Copter: incorporate comment to remove unnecessary function

bnsgeyer · Jan 31, 2024 · 940743e · 940743e
1 parent cf33610
commit 940743e
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 16 deletions.
diff --git 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
@@ -703,7 +703,6 @@ 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_systemid.cpp b/ArduCopter/mode_systemid.cpp
@@ -306,36 +306,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;