From 1d2d8c903aee7a5e9a29a023d87dffcd60d35b93 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Nov 2014 14:47:09 +1100 Subject: [PATCH] Rover: make preflight calibration follow plane conventions --- APMrover2/GCS_Mavlink.pde | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 5210381614..bc2e9e90ee 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -829,15 +829,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_PREFLIGHT_CALIBRATION: - if (packet.param1 == 1 || - packet.param2 == 1 || - packet.param3 == 1) { + if ((packet.param1 == 1 || + packet.param2 == 1) && + packet.param3 == 0) { startup_INS_ground(true); - } - if (packet.param4 == 1) { + result = MAV_RESULT_ACCEPTED; + } else if (packet.param4 == 1) { trim_radio(); + result = MAV_RESULT_ACCEPTED; + } else { + send_text_P(SEVERITY_LOW, PSTR("Unsupported preflight calibration")); } - result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: