|
|
|
@ -101,18 +101,10 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
@@ -101,18 +101,10 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
|
|
|
|
|
{ |
|
|
|
|
Vector3f trim = _trim.get(); |
|
|
|
|
|
|
|
|
|
// debug -- remove me!
|
|
|
|
|
hal.console->printf_P(PSTR("\nadd_trim before R:%4.2f P:%4.2f\n"), |
|
|
|
|
ToDeg(trim.x),ToDeg(trim.y)); |
|
|
|
|
|
|
|
|
|
// add new trim
|
|
|
|
|
trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0)); |
|
|
|
|
trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0), ToRad(10.0)); |
|
|
|
|
|
|
|
|
|
// set and save new trim values
|
|
|
|
|
_trim.set_and_save(trim); |
|
|
|
|
|
|
|
|
|
// debug -- remove me!
|
|
|
|
|
hal.console->printf_P(PSTR("add_trim after R:%4.2f P:%4.2f\n"), |
|
|
|
|
ToDeg(trim.x),ToDeg(trim.y)); |
|
|
|
|
} |
|
|
|
|