From d0dfe86a4bae76feda05203f8cb44456f5540ca2 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Mon, 29 Jan 2024 19:05:34 -0500 Subject: [PATCH] Copter: incorporate comment to remove unnecessary function --- ArduCopter/Attitude.cpp | 9 --------- ArduCopter/Copter.h | 1 - ArduCopter/mode_systemid.cpp | 12 ++++++------ 3 files changed, 6 insertions(+), 16 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 1c0cb723bf..67646caed9 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -122,15 +122,6 @@ 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 index 04c34a8dac..a872dc67a2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -722,7 +722,6 @@ private: 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_systemid.cpp b/ArduCopter/mode_systemid.cpp index 5c9c8e6033..95c80bce99 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -308,36 +308,36 @@ void ModeSystemId::run() case AxisType::DISTURB_POS_LAT: disturb_state.x = 0.0f; disturb_state.y = waveform_sample * 100.0f; - copter.rotate_target_body_frame_to_NE(disturb_state.x, disturb_state.y); + disturb_state.rotate(attitude_control->get_att_target_euler_rad().z); pos_control->set_disturb_pos_cm(disturb_state); break; case AxisType::DISTURB_POS_LONG: disturb_state.x = waveform_sample * 100.0f; disturb_state.y = 0.0f; - copter.rotate_target_body_frame_to_NE(disturb_state.x, disturb_state.y); + disturb_state.rotate(attitude_control->get_att_target_euler_rad().z); pos_control->set_disturb_pos_cm(disturb_state); break; case AxisType::DISTURB_VEL_LAT: disturb_state.x = 0.0f; disturb_state.y = waveform_sample * 100.0f; - copter.rotate_target_body_frame_to_NE(disturb_state.x, disturb_state.y); + disturb_state.rotate(attitude_control->get_att_target_euler_rad().z); pos_control->set_disturb_vel_cms(disturb_state); break; case AxisType::DISTURB_VEL_LONG: disturb_state.x = waveform_sample * 100.0f; disturb_state.y = 0.0f; - copter.rotate_target_body_frame_to_NE(disturb_state.x, disturb_state.y); + disturb_state.rotate(attitude_control->get_att_target_euler_rad().z); pos_control->set_disturb_vel_cms(disturb_state); break; case AxisType::INPUT_LOITER_LAT: 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); + input_vel.rotate(attitude_control->get_att_target_euler_rad().z); break; case AxisType::INPUT_LOITER_LONG: 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); + input_vel.rotate(attitude_control->get_att_target_euler_rad().z); break; } break;