diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 69ffa1cc94..1dc0385cb9 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -9,36 +9,29 @@ float get_smoothing_gain() // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle // returns desired angle in centi-degrees -static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out) +static void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out) { - static float _scaler = 1.0; - static int16_t _angle_max = 0; + float angle_max = constrain_float(aparm.angle_max,0,8000); + float scaler = (float)angle_max/(float)ROLL_PITCH_INPUT_MAX; - // apply circular limit to pitch and roll inputs + // scale roll_in, pitch_in to correct units + roll_in *= scaler; + pitch_in *= scaler; + + // do circular limit float total_in = pythagorous2((float)pitch_in, (float)roll_in); - - if (total_in > ROLL_PITCH_INPUT_MAX) { - float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; + if (total_in > angle_max) { + float ratio = angle_max / total_in; roll_in *= ratio; pitch_in *= ratio; } - // return filtered roll if no scaling required - if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) { - roll_out = roll_in; - pitch_out = pitch_in; - return; - } + // do lateral tilt to euler roll conversion + roll_in = (18000/M_PI_F) * atanf(cosf(pitch_in*(M_PI_F/18000))*tanf(roll_in*(M_PI_F/18000))); - // check if angle_max has been updated and redo scaler - if (aparm.angle_max != _angle_max) { - _angle_max = aparm.angle_max; - _scaler = (float)aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX; - } - - // convert pilot input to lean angle - roll_out = (int16_t)((float)roll_in * _scaler); - pitch_out = (int16_t)((float)pitch_in * _scaler); + // return + roll_out = roll_in; + pitch_out = pitch_in; } // get_pilot_desired_heading - transform pilot's yaw input into a desired heading diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index dbc0f7fdf6..7b6029bc74 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -21,7 +21,7 @@ static bool althold_init(bool ignore_checks) // should be called at 100hz or more static void althold_run() { - int16_t target_roll, target_pitch; + float target_roll, target_pitch; float target_yaw_rate; int16_t target_climb_rate; diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index d6d42a2308..adb2bff2f2 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -249,7 +249,7 @@ static bool autotune_start(bool ignore_checks) // should be called at 100hz or more static void autotune_run() { - int16_t target_roll, target_pitch; + float target_roll, target_pitch; float target_yaw_rate; int16_t target_climb_rate; diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index 7378cc43aa..a32dfe7c1a 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -42,7 +42,7 @@ static void drift_run() { static float breaker = 0.0; static float roll_input = 0.0; - int16_t target_roll, target_pitch; + float target_roll, target_pitch; float target_yaw_rate; int16_t pilot_throttle_scaled; diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 47a5bea205..3e663fe50a 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -122,7 +122,7 @@ static void land_gps_run() // should be called at 100hz or more static void land_nogps_run() { - int16_t target_roll = 0, target_pitch = 0; + float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; // if not auto armed or landed set throttle to zero and exit immediately diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index d48d6b6b47..3659906710 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -36,7 +36,7 @@ #define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s // declare some function to keep compiler happy -static void poshold_update_pilot_lean_angle(int16_t &lean_angle_filtered, int16_t &lean_angle_raw); +static void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); static int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); static void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); static void poshold_update_wind_comp_estimate(); @@ -62,8 +62,8 @@ static struct { uint8_t loiter_reset_I : 1; // true the very first time PosHold enters loiter, thereafter we trust the i terms loiter has // pilot input related variables - int16_t pilot_roll; // pilot requested roll angle (filtered to slow returns to zero) - int16_t pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero) + float pilot_roll; // pilot requested roll angle (filtered to slow returns to zero) + float pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero) // braking related variables float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate) @@ -144,7 +144,7 @@ static bool poshold_init(bool ignore_checks) // should be called at 100hz or more static void poshold_run() { - int16_t target_roll, target_pitch; // pilot's roll and pitch angle inputs + float target_roll, target_pitch; // pilot's roll and pitch angle inputs float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec int16_t target_climb_rate = 0; // pilot desired climb rate in centimeters/sec float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls @@ -530,7 +530,7 @@ static void poshold_run() } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received -static void poshold_update_pilot_lean_angle(int16_t &lean_angle_filtered, int16_t &lean_angle_raw) +static void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) { // if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (abs(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) { diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index 387d7cde27..c5ceb50bc6 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -19,7 +19,7 @@ static bool stabilize_init(bool ignore_checks) // should be called at 100hz or more static void stabilize_run() { - int16_t target_roll, target_pitch; + float target_roll, target_pitch; float target_yaw_rate; int16_t pilot_throttle_scaled; diff --git a/ArduCopter/heli_control_stabilize.pde b/ArduCopter/heli_control_stabilize.pde index 80ce718c4d..f4de9d3277 100644 --- a/ArduCopter/heli_control_stabilize.pde +++ b/ArduCopter/heli_control_stabilize.pde @@ -17,7 +17,7 @@ static bool heli_stabilize_init(bool ignore_checks) // should be called at 100hz or more static void heli_stabilize_run() { - int16_t target_roll, target_pitch; + float target_roll, target_pitch; float target_yaw_rate; int16_t pilot_throttle_scaled;