Convert input, output and navigation to use [-1, 1]

make/source.mk

@@ -66,6 +66,8 @@ COMMON_SRC = \
             fc/fc_msp_box.c \
             fc/rc_smoothing.c \
             fc/rc_adjustments.c \
+            fc/rc_command.c \
+            fc/rc_control.c \
             fc/rc_controls.c \
             fc/rc_curves.c \
             fc/rc_modes.c \

src/main/blackbox/blackbox.c

@@ -49,6 +49,7 @@
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
+#include "fc/rc_control.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
@@ -58,14 +59,13 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
+#include "flight/wind_estimator.h"
 
 #include "io/beeper.h"
 #include "io/gps.h"
 
 #include "navigation/navigation.h"
 
-#include "rx/rx.h"
-
 #include "sensors/diagnostics.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
@@ -75,9 +75,9 @@
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
-#include "flight/wind_estimator.h"
 #include "sensors/temperature.h"
 
+#include "rx/rx.h"
 
 #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
 #define DEFAULT_BLACKBOX_DEVICE     BLACKBOX_DEVICE_FLASH
@@ -1330,9 +1330,13 @@ static void loadMainState(timeUs_t currentTimeUs)
     }
 #endif
 
+    const rcCommand_t *controlOutput = rcControlGetOutput();
     for (int i = 0; i < 4; i++) {
-        blackboxCurrent->rcData[i] = rcData[i];
-        blackboxCurrent->rcCommand[i] = rcCommand[i];
+        blackboxCurrent->rcData[i] = rxGetChannelValue(i);
+        // TODO: This changes THR to always be represented
+        // in bidirectional mode in logs. What should be
+        // done here?
+        blackboxCurrent->rcCommand[i] = rcCommandMapPWMValue(controlOutput->axes[i]);
     }
 
     blackboxCurrent->attitude[0] = attitude.values.roll;
@@ -1367,7 +1371,7 @@ static void loadMainState(timeUs_t currentTimeUs)
     blackboxCurrent->rssi = getRSSI();
 
     //Tail servo for tricopters
-    blackboxCurrent->servo[5] = servo[5];
+    blackboxCurrent->servo[5] = servosGetPWMValue(5);
 
 #ifdef NAV_BLACKBOX
     blackboxCurrent->navState = navCurrentState;

src/main/cms/cms.c

@@ -780,9 +780,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration)
 
 // Stick/key detection and key codes
 
-#define IS_HI(X)  (rcData[X] > 1750)
-#define IS_LO(X)  (rcData[X] < 1250)
-#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
+#define IS_HI(X)  (rxGetChannelValue(X) > 1750)
+#define IS_LO(X)  (rxGetChannelValue(X) < 1250)
+#define IS_MID(X) (rxGetChannelValue(X) > 1250 && rxGetChannelValue(X) < 1750)
 
 #define KEY_NONE    0
 #define KEY_UP      1

src/main/cms/cms_menu_misc.c

@@ -56,7 +56,7 @@ static long cmsx_menuRcConfirmBack(const OSD_Entry *self)
 static const OSD_Entry cmsx_menuRcEntries[] =
 {
     OSD_LABEL_ENTRY("-- RC PREV --"),
-
+#if 0 // TODO: Do we really want this menu?
     OSD_INT16_RO_ENTRY("ROLL", &rcData[ROLL]),
     OSD_INT16_RO_ENTRY("PITCH", &rcData[PITCH]),
     OSD_INT16_RO_ENTRY("THR", &rcData[THROTTLE]),
@@ -66,7 +66,7 @@ static const OSD_Entry cmsx_menuRcEntries[] =
     OSD_INT16_RO_ENTRY("AUX2", &rcData[AUX2]),
     OSD_INT16_RO_ENTRY("AUX3", &rcData[AUX3]),
     OSD_INT16_RO_ENTRY("AUX4", &rcData[AUX4]),
-
+#endif
     OSD_BACK_ENTRY,
     OSD_END_ENTRY,
 };

src/main/common/maths.c

@@ -139,6 +139,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
     return value;
 }
 
+float applyDeadbandf(float value, float deadband)
+{
+    if (ABS(value) < deadband) {
+        value = 0;
+    } else if (value > 0) {
+        value -= deadband;
+    } else if (value < 0) {
+        value += deadband;
+    }
+    return value;
+}
+
 int constrain(int amt, int low, int high)
 {
     if (amt < low)

src/main/common/maths.h

@@ -123,6 +123,7 @@ void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
 
 int gcd(int num, int denom);
 int32_t applyDeadband(int32_t value, int32_t deadband);
+float applyDeadbandf(float value, float deadband);
 
 int constrain(int amt, int low, int high);
 float constrainf(float amt, float low, float high);

src/main/fc/controlrate_profile.c

@@ -80,7 +80,7 @@ void setControlRateProfile(uint8_t profileIndex)
 
 void activateControlRateConfig(void)
 {
-    generateThrottleCurve(currentControlRateProfile);
+    rcCurveGenerateThrottle(currentControlRateProfile);
 }
 
 void changeControlRateProfile(uint8_t profileIndex)

src/main/fc/controlrate_profile.h

@@ -40,6 +40,12 @@ and so on.
 
 #define CONTROL_RATE_CONFIG_TPA_MAX              100
 
+typedef struct controlRateAxes_s {
+    uint8_t rcExpo8;
+    uint8_t rcYawExpo8;
+    uint8_t rates[3];
+} controlRateAxes_t;
+
 typedef struct controlRateConfig_s {
 
     struct {
@@ -50,17 +56,8 @@ typedef struct controlRateConfig_s {
         uint16_t fixedWingTauMs;               // Time constant of airplane TPA PT1-filter
     } throttle;
 
-    struct {
-        uint8_t rcExpo8;
-        uint8_t rcYawExpo8;
-        uint8_t rates[3];
-    } stabilized;
-
-    struct {
-        uint8_t rcExpo8;
-        uint8_t rcYawExpo8;
-        uint8_t rates[3];
-    } manual;
+    controlRateAxes_t stabilized;
+    controlRateAxes_t manual;
 
     struct {
         uint8_t fpvCamAngleDegrees;             // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis)

src/main/fc/fc_core.c

@@ -54,6 +54,7 @@
 #include "fc/controlrate_profile.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_smoothing.h"
+#include "fc/rc_control.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_curves.h"
 #include "fc/rc_modes.h"
@@ -142,16 +143,6 @@ bool isCalibrating(void)
     return false;
 }
 
-int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
-{
-    int16_t stickDeflection;
-
-    stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);
-    stickDeflection = applyDeadband(stickDeflection, deadband);
-
-    return rcLookup(stickDeflection, rate);
-}
-
 static void updateArmingStatus(void)
 {
     if (ARMING_FLAG(ARMED)) {
@@ -294,48 +285,6 @@ static void updateArmingStatus(void)
     }
 }
 
-void annexCode(void)
-{
-    int32_t throttleValue;
-
-    if (failsafeShouldApplyControlInput()) {
-        // Failsafe will apply rcCommand for us
-        failsafeApplyControlInput();
-    }
-    else {
-        // Compute ROLL PITCH and YAW command
-        rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
-        rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
-        rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.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]
-        rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
-
-        // Signal updated rcCommand values to Failsafe system
-        failsafeUpdateRcCommandValues();
-
-        if (FLIGHT_MODE(HEADFREE_MODE)) {
-            const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
-            const float cosDiff = cos_approx(radDiff);
-            const float sinDiff = sin_approx(radDiff);
-            const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
-            rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
-            rcCommand[PITCH] = rcCommand_PITCH;
-        }
-    }
-
-    updateArmingStatus();
-}
-
 void disarm(disarmReason_t disarmReason)
 {
     if (ARMING_FLAG(ARMED)) {
@@ -595,7 +544,7 @@ void processRx(timeUs_t currentTimeUs)
                 rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
 
                 // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs
-                if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) {
+                if ((rollPitchStatus == ROLL_PITCH_CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) {
                     ENABLE_STATE(ANTI_WINDUP);
                 }
                 else {
@@ -674,6 +623,8 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens
 
 void taskMainPidLoop(timeUs_t currentTimeUs)
 {
+    rcCommand_t controlOutput;
+
     cycleTime = getTaskDeltaTime(TASK_SELF);
     dT = (float)cycleTime * 0.000001f;
 
@@ -685,10 +636,27 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     imuUpdateAccelerometer();
     imuUpdateAttitude(currentTimeUs);
 
-    annexCode();
+    rcControlUpdateFromRX();
+
+    if (failsafeShouldApplyControlOutput()) {
+        // Failsafe will apply rcCommand for us
+        failsafeApplyControlOutput(&controlOutput);
+        rcControlUpdateOutput(&controlOutput, RC_CONTROL_SOURCE_FAILSAFE);
+    } else {
+        if (FLIGHT_MODE(HEADFREE_MODE)) {
+            float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
+            rcCommandRotate(&controlOutput, rcControlGetOutput(), radDiff);
+            rcControlUpdateOutput(&controlOutput, RC_CONTROL_SOURCE_HEADFREE);
+        }
+
+        // Signal updated rcCommand values to Failsafe system
+        failsafeUpdateLastGoodRcCommand(rcControlGetInput());
+    }
+
+    updateArmingStatus();
 
-    if (rxConfig()->rcFilterFrequency) {
-        rcInterpolationApply(isRXDataNew);
+    if (rxConfig()->rcFilterFrequency && rcInterpolationApply(&controlOutput, rcControlGetOutput(), isRXDataNew)) {
+        rcControlUpdateOutput(&controlOutput, RC_CONTROL_SOURCE_KEEP);
     }
 
 #if defined(USE_NAV)
@@ -708,11 +676,13 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     // sticks, do not process yaw input from the rx.  We do this so the
     // motors do not spin up while we are trying to arm or disarm.
     // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
-    if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
+    if (isUsingSticksForArming() && rxGetChannelValue(THROTTLE) <= rxConfig()->mincheck
             && !((mixerConfig()->platformType == PLATFORM_TRICOPTER) && servoConfig()->tri_unarmed_servo)
             && mixerConfig()->platformType != PLATFORM_AIRPLANE
     ) {
-        rcCommand[YAW] = 0;
+        rcCommandCopy(&controlOutput, rcControlGetOutput());
+        controlOutput.yaw = 0;
+        rcControlUpdateOutput(&controlOutput, RC_CONTROL_SOURCE_KEEP);
     }
 
     // Apply throttle tilt compensation
@@ -727,10 +697,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         }
 
         if (thrTiltCompStrength) {
-            rcCommand[THROTTLE] = constrain(motorConfig()->minthrottle
-                                            + (rcCommand[THROTTLE] - motorConfig()->minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
-                                            motorConfig()->minthrottle,
-                                            motorConfig()->maxthrottle);
+            rcCommandCopy(&controlOutput, rcControlGetOutput());
+            controlOutput.throttle = constrainf(controlOutput.throttle * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
+                                        RC_COMMAND_MIN, RC_COMMAND_MAX);
+            rcControlUpdateOutput(&controlOutput, RC_CONTROL_SOURCE_KEEP);
         }
     }
     else {

src/main/fc/fc_msp.c

@@ -436,7 +436,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 
     case MSP_SERVO:
-        sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
+        for (int ii = 0; ii < MAX_SUPPORTED_SERVOS; ii++) {
+            int16_t s = servosGetPWMValue(ii);
+            sbufWriteU16(dst, s);
+        }
         break;
     case MSP_SERVO_CONFIGURATIONS:
         for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
@@ -478,7 +481,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 
     case MSP_RC:
         for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
-            sbufWriteU16(dst, rcRaw[i]);
+            sbufWriteU16(dst, rxGetRawChannelValue(i));
         }
         break;
 
@@ -1762,7 +1765,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             sbufReadU8(src);
             sbufReadU8(src); // used to be forwardFromChannel, ignored
             sbufReadU32(src); // used to be reversedSources
-            servoComputeScalingFactors(tmp_u8);
+            servoComputeMetadata(tmp_u8);
         }
         break;
 

src/main/fc/rc_adjustments.c

@@ -601,9 +601,10 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxD
 
         if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
             int delta;
-            if (rcData[channelIndex] > PWM_RANGE_MIDDLE + 200) {
+            int16_t channelValue = rxGetChannelValue(channelIndex);
+            if (channelValue > PWM_RANGE_MIDDLE + 200) {
                 delta = adjustmentState->config->data.stepConfig.step;
-            } else if (rcData[channelIndex] < PWM_RANGE_MIDDLE - 200) {
+            } else if (channelValue < PWM_RANGE_MIDDLE - 200) {
                 delta = 0 - adjustmentState->config->data.stepConfig.step;
             } else {
                 // returning the switch to the middle immediately resets the ready state