|
|
|
@ -601,17 +601,24 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -601,17 +601,24 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
} |
|
|
|
|
if (is_equal(packet.param4,1.0f)) { |
|
|
|
|
// Cant trim radio |
|
|
|
|
} |
|
|
|
|
#if !defined( __AVR_ATmega1280__ ) |
|
|
|
|
else if (is_equal(packet.param5,1.0f)) { |
|
|
|
|
} else if (is_equal(packet.param5,1.0f)) { |
|
|
|
|
float trim_roll, trim_pitch; |
|
|
|
|
AP_InertialSensor_UserInteract_MAVLink interact(this); |
|
|
|
|
if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { |
|
|
|
|
// reset ahrs's trim to suggested values from calibration routine |
|
|
|
|
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); |
|
|
|
|
} |
|
|
|
|
} else if (is_equal(packet.param5,2.0f)) { |
|
|
|
|
// accel trim |
|
|
|
|
float trim_roll, trim_pitch; |
|
|
|
|
if(ins.calibrate_trim(trim_roll, trim_pitch)) { |
|
|
|
|
// reset ahrs's trim to suggested values from calibration routine |
|
|
|
|
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); |
|
|
|
|
result = MAV_RESULT_ACCEPTED; |
|
|
|
|
} else { |
|
|
|
|
result = MAV_RESULT_FAILED; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
result = MAV_RESULT_ACCEPTED; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|