diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index dd6d5e3a9d..488f662e11 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -595,7 +595,8 @@ int do_level_calibration(int mavlink_fd) { param_get(pitch_offset_handle, &pitch_offset_current); param_get(board_rot_handle, &board_rot_current); - if (board_rot_current == 0) { + // give attitude some time to settle if there have been changes to the board rotation parameters + if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) { settle_time = 0; }