AntennaTracker: add support for MANUAL and AUTO modes

Tools/AntennaTracker/AntennaTracker.pde

@@ -192,6 +192,10 @@ static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
 ////////////////////////////////////////////////////////////////////////////////
 static struct   Location current_loc;
 
+// This is the state of the antenna control system
+// There are multiple states defined such as MANUAL, FBW-A, AUTO
+static enum ControlMode control_mode  = INITIALISING;
+
 /*
   scheduler table - all regular tasks apart from the fast_loop()
   should be listed here, along with how often they should be called

Tools/AntennaTracker/GCS_Mavlink.pde

@@ -24,13 +24,43 @@ static bool mavlink_active;
 
 static NOINLINE void send_heartbeat(mavlink_channel_t chan)
 {
+    uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+    uint8_t system_status = MAV_STATE_ACTIVE;
+    uint32_t custom_mode = control_mode;
+
+    // work out the base_mode. This value is not very useful
+    // for APM, but we calculate it as best we can so a generic
+    // MAVLink enabled ground station can work out something about
+    // what the MAV is up to. The actual bit values are highly
+    // ambiguous for most of the APM flight modes. In practice, you
+    // only get useful information from the custom_mode, which maps to
+    // the APM flight mode and has a well defined meaning in the
+    // ArduPlane documentation
+    switch (control_mode) {
+    case MANUAL:
+        base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+        break;
+
+    case AUTO:
+        base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
+                    MAV_MODE_FLAG_STABILIZE_ENABLED;
+        // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
+        // APM does in any mode, as that is defined as "system finds its own goal
+        // positions", which APM does not currently do
+        break;
+
+    case INITIALISING:
+        system_status = MAV_STATE_CALIBRATING;
+        break;
+    }
+
     mavlink_msg_heartbeat_send(
         chan,
         MAV_TYPE_ANTENNA_TRACKER,
         MAV_AUTOPILOT_ARDUPILOTMEGA,
-        MAV_MODE_FLAG_AUTO_ENABLED,
+        base_mode,
-        0,
+        custom_mode,
-        MAV_STATE_ACTIVE);
+        system_status);
 }
 
 static NOINLINE void send_attitude(mavlink_channel_t chan)
@@ -631,8 +661,6 @@ GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
 
 void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 {
-//    hal.uartA->printf("handleMessage %d\n", msg->msgid);
-
     switch (msg->msgid) {
 
     case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@@ -859,21 +887,46 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 break;
             }
 
-        case MAV_CMD_COMPONENT_ARM_DISARM:
+            case MAV_CMD_COMPONENT_ARM_DISARM:
-            if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
+                if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
-                if (packet.param1 == 1.0f) {
+                    if (packet.param1 == 1.0f) {
-                    arm_servos();
+                        arm_servos();
-                    result = MAV_RESULT_ACCEPTED;
+                        result = MAV_RESULT_ACCEPTED;
-                } else if (packet.param1 == 0.0f)  {
+                    } else if (packet.param1 == 0.0f)  {
-                    disarm_servos();
+                        disarm_servos();
-                    result = MAV_RESULT_ACCEPTED;
+                        result = MAV_RESULT_ACCEPTED;
+                    } else {
+                        result = MAV_RESULT_UNSUPPORTED;
+                    }
                 } else {
                     result = MAV_RESULT_UNSUPPORTED;
                 }
-            } else {
-                result = MAV_RESULT_UNSUPPORTED;
-            }
             break;
+            
+            case MAV_CMD_DO_SET_MODE:
+                switch ((uint16_t)packet.param1) {
+                    case MAV_MODE_MANUAL_ARMED:
+                    case MAV_MODE_MANUAL_DISARMED:
+                        set_mode(MANUAL);
+                        result = MAV_RESULT_ACCEPTED;
+                        break;
+
+                    case MAV_MODE_AUTO_ARMED:
+                    case MAV_MODE_AUTO_DISARMED:
+                        set_mode(AUTO);
+                        result = MAV_RESULT_ACCEPTED;
+                        break;
+
+                    default:
+                        result = MAV_RESULT_UNSUPPORTED;
+                }
+                break;
+
+                // mavproxy/mavutil sends this when auto command is entered 
+            case MAV_CMD_MISSION_START:
+                set_mode(AUTO);
+                result = MAV_RESULT_ACCEPTED;
+                break;
 
             case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
             {

Tools/AntennaTracker/defines.h

@@ -50,5 +50,14 @@
 #define MASK_OPTIONS_LOITER_DIRECTION   (1<<2)          // 0 = CW
                                                         // 1 = CCW
 
+// Controller modes
+// ----------------
+
+enum ControlMode {
+    MANUAL=0,
+    AUTO=10,
+    INITIALISING=16
+};
+
 #endif // _DEFINES_H
 

Tools/AntennaTracker/system.pde

@@ -79,6 +79,8 @@ static void init_tracker()
 
     gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
     hal.scheduler->delay(1000); // Why????
+
+    set_mode(AUTO); // tracking
 }
 
 // Level the tracker by calibrating the INS
@@ -203,3 +205,13 @@ static void disarm_servos()
     channel_yaw.disable_out();
     channel_pitch.disable_out();
 }
+
+static void set_mode(enum ControlMode mode)
+{
+    if(control_mode == mode) {
+        // don't switch modes if we are already in the correct mode.
+        return;
+    }
+    control_mode = mode;
+}
+

Tools/AntennaTracker/tracking.pde

@@ -172,24 +172,26 @@ static void update_tracking(void)
         current_loc.options = 0; // Absolute altitude
     }
 
-    // calculate the bearing to the vehicle
+    if (control_mode == AUTO)
-    float bearing  = get_bearing_cd(current_loc, vehicle.location) * 0.01f;
+    {
-    float distance = get_distance(current_loc, vehicle.location);
+        // calculate the bearing to the vehicle
-    int32_t alt_diff_cm = current_loc.options & MASK_OPTIONS_RELATIVE_ALT  // Do we know our absolute altitude?
+        float bearing  = get_bearing_cd(current_loc, vehicle.location) * 0.01f;
-        ? (vehicle.relative_alt - current_loc.alt) 
+        float distance = get_distance(current_loc, vehicle.location);
-        : (vehicle.location.alt - current_loc.alt); // cm
+        int32_t alt_diff_cm = current_loc.options & MASK_OPTIONS_RELATIVE_ALT  // Do we know our absolute altitude?
-    float pitch    = degrees(atan2(alt_diff_cm/100, distance));
+            ? (vehicle.relative_alt - current_loc.alt) 
-    // update the servos
+            : (vehicle.location.alt - current_loc.alt); // cm
-    update_pitch_servo(pitch);
+        float pitch    = degrees(atan2(alt_diff_cm/100, distance));
-    update_yaw_servo(bearing);
+        // update the servos
+        update_pitch_servo(pitch);
+        update_yaw_servo(bearing);
+        
+        // update nav_status for NAV_CONTROLLER_OUTPUT
+        nav_status.bearing  = bearing;
+        nav_status.pitch    = pitch;
+        nav_status.distance = distance;
+    }
 
-    // update nav_status for NAV_CONTROLLER_OUTPUT
-    nav_status.bearing  = bearing;
-    nav_status.pitch    = pitch;
-    nav_status.distance = distance;
 }
-
-
 /**
    handle an updated position from the aircraft
  */