Copter: add support for sysid of vel and pos loops

bnsgeyer · Oct 31, 2023 · a49ab87 · a49ab87
1 parent 5c95a09
commit a49ab87
Show file tree

Hide file tree

Showing 4 changed files with 286 additions and 57 deletions.
diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
@@ -122,6 +122,15 @@ void Copter::rotate_body_frame_to_NE(float &x, float &y)
     y = ne_y;
 }
 
+// rotate vector from vehicle's target frame perspective to North-East frame
+void Copter::rotate_target_body_frame_to_NE(float &x, float &y)
+{
+    float ne_x = x*cosf(attitude_control->get_att_target_euler_rad().z) - y*sinf(attitude_control->get_att_target_euler_rad().z);
+    float ne_y = x*sinf(attitude_control->get_att_target_euler_rad().z) + y*cosf(attitude_control->get_att_target_euler_rad().z);
+    x = ne_x;
+    y = ne_y;
+}
+
 // It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
 uint16_t Copter::get_pilot_speed_dn() const
 {

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
@@ -724,6 +724,7 @@ class Copter : public AP_Vehicle {
     float get_non_takeoff_throttle();
     void set_accel_throttle_I_from_pilot_throttle();
     void rotate_body_frame_to_NE(float &x, float &y);
+    void rotate_target_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn() const;
     void run_rate_controller();
 

diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
@@ -1617,6 +1617,12 @@ class ModeSystemId : public Mode {
         MIX_PITCH = 11,     // mixer pitch axis is being excited
         MIX_YAW = 12,       // mixer pitch axis is being excited
         MIX_THROTTLE = 13,  // mixer throttle axis is being excited
+        DISTURB_POS_LAT = 14,
+        DISTURB_POS_LONG = 15,
+        DISTURB_VEL_LAT = 16,
+        DISTURB_VEL_LONG = 17,
+        INPUT_LOITER_LAT = 18,
+        INPUT_LOITER_LONG = 19,
     };
 
     AP_Int8 axis;               // Controls which axis are being excited. Set to non-zero to display other parameters
@@ -1633,7 +1639,8 @@ class ModeSystemId : public Mode {
     float waveform_freq_rads;   // Instantaneous waveform frequency
     float time_const_freq;      // Time at constant frequency before chirp starts
     int8_t log_subsample;       // Subsample multiple for logging.
-
+    Vector2f target_vel;        // target velocity for position controller modes
+
     // System ID states
     enum class SystemIDModeState {
         SYSTEMID_STATE_STOPPED,