-#if HAS_GYRO
- if (r == 0) {
- ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2;
- ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2;
- ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2;
+#if HAS_IMU
+ if (auto_cal) {
+ ao_config.accel_zero_along = (int16_t) ((accel_along_up + accel_along_down) / 2);
+ ao_config.accel_zero_across = (int16_t) ((accel_across_up + accel_across_down) / 2);
+ ao_config.accel_zero_through = (int16_t) ((accel_through_up + accel_through_down) / 2);
+ } else {
+ int16_t v;
+
+ v = (int16_t) ao_cmd_decimal();
+ if (ao_cmd_status == ao_cmd_success) {
+ ao_config.accel_zero_along = v;
+ v = (int16_t) ao_cmd_decimal();
+ if (ao_cmd_status == ao_cmd_success) {
+ ao_config.accel_zero_across = v;
+ v = (int16_t) ao_cmd_decimal();
+ if (ao_cmd_status == ao_cmd_success)
+ ao_config.accel_zero_through = v;
+ }
+ }