Copter: update for position control in sysid

bnsgeyer · Aug 28, 2023 · b169275 · b169275
1 parent 619160f
commit b169275
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 15 deletions.
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
@@ -1599,7 +1599,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,

diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp
@@ -121,8 +121,8 @@ void ModeSystemId::run()
     float target_yaw_rate = 0.0f;
     float pilot_throttle_scaled = 0.0f;
     float target_climb_rate = 0.0f;
-//    Vector3f target_accel = Vector3f(0.0f,0.0f,GRAVITY_MSS);
-    Vector2f target_vel;
+    Vector2f input_vel;
+    Vector2f pilot_vel;
 
     if ((AxisType)axis.get() != AxisType::DISTURB_POS_LAT && (AxisType)axis.get() != AxisType::DISTURB_POS_LONG 
          && (AxisType)axis.get() != AxisType::DISTURB_VEL_LAT && (AxisType)axis.get() != AxisType::DISTURB_VEL_LONG
@@ -188,8 +188,8 @@ void ModeSystemId::run()
 
     } else {
         // set xy speed and acceleration limits
-        pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
-        pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+        pos_control->set_max_speed_accel_xy(500.0f, 250.0f);
+        pos_control->set_correction_speed_accel_xy(500.0f, 250.0f);
 
         // set vertical speed and acceleration limits
         pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
@@ -201,7 +201,8 @@ void ModeSystemId::run()
             update_simple_mode();
 
             // convert pilot input to lean angles
-            get_pilot_desired_lean_angles(target_roll, target_pitch, pos_control->get_lean_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
+//            get_pilot_desired_lean_angles(target_roll, target_pitch, pos_control->get_lean_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
+            pilot_vel = get_pilot_desired_velocity(250.0f);
 
         }
 
@@ -311,10 +312,14 @@ void ModeSystemId::run()
                     pos_control->set_disturb_vel_cms(disturb_state);
                     break;
                 case AxisType::INPUT_LOITER_LAT:
-                    target_roll += waveform_sample*100.0f;                    
+                    input_vel.x = 0.0f;
+                    input_vel.y = waveform_sample * 100.0f;
+                    copter.rotate_target_body_frame_to_NE(input_vel.x, input_vel.y);
                     break;
                 case AxisType::INPUT_LOITER_LONG:
-                    target_pitch += waveform_sample*100.0f;                    
+                    input_vel.x = waveform_sample * 100.0f;
+                    input_vel.y = 0.0f;
+                    copter.rotate_target_body_frame_to_NE(input_vel.x, input_vel.y);
                     break;
             }
             break;
@@ -340,11 +345,6 @@ void ModeSystemId::run()
         // this was split into two parts to allow pilot input to accomplish closed loop sweeps
         if (!copter.failsafe.radio) {
 
-            target_vel.x = -0.001f * target_pitch;
-            target_vel.y = 0.001f * target_roll;
-            copter.rotate_target_body_frame_to_NE(target_vel.x, target_vel.y);
-
-
             // set target accels in position controller
 //            Vector3f target_attitude_rad = Vector3f(radians(target_roll*0.01), radians(target_pitch*0.01), attitude_control->get_att_target_euler_rad().z);
 //            target_accel = pos_control->lean_angles_to_accel(target_attitude_rad);
@@ -413,8 +413,10 @@ void ModeSystemId::run()
             // set motors to full range
             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-
-            Vector2f accel = Vector2f(target_vel.x / G_Dt, target_vel.y / G_Dt);
+            input_vel += pilot_vel;
+            Vector2f accel = Vector2f((input_vel.x - target_vel.x) / G_Dt, (input_vel.y - target_vel.y) / G_Dt);
+//            Vector2f accel;
+            target_vel = input_vel;
             pos_control->input_vel_accel_xy(target_vel, accel);
             pos_control->update_xy_controller();