AntennaTracker: auto mode internal state verbosity

Collaterally fixes a small overight where the target location would be considered automatically valid for the first 5 seconds after startup
Ecognize · Jul 27, 2024 · 9b5405d · 9b5405d
1 parent 9214be1
commit 9b5405d
Show file tree

Hide file tree

Showing 5 changed files with 69 additions and 3 deletions.
diff --git a/AntennaTracker/mode.cpp b/AntennaTracker/mode.cpp
@@ -20,7 +20,7 @@ void Mode::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) || !tracker.vehicle.location_valid) {
+    if (target_range_acceptable() || !tracker.vehicle.location_valid) {
         tracker.update_pitch_servo(bf_pitch);
         tracker.update_yaw_servo(bf_yaw);
     }
@@ -174,3 +174,7 @@ bool Mode::get_ef_yaw_direction()
     // if we get this far we can use the regular, shortest path to the target
     return false;
 }
+
+bool Mode::target_range_acceptable() const {
+    return (tracker.g.distance_min <= 0) || (tracker.nav_status.distance >= tracker.g.distance_min);
+}
diff --git a/AntennaTracker/mode.h b/AntennaTracker/mode.h
@@ -36,13 +36,29 @@ class Mode {
     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 target_range_acceptable() const;
 };
 
 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;
+
+    // Specific states of AUTO mode
+    enum class State {
+        IDLE=0,             // Not doing anything in particular
+        TRACKING=1,         // Actively tracking a valid target
+        SUPPRESSED=2,       // Target within minimum tracking range
+        SCANNING=3,         // Target lost, scanning for target
+    };
+
+    void switch_state(State st) { _state = st; }
+
+private:
+    State _state{State::IDLE};
+    bool scan_for_any_target() const;
 };
 
 class ModeGuided : public Mode {

diff --git a/AntennaTracker/mode_auto.cpp b/AntennaTracker/mode_auto.cpp
@@ -4,9 +4,48 @@
 
 void ModeAuto::update()
 {
+    State old_state = _state;
+
     if (tracker.vehicle.location_valid) {
         update_auto();
-    } else if (tracker.target_set || (tracker.g.auto_opts.get() & (1 << 0)) != 0) {
+        _state = target_range_acceptable() ? State::TRACKING : State::SUPPRESSED;
+    } else if (tracker.target_set || scan_for_any_target()) {
         update_scan();
+        _state = State::SCANNING;
+    } else {
+        _state = State::IDLE;
+    }
+
+    // Announce the state change if necessary
+    if (old_state != _state) {
+        switch (_state) {
+            case State::TRACKING:
+                gcs().send_text(MAV_SEVERITY_INFO, "Tracker locked on the vehicle %d", (short)tracker.g.sysid_target);
+                break;
+
+            case State::SCANNING:
+                if (scan_for_any_target()) {
+                    gcs().send_text(MAV_SEVERITY_INFO, "Tracker scanning for a vehicle");
+                } else {
+                    gcs().send_text(MAV_SEVERITY_INFO, "Tracker scanning for the vehicle %d", (short)tracker.g.sysid_target);
+                }
+                break;
+
+            case State::SUPPRESSED:
+                gcs().send_text(MAV_SEVERITY_INFO, "Tracker suppressed while the target is within %d m", (short)tracker.g.distance_min);
+                break;
+
+            case State::IDLE:
+                gcs().send_text(MAV_SEVERITY_INFO, "Tracker idle");
+                break;
+
+            default:
+                break;
+        }
     }
 }
+
+bool ModeAuto::scan_for_any_target() const
+{
+    return (tracker.g.auto_opts.get() & (1 << 0)) != 0;
+}
diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
@@ -1,4 +1,5 @@
 #include "Tracker.h"
+#include "mode.h"
 
 // mission storage
 static const StorageAccess wp_storage(StorageManager::StorageMission);
@@ -191,6 +192,11 @@ void Tracker::set_mode(Mode &newmode, const ModeReason reason)
     gcs().send_message(MSG_HEARTBEAT);
 
     nav_status.bearing = ahrs.yaw_sensor * 0.01f;
+
+    // reset the auto mode state
+    if (&mode_auto == &newmode) {
+        mode_auto.switch_state(ModeAuto::State::IDLE);
+    }
 }
 
 bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)

diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp
@@ -10,7 +10,8 @@ void Tracker::update_vehicle_pos_estimate()
     float dt = (AP_HAL::micros() - vehicle.last_update_us) * 1.0e-6f;
 
     // if less than 5 seconds since last position update estimate the position
-    if (dt < TRACKING_TIMEOUT_SEC) {
+    // and the vehicle updated at least once
+    if (dt < TRACKING_TIMEOUT_SEC && vehicle.last_update_us != 0) {
         // project the vehicle position to take account of lost radio packets
         vehicle.location_estimate = vehicle.location;
         float north_offset = vehicle.vel.x * dt;