Plane: add system ID to quadplane

enable as an AUX switch
ArduPilot · Feb 7, 2025 · 5699218 · 5699218
1 parent 0615c45
commit 5699218
Show file tree

Hide file tree

Showing 11 changed files with 455 additions and 10 deletions.
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1049,7 +1049,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @Path: ../libraries/AP_Quicktune/AP_Quicktune.cpp
     GOBJECT(quicktune, "QWIK_",  AP_Quicktune),
 #endif
-    
+
     AP_VAREND
 };
 
@@ -1309,6 +1309,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("RNGFND_LND_ORNT", 36, ParametersG2, rangefinder_land_orient, ROTATION_PITCH_270),
 #endif
+
+#if AP_PLANE_SYSTEMID_ENABLED
+    // @Group: SID
+    // @Path: systemid.cpp
+    AP_SUBGROUPINFO(systemid, "SID", 37, ParametersG2, AP_SystemID),
+#endif
 
     AP_GROUPEND
 };

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
@@ -590,6 +590,10 @@ class ParametersG2 {
     // orientation of rangefinder to use for landing
     AP_Int8 rangefinder_land_orient;
 #endif
+
+#if AP_PLANE_SYSTEMID_ENABLED
+    AP_SystemID systemid;
+#endif
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -122,6 +122,7 @@
 #endif
 #include "AP_Arming_Plane.h"
 #include "pullup.h"
+#include "systemid.h"
 
 /*
   main APM:Plane class
@@ -181,6 +182,9 @@ class Plane : public AP_Vehicle {
 #if AP_PLANE_GLIDER_PULLUP_ENABLED
     friend class GliderPullup;
 #endif
+#if AP_PLANE_SYSTEMID_ENABLED
+    friend class AP_SystemID;
+#endif
 
     Plane(void);
 

diff --git a/ArduPlane/RC_Channel_Plane.cpp b/ArduPlane/RC_Channel_Plane.cpp
@@ -177,6 +177,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
 #endif
 #if AP_QUICKTUNE_ENABLED
     case AUX_FUNC::QUICKTUNE:
+#endif
+#if AP_PLANE_SYSTEMID_ENABLED
+    case AUX_FUNC::SYSTEMID:
 #endif
         break;
 
@@ -468,6 +471,16 @@ bool RC_Channel_Plane::do_aux_function(const AuxFuncTrigger &trigger)
         break;
 #endif
 
+#if AP_PLANE_SYSTEMID_ENABLED
+    case AUX_FUNC::SYSTEMID:
+        if (ch_flag == AuxSwitchPos::HIGH) {
+            plane.g2.systemid.start();
+        } else if (ch_flag == AuxSwitchPos::LOW) {
+            plane.g2.systemid.stop();
+        }
+        break;
+#endif
+
 #if AP_ICENGINE_ENABLED
     case AUX_FUNC::ICE_START_STOP:
         plane.g2.ice_control.do_aux_function(trigger);

diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp
@@ -101,6 +101,10 @@ bool Mode::enter()
     plane.target_altitude.terrain_following_pending = false;
 #endif
 
+#if AP_PLANE_SYSTEMID_ENABLED
+    plane.g2.systemid.stop();
+#endif
+
     // disable auto mode servo idle during altitude wait command
     plane.auto_state.idle_mode = false;
 

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -10,6 +10,7 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Mission/AP_Mission.h>
 #include "pullup.h"
+#include "systemid.h"
 
 #ifndef AP_QUICKTUNE_ENABLED
 #define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED
@@ -167,6 +168,11 @@ class Mode
     virtual bool supports_quicktune() const { return false; }
 #endif
 
+#if AP_PLANE_SYSTEMID_ENABLED
+    // does this mode support systemid?
+    virtual bool supports_systemid() const { return false; }
+#endif
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -697,6 +703,11 @@ class ModeQStabilize : public Mode
 
     void run() override;
 
+#if AP_PLANE_SYSTEMID_ENABLED
+    // does this mode support systemid?
+    bool supports_systemid() const override { return true; }
+#endif
+
 protected:
 private:
 
@@ -721,6 +732,11 @@ class ModeQHover : public Mode
 
     void run() override;
 
+#if AP_PLANE_SYSTEMID_ENABLED
+    // does this mode support systemid?
+    bool supports_systemid() const override { return true; }
+#endif
+
 protected:
 
     bool _enter() override;
@@ -749,6 +765,11 @@ friend class Plane;
 
     void run() override;
 
+#if AP_PLANE_SYSTEMID_ENABLED
+    // does this mode support systemid?
+    bool supports_systemid() const override { return true; }
+#endif
+
 protected:
 
     bool _enter() override;

diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp
@@ -126,10 +126,18 @@ void ModeQLoiter::run()
     // Pilot input, use yaw rate time constant
     quadplane.set_pilot_yaw_rate_time_constant();
 
+    Vector3f target { plane.nav_roll_cd*0.01, plane.nav_pitch_cd*0.01, quadplane.get_desired_yaw_rate_cds() * 0.01 };
+
+#if AP_PLANE_SYSTEMID_ENABLED
+    auto &systemid = plane.g2.systemid;
+    systemid.update();
+    target += systemid.get_attitude_offset_deg();
+#endif
+
     // call attitude controller with conservative smoothing gain of 4.0f
-    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
-                                                                  plane.nav_pitch_cd,
-                                                                  quadplane.get_desired_yaw_rate_cds());
+    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target.x*100,
+                                                                  target.y*100,
+                                                                  target.z*100);
 
     if (plane.control_mode == &plane.mode_qland) {
         if (poscontrol.get_state() < QuadPlane::QPOS_LAND_FINAL && quadplane.check_land_final()) {

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
@@ -936,16 +936,25 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
             }
         }
 
+        // note this is actually in deg/s for some SID_AXIS values for yaw
+        Vector3f offset_deg;
+
+#if AP_PLANE_SYSTEMID_ENABLED
+        auto &systemid = plane.g2.systemid;
+        systemid.update();
+        offset_deg = systemid.get_attitude_offset_deg();
+#endif
+
         if (use_yaw_target) {
-            attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd,
-                                                               plane.nav_pitch_cd,
-                                                               yaw_target_cd,
+            attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd + offset_deg.x*100,
+                                                               plane.nav_pitch_cd + offset_deg.y*100,
+                                                               yaw_target_cd + offset_deg.z*100,
                                                                true);
         } else {
             // use euler angle attitude control
-            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
-                                                                          plane.nav_pitch_cd,
-                                                                          yaw_rate_cds);
+            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd + offset_deg.x*100,
+                                                                          plane.nav_pitch_cd + offset_deg.y*100,
+                                                                          yaw_rate_cds + offset_deg.z*100);
         }
     } else {
         // use the fixed wing desired rates
@@ -980,6 +989,9 @@ void QuadPlane::hold_stabilize(float throttle_in)
             // tailsitters in forward flight should not use angle boost
             should_boost = false;
         }
+#if AP_PLANE_SYSTEMID_ENABLED
+        throttle_in += plane.g2.systemid.get_throttle_offset();
+#endif
         attitude_control->set_throttle_out(throttle_in, should_boost, 0);
     }
 }

diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
@@ -63,6 +63,7 @@ class QuadPlane
     friend class ModeQAcro;
     friend class ModeLoiterAltQLand;
     friend class ModeAutoLand;
+    friend class AP_SystemID;
 
     QuadPlane(AP_AHRS &_ahrs);