diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9d1976430a..6820de44c7 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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 diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 2c6d23b4f5..0eed603f7d 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -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. diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dbb78cc978..d438cb624f 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -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(); };