Copter: change position controller method for making inputs

bnsgeyer · May 14, 2024 · d1fb594 · d1fb594
1 parent 97d91b4
commit d1fb594
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
@@ -3,6 +3,7 @@
 #include "Copter.h"
 #include <AP_Math/chirp.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
+#include <AP_Math/control.h>
 class Parameters;
 class ParametersG2;
 
@@ -1671,7 +1672,8 @@ class ModeSystemId : public Mode {
     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
-
+    Vector2f target_pos;       // target positon
+    Vector2f input_vel_last;    // last cycle input velocity
     // System ID states
     enum class SystemIDModeState {
         SYSTEMID_STATE_STOPPED,

diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp
@@ -127,6 +127,10 @@ bool ModeSystemId::init(bool ignore_checks)
         if (!pos_control->is_active_z()) {
             pos_control->init_z_controller();
         }
+        Vector3f curr_pos;
+        curr_pos = inertial_nav.get_position_neu_cm();
+        target_pos.x = curr_pos.x;
+        target_pos.y = curr_pos.y;
     }
 
     att_bf_feedforward = attitude_control->get_bf_feedforward();
@@ -358,7 +362,12 @@ void ModeSystemId::run()
         }
 
         Vector2f accel;
-        pos_control->input_vel_accel_xy(input_vel, accel);
+        target_pos += input_vel * G_Dt;
+        if (is_positive(G_Dt)) {
+            accel = (input_vel - input_vel_last) / G_Dt;
+            input_vel_last = input_vel;
+        }
+        pos_control->set_pos_vel_accel_xy(target_pos.topostype(), input_vel, accel);
 
         // run pos controller
         pos_control->update_xy_controller();