diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index bf5dacf293..12e578c0da 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -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 diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 1463c48897..9d1976430a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 4f8378f4d2..2c6d23b4f5 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -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) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 6da5e76e74..dbb78cc978 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -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); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index d2b2ee58e1..af0f61dd48 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -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);