Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: incorporate comment to remove unnecessary function

This commit is contained in:
bnsgeyer 2024-01-29 19:05:34 -05:00 committed by Bill Geyer
parent f74ccd569a
commit d0dfe86a4b
3 changed files with 6 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/Attitude.cpp
View File

@ -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
{

1
ArduCopter/Copter.h
View File

@ -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();

12
ArduCopter/mode_systemid.cpp
View File

@ -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;