mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 21:35:44 +03:00
Preserve resolution of RC input
This commit is contained in:
parent
858d593c51
commit
8c28533c69
19 changed files with 63 additions and 47 deletions
|
@ -22,6 +22,7 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <ctype.h>
|
#include <ctype.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
@ -79,6 +80,20 @@ static const void *cmsx_menuRcConfirmBack(displayPort_t *pDisp, const OSD_Entry
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static int16_t rcDataInt[AUX4 + 1];
|
||||||
|
|
||||||
|
static const void *cmsx_menuRcOnDisplayUpdate(displayPort_t *pDisp, const OSD_Entry *selected)
|
||||||
|
{
|
||||||
|
UNUSED(pDisp);
|
||||||
|
UNUSED(selected);
|
||||||
|
|
||||||
|
for (int i = 0; i <= AUX4; i++) {
|
||||||
|
rcDataInt[i] = lroundf(rcData[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
// RC preview
|
// RC preview
|
||||||
//
|
//
|
||||||
|
@ -86,15 +101,15 @@ static const OSD_Entry cmsx_menuRcEntries[] =
|
||||||
{
|
{
|
||||||
{ "-- RC PREV --", OME_Label, NULL, NULL, 0},
|
{ "-- RC PREV --", OME_Label, NULL, NULL, 0},
|
||||||
|
|
||||||
{ "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[ROLL], 1, 2500, 0 }, DYNAMIC },
|
{ "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[ROLL], 1, 2500, 0 }, DYNAMIC },
|
||||||
{ "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[PITCH], 1, 2500, 0 }, DYNAMIC },
|
{ "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[PITCH], 1, 2500, 0 }, DYNAMIC },
|
||||||
{ "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[THROTTLE], 1, 2500, 0 }, DYNAMIC },
|
{ "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[THROTTLE], 1, 2500, 0 }, DYNAMIC },
|
||||||
{ "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[YAW], 1, 2500, 0 }, DYNAMIC },
|
{ "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[YAW], 1, 2500, 0 }, DYNAMIC },
|
||||||
|
|
||||||
{ "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX1], 1, 2500, 0 }, DYNAMIC },
|
{ "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX1], 1, 2500, 0 }, DYNAMIC },
|
||||||
{ "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX2], 1, 2500, 0 }, DYNAMIC },
|
{ "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX2], 1, 2500, 0 }, DYNAMIC },
|
||||||
{ "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX3], 1, 2500, 0 }, DYNAMIC },
|
{ "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX3], 1, 2500, 0 }, DYNAMIC },
|
||||||
{ "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX4], 1, 2500, 0 }, DYNAMIC },
|
{ "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX4], 1, 2500, 0 }, DYNAMIC },
|
||||||
|
|
||||||
{ "BACK", OME_Back, NULL, NULL, 0},
|
{ "BACK", OME_Back, NULL, NULL, 0},
|
||||||
{NULL, OME_END, NULL, NULL, 0}
|
{NULL, OME_END, NULL, NULL, 0}
|
||||||
|
@ -107,7 +122,7 @@ CMS_Menu cmsx_menuRcPreview = {
|
||||||
#endif
|
#endif
|
||||||
.onEnter = cmsx_menuRcOnEnter,
|
.onEnter = cmsx_menuRcOnEnter,
|
||||||
.onExit = cmsx_menuRcConfirmBack,
|
.onExit = cmsx_menuRcConfirmBack,
|
||||||
.onDisplayUpdate = NULL,
|
.onDisplayUpdate = cmsx_menuRcOnDisplayUpdate,
|
||||||
.entries = cmsx_menuRcEntries
|
.entries = cmsx_menuRcEntries
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -763,7 +763,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
|
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
|
||||||
|
|
||||||
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
|
float tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
|
||||||
if (axis == ROLL || axis == PITCH) {
|
if (axis == ROLL || axis == PITCH) {
|
||||||
if (tmp > rcControlsConfig()->deadband) {
|
if (tmp > rcControlsConfig()->deadband) {
|
||||||
tmp -= rcControlsConfig()->deadband;
|
tmp -= rcControlsConfig()->deadband;
|
||||||
|
|
|
@ -345,7 +345,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
return RX_FRAME_PENDING;
|
return RX_FRAME_PENDING;
|
||||||
}
|
}
|
||||||
|
|
||||||
STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
STATIC_UNIT_TESTED float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
/* conversion from RC value to PWM
|
/* conversion from RC value to PWM
|
||||||
|
@ -356,7 +356,7 @@ STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState
|
||||||
* scale factor = (2012-988) / (1811-172) = 0.62477120195241
|
* scale factor = (2012-988) / (1811-172) = 0.62477120195241
|
||||||
* offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
|
* offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
|
||||||
*/
|
*/
|
||||||
return (0.62477120195241f * crsfChannelData[chan]) + 881;
|
return (0.62477120195241f * (float)crsfChannelData[chan]) + 881;
|
||||||
}
|
}
|
||||||
|
|
||||||
void crsfRxWriteTelemetryData(const void *data, int len)
|
void crsfRxWriteTelemetryData(const void *data, int len)
|
||||||
|
|
|
@ -289,7 +289,7 @@ static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
STATIC_UNIT_TESTED float ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
|
|
||||||
|
@ -302,7 +302,7 @@ STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState
|
||||||
// max 1024 1811 2012us
|
// max 1024 1811 2012us
|
||||||
//
|
//
|
||||||
|
|
||||||
return (5 * (ghstChannelData[chan]+1) / 8) + 880;
|
return (5 * ((float)ghstChannelData[chan] + 1) / 8) + 880;
|
||||||
}
|
}
|
||||||
|
|
||||||
static timeUs_t ghstFrameTimeUs(void)
|
static timeUs_t ghstFrameTimeUs(void)
|
||||||
|
|
|
@ -196,7 +196,7 @@ static uint8_t ibusFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static uint16_t ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
static float ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
return ibusChannelData[chan];
|
return ibusChannelData[chan];
|
||||||
|
|
|
@ -237,7 +237,7 @@ static uint8_t jetiExBusFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
return frameStatus;
|
return frameStatus;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t jetiExBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
static float jetiExBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
if (chan >= rxRuntimeState->channelCount)
|
if (chan >= rxRuntimeState->channelCount)
|
||||||
return 0;
|
return 0;
|
||||||
|
|
|
@ -36,7 +36,7 @@
|
||||||
static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||||
static bool rxMspFrameDone = false;
|
static bool rxMspFrameDone = false;
|
||||||
|
|
||||||
uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
float rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
return mspFrame[chan];
|
return mspFrame[chan];
|
||||||
|
|
|
@ -22,6 +22,6 @@
|
||||||
|
|
||||||
struct rxConfig_s;
|
struct rxConfig_s;
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
|
float rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
|
||||||
void rxMspInit(const struct rxConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState);
|
void rxMspInit(const struct rxConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState);
|
||||||
void rxMspFrameReceive(uint16_t *frame, int channelCount);
|
void rxMspFrameReceive(uint16_t *frame, int channelCount);
|
||||||
|
|
|
@ -41,13 +41,13 @@
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "rx/pwm.h"
|
#include "rx/pwm.h"
|
||||||
|
|
||||||
static uint16_t pwmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
static float pwmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
return pwmRead(channel);
|
return pwmRead(channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t ppmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
static float ppmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
return ppmRead(channel);
|
return ppmRead(channel);
|
||||||
|
|
|
@ -107,8 +107,8 @@ static uint32_t needRxSignalMaxDelayUs;
|
||||||
static uint32_t suspendRxSignalUntil = 0;
|
static uint32_t suspendRxSignalUntil = 0;
|
||||||
static uint8_t skipRxSamples = 0;
|
static uint8_t skipRxSamples = 0;
|
||||||
|
|
||||||
static int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||||
uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||||
|
|
||||||
#define MAX_INVALID_PULS_TIME 300
|
#define MAX_INVALID_PULS_TIME 300
|
||||||
|
@ -154,7 +154,7 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRange
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t nullReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
static float nullReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
UNUSED(channel);
|
UNUSED(channel);
|
||||||
|
@ -584,15 +584,15 @@ static uint16_t getRxfailValue(uint8_t channel)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range)
|
STATIC_UNIT_TESTED float applyRxChannelRangeConfiguraton(float sample, const rxChannelRangeConfig_t *range)
|
||||||
{
|
{
|
||||||
// Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
|
// Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
|
||||||
if (sample == PPM_RCVR_TIMEOUT) {
|
if (sample == PPM_RCVR_TIMEOUT) {
|
||||||
return PPM_RCVR_TIMEOUT;
|
return PPM_RCVR_TIMEOUT;
|
||||||
}
|
}
|
||||||
|
|
||||||
sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
sample = scaleRangef(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||||
sample = constrain(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
|
sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
|
||||||
|
|
||||||
return sample;
|
return sample;
|
||||||
}
|
}
|
||||||
|
@ -604,7 +604,7 @@ static void readRxChannelsApplyRanges(void)
|
||||||
const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
|
const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
|
||||||
|
|
||||||
// sample the channel
|
// sample the channel
|
||||||
uint16_t sample;
|
float sample;
|
||||||
#if defined(USE_RX_MSP_OVERRIDE)
|
#if defined(USE_RX_MSP_OVERRIDE)
|
||||||
if (rxConfig()->msp_override_channels_mask) {
|
if (rxConfig()->msp_override_channels_mask) {
|
||||||
sample = rxMspOverrideReadRawRc(&rxRuntimeState, rxConfig(), rawChannel);
|
sample = rxMspOverrideReadRawRc(&rxRuntimeState, rxConfig(), rawChannel);
|
||||||
|
@ -634,7 +634,7 @@ static void detectAndApplySignalLossBehaviour(void)
|
||||||
|
|
||||||
rxFlightChannelsValid = true;
|
rxFlightChannelsValid = true;
|
||||||
for (int channel = 0; channel < rxChannelCount; channel++) {
|
for (int channel = 0; channel < rxChannelCount; channel++) {
|
||||||
uint16_t sample = rcRaw[channel];
|
float sample = rcRaw[channel];
|
||||||
|
|
||||||
const bool validPulse = useValueFromRx && isPulseValid(sample);
|
const bool validPulse = useValueFromRx && isPulseValid(sample);
|
||||||
|
|
||||||
|
|
|
@ -86,7 +86,7 @@ typedef enum {
|
||||||
|
|
||||||
extern const char rcChannelLetters[];
|
extern const char rcChannelLetters[];
|
||||||
|
|
||||||
extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
extern float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||||
|
|
||||||
#define RSSI_SCALE_MIN 1
|
#define RSSI_SCALE_MIN 1
|
||||||
#define RSSI_SCALE_MAX 255
|
#define RSSI_SCALE_MAX 255
|
||||||
|
@ -124,7 +124,7 @@ typedef struct rxChannelRangeConfig_s {
|
||||||
PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs);
|
PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs);
|
||||||
|
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeState, uint8_t chan); // used by receiver driver to return channel data
|
typedef float (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeState, uint8_t chan); // used by receiver driver to return channel data
|
||||||
typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeState_s *rxRuntimeState);
|
typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeState_s *rxRuntimeState);
|
||||||
typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeState_s *rxRuntimeState);
|
typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeState_s *rxRuntimeState);
|
||||||
typedef timeUs_t rcGetFrameTimeUsFn(void); // used to retrieve the timestamp in microseconds for the last channel data frame
|
typedef timeUs_t rcGetFrameTimeUsFn(void); // used to retrieve the timestamp in microseconds for the last channel data frame
|
||||||
|
|
|
@ -67,7 +67,7 @@ static protocolDataReceivedFnPtr protocolDataReceived;
|
||||||
static protocolProcessFrameFnPtr protocolProcessFrame;
|
static protocolProcessFrameFnPtr protocolProcessFrame;
|
||||||
static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
|
static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
|
||||||
|
|
||||||
STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
STATIC_UNIT_TESTED float rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||||
{
|
{
|
||||||
STATIC_ASSERT(NRF24L01_MAX_PAYLOAD_SIZE <= RX_SPI_MAX_PAYLOAD_SIZE, NRF24L01_MAX_PAYLOAD_SIZE_larger_than_RX_SPI_MAX_PAYLOAD_SIZE);
|
STATIC_ASSERT(NRF24L01_MAX_PAYLOAD_SIZE <= RX_SPI_MAX_PAYLOAD_SIZE, NRF24L01_MAX_PAYLOAD_SIZE_larger_than_RX_SPI_MAX_PAYLOAD_SIZE);
|
||||||
|
|
||||||
|
|
|
@ -87,11 +87,11 @@ uint8_t sbusChannelsDecode(rxRuntimeState_t *rxRuntimeState, const sbusChannels_
|
||||||
return RX_FRAME_COMPLETE;
|
return RX_FRAME_COMPLETE;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t sbusChannelsReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
static float sbusChannelsReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
// Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
|
// Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
|
||||||
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
|
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
|
||||||
return (5 * rxRuntimeState->channelData[chan] / 8) + 880;
|
return (5 * (float)rxRuntimeState->channelData[chan] / 8) + 880;
|
||||||
}
|
}
|
||||||
|
|
||||||
void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
|
void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
|
||||||
|
|
|
@ -174,18 +174,19 @@ static uint8_t spektrumFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t spektrumReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
static float spektrumReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
uint16_t data;
|
float data;
|
||||||
|
|
||||||
if (chan >= rxRuntimeState->channelCount) {
|
if (chan >= rxRuntimeState->channelCount) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (spekHiRes)
|
if (spekHiRes) {
|
||||||
data = 988 + (spekChannelData[chan] >> 1); // 2048 mode
|
data = 0.5f * (float)spekChannelData[chan] + 988; // 2048 mode
|
||||||
else
|
} else {
|
||||||
data = 988 + spekChannelData[chan]; // 1024 mode
|
data = spekChannelData[chan] + 988; // 1024 mode
|
||||||
|
}
|
||||||
|
|
||||||
return data;
|
return data;
|
||||||
}
|
}
|
||||||
|
|
|
@ -55,7 +55,7 @@
|
||||||
|
|
||||||
#define SRXL2_MAX_CHANNELS 32
|
#define SRXL2_MAX_CHANNELS 32
|
||||||
#define SRXL2_FRAME_PERIOD_US 11000 // 5500 for DSMR
|
#define SRXL2_FRAME_PERIOD_US 11000 // 5500 for DSMR
|
||||||
#define SRXL2_CHANNEL_SHIFT 5
|
#define SRXL2_CHANNEL_SHIFT 2
|
||||||
#define SRXL2_CHANNEL_CENTER 0x8000
|
#define SRXL2_CHANNEL_CENTER 0x8000
|
||||||
|
|
||||||
#define SRXL2_PORT_BAUDRATE_DEFAULT 115200
|
#define SRXL2_PORT_BAUDRATE_DEFAULT 115200
|
||||||
|
@ -458,13 +458,13 @@ static bool srxl2ProcessFrame(const rxRuntimeState_t *rxRuntimeState)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t srxl2ReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channelIdx)
|
static float srxl2ReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channelIdx)
|
||||||
{
|
{
|
||||||
if (channelIdx >= rxRuntimeState->channelCount) {
|
if (channelIdx >= rxRuntimeState->channelCount) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
return SPEKTRUM_PULSE_OFFSET + ((rxRuntimeState->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) >> 1);
|
return ((float)(rxRuntimeState->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) / 16) + SPEKTRUM_PULSE_OFFSET;
|
||||||
}
|
}
|
||||||
|
|
||||||
void srxl2RxWriteData(const void *data, int len)
|
void srxl2RxWriteData(const void *data, int len)
|
||||||
|
|
|
@ -162,10 +162,10 @@ static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
return frameStatus;
|
return frameStatus;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
static float sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
return sumdChannels[chan] / 8;
|
return (float)sumdChannels[chan] / 8;
|
||||||
}
|
}
|
||||||
|
|
||||||
static timeUs_t sumdFrameTimeUsFn(void)
|
static timeUs_t sumdFrameTimeUsFn(void)
|
||||||
|
|
|
@ -108,7 +108,7 @@ static uint8_t sumhFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
return RX_FRAME_COMPLETE;
|
return RX_FRAME_COMPLETE;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t sumhReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
static float sumhReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
UNUSED(rxRuntimeState);
|
UNUSED(rxRuntimeState);
|
||||||
|
|
||||||
|
|
|
@ -248,7 +248,7 @@ static uint8_t xBusFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
return RX_FRAME_COMPLETE;
|
return RX_FRAME_COMPLETE;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t xBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
static float xBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
uint16_t data;
|
uint16_t data;
|
||||||
|
|
||||||
|
|
|
@ -201,7 +201,7 @@ static uint8_t frameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
return RX_FRAME_COMPLETE;
|
return RX_FRAME_COMPLETE;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t readRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
static float readRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||||
{
|
{
|
||||||
if (chan >= rxRuntimeState->channelCount) {
|
if (chan >= rxRuntimeState->channelCount) {
|
||||||
return 0;
|
return 0;
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue