Copter: interpret input in stabilize as a tilt vector

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

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;
 

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;
 

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;
 

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

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)) {

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;
 

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;