From 4d208fcd47f7a0ddef38c3b109986cd6359ee602 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 22 Jan 2016 11:09:32 +0900 Subject: [PATCH] AP_MotorsTri: roll, pitch, yaw input in -1 to +1 range --- libraries/AP_Motors/AP_MotorsTri.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 0981358ac6..ec50c67b55 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -186,9 +186,9 @@ void AP_MotorsTri::output_armed_stabilizing() float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0 // apply voltage and air pressure compensation - roll_thrust = get_roll_thrust() * get_compensation_gain(); - pitch_thrust = get_pitch_thrust() * get_compensation_gain(); - yaw_thrust = get_yaw_thrust() * get_compensation_gain()*sinf(_pivot_angle_max); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle + roll_thrust = _roll_in * get_compensation_gain(); + pitch_thrust = _pitch_in * get_compensation_gain(); + yaw_thrust = _yaw_in * get_compensation_gain()*sinf(_pivot_angle_max); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle throttle_thrust = get_throttle() * get_compensation_gain(); float pivot_angle_request_max = asin(yaw_thrust); float pivot_thrust_max = cosf(pivot_angle_request_max);