manual mode: move rates applications in fc_core

iNavFlight · Jan 24, 2018 · 9c28150 · 9c28150
1 parent bba8620
commit 9c28150
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 3 deletions.
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
@@ -298,6 +298,13 @@ void annexCode(void)
         rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcExpo8 : currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
         rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcYawExpo8 : currentControlRateProfile->rcYawExpo8, rcControlsConfig()->yaw_deadband);
 
+        // Apply manual control rates
+        if (FLIGHT_MODE(MANUAL_MODE)) {
+            rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual_rates[FD_ROLL] / 100L;
+            rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual_rates[FD_PITCH] / 100L;
+            rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual_rates[FD_YAW] / 100L;
+        }
+
         //Compute THROTTLE command
         throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
         throttleValue = (uint32_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);       // [MINCHECK;2000] -> [0;1000]

diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
@@ -367,9 +367,9 @@ void servoMixer(float dT)
     int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
 
     if (FLIGHT_MODE(MANUAL_MODE)) {
-        input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual_rates[FD_ROLL] / 100L;
-        input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual_rates[FD_PITCH] / 100L;
-        input[INPUT_STABILIZED_YAW] = rcCommand[YAW] * currentControlRateProfile->manual_rates[FD_YAW] / 100L;
+        input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
+        input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
+        input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
     } else {
         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
         input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];