Tracker: factor out preflight calibration function

AntennaTracker/GCS_Mavlink.cpp

@@ -446,84 +446,88 @@ uint8_t GCS_MAVLINK_Tracker::sysid_my_gcs() const
     return tracker.g.sysid_my_gcs;
 }
 
-void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
+MAV_RESULT GCS_MAVLINK_Tracker::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
 {
-    switch (msg->msgid) {
-
-    // If we are currently operating as a proxy for a remote, 
-    // alas we have to look inside each packet to see if it's for us or for the remote
-    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
-    {
-        handle_request_data_stream(msg, false);
-        break;
-    }
-
-
-    case MAVLINK_MSG_ID_HEARTBEAT:
-        break;
-
-    case MAVLINK_MSG_ID_COMMAND_LONG:
-    {
-        // decode
-        mavlink_command_long_t packet;
-        mavlink_msg_command_long_decode(msg, &packet);
-        
-        MAV_RESULT result = MAV_RESULT_UNSUPPORTED;
-        
-        // do command
-        send_text(MAV_SEVERITY_INFO,"Command received: ");
-        
-        switch(packet.command) {
-            
-            case MAV_CMD_PREFLIGHT_CALIBRATION:
-            {
     if (is_equal(packet.param1,1.0f)) {
         tracker.ins.init_gyro();
-                    if (tracker.ins.gyro_calibrated_ok_all()) {
-                        tracker.ahrs.reset_gyro_drift();
-                        result = MAV_RESULT_ACCEPTED;
-                    } else {
-                        result = MAV_RESULT_FAILED;
+        if (!tracker.ins.gyro_calibrated_ok_all()) {
+            return MAV_RESULT_FAILED;
         }
+        tracker.ahrs.reset_gyro_drift();
+        return MAV_RESULT_ACCEPTED;
     }
+
     if (is_equal(packet.param3,1.0f)) {
         tracker.init_barometer(false);
         // zero the altitude difference on next baro update
         tracker.nav_status.need_altitude_calibration = true;
-                    result = MAV_RESULT_ACCEPTED;
+        return MAV_RESULT_ACCEPTED;
     }
     if (is_equal(packet.param4,1.0f)) {
         // Can't trim radio
-                    result = MAV_RESULT_UNSUPPORTED;
-                } else if (is_equal(packet.param5,1.0f)) {
-                    result = MAV_RESULT_ACCEPTED;
+        return MAV_RESULT_UNSUPPORTED;
+    }
+
+    if (is_equal(packet.param5,1.0f)) {
         // start with gyro calibration
         tracker.ins.init_gyro();
         // reset ahrs gyro bias
-                    if (tracker.ins.gyro_calibrated_ok_all()) {
-                        tracker.ahrs.reset_gyro_drift();
-                    } else {
-                        result = MAV_RESULT_FAILED;
+        if (!tracker.ins.gyro_calibrated_ok_all()) {
+            return MAV_RESULT_FAILED;
         }
+        tracker.ahrs.reset_gyro_drift();
         // start accel cal
         tracker.ins.acal_init();
         tracker.ins.get_acal()->start(this);
-                } else if (is_equal(packet.param5,2.0f)) {
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    if (is_equal(packet.param5,2.0f)) {
         // start with gyro calibration
         tracker.ins.init_gyro();
         // accel trim
         float trim_roll, trim_pitch;
-                    if (tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
+        if (!tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
+            return MAV_RESULT_FAILED;
+        }
         // reset ahrs's trim to suggested values from calibration routine
         tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
-                        result = MAV_RESULT_ACCEPTED;
-                    } else {
-                        result = MAV_RESULT_FAILED;
-                    }
+        return MAV_RESULT_ACCEPTED;
     }
+
+    return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
+}
+
+
+void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
+{
+    switch (msg->msgid) {
+
+    // If we are currently operating as a proxy for a remote, 
+    // alas we have to look inside each packet to see if it's for us or for the remote
+    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+    {
+        handle_request_data_stream(msg, false);
         break;
     }
 
+
+    case MAVLINK_MSG_ID_HEARTBEAT:
+        break;
+
+    case MAVLINK_MSG_ID_COMMAND_LONG:
+    {
+        // decode
+        mavlink_command_long_t packet;
+        mavlink_msg_command_long_decode(msg, &packet);
+        
+        MAV_RESULT result = MAV_RESULT_UNSUPPORTED;
+        
+        // do command
+        send_text(MAV_SEVERITY_INFO,"Command received: ");
+        
+        switch(packet.command) {
+
             case MAV_CMD_COMPONENT_ARM_DISARM:
                 if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
                     if (is_equal(packet.param1,1.0f)) {

AntennaTracker/GCS_Mavlink.h

@@ -28,6 +28,8 @@ protected:
 
     bool set_mode(uint8_t mode) override;
 
+    MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
+
 private:
 
     void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override;