diff --git a/docs/Configuration.md b/docs/Configuration.md index 253187620..11eb52b2d 100644 --- a/docs/Configuration.md +++ b/docs/Configuration.md @@ -120,8 +120,18 @@ set frsky_inversion = 1 ## CLI command differences from baseflight -``` -gps_provider = gps_type -serialrx_provider = serialrx_type -``` +### gps_provider / gps_type +reason: renamed for consistency + +### serialrx_provider / serialrx_type +reason: renamed for consistency + +### rssi_channel / rssi_aux_channel +reason: improved functionality + +Cleanflight supports using any RX channel for rssi. Baseflight only supports AUX1 to 4. + +In Cleanflight a value of 0 disables the feature, a higher value indicates the channel number to read RSSI information from. + +Example, to use RSSI on AUX1 in Cleanflight set the value to 5, since 5 is the first AUX channel. diff --git a/docs/Rx.md b/docs/Rx.md new file mode 100644 index 000000000..8e5bf19ef --- /dev/null +++ b/docs/Rx.md @@ -0,0 +1,29 @@ +# Receivers (RX) + +## Parallel PWM + +8 channel support, 1 channel per input pin. On some platforms using parallel input will disable the use of serial ports +and softserial making it hard to use telemetry or gps features. + +## PPM (PPM SUM) + +12 channels via a single input pin, not as accurate or jitter free as methods that use serial communications. + +## MultiWii serial protocol (MSP) + +Allows you to use MSP commands as the RC input. Only 8 channel support to maintain compatibility with MSP. + +## Spektrum + +12 channels via serial currently supported. + +## SUMD + +8 channels supported currently, 12 or more is technically possible. + +## SBUS + +12 channels via serial supported currently. + + + diff --git a/src/config.c b/src/config.c index 4bb846f26..3ba2eea33 100755 --- a/src/config.c +++ b/src/config.c @@ -54,7 +54,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon master_t masterConfig; // master config struct with data independent from profiles profile_t currentProfile; // profile config struct -static const uint8_t EEPROM_CONF_VERSION = 66; +static const uint8_t EEPROM_CONF_VERSION = 67; static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims) { @@ -198,6 +198,8 @@ static void resetConf(void) masterConfig.rxConfig.midrc = 1500; masterConfig.rxConfig.mincheck = 1100; masterConfig.rxConfig.maxcheck = 1900; + masterConfig.rxConfig.rssi_channel = 0; + masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right masterConfig.airplaneConfig.flaps_speed = 0; masterConfig.fixedwing_althold_dir = 1; @@ -215,7 +217,6 @@ static void resetConf(void) masterConfig.looptime = 3500; masterConfig.emf_avoidance = 0; - masterConfig.rssi_aux_channel = 0; currentProfile.pidController = 0; resetPidProfile(¤tProfile.pidProfile); diff --git a/src/config_master.h b/src/config_master.h index 32da38897..b74f6ab37 100644 --- a/src/config_master.h +++ b/src/config_master.h @@ -48,7 +48,7 @@ typedef struct master_t { airplaneConfig_t airplaneConfig; int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign - uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable. + uint8_t rssi_channel; // Read rssi from channel. 0 to disable, 1 = channel 1, etc. // gps-related stuff gpsProvider_e gps_provider; diff --git a/src/drivers/pwm_mapping.h b/src/drivers/pwm_mapping.h index e34a43fc2..fb82eca7d 100755 --- a/src/drivers/pwm_mapping.h +++ b/src/drivers/pwm_mapping.h @@ -3,8 +3,6 @@ #define MAX_PWM_MOTORS 12 #define MAX_PWM_SERVOS 8 -#define MAX_PWM_INPUT_PORTS 8 - #define MAX_MOTORS 12 #define MAX_SERVOS 8 #define MAX_PWM_OUTPUT_PORTS MAX_PWM_MOTORS // must be set to the largest of either MAX_MOTORS or MAX_SERVOS diff --git a/src/drivers/pwm_rx.c b/src/drivers/pwm_rx.c index 0ede9d6b7..71149481f 100644 --- a/src/drivers/pwm_rx.c +++ b/src/drivers/pwm_rx.c @@ -12,6 +12,16 @@ #include "pwm_rx.h" + +#define PPM_CAPTURE_COUNT 12 +#define PWM_INPUT_PORT_COUNT 8 + +#if PPM_CAPTURE_COUNT > MAX_PWM_INPUT_PORTS +#define PWM_PORTS_OR_PPM_CAPTURE_COUNT PPM_CAPTURE_COUNT +#else +#define PWM_PORTS_OR_PPM_CAPTURE_COUNT PWM_INPUT_PORT_COUNT +#endif + void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); // from pwm_output.c typedef enum { @@ -31,9 +41,9 @@ typedef struct { const timerHardware_t *timerHardware; } pwmInputPort_t; -static pwmInputPort_t pwmInputPorts[MAX_PWM_INPUT_PORTS]; +static pwmInputPort_t pwmInputPorts[PWM_INPUT_PORT_COUNT]; -static uint16_t captures[MAX_PWM_INPUT_PORTS]; +static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT]; static void ppmCallback(uint8_t port, captureCompare_t capture) { @@ -50,7 +60,7 @@ static void ppmCallback(uint8_t port, captureCompare_t capture) if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe." chan = 0; } else { - if (chan < MAX_PWM_INPUT_PORTS) { + if (chan < PPM_CAPTURE_COUNT) { captures[chan] = diff; } chan++; diff --git a/src/flight_mixer.c b/src/flight_mixer.c index 878c0b75a..c4c864e59 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -197,7 +197,7 @@ int16_t determineServoMiddleOrForwardFromChannel(int nr) { uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel; - if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom << MAX_SUPPORTED_RC_CHANNEL_COUNT) { + if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) { return rcData[channelToForwardFrom]; } diff --git a/src/mw.c b/src/mw.c index 26b7abd35..9a1270b62 100755 --- a/src/mw.c +++ b/src/mw.c @@ -103,7 +103,7 @@ void annexCode(void) for (axis = 0; axis < 3; axis++) { tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); - if (axis != 2) { // ROLL & PITCH + if (axis == ROLL || axis == PITCH) { if (currentProfile.deadband) { if (tmp > currentProfile.deadband) { tmp -= currentProfile.deadband; @@ -116,7 +116,8 @@ void annexCode(void) rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; - } else { // YAW + } + if (axis == YAW) { if (currentProfile.yaw_deadband) { if (tmp > currentProfile.yaw_deadband) { tmp -= currentProfile.yaw_deadband; @@ -127,6 +128,7 @@ void annexCode(void) rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500; } + // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile.pidProfile.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile.pidProfile.D8[axis] * prop1 / 100; @@ -262,9 +264,8 @@ void loop(void) } // Read value of AUX channel as rssi - // 0 is disable, 1-4 is AUX{1..4} - if (masterConfig.rssi_aux_channel > 0) { - const int16_t rssiChannelData = rcData[AUX1 + masterConfig.rssi_aux_channel - 1]; + if (masterConfig.rxConfig.rssi_channel > 0) { + const int16_t rssiChannelData = rcData[masterConfig.rxConfig.rssi_channel - 1]; // Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023]; rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f); } diff --git a/src/rx_common.c b/src/rx_common.c index 1bfe901dc..04ee5112a 100644 --- a/src/rx_common.c +++ b/src/rx_common.c @@ -128,14 +128,19 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig) return false; } -uint8_t calculateChannelRemapping(uint8_t *rcmap, uint8_t channelToRemap) { - return rcmap[channelToRemap]; +uint8_t calculateChannelRemapping(uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap) +{ + if (channelToRemap < channelMapEntryCount) { + return channelMap[channelToRemap]; + } + return channelToRemap; } void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { uint8_t chan; - static int16_t rcSamples[MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT], rcDataMean[MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT]; + static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT]; + static int16_t rcDataMean[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT]; static uint8_t rcSampleIndex = 0; uint8_t currentSampleIndex = 0; @@ -148,14 +153,14 @@ void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT; } - for (chan = 0; chan < MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT; chan++) { + for (chan = 0; chan < rxRuntimeConfig->channelCount; chan++) { if (!rcReadRawFunc) { rcData[chan] = rxConfig->midrc; continue; } - uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, chan); + uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, chan); // sample the channel uint16_t sample = rcReadRawFunc(rxRuntimeConfig, rawChannel); diff --git a/src/rx_common.h b/src/rx_common.h index 878b987ec..9ccf48fbd 100644 --- a/src/rx_common.h +++ b/src/rx_common.h @@ -18,9 +18,16 @@ typedef enum { #define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1) -#define MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT 8 +#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12 +#define MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT 8 #define MAX_SUPPORTED_RC_CHANNEL_COUNT (18) +#if MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT > MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT +#define MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT +#else +#define MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT +#endif + extern const char rcChannelLetters[]; extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] @@ -31,8 +38,11 @@ typedef struct rxConfig_s { uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end + uint8_t rssi_channel; } rxConfig_t; +#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) + typedef struct rxRuntimeConfig_s { uint8_t channelCount; // number of rc channels as reported by current input driver } rxRuntimeConfig_t; diff --git a/src/rx_msp.c b/src/rx_msp.c index e81cf0f23..a7746cce1 100644 --- a/src/rx_msp.c +++ b/src/rx_msp.c @@ -35,6 +35,7 @@ bool rxMspFrameComplete(void) bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { + rxRuntimeConfig->channelCount = 8; // See MSP_SET_RAW_RC if (callback) *callback = rxMspReadRawRC; diff --git a/src/rx_pwm.c b/src/rx_pwm.c index 2db3cdc7d..abad0b3db 100644 --- a/src/rx_pwm.c +++ b/src/rx_pwm.c @@ -22,6 +22,12 @@ void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled *callback = pwmReadRawRC; - rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT; + + if (feature(FEATURE_RX_PARALLEL_PWM)) { + rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; + } + if (feature(FEATURE_RX_PPM)) { + rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; + } } diff --git a/src/rx_sbus.c b/src/rx_sbus.c index f20689d35..69f58bea5 100644 --- a/src/rx_sbus.c +++ b/src/rx_sbus.c @@ -13,9 +13,7 @@ #include "rx_common.h" #include "rx_sbus.h" -// driver for SBUS receiver using UART2 - -#define SBUS_MAX_CHANNEL 8 +#define SBUS_MAX_CHANNEL 12 #define SBUS_FRAME_SIZE 25 #define SBUS_SYNCBYTE 0x0F #define SBUS_OFFSET 988 @@ -118,7 +116,10 @@ bool sbusFrameComplete(void) sbusChannelData[5] = sbus.msg.chan5; sbusChannelData[6] = sbus.msg.chan6; sbusChannelData[7] = sbus.msg.chan7; - // need more channels? No problem. Add them. + sbusChannelData[8] = sbus.msg.chan8; + sbusChannelData[9] = sbus.msg.chan9; + sbusChannelData[10] = sbus.msg.chan10; + sbusChannelData[11] = sbus.msg.chan11; return true; } diff --git a/src/rx_spektrum.c b/src/rx_spektrum.c index f371f0b2a..21375188e 100644 --- a/src/rx_spektrum.c +++ b/src/rx_spektrum.c @@ -13,7 +13,7 @@ #include "rx_common.h" #include "rx_spektrum.h" -// driver for spektrum satellite receiver / sbus using UART2 (freeing up more motor outputs for stuff) +// driver for spektrum satellite receiver / sbus #define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12 #define SPEKTRUM_2048_CHANNEL_COUNT 12 diff --git a/src/rx_sumd.c b/src/rx_sumd.c index a8f2508f8..4645c33a1 100644 --- a/src/rx_sumd.c +++ b/src/rx_sumd.c @@ -15,9 +15,11 @@ // driver for SUMD receiver using UART2 +// FIXME test support for more than 8 channels, should probably work up to 12 channels + #define SUMD_SYNCBYTE 0xA8 #define SUMD_MAX_CHANNEL 8 -#define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 -> 17 bytes for 6 channels +#define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 = 17 bytes for 6 channels #define SUMD_BAUDRATE 115200 diff --git a/src/serial_cli.c b/src/serial_cli.c index c503a5329..ba4c66251 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -149,6 +149,7 @@ const clivalue_t valueTable[] = { { "midrc", VAR_UINT16, &masterConfig.rxConfig.midrc, 1200, 1700 }, { "mincheck", VAR_UINT16, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "maxcheck", VAR_UINT16, &masterConfig.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "rssi_channel", VAR_INT8, &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT }, { "minthrottle", VAR_UINT16, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "maxthrottle", VAR_UINT16, &masterConfig.escAndServoConfig.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, @@ -235,9 +236,6 @@ const clivalue_t valueTable[] = { { "failsafe_throttle", VAR_UINT16, ¤tProfile.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX }, { "failsafe_detect_threshold", VAR_UINT16, ¤tProfile.failsafeConfig.failsafe_detect_threshold, 100, PWM_RANGE_MAX }, - { "rssi_aux_channel", VAR_INT8, &masterConfig.rssi_aux_channel, 0, 4 }, - - { "gimbal_flags", VAR_UINT8, ¤tProfile.gimbalConfig.gimbal_flags, 0, 255}, { "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 }, diff --git a/src/serial_msp.c b/src/serial_msp.c index b92252177..2bd3d2f31 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -330,6 +330,7 @@ static void evaluateCommand(void) switch (cmdMSP) { case MSP_SET_RAW_RC: + // FIXME need support for more than 8 channels for (i = 0; i < 8; i++) rcData[i] = read16(); headSerialReply(0); @@ -505,8 +506,8 @@ static void evaluateCommand(void) s_struct((uint8_t *)motor, 16); break; case MSP_RC: - headSerialReply(16); - for (i = 0; i < 8; i++) + headSerialReply(2 * rxRuntimeConfig.channelCount); + for (i = 0; i < rxRuntimeConfig.channelCount; i++) serialize16(rcData[i]); break; case MSP_RAW_GPS: