From ad1c379445d2c3de71949f6905bf19039e69445c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 17 Jul 2017 11:04:51 +1000 Subject: [PATCH] Copter: move handling of PREFLIGHT_SET_SENSOR_OFFSETS up --- ArduCopter/GCS_Mavlink.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 45a5386b74..d9e8eb4a89 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1214,23 +1214,6 @@ void GCS_MAVLINK_Copter::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) { - copter.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); - result = MAV_RESULT_ACCEPTED; - } - break; - } - case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1,1.0f)) { // attempt to arm and return success or failure