Tracker: create a Mode hierarchy to handle different control modes

AntennaTracker/GCS_Mavlink.cpp

@@ -30,17 +30,17 @@ MAV_MODE GCS_MAVLINK_Tracker::base_mode() const
     // 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 (tracker.control_mode) {
-    case MANUAL:
+    switch (tracker.mode->number()) {
+    case Mode::Number::MANUAL:
         _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
         break;
 
-    case STOP:
+    case Mode::Number::STOP:
         break;
 
-    case SCAN:
-    case SERVO_TEST:
-    case AUTO:
+    case Mode::Number::SCAN:
+    case Mode::Number::SERVOTEST:
+    case Mode::Number::AUTO:
         _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
             MAV_MODE_FLAG_STABILIZE_ENABLED;
         // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
@@ -48,13 +48,13 @@ MAV_MODE GCS_MAVLINK_Tracker::base_mode() const
         // positions", which APM does not currently do
         break;
 
-    case INITIALISING:
+    case Mode::Number::INITIALISING:
         break;
     }
 
     // we are armed if safety switch is not disarmed
     if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED &&
-        tracker.control_mode != INITIALISING &&
+        tracker.mode != &tracker.mode_initialising &&
         hal.util->get_soft_armed()) {
         _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
     }
@@ -64,12 +64,12 @@ MAV_MODE GCS_MAVLINK_Tracker::base_mode() const
 
 uint32_t GCS_Tracker::custom_mode() const
 {
-    return tracker.control_mode;
+    return (uint32_t)tracker.mode->number();
 }
 
 MAV_STATE GCS_MAVLINK_Tracker::system_status() const
 {
-    if (tracker.control_mode == INITIALISING) {
+    if (tracker.mode == &tracker.mode_initialising) {
         return MAV_STATE_CALIBRATING;
     }
     return MAV_STATE_ACTIVE;
@@ -399,14 +399,17 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_long_packet(const mavlink_command
         return MAV_RESULT_UNSUPPORTED;
 
     case MAV_CMD_DO_SET_SERVO:
-        if (!tracker.servo_test_set_servo(packet.param1, packet.param2)) {
+        // ensure we are in servo test mode
+        tracker.set_mode(tracker.mode_servotest, ModeReason::SERVOTEST);
+
+        if (!tracker.mode_servotest.set_servo(packet.param1, packet.param2)) {
             return MAV_RESULT_FAILED;
         }
         return MAV_RESULT_ACCEPTED;
 
         // mavproxy/mavutil sends this when auto command is entered 
     case MAV_CMD_MISSION_START:
-        tracker.set_mode(AUTO, ModeReason::GCS_COMMAND);
+        tracker.set_mode(tracker.mode_auto, ModeReason::GCS_COMMAND);
         return MAV_RESULT_ACCEPTED;
 
     default:

AntennaTracker/Log.cpp

@@ -78,7 +78,7 @@ const struct LogStructure Tracker::log_structure[] = {
 
 void Tracker::Log_Write_Vehicle_Startup_Messages()
 {
-    logger.Write_Mode(control_mode, ModeReason::INITIALISED);
+    logger.Write_Mode((uint8_t)mode->number(), ModeReason::INITIALISED);
     gps.Write_AP_Logger_Log_Startup_messages();
 }
 

AntennaTracker/Tracker.cpp

@@ -130,6 +130,32 @@ void Tracker::ten_hz_logging_loop()
     }
 }
 
+Mode *Tracker::mode_from_mode_num(const Mode::Number num)
+{
+    Mode *ret = nullptr;
+    switch (num) {
+    case Mode::Number::MANUAL:
+        ret = &mode_manual;
+        break;
+    case Mode::Number::STOP:
+        ret = &mode_stop;
+        break;
+    case Mode::Number::SCAN:
+        ret = &mode_scan;
+        break;
+    case Mode::Number::SERVOTEST:
+        ret = &mode_servotest;
+        break;
+    case Mode::Number::AUTO:
+        ret = &mode_auto;
+        break;
+    case Mode::Number::INITIALISING:
+        ret = &mode_initialising;
+        break;
+    }
+    return ret;
+}
+
 /*
   update AP_Stats
 */

AntennaTracker/Tracker.h

@@ -75,11 +75,15 @@
 #include <SITL/SITL.h>
 #endif
 
+#include "mode.h"
+
 class Tracker : public AP_Vehicle {
 public:
     friend class GCS_MAVLINK_Tracker;
     friend class GCS_Tracker;
     friend class Parameters;
+    friend class ModeAuto;
+    friend class Mode;
 
     Tracker(void);
 
@@ -135,7 +139,16 @@ private:
                            nullptr};
     struct Location current_loc;
 
-    enum ControlMode control_mode  = INITIALISING;
+    Mode *mode_from_mode_num(enum Mode::Number num);
+
+    Mode *mode = &mode_initialising;
+
+    ModeAuto mode_auto;
+    ModeInitialising mode_initialising;
+    ModeManual mode_manual;
+    ModeScan mode_scan;
+    ModeServoTest mode_servotest;
+    ModeStop mode_stop;
 
 #ifdef ENABLE_SCRIPTING
     AP_Scripting scripting;
@@ -153,7 +166,7 @@ private:
     } vehicle;
 
     // Navigation controller state
-    struct {
+    struct NavStatus {
         float bearing;                  // bearing to vehicle in centi-degrees
         float distance;                 // distance to vehicle in meters
         float pitch;                    // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below)
@@ -167,7 +180,7 @@ private:
         bool need_altitude_calibration  : 1;// true if tracker altitude has not been determined (true after startup)
         bool scan_reverse_pitch         : 1;// controls direction of pitch movement in SCAN mode
         bool scan_reverse_yaw           : 1;// controls direction of yaw movement in SCAN mode
-    } nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
+    } nav_status;
 
     // setup the var_info table
     AP_Param param_loader{var_info};
@@ -187,22 +200,6 @@ private:
     void ten_hz_logging_loop();
     void stats_update();
 
-    // control_auto.cpp
-    void update_auto(void);
-    void calc_angle_error(float pitch, float yaw, bool direction_reversed);
-    void convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
-    bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
-    bool get_ef_yaw_direction();
-
-    // control_manual.cpp
-    void update_manual(void);
-
-    // control_scan.cpp
-    void update_scan(void);
-
-    // control_servo_test.cpp
-    bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm);
-
     // GCS_Mavlink.cpp
     void send_nav_controller_output(mavlink_channel_t chan);
 
@@ -246,8 +243,8 @@ private:
     void arm_servos();
     void disarm_servos();
     void prepare_servos();
-    void set_mode(enum ControlMode mode, ModeReason reason);
-    bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
+    void set_mode(Mode &newmode, ModeReason reason);
+    bool set_mode(uint8_t new_mode, ModeReason reason) override;
     bool should_log(uint32_t mask);
     bool start_command_callback(const AP_Mission::Mission_Command& cmd) { return false; }
     void exit_mission_callback() { return; }

AntennaTracker/control_auto.cpp

@@ -6,10 +6,14 @@
 
 /*
  * update_auto - runs the auto controller
- *  called at 50hz while control_mode is 'AUTO'
+ *  called at 50hz while control mode is 'AUTO'
  */
-void Tracker::update_auto(void)
+void Mode::update_auto(void)
 {
+    struct Tracker::NavStatus &nav_status = tracker.nav_status;
+
+    Parameters &g = tracker.g;
+
     float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
     float pitch = constrain_float(nav_status.pitch+g.pitch_trim, g.pitch_min, g.pitch_max) * 100; // target pitch in centidegrees
 
@@ -22,17 +26,18 @@ void Tracker::update_auto(void)
     convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw);
 
     // only move servos if target is at least distance_min away if we  have a target
-    if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !vehicle.location_valid) {
-        update_pitch_servo(bf_pitch);
-        update_yaw_servo(bf_yaw);
+    if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !tracker.vehicle.location_valid) {
+        tracker.update_pitch_servo(bf_pitch);
+        tracker.update_yaw_servo(bf_yaw);
     }
 }
 
-void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
+void Mode::calc_angle_error(float pitch, float yaw, bool direction_reversed)
 {
     // Pitch angle error in centidegrees
     // Positive error means the target is above current pitch
     // Negative error means the target is below current pitch
+    const AP_AHRS &ahrs = AP::ahrs();
     float ahrs_pitch = ahrs.pitch_sensor;
     int32_t ef_pitch_angle_error = pitch - ahrs_pitch;
 
@@ -53,25 +58,29 @@ void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
     float bf_pitch_err;
     float bf_yaw_err;
     convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
+    struct Tracker::NavStatus &nav_status = tracker.nav_status;
     nav_status.angle_error_pitch = bf_pitch_err;
     nav_status.angle_error_yaw = bf_yaw_err;
 
     // set actual and desired for logging, note we are using angles not rates
+    Parameters &g = tracker.g;
     g.pidPitch2Srv.set_target_rate(pitch * 0.01);
     g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01);
     g.pidYaw2Srv.set_target_rate(yaw * 0.01);
     g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01);
 }
 
-void Tracker::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
+void Mode::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
 {
 	// earth frame to body frame pitch and yaw conversion
+    const AP_AHRS &ahrs = AP::ahrs();
     bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
     bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
 }
 
-bool Tracker::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
+bool Mode::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
 {
+    const AP_AHRS &ahrs = AP::ahrs();
     // avoid divide by zero
     if (is_zero(ahrs.cos_pitch())) {
         return false;
@@ -83,13 +92,14 @@ bool Tracker::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& e
 }
 
 // return value is true if taking the long road to the target, false if normal, shortest direction should be used
-bool Tracker::get_ef_yaw_direction()
+bool Mode::get_ef_yaw_direction()
 {
     // calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
-    float yaw_angle_limit_lower =   (-g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
-    float yaw_angle_limit_upper =   (g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
-    float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - pitch_servo_out_filt.get();
-    float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - pitch_servo_out_filt.get();
+    Parameters &g = tracker.g;
+    float yaw_angle_limit_lower =   (-g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
+    float yaw_angle_limit_upper =   (g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
+    float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - tracker.pitch_servo_out_filt.get();
+    float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - tracker.pitch_servo_out_filt.get();
 
     // distances to earthframe angle limits in centi-degrees
     float ef_yaw_limit_lower = yaw_angle_limit_lower;
@@ -99,7 +109,9 @@ bool Tracker::get_ef_yaw_direction()
     convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);
     convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);
 
+    const AP_AHRS &ahrs = AP::ahrs();
     float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
+    struct Tracker::NavStatus &nav_status = tracker.nav_status;
     float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);
     float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);
 

AntennaTracker/control_manual.cpp

@@ -6,9 +6,9 @@
 
 /*
  * update_manual - runs the manual controller
- *  called at 50hz while control_mode is 'MANUAL'
+ *  called at 50hz while control mode is 'MANUAL'
  */
-void Tracker::update_manual(void)
+void ModeManual::update()
 {
     // copy yaw and pitch input to output
     SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, RC_Channels::rc_channel(CH_YAW)->get_radio_in());

AntennaTracker/control_scan.cpp

@@ -6,10 +6,14 @@
 
 /*
  * update_scan - runs the scan controller
- *  called at 50hz while control_mode is 'SCAN'
+ *  called at 50hz while control mode is 'SCAN'
  */
-void Tracker::update_scan(void)
+void Mode::update_scan(void)
 {
+    struct Tracker::NavStatus &nav_status = tracker.nav_status;
+
+    Parameters &g = tracker.g;
+
     if (!nav_status.manual_control_yaw) {
         float yaw_delta = g.scan_speed_yaw * 0.02f;
         nav_status.bearing   += yaw_delta   * (nav_status.scan_reverse_yaw?-1:1);

AntennaTracker/control_servo_test.cpp

@@ -5,10 +5,10 @@
  */
 
 /*
- * servo_test_set_servo - sets the yaw or pitch servo pwm directly
+ * set_servo - sets the yaw or pitch servo pwm directly
  *  servo_num are 1 for yaw, 2 for pitch
  */
-bool Tracker::servo_test_set_servo(uint8_t servo_num, uint16_t pwm)
+bool ModeServoTest::set_servo(uint8_t servo_num, uint16_t pwm)
 {
     // convert servo_num from 1~2 to 0~1 range
     servo_num--;
@@ -18,11 +18,6 @@ bool Tracker::servo_test_set_servo(uint8_t servo_num, uint16_t pwm)
         return false;
     }
 
-    // ensure we are in servo test mode
-    if (control_mode != SERVO_TEST) {
-        set_mode(SERVO_TEST, ModeReason::SERVOTEST);
-    }
-
     // set yaw servo pwm and send output to servo
     if (servo_num == CH_YAW) {
         SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, pwm);

AntennaTracker/defines.h

@@ -3,15 +3,6 @@
 // Controller modes
 // ----------------
 
-enum ControlMode {
-    MANUAL=0,
-    STOP=1,
-    SCAN=2,
-    SERVO_TEST=3,
-    AUTO=10,
-    INITIALISING=16
-};
-
 enum ServoType {
     SERVO_TYPE_POSITION=0,
     SERVO_TYPE_ONOFF=1,

AntennaTracker/mode.cpp

@@ -0,0 +1,3 @@
+#include "mode.h"
+
+#include "Tracker.h"

AntennaTracker/mode.h

@@ -0,0 +1,82 @@
+#pragma once
+
+#include <stdint.h>
+
+class Mode {
+public:
+    enum class Number {
+        MANUAL=0,
+        STOP=1,
+        SCAN=2,
+        SERVOTEST=3,
+        AUTO=10,
+        INITIALISING=16
+    };
+
+    Mode() {}
+
+    // do not allow copying
+    Mode(const Mode &other) = delete;
+    Mode &operator=(const Mode&) = delete;
+
+    // returns a unique number specific to this mode
+    virtual Mode::Number number() const = 0;
+
+    virtual bool requires_armed_servos() const = 0;
+
+    virtual void update() = 0;
+
+protected:
+    void update_scan();
+    void update_auto();
+
+    bool get_ef_yaw_direction();
+
+    void calc_angle_error(float pitch, float yaw, bool direction_reversed);
+    void convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
+    bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
+};
+
+class ModeAuto : public Mode {
+public:
+    Mode::Number number() const override { return Mode::Number::AUTO; }
+    bool requires_armed_servos() const override { return true; }
+    void update() override;
+};
+
+class ModeInitialising : public Mode {
+public:
+    Mode::Number number() const override { return Mode::Number::INITIALISING; }
+    bool requires_armed_servos() const override { return false; }
+    void update() override {};
+};
+
+class ModeManual : public Mode {
+public:
+    Mode::Number number() const override { return Mode::Number::MANUAL; }
+    bool requires_armed_servos() const override { return true; }
+    void update() override;
+};
+
+class ModeScan : public Mode {
+public:
+    Mode::Number number() const override { return Mode::Number::SCAN; }
+    bool requires_armed_servos() const override { return true; }
+    void update() override;
+};
+
+class ModeServoTest : public Mode {
+public:
+    Mode::Number number() const override { return Mode::Number::SERVOTEST; }
+    bool requires_armed_servos() const override { return true; }
+    void update() override {};
+
+    bool set_servo(uint8_t servo_num, uint16_t pwm);
+};
+
+class ModeStop : public Mode {
+public:
+    Mode::Number number() const override { return Mode::Number::STOP; }
+    bool requires_armed_servos() const override { return false; }
+    void update() override {};
+};

AntennaTracker/mode_auto.cpp

@@ -0,0 +1,12 @@
+#include "mode.h"
+
+#include "Tracker.h"
+
+void ModeAuto::update()
+{
+    if (tracker.vehicle.location_valid) {
+        update_auto();
+    } else if (tracker.target_set) {
+        update_scan();
+    }
+}

AntennaTracker/mode_scan.cpp

@@ -0,0 +1,6 @@
+#include "mode.h"
+
+void ModeScan::update()
+{
+    update_scan();
+}

AntennaTracker/system.cpp

@@ -108,24 +108,11 @@ void Tracker::init_tracker()
     gcs().send_text(MAV_SEVERITY_INFO,"Ready to track");
     hal.scheduler->delay(1000); // Why????
 
-    switch (g.initial_mode) {
-    case MANUAL:
-        set_mode(MANUAL, ModeReason::STARTUP);
-        break;
-
-    case SCAN:
-        set_mode(SCAN, ModeReason::STARTUP);
-        break;
-
-    case STOP:
-        set_mode(STOP, ModeReason::STARTUP);
-        break;
-
-    case AUTO:
-    default:
-        set_mode(AUTO, ModeReason::STARTUP);
-        break;
+    Mode *newmode = mode_from_mode_num((Mode::Number)g.initial_mode.get());
+    if (newmode == nullptr) {
+        newmode = &mode_manual;
     }
+    set_mode(*newmode, ModeReason::STARTUP);
 
     if (g.startup_delay > 0) {
         // arm servos with trim value to allow them to start up (required
@@ -214,47 +201,54 @@ void Tracker::prepare_servos()
     SRV_Channels::output_ch_all();
 }
 
-void Tracker::set_mode(enum ControlMode mode, ModeReason reason)
+void Tracker::set_mode(Mode &newmode, const ModeReason reason)
 {
-    if (control_mode == mode) {
+    if (mode == &newmode) {
         // don't switch modes if we are already in the correct mode.
         return;
     }
-    control_mode = mode;
+    mode = &newmode;
 
-	switch (control_mode) {
-    case AUTO:
-    case MANUAL:
-    case SCAN:
-    case SERVO_TEST:
+    if (mode->requires_armed_servos()) {
         arm_servos();
-        break;
-
-    case STOP:
-    case INITIALISING:
+    } else {
         disarm_servos();
-        break;
     }
 
 	// log mode change
-	logger.Write_Mode(control_mode, reason);
-  gcs().send_message(MSG_HEARTBEAT);
+	logger.Write_Mode((uint8_t)mode->number(), reason);
+    gcs().send_message(MSG_HEARTBEAT);
 
     nav_status.bearing = ahrs.yaw_sensor * 0.01f;
 }
 
 bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)
 {
-    switch (new_mode) {
-    case AUTO:
-    case MANUAL:
-    case SCAN:
-    case SERVO_TEST:
-    case STOP:
-        set_mode((enum ControlMode)new_mode, reason);
-        return true;
+    Mode *fred = nullptr;
+    switch ((Mode::Number)new_mode) {
+    case Mode::Number::INITIALISING:
+        return false;
+    case Mode::Number::AUTO:
+        fred = &mode_auto;
+        break;
+    case Mode::Number::MANUAL:
+        fred = &mode_manual;
+        break;
+    case Mode::Number::SCAN:
+        fred = &mode_scan;
+        break;
+    case Mode::Number::SERVOTEST:
+        fred = &mode_servotest;
+        break;
+    case Mode::Number::STOP:
+        fred = &mode_stop;
+        break;
     }
-    return false;
+    if (fred == nullptr) {
+        return false;
+    }
+    set_mode(*fred, reason);
+    return true;
 }
 
 /*

AntennaTracker/tracking.cpp

@@ -53,7 +53,7 @@ void Tracker::update_bearing_and_distance()
 
     // calculate bearing to vehicle
     // To-Do: remove need for check of control_mode
-    if (control_mode != SCAN && !nav_status.manual_control_yaw) {
+    if (mode != &mode_scan && !nav_status.manual_control_yaw) {
         nav_status.bearing  = current_loc.get_bearing_to(vehicle.location_estimate) * 0.01f;
     }
 
@@ -70,7 +70,7 @@ void Tracker::update_bearing_and_distance()
 
     // calculate pitch to vehicle
     // To-Do: remove need for check of control_mode
-    if (control_mode != SCAN && !nav_status.manual_control_pitch) {
+    if (mode->number() != Mode::Number::SCAN && !nav_status.manual_control_pitch) {
     	if (g.alt_source == ALT_SOURCE_BARO) {
     	    nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
     	} else {
@@ -117,28 +117,7 @@ void Tracker::update_tracking(void)
             break;
         }
     } else {
-        switch (control_mode) {
-        case AUTO:
-            if (vehicle.location_valid) {
-                update_auto();
-            } else if (tracker.target_set) {
-                update_scan();
-            }
-            break;
-
-        case MANUAL:
-            update_manual();
-            break;
-
-        case SCAN:
-            update_scan();
-            break;
-
-        case SERVO_TEST:
-        case STOP:
-        case INITIALISING:
-            break;
-        }
+        mode->update();
     }
 
     // convert servo_out to radio_out and send to servo