Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: move get_pilot_desired_lean_angles higher in cpp

Browse Source 
this makes the definition in the .h and implementation in .cpp files appear in the same order
This commit is contained in:
Randy Mackay 2018-03-14 09:23:30 +09:00
parent 20995b909f
commit af207caa1f
3 changed files with 43 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -177,7 +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

64
ArduCopter/mode.cpp
View File

@ -313,6 +313,38 @@ void Copter::Mode::update_navigation()
run_autopilot();
}
// 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
const float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
roll_in *= scaler;
pitch_in *= scaler;
// do circular limit
const 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;
}
bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const
{
if (!ap.land_complete) {
@ -345,38 +377,6 @@ 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.

19
ArduCopter/mode.h
View File

@ -28,13 +28,22 @@ protected:
// returns a string for this flightmode, exactly 4 bytes
virtual const char *name4() const = 0;
// navigation support functions:
// navigation support functions
void update_navigation();
virtual void run_autopilot() {}
virtual uint32_t wp_distance() const { return 0; }
virtual int32_t wp_bearing() const { return 0; }
virtual bool in_guided_mode() const { return false; }
// pilot input processing
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
// takeoff support
bool takeoff_triggered(float target_climb_rate) const;
// helper functions
void zero_throttle_and_relax_ac();
// convenience references to avoid code churn in conversion:
Parameters &g;
ParametersG2 &g2;
@ -52,9 +61,6 @@ protected:
ap_t ≈
takeoff_state_t &takeoff_state;
// takeoff support
bool takeoff_triggered(float target_climb_rate) const;
// gnd speed limit required to observe optical flow sensor limits
float &ekfGndSpdLimit;
@ -69,9 +75,6 @@ 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.
@ -99,8 +102,6 @@ protected:
uint16_t get_pilot_speed_dn(void);
// end pass-through functions
void zero_throttle_and_relax_ac();
};