AntennaTracker: mode/enter exit method infrastructure

Ecognize · Jul 28, 2024 · ca934c5 · ca934c5
1 parent 0ba2793
commit ca934c5
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 5 deletions.
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
@@ -200,7 +200,7 @@ class Tracker : public AP_Vehicle {
     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
     bool set_home(const Location &temp, bool lock) override WARN_IF_UNUSED;
     void prepare_servos();
-    void set_mode(Mode &newmode, ModeReason reason);
+    bool set_mode(Mode &newmode, ModeReason reason);
     bool set_mode(uint8_t new_mode, ModeReason reason) override;
     uint8_t get_mode() const override { return (uint8_t)mode->number(); }
     bool should_log(uint32_t mask);

diff --git a/AntennaTracker/mode.h b/AntennaTracker/mode.h
@@ -27,7 +27,19 @@ class Mode {
 
     virtual void update() = 0;
 
+    // enter this mode, returns true if mode change is possible
+    bool enter() { return _enter(); }
+
+    // perform any cleanups required:
+    void exit() { _exit(); }
+
 protected:
+    // subclasses override this to perform checks before entering the mode
+    virtual bool _enter() { return true; }
+
+    // subclasses override this to perform any required cleanup when exiting the mode
+    virtual void _exit() { return; }
+
     void update_scan();
     void update_auto();
 
@@ -56,6 +68,11 @@ class ModeAuto : public Mode {
 
     void switch_state(State st);
 
+protected:
+    // Ensure Auto considers its starting state to be idle so it
+    // prints out a meaningful state after the first update()
+    bool _enter() override { _state = State::IDLE; return true; }
+
 private:
     State _state;
 
@@ -79,6 +96,11 @@ class ModeGuided : public Mode {
         _last_set_angle_ms = AP_HAL::millis();
     }
 
+protected:
+    // Reset the last command time so that entering the mode always
+    // prints the current target angle
+    bool _enter() override { _last_set_angle_ms = 0; return true; }
+
 private:
     Quaternion _target_att;
     bool _use_yaw_rate;

diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
@@ -168,14 +168,23 @@ void Tracker::prepare_servos()
     SRV_Channels::output_ch_all();
 }
 
-void Tracker::set_mode(Mode &newmode, const ModeReason reason)
+bool Tracker::set_mode(Mode &newmode, const ModeReason reason)
 {
     control_mode_reason = reason;
 
     if (mode == &newmode) {
         // don't switch modes if we are already in the correct mode.
-        return;
+        return false;
+    }
+
+    // Try entering the new mode
+    // NOTE: futureproofing, currently all modes return true
+    if (!newmode.enter()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Tracker mode change failed");
+        return false;
     }
+    // Exit the previous one and swap
+    mode->exit();
     mode = &newmode;
 
     if (mode->requires_armed_servos()) {
@@ -191,6 +200,7 @@ void Tracker::set_mode(Mode &newmode, const ModeReason reason)
     gcs().send_message(MSG_HEARTBEAT);
 
     nav_status.bearing = ahrs.yaw_sensor * 0.01f;
+    return true;
 }
 
 bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)
@@ -221,8 +231,7 @@ bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)
     if (fred == nullptr) {
         return false;
     }
-    set_mode(*fred, reason);
-    return true;
+    return set_mode(*fred, reason);
 }
 
 #if HAL_LOGGING_ENABLED