From cee7ce057154b8ade2c5750006b7411fb0f07f69 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 17 Jul 2017 11:04:43 +1000 Subject: [PATCH] Rover: move handling of PREFLIGHT_SET_SENSOR_OFFSETS up --- APMrover2/GCS_Mavlink.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 8d2f47f5db..324ae14741 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -920,23 +920,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) } break; - case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: - { - uint8_t compassNumber = -1; - if (is_equal(packet.param1, 2.0f)) { - compassNumber = 0; - } else if (is_equal(packet.param1, 5.0f)) { - compassNumber = 1; - } else if (is_equal(packet.param1, 6.0f)) { - compassNumber = 2; - } - if (compassNumber != (uint8_t) -1) { - rover.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); - result = MAV_RESULT_ACCEPTED; - } - break; - } - case MAV_CMD_DO_SET_MODE: switch (static_cast(packet.param1)) { case MAV_MODE_MANUAL_ARMED: