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

Copter: Moved get_pilot_desired_lean_angles to mode.cpp

This commit is contained in:
Ebin 2018-03-13 23:24:55 +05:30 committed by Randy Mackay
parent 96793a3ae7
commit 20995b909f
5 changed files with 36 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ArduCopter/Attitude.cpp
View File

@ -7,37 +7,6 @@ float Copter::get_smoothing_gain()
return (2.0f + (float)g.rc_feel_rp/10.0f);
}
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
// sanity check angle max parameter
aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);
// limit max lean angle
angle_max = constrain_float(angle_max, 1000, aparm.angle_max);
// scale roll_in, pitch_in to ANGLE_MAX parameter range
float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
roll_in *= scaler;
pitch_in *= scaler;
// do circular limit
float total_in = norm(pitch_in, roll_in);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// do lateral tilt to euler roll conversion
roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
// return
roll_out = roll_in;
pitch_out = pitch_in;
}
// get_pilot_desired_heading - transform pilot's yaw input into a
// desired yaw rate
// returns desired yaw rate in centi-degrees per second

2
ArduCopter/Copter.h
View File

@ -177,6 +177,7 @@ public:
friend class Parameters;
friend class ParametersG2;
friend class AP_Avoidance_Copter;
friend class Mode;
#if ADVANCED_FAILSAFE == ENABLED
friend class AP_AdvancedFailsafe_Copter;
#endif
@ -658,7 +659,6 @@ private:
// Attitude.cpp
float get_smoothing_gain();
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
float get_pilot_desired_yaw_rate(int16_t stick_angle);
float get_roi_yaw();
float get_look_ahead_yaw();

35
ArduCopter/mode.cpp
View File

@ -345,14 +345,41 @@ void Copter::Mode::zero_throttle_and_relax_ac()
#endif
}
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
AP_Vehicle::MultiCopter &aparm = copter.aparm;
// sanity check angle max parameter
aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);
// limit max lean angle
angle_max = constrain_float(angle_max, 1000, aparm.angle_max);
// scale roll_in, pitch_in to ANGLE_MAX parameter range
float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
roll_in *= scaler;
pitch_in *= scaler;
// do circular limit
float total_in = norm(pitch_in, roll_in);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// do lateral tilt to euler roll conversion
roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
// return
roll_out = roll_in;
pitch_out = pitch_in;
}
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
}
float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
{

4
ArduCopter/mode.h
View File

@ -69,10 +69,12 @@ protected:
heli_flags_t &heli_flags;
#endif
// mode.cpp
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
float get_pilot_desired_yaw_rate(int16_t stick_angle);
float get_pilot_desired_climb_rate(float throttle_control);

2
ArduCopter/mode_flowhold.cpp
View File

@ -267,7 +267,7 @@ void Copter::ModeFlowHold::run()
int16_t roll_in = copter.channel_roll->get_control_in();
int16_t pitch_in = copter.channel_pitch->get_control_in();
float angle_max = copter.attitude_control->get_althold_lean_angle_max();
copter.get_pilot_desired_lean_angles(roll_in, pitch_in,
get_pilot_desired_lean_angles(roll_in, pitch_in,
bf_angles.x, bf_angles.y,
angle_max);