Improvement: Add support for upto 18 channels. Allow RSSI to be read
from any channel, not just AUX1-4. See documentation changes in this commit.
This commit is contained in:
parent
126f94b2c1
commit
21207ca940
|
@ -120,8 +120,18 @@ set frsky_inversion = 1
|
||||||
|
|
||||||
## CLI command differences from baseflight
|
## CLI command differences from baseflight
|
||||||
|
|
||||||
```
|
### gps_provider / gps_type
|
||||||
gps_provider = gps_type
|
reason: renamed for consistency
|
||||||
serialrx_provider = serialrx_type
|
|
||||||
```
|
### 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.
|
||||||
|
|
||||||
|
|
|
@ -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.
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -54,7 +54,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
|
||||||
master_t masterConfig; // master config struct with data independent from profiles
|
master_t masterConfig; // master config struct with data independent from profiles
|
||||||
profile_t currentProfile; // profile config struct
|
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)
|
static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
|
||||||
{
|
{
|
||||||
|
@ -198,6 +198,8 @@ static void resetConf(void)
|
||||||
masterConfig.rxConfig.midrc = 1500;
|
masterConfig.rxConfig.midrc = 1500;
|
||||||
masterConfig.rxConfig.mincheck = 1100;
|
masterConfig.rxConfig.mincheck = 1100;
|
||||||
masterConfig.rxConfig.maxcheck = 1900;
|
masterConfig.rxConfig.maxcheck = 1900;
|
||||||
|
masterConfig.rxConfig.rssi_channel = 0;
|
||||||
|
|
||||||
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
|
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
|
||||||
masterConfig.airplaneConfig.flaps_speed = 0;
|
masterConfig.airplaneConfig.flaps_speed = 0;
|
||||||
masterConfig.fixedwing_althold_dir = 1;
|
masterConfig.fixedwing_althold_dir = 1;
|
||||||
|
@ -215,7 +217,6 @@ static void resetConf(void)
|
||||||
|
|
||||||
masterConfig.looptime = 3500;
|
masterConfig.looptime = 3500;
|
||||||
masterConfig.emf_avoidance = 0;
|
masterConfig.emf_avoidance = 0;
|
||||||
masterConfig.rssi_aux_channel = 0;
|
|
||||||
|
|
||||||
currentProfile.pidController = 0;
|
currentProfile.pidController = 0;
|
||||||
resetPidProfile(¤tProfile.pidProfile);
|
resetPidProfile(¤tProfile.pidProfile);
|
||||||
|
|
|
@ -48,7 +48,7 @@ typedef struct master_t {
|
||||||
airplaneConfig_t airplaneConfig;
|
airplaneConfig_t airplaneConfig;
|
||||||
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
|
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
|
// gps-related stuff
|
||||||
gpsProvider_e gps_provider;
|
gpsProvider_e gps_provider;
|
||||||
|
|
|
@ -3,8 +3,6 @@
|
||||||
#define MAX_PWM_MOTORS 12
|
#define MAX_PWM_MOTORS 12
|
||||||
#define MAX_PWM_SERVOS 8
|
#define MAX_PWM_SERVOS 8
|
||||||
|
|
||||||
#define MAX_PWM_INPUT_PORTS 8
|
|
||||||
|
|
||||||
#define MAX_MOTORS 12
|
#define MAX_MOTORS 12
|
||||||
#define MAX_SERVOS 8
|
#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
|
#define MAX_PWM_OUTPUT_PORTS MAX_PWM_MOTORS // must be set to the largest of either MAX_MOTORS or MAX_SERVOS
|
||||||
|
|
|
@ -12,6 +12,16 @@
|
||||||
|
|
||||||
#include "pwm_rx.h"
|
#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
|
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); // from pwm_output.c
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -31,9 +41,9 @@ typedef struct {
|
||||||
const timerHardware_t *timerHardware;
|
const timerHardware_t *timerHardware;
|
||||||
} pwmInputPort_t;
|
} 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)
|
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."
|
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;
|
chan = 0;
|
||||||
} else {
|
} else {
|
||||||
if (chan < MAX_PWM_INPUT_PORTS) {
|
if (chan < PPM_CAPTURE_COUNT) {
|
||||||
captures[chan] = diff;
|
captures[chan] = diff;
|
||||||
}
|
}
|
||||||
chan++;
|
chan++;
|
||||||
|
|
|
@ -197,7 +197,7 @@ int16_t determineServoMiddleOrForwardFromChannel(int nr)
|
||||||
{
|
{
|
||||||
uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel;
|
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];
|
return rcData[channelToForwardFrom];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
11
src/mw.c
11
src/mw.c
|
@ -103,7 +103,7 @@ void annexCode(void)
|
||||||
|
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (axis = 0; axis < 3; axis++) {
|
||||||
tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
|
tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
|
||||||
if (axis != 2) { // ROLL & PITCH
|
if (axis == ROLL || axis == PITCH) {
|
||||||
if (currentProfile.deadband) {
|
if (currentProfile.deadband) {
|
||||||
if (tmp > currentProfile.deadband) {
|
if (tmp > currentProfile.deadband) {
|
||||||
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;
|
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
|
||||||
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500;
|
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500;
|
||||||
prop1 = (uint16_t)prop1 * prop2 / 100;
|
prop1 = (uint16_t)prop1 * prop2 / 100;
|
||||||
} else { // YAW
|
}
|
||||||
|
if (axis == YAW) {
|
||||||
if (currentProfile.yaw_deadband) {
|
if (currentProfile.yaw_deadband) {
|
||||||
if (tmp > currentProfile.yaw_deadband) {
|
if (tmp > currentProfile.yaw_deadband) {
|
||||||
tmp -= currentProfile.yaw_deadband;
|
tmp -= currentProfile.yaw_deadband;
|
||||||
|
@ -127,6 +128,7 @@ void annexCode(void)
|
||||||
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
|
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
|
||||||
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500;
|
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;
|
dynP8[axis] = (uint16_t)currentProfile.pidProfile.P8[axis] * prop1 / 100;
|
||||||
dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100;
|
dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100;
|
||||||
dynD8[axis] = (uint16_t)currentProfile.pidProfile.D8[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
|
// Read value of AUX channel as rssi
|
||||||
// 0 is disable, 1-4 is AUX{1..4}
|
if (masterConfig.rxConfig.rssi_channel > 0) {
|
||||||
if (masterConfig.rssi_aux_channel > 0) {
|
const int16_t rssiChannelData = rcData[masterConfig.rxConfig.rssi_channel - 1];
|
||||||
const int16_t rssiChannelData = rcData[AUX1 + masterConfig.rssi_aux_channel - 1];
|
|
||||||
// Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023];
|
// Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023];
|
||||||
rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f);
|
rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f);
|
||||||
}
|
}
|
||||||
|
|
|
@ -128,14 +128,19 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t calculateChannelRemapping(uint8_t *rcmap, uint8_t channelToRemap) {
|
uint8_t calculateChannelRemapping(uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap)
|
||||||
return rcmap[channelToRemap];
|
{
|
||||||
|
if (channelToRemap < channelMapEntryCount) {
|
||||||
|
return channelMap[channelToRemap];
|
||||||
|
}
|
||||||
|
return channelToRemap;
|
||||||
}
|
}
|
||||||
|
|
||||||
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
{
|
{
|
||||||
uint8_t chan;
|
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;
|
static uint8_t rcSampleIndex = 0;
|
||||||
uint8_t currentSampleIndex = 0;
|
uint8_t currentSampleIndex = 0;
|
||||||
|
|
||||||
|
@ -148,14 +153,14 @@ void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT;
|
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) {
|
if (!rcReadRawFunc) {
|
||||||
rcData[chan] = rxConfig->midrc;
|
rcData[chan] = rxConfig->midrc;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, chan);
|
uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, chan);
|
||||||
|
|
||||||
// sample the channel
|
// sample the channel
|
||||||
uint16_t sample = rcReadRawFunc(rxRuntimeConfig, rawChannel);
|
uint16_t sample = rcReadRawFunc(rxRuntimeConfig, rawChannel);
|
||||||
|
|
|
@ -18,9 +18,16 @@ typedef enum {
|
||||||
|
|
||||||
#define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1)
|
#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)
|
#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 const char rcChannelLetters[];
|
||||||
|
|
||||||
extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
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 midrc; // Some radios have not a neutral point centered on 1500. can be changed here
|
||||||
uint16_t mincheck; // minimum rc end
|
uint16_t mincheck; // minimum rc end
|
||||||
uint16_t maxcheck; // maximum rc end
|
uint16_t maxcheck; // maximum rc end
|
||||||
|
uint8_t rssi_channel;
|
||||||
} rxConfig_t;
|
} rxConfig_t;
|
||||||
|
|
||||||
|
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
|
||||||
|
|
||||||
typedef struct rxRuntimeConfig_s {
|
typedef struct rxRuntimeConfig_s {
|
||||||
uint8_t channelCount; // number of rc channels as reported by current input driver
|
uint8_t channelCount; // number of rc channels as reported by current input driver
|
||||||
} rxRuntimeConfig_t;
|
} rxRuntimeConfig_t;
|
||||||
|
|
|
@ -35,6 +35,7 @@ bool rxMspFrameComplete(void)
|
||||||
|
|
||||||
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||||
{
|
{
|
||||||
|
rxRuntimeConfig->channelCount = 8; // See MSP_SET_RAW_RC
|
||||||
if (callback)
|
if (callback)
|
||||||
*callback = rxMspReadRawRC;
|
*callback = rxMspReadRawRC;
|
||||||
|
|
||||||
|
|
|
@ -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
|
// configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled
|
||||||
*callback = pwmReadRawRC;
|
*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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -13,9 +13,7 @@
|
||||||
#include "rx_common.h"
|
#include "rx_common.h"
|
||||||
#include "rx_sbus.h"
|
#include "rx_sbus.h"
|
||||||
|
|
||||||
// driver for SBUS receiver using UART2
|
#define SBUS_MAX_CHANNEL 12
|
||||||
|
|
||||||
#define SBUS_MAX_CHANNEL 8
|
|
||||||
#define SBUS_FRAME_SIZE 25
|
#define SBUS_FRAME_SIZE 25
|
||||||
#define SBUS_SYNCBYTE 0x0F
|
#define SBUS_SYNCBYTE 0x0F
|
||||||
#define SBUS_OFFSET 988
|
#define SBUS_OFFSET 988
|
||||||
|
@ -118,7 +116,10 @@ bool sbusFrameComplete(void)
|
||||||
sbusChannelData[5] = sbus.msg.chan5;
|
sbusChannelData[5] = sbus.msg.chan5;
|
||||||
sbusChannelData[6] = sbus.msg.chan6;
|
sbusChannelData[6] = sbus.msg.chan6;
|
||||||
sbusChannelData[7] = sbus.msg.chan7;
|
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -13,7 +13,7 @@
|
||||||
#include "rx_common.h"
|
#include "rx_common.h"
|
||||||
#include "rx_spektrum.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_MAX_SUPPORTED_CHANNEL_COUNT 12
|
||||||
#define SPEKTRUM_2048_CHANNEL_COUNT 12
|
#define SPEKTRUM_2048_CHANNEL_COUNT 12
|
||||||
|
|
|
@ -15,9 +15,11 @@
|
||||||
|
|
||||||
// driver for SUMD receiver using UART2
|
// 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_SYNCBYTE 0xA8
|
||||||
#define SUMD_MAX_CHANNEL 8
|
#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
|
#define SUMD_BAUDRATE 115200
|
||||||
|
|
||||||
|
|
|
@ -149,6 +149,7 @@ const clivalue_t valueTable[] = {
|
||||||
{ "midrc", VAR_UINT16, &masterConfig.rxConfig.midrc, 1200, 1700 },
|
{ "midrc", VAR_UINT16, &masterConfig.rxConfig.midrc, 1200, 1700 },
|
||||||
{ "mincheck", VAR_UINT16, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
{ "mincheck", VAR_UINT16, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
||||||
{ "maxcheck", VAR_UINT16, &masterConfig.rxConfig.maxcheck, 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 },
|
{ "minthrottle", VAR_UINT16, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
||||||
{ "maxthrottle", VAR_UINT16, &masterConfig.escAndServoConfig.maxthrottle, 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_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 },
|
{ "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},
|
{ "gimbal_flags", VAR_UINT8, ¤tProfile.gimbalConfig.gimbal_flags, 0, 255},
|
||||||
|
|
||||||
{ "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 },
|
{ "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 },
|
||||||
|
|
|
@ -330,6 +330,7 @@ static void evaluateCommand(void)
|
||||||
|
|
||||||
switch (cmdMSP) {
|
switch (cmdMSP) {
|
||||||
case MSP_SET_RAW_RC:
|
case MSP_SET_RAW_RC:
|
||||||
|
// FIXME need support for more than 8 channels
|
||||||
for (i = 0; i < 8; i++)
|
for (i = 0; i < 8; i++)
|
||||||
rcData[i] = read16();
|
rcData[i] = read16();
|
||||||
headSerialReply(0);
|
headSerialReply(0);
|
||||||
|
@ -505,8 +506,8 @@ static void evaluateCommand(void)
|
||||||
s_struct((uint8_t *)motor, 16);
|
s_struct((uint8_t *)motor, 16);
|
||||||
break;
|
break;
|
||||||
case MSP_RC:
|
case MSP_RC:
|
||||||
headSerialReply(16);
|
headSerialReply(2 * rxRuntimeConfig.channelCount);
|
||||||
for (i = 0; i < 8; i++)
|
for (i = 0; i < rxRuntimeConfig.channelCount; i++)
|
||||||
serialize16(rcData[i]);
|
serialize16(rcData[i]);
|
||||||
break;
|
break;
|
||||||
case MSP_RAW_GPS:
|
case MSP_RAW_GPS:
|
||||||
|
|
Loading…
Reference in New Issue