From d2b4a6fd2e2f27fa25bd195046ff94b62e209b16 Mon Sep 17 00:00:00 2001
From: bnsgeyer <bnsgeyer@aol.com>
Date: Wed, 29 Jan 2025 00:03:23 -0500
Subject: [PATCH] Plane: add system ID to quadplane

---
 ArduPlane/GCS_MAVLink_Plane.cpp | 2 ++
 ArduPlane/GCS_Plane.cpp         | 1 +
 ArduPlane/events.cpp            | 2 ++
 ArduPlane/mode.h                | 2 +-
 ArduPlane/mode_qsystemid.cpp    | 2 +-
 ArduPlane/quadplane.cpp         | 4 ++++
 ArduPlane/quadplane.h           | 2 ++
 7 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/GCS_MAVLink_Plane.cpp b/ArduPlane/GCS_MAVLink_Plane.cpp
index 3ae02911466d5..557feb377479d 100644
--- a/ArduPlane/GCS_MAVLink_Plane.cpp
+++ b/ArduPlane/GCS_MAVLink_Plane.cpp
@@ -41,6 +41,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
     case Mode::Number::FLY_BY_WIRE_B:
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QSTABILIZE:
+    case Mode::Number::QSYSTEMID:
     case Mode::Number::QHOVER:
     case Mode::Number::QLOITER:
     case Mode::Number::QLAND:
@@ -1606,6 +1607,7 @@ uint8_t GCS_MAVLINK_Plane::send_available_mode(uint8_t index) const
     // Quadplane modes
     const Mode* q_modes[] {
         &plane.mode_qstabilize,
+        &plane.mode_qsystemid,
         &plane.mode_qhover,
         &plane.mode_qloiter,
         &plane.mode_qland,
diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp
index 6b76d734cb3d7..93ccb9ceb93ab 100644
--- a/ArduPlane/GCS_Plane.cpp
+++ b/ArduPlane/GCS_Plane.cpp
@@ -43,6 +43,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
     case Mode::Number::AUTOTUNE:
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QSTABILIZE:
+    case Mode::Number::QSYSTEMID:
     case Mode::Number::QHOVER:
     case Mode::Number::QLAND:
     case Mode::Number::QLOITER:
diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp
index d5ab51c603f5b..c15164a613064 100644
--- a/ArduPlane/events.cpp
+++ b/ArduPlane/events.cpp
@@ -49,6 +49,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
 
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QSTABILIZE:
+    case Mode::Number::QSYSTEMID:
     case Mode::Number::QLOITER:
     case Mode::Number::QHOVER:
 #if QAUTOTUNE_ENABLED
@@ -162,6 +163,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
 
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QSTABILIZE:
+    case Mode::Number::QSYSTEMID:
     case Mode::Number::QHOVER:
     case Mode::Number::QLOITER:
     case Mode::Number::QACRO:
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 7edfc894a9639..e45bc5e1a4e26 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -721,7 +721,7 @@ class ModeQSystemId : public Mode {
     bool allows_throttle_nudging() const override { return true; }
 
     // methods that affect movement of the vehicle in this mode
-//    void update() override;
+    void update() override;
 
     // used as a base class for all Q modes
     bool _enter() override;
diff --git a/ArduPlane/mode_qsystemid.cpp b/ArduPlane/mode_qsystemid.cpp
index b5f1c0bf715f1..fc44a16d59b2e 100644
--- a/ArduPlane/mode_qsystemid.cpp
+++ b/ArduPlane/mode_qsystemid.cpp
@@ -115,7 +115,7 @@ bool ModeQSystemId::_enter()
     return true;
 }
 
-void ModeQStabilize::update()
+void ModeQSystemId::update()
 {
 
 }
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 8480a19d03b3d..edd583a2393c0 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -547,6 +547,10 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Standard
     AP_GROUPINFO("APPROACH_DIST", 39, QuadPlane, approach_distance, 0),
     
+    // @Group: SID
+    // @Path: mode_qsystemid.cpp
+    AP_SUBGROUPINFO(mode_qsystemid, "SID", 40, QuadPlane, ModeQSystemId),
+
     AP_GROUPEND
 };
 
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 5350dfb09c466..2cf701e170e0d 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -429,6 +429,8 @@ class QuadPlane
 
     AC_WeatherVane *weathervane;
 
+    void mode_qsystemid;
+
     bool initialised;
 
     Location last_auto_target;