diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8e24b1d2478b15cce832ce20e629c25c32ae99b4..44be364e77516d0e20bf259f077cad8c59aadd53 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1100,6 +1100,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // this blocks ins.calibrate_accel(mavlink_delay, flash_leds, gcs_send_text_fmt, setup_wait_key, trim_roll, trim_pitch); // reset ahrs's trim to suggested values from calibration routine + trim_roll = constrain(trim_roll, ToRad(-10.0f), ToRad(10.0f)); + trim_pitch = constrain(trim_pitch, ToRad(-10.0f), ToRad(10.0f)); ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } result = MAV_RESULT_ACCEPTED; diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index e1c414fe176987228582ba28ea0ef1691ff568f5..c8c844fe976811373bdf0a1a6c4be69b03fa1bcf 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -292,6 +292,8 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv) delay, flash_leds, &timer_scheduler); ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key, trim_roll, trim_pitch); // reset ahrs's trim to suggested values from calibration routine + trim_roll = constrain(trim_roll, ToRad(-10.0f), ToRad(10.0f)); + trim_pitch = constrain(trim_pitch, ToRad(-10.0f), ToRad(10.0f)); ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); report_ins(); return(0);