Browse Source

Fixed gyro scale calibration (it was storing scale even though the scale calibration was skipped

sbg
Julian Oes 12 years ago committed by Anton Babushkin
parent
commit
53def47216
  1. 8
      src/modules/commander/gyro_calibration.cpp

8
src/modules/commander/gyro_calibration.cpp

@ -177,8 +177,12 @@ void do_gyro_calibration(int mavlink_fd)
/* abort this loop if not rotated more than 180 degrees within 5 seconds */ /* abort this loop if not rotated more than 180 degrees within 5 seconds */
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) && (hrt_absolute_time() - start_time > 5 * 1e6)) {
break; mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
return;
}
/* wait blocking for new data */ /* wait blocking for new data */
struct pollfd fds[1]; struct pollfd fds[1];

Loading…
Cancel
Save