mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-09 01:13:57 -04:00
Copter: move get_pilot_desired_lean_angles higher in cpp
this makes the definition in the .h and implementation in .cpp files appear in the same order
This commit is contained in:
parent
20995b909f
commit
af207caa1f
@ -177,7 +177,7 @@ public:
|
|||||||
friend class Parameters;
|
friend class Parameters;
|
||||||
friend class ParametersG2;
|
friend class ParametersG2;
|
||||||
friend class AP_Avoidance_Copter;
|
friend class AP_Avoidance_Copter;
|
||||||
friend class Mode;
|
|
||||||
#if ADVANCED_FAILSAFE == ENABLED
|
#if ADVANCED_FAILSAFE == ENABLED
|
||||||
friend class AP_AdvancedFailsafe_Copter;
|
friend class AP_AdvancedFailsafe_Copter;
|
||||||
#endif
|
#endif
|
||||||
|
@ -313,6 +313,38 @@ void Copter::Mode::update_navigation()
|
|||||||
run_autopilot();
|
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
|
bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const
|
||||||
{
|
{
|
||||||
if (!ap.land_complete) {
|
if (!ap.land_complete) {
|
||||||
@ -345,38 +377,6 @@ void Copter::Mode::zero_throttle_and_relax_ac()
|
|||||||
#endif
|
#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;
|
// pass-through functions to reduce code churn on conversion;
|
||||||
// these are candidates for moving into the Mode base
|
// these are candidates for moving into the Mode base
|
||||||
// class.
|
// class.
|
||||||
|
@ -28,13 +28,22 @@ protected:
|
|||||||
// returns a string for this flightmode, exactly 4 bytes
|
// returns a string for this flightmode, exactly 4 bytes
|
||||||
virtual const char *name4() const = 0;
|
virtual const char *name4() const = 0;
|
||||||
|
|
||||||
// navigation support functions:
|
// navigation support functions
|
||||||
void update_navigation();
|
void update_navigation();
|
||||||
virtual void run_autopilot() {}
|
virtual void run_autopilot() {}
|
||||||
virtual uint32_t wp_distance() const { return 0; }
|
virtual uint32_t wp_distance() const { return 0; }
|
||||||
virtual int32_t wp_bearing() const { return 0; }
|
virtual int32_t wp_bearing() const { return 0; }
|
||||||
virtual bool in_guided_mode() const { return false; }
|
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:
|
// convenience references to avoid code churn in conversion:
|
||||||
Parameters &g;
|
Parameters &g;
|
||||||
ParametersG2 &g2;
|
ParametersG2 &g2;
|
||||||
@ -52,9 +61,6 @@ protected:
|
|||||||
ap_t ≈
|
ap_t ≈
|
||||||
takeoff_state_t &takeoff_state;
|
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
|
// gnd speed limit required to observe optical flow sensor limits
|
||||||
float &ekfGndSpdLimit;
|
float &ekfGndSpdLimit;
|
||||||
|
|
||||||
@ -69,9 +75,6 @@ protected:
|
|||||||
heli_flags_t &heli_flags;
|
heli_flags_t &heli_flags;
|
||||||
#endif
|
#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;
|
// pass-through functions to reduce code churn on conversion;
|
||||||
// these are candidates for moving into the Mode base
|
// these are candidates for moving into the Mode base
|
||||||
// class.
|
// class.
|
||||||
@ -99,8 +102,6 @@ protected:
|
|||||||
uint16_t get_pilot_speed_dn(void);
|
uint16_t get_pilot_speed_dn(void);
|
||||||
|
|
||||||
// end pass-through functions
|
// end pass-through functions
|
||||||
|
|
||||||
void zero_throttle_and_relax_ac();
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user