From 59a43a42f8513e1b816f598a28647eb98d7c707c Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Thu, 30 Nov 2023 20:16:22 -0500 Subject: [PATCH] Copter: clean up --- ArduCopter/mode_systemid.cpp | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index b7d894f1fbc58c..269a9e5346f6e3 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -159,8 +159,6 @@ void ModeSystemId::run() float pilot_throttle_scaled = 0.0f; float target_climb_rate = 0.0f; Vector2f input_vel; - Vector2f pilot_vel; - Vector2f vel_correction; 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 @@ -224,17 +222,6 @@ void ModeSystemId::run() pilot_throttle_scaled = get_pilot_desired_throttle(); #endif - } else { - if (!copter.failsafe.radio) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // convert pilot input to reposition velocity - // use half maximum acceleration as the maximum velocity to ensure aircraft will - // stop from full reposition speed in less than 1 second. - const float max_pilot_vel = wp_nav->get_wp_acceleration() * 0.5; - vel_correction = get_pilot_desired_velocity(max_pilot_vel); - } } if ((systemid_state == SystemIDModeState::SYSTEMID_STATE_TESTING) && @@ -372,7 +359,7 @@ void ModeSystemId::run() } Vector2f accel; - pos_control->input_vel_accel_xy(vel_correction, accel); + pos_control->input_vel_accel_xy(input_vel, accel); // run pos controller pos_control->update_xy_controller();