Copter: AP_Math: Move rc_input_to_roll_pitch conversion to AP_Math

ArduCopter/mode.cpp

@@ -406,27 +406,14 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd
         return;
     }
 
-    float rc_2_rad = radians(angle_max_cd * 0.01) / (float)ROLL_PITCH_YAW_INPUT_MAX;
-
-    // fetch roll and pitch stick positions and convert them to normalised horizontal thrust
-    Vector2f thrust;
-    thrust.x = - tanf(rc_2_rad * channel_pitch->get_control_in());
-    thrust.y = tanf(rc_2_rad * channel_roll->get_control_in());
-
-    // calculate the horizontal thrust limit based on the angle limit
-    angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd);
-    float thrust_limit = tanf(radians(angle_limit_cd * 0.01));
-
-    // apply horizontal thrust limit
-    thrust.limit_length(thrust_limit);
-
-    // Conversion from angular thrust vector to euler angles.
-    float pitch_rad = - atanf(thrust.x);
-    float roll_rad = atanf(cosf(pitch_rad) * thrust.y);
+    //transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command
+    float roll_out_deg;
+    float pitch_out_deg;
+    rc_input_to_roll_pitch(channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), angle_max_cd * 0.01,  angle_limit_cd * 0.01, roll_out_deg, pitch_out_deg);
 
     // Convert to centi-degrees
-    roll_out_cd = degrees(roll_rad) * 100.0;
-    pitch_out_cd = degrees(pitch_rad) * 100.0;
+    roll_out_cd = roll_out_deg * 100.0;
+    pitch_out_cd = pitch_out_deg * 100.0;
 }
 
 // transform pilot's roll or pitch input into a desired velocity