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,
-        0,
-        MAV_STATE_ACTIVE);
+        base_mode,
+        custom_mode,
+        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:
@@ -875,6 +903,31 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 }
             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:
             {
                 if (packet.param1 == 1 || packet.param1 == 3) {

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,6 +172,8 @@ static void update_tracking(void)
         current_loc.options = 0; // Absolute altitude
     }
 
+    if (control_mode == AUTO)
+    {
         // calculate the bearing to the vehicle
         float bearing  = get_bearing_cd(current_loc, vehicle.location) * 0.01f;
         float distance = get_distance(current_loc, vehicle.location);
@@ -189,7 +191,7 @@ static void update_tracking(void)
         nav_status.distance = distance;
     }
 
-
+}
 /**
    handle an updated position from the aircraft
  */