src/main/rx/msp_override.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include "platform.h"
  22
  23 #include "build/build_config.h"
  24 #include "build/debug.h"
  25
  26 #include "common/maths.h"
  27 #include "common/utils.h"
  28
  29 #include "config/feature.h"
  30 #include "config/parameter_group.h"
  31 #include "config/parameter_group_ids.h"
  32
  33 #include "drivers/time.h"
  34
  35 #include "fc/config.h"
  36 #include "fc/rc_controls.h"
  37 #include "fc/rc_modes.h"
  38
  39 #include "flight/failsafe.h"
  40
  41 #include "rx/rx.h"
  42 #include "rx/msp.h"
  43 #include "rx/msp_override.h"
  44
  45
  46 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
  47
  48 static bool rxDataProcessingRequired = false;
  49
  50 static bool rxSignalReceived = false;
  51 static bool rxFlightChannelsValid = false;
  52 static bool rxFailsafe = true;
  53
  54 static timeMs_t rxDataFailurePeriod;
  55 static timeMs_t rxDataRecoveryPeriod;
  56 static timeMs_t validRxDataReceivedAt = 0;
  57 static timeMs_t validRxDataFailedAt = 0;
  58
  59 static timeUs_t rxNextUpdateAtUs = 0;
  60 static timeUs_t needRxSignalBefore = 0;
  61
  62 static uint16_t mspOverrideCtrlChannels = 0; // bitmask representing which channels are used to control MSP override
  63 static rcChannel_t mspRcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  64
  65 static rxRuntimeConfig_t rxRuntimeConfigMSP;
  66
  67
  68 void mspOverrideInit(void)
  69 {
  70     timeMs_t nowMs = millis();
  71
  72     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
  73         mspRcChannels[i].raw = PWM_RANGE_MIDDLE;
  74         mspRcChannels[i].data = PWM_RANGE_MIDDLE;
  75         mspRcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME;
  76     }
  77
  78     mspRcChannels[THROTTLE].raw = (feature(FEATURE_REVERSIBLE_MOTORS)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
  79     mspRcChannels[THROTTLE].data = mspRcChannels[THROTTLE].raw;
  80
  81     // Initialize ARM switch to OFF position when arming via switch is defined
  82     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
  83         if (modeActivationConditions(i)->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) {
  84             // ARM switch is defined, determine an OFF value
  85             uint16_t value;
  86             if (modeActivationConditions(i)->range.startStep > 0) {
  87                 value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.startStep - 1));
  88             } else {
  89                 value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.endStep + 1));
  90             }
  91             // Initialize ARM AUX channel to OFF value
  92             rcChannel_t *armChannel = &mspRcChannels[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT];
  93             armChannel->raw = value;
  94             armChannel->data = value;
  95         }
  96
  97         // Find which channels are used to control MSP override
  98         if (modeActivationConditions(i)->modeId == BOXMSPRCOVERRIDE && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) {
  99             mspOverrideCtrlChannels |= 1 << (modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT);
 100         }
 101     }
 102
 103     rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
 104     rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
 105
 106     rxMspInit(rxConfig(), &rxRuntimeConfigMSP);
 107 }
 108
 109 bool mspOverrideIsReceivingSignal(void)
 110 {
 111     return rxSignalReceived;
 112 }
 113
 114 bool mspOverrideAreFlightChannelsValid(void)
 115 {
 116     return rxFlightChannelsValid;
 117 }
 118
 119 bool mspOverrideIsInFailsafe(void)
 120 {
 121     return rxFailsafe;
 122 }
 123
 124 bool mspOverrideUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
 125 {
 126     UNUSED(currentDeltaTime);
 127
 128     if (rxSignalReceived) {
 129         if (currentTimeUs >= needRxSignalBefore) {
 130             rxSignalReceived = false;
 131         }
 132     }
 133
 134     const uint8_t frameStatus = rxRuntimeConfigMSP.rcFrameStatusFn(&rxRuntimeConfigMSP);
 135     if (frameStatus & RX_FRAME_COMPLETE) {
 136         rxDataProcessingRequired = true;
 137         rxSignalReceived = true;
 138         needRxSignalBefore = currentTimeUs + rxRuntimeConfigMSP.rxSignalTimeout;
 139     }
 140
 141     if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
 142         rxDataProcessingRequired = true;
 143     }
 144
 145     return rxDataProcessingRequired; // data driven or 50Hz
 146 }
 147
 148 bool mspOverrideCalculateChannels(timeUs_t currentTimeUs)
 149 {
 150     int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 151     const timeMs_t currentTimeMs = millis();
 152
 153     if (!rxDataProcessingRequired) {
 154         return false;
 155     }
 156
 157     rxDataProcessingRequired = false;
 158     rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ;
 159
 160     rxFlightChannelsValid = true;
 161
 162     // Read and process channel data
 163     for (int channel = 0; channel < rxRuntimeConfigMSP.channelCount; channel++) {
 164         const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
 165
 166         // sample the channel
 167         uint16_t sample = (*rxRuntimeConfigMSP.rcReadRawFn)(&rxRuntimeConfigMSP, rawChannel);
 168
 169         // apply the rx calibration to flight channel
 170         if (channel < NON_AUX_CHANNEL_COUNT && sample != 0) {
 171             sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
 172             sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
 173         }
 174
 175         // Store as rxRaw
 176         mspRcChannels[channel].raw = sample;
 177
 178         // Apply invalid pulse value logic
 179         if (!isRxPulseValid(sample)) {
 180             sample = mspRcChannels[channel].data;   // hold channel, replace with old value
 181             if ((currentTimeMs > mspRcChannels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) {
 182                 rxFlightChannelsValid = false;
 183             }
 184         } else {
 185             mspRcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_RX_PULSE_TIME;
 186         }
 187
 188         // Save channel value
 189         rcStaging[channel] = sample;
 190     }
 191
 192     // Update channel input value if receiver is not in failsafe mode
 193     // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained
 194     if (rxFlightChannelsValid && rxSignalReceived) {
 195         for (int channel = 0; channel < rxRuntimeConfigMSP.channelCount; channel++) {
 196             mspRcChannels[channel].data = rcStaging[channel];
 197         }
 198     }
 199
 200     // Update failsafe
 201     if (rxFlightChannelsValid && rxSignalReceived) {
 202         validRxDataReceivedAt = millis();
 203         if ((validRxDataReceivedAt - validRxDataFailedAt) > rxDataRecoveryPeriod) {
 204             rxFailsafe = false;
 205         }
 206     } else {
 207         validRxDataFailedAt = millis();
 208         if ((validRxDataFailedAt - validRxDataReceivedAt) > rxDataFailurePeriod) {
 209             rxFailsafe = true;
 210         }
 211     }
 212
 213     return true;
 214 }
 215
 216 void mspOverrideChannels(rcChannel_t *rcChannels)
 217 {
 218     for (uint16_t channel = 0, channelMask = 1; channel < rxRuntimeConfigMSP.channelCount; ++channel, channelMask <<= 1) {
 219         if (rxConfig()->mspOverrideChannels & ~mspOverrideCtrlChannels & channelMask) {
 220             rcChannels[channel].raw = rcChannels[channel].data = mspRcChannels[channel].data;
 221         }
 222     }
 223 }
 224
 225 int16_t mspOverrideGetChannelValue(unsigned channelNumber)
 226 {
 227     return mspRcChannels[channelNumber].data;
 228 }
 229
 230 int16_t mspOverrideGetRawChannelValue(unsigned channelNumber)
 231 {
 232     return mspRcChannels[channelNumber].raw;
 233 }
 234
 235 #endif // defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)