Merge branch 'digitalentity-rc-calibration'
This commit is contained in:
commit
26ac6115e7
|
@ -92,6 +92,7 @@ Re-apply any new defaults as desired.
|
|||
| `play_sound` | index, or none for next |
|
||||
| `profile` | index (0 to 2) |
|
||||
| `rateprofile` | index (0 to 2) |
|
||||
| `rxrange` | configure rx channel ranges (end-points) |
|
||||
| `save` | save and reboot |
|
||||
| `set` | name=value or blank or * for list |
|
||||
| `status` | show system status |
|
||||
|
|
37
docs/Rx.md
37
docs/Rx.md
|
@ -206,7 +206,7 @@ Use the `input_filtering_mode` CLI setting to select a mode.
|
|||
| 0 | Disabled |
|
||||
| 1 | Enabled |
|
||||
|
||||
## Receiver configuration
|
||||
## Receiver configuration.
|
||||
|
||||
### FrSky D4R-II
|
||||
|
||||
|
@ -216,3 +216,38 @@ Set the RX for 'No Pulses'. Turn OFF TX and RX, Turn ON RX. Press and release
|
|||
|
||||
Set failsafe on channels 1-4 set to OFF in the receiver settings (via transmitter menu).
|
||||
|
||||
|
||||
## Receiver Channel Range Configuration.
|
||||
|
||||
If you have a transmitter/receiver, that output a non-standard pulse range (i.e. 1070-1930 as some Spektrum receivers)
|
||||
you could use rx channel range configuration to map actual range of your transmitter to 1000-2000 as expected by Cleanflight.
|
||||
|
||||
The low and high value of a channel range are often referred to as 'End-points'. e.g. 'End-point adjustments / EPA'.
|
||||
|
||||
All attempts should be made to configure your transmitter/receiver to use the range 1000-2000 *before* using this feature
|
||||
as you will have less preceise control if it is used.
|
||||
|
||||
To do this you should figure out what range your transmitter outputs and use these values for rx range configuration.
|
||||
You can do this in a few simple steps:
|
||||
|
||||
If you have used rc range configuration previously you should reset it to prevent it from altering rc input. Do so
|
||||
by entering the following command in CLI:
|
||||
```
|
||||
rxrange reset
|
||||
save
|
||||
```
|
||||
|
||||
Now reboot your FC, connect the configurator, go to the `Receiver` tab move sticks on your transmitter and note min and
|
||||
max values of first 4 channels. Take caution as you can accidentally arm your craft. Best way is to move one channel at
|
||||
a time.
|
||||
|
||||
Go to CLI and set the min and max values with the following command:
|
||||
```
|
||||
rxrange <channel_number> <min> <max>
|
||||
```
|
||||
|
||||
For example, if you have the range 1070-1930 for the first channel you should use `rxrange 0 1070 1930` in
|
||||
the CLI. Be sure to enter the `save` command to save the settings.
|
||||
|
||||
After configuring channel ranges use the sub-trim on your transmitter to set the middle point of pitch, roll, yaw and throttle.
|
||||
|
||||
|
|
|
@ -1388,4 +1388,4 @@ void initBlackbox(void)
|
|||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif
|
|
@ -414,6 +414,8 @@ static void resetConf(void)
|
|||
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
|
||||
masterConfig.rxConfig.rssi_ppm_invert = 0;
|
||||
|
||||
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
|
||||
|
||||
masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED;
|
||||
|
||||
masterConfig.retarded_arm = 0;
|
||||
|
|
|
@ -106,7 +106,6 @@ ppmDevice_t ppmDev;
|
|||
#define PPM_STABLE_FRAMES_REQUIRED_COUNT 25
|
||||
#define PPM_IN_MIN_NUM_CHANNELS 4
|
||||
#define PPM_IN_MAX_NUM_CHANNELS PWM_PORTS_OR_PPM_CAPTURE_COUNT
|
||||
#define PPM_RCVR_TIMEOUT 0
|
||||
|
||||
|
||||
bool isPPMDataBeingReceived(void)
|
||||
|
|
|
@ -22,6 +22,9 @@ typedef enum {
|
|||
INPUT_FILTERING_ENABLED
|
||||
} inputFilteringMode_e;
|
||||
|
||||
#define PPM_RCVR_TIMEOUT 0
|
||||
|
||||
|
||||
void ppmInConfig(const timerHardware_t *timerHardwarePtr);
|
||||
void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer);
|
||||
|
||||
|
|
|
@ -421,4 +421,3 @@ void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar)
|
|||
systemResetToBootloader();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -117,6 +117,7 @@ static void cliSet(char *cmdline);
|
|||
static void cliGet(char *cmdline);
|
||||
static void cliStatus(char *cmdline);
|
||||
static void cliVersion(char *cmdline);
|
||||
static void cliRxRange(char *cmdline);
|
||||
|
||||
#ifdef GPS
|
||||
static void cliGpsPassthrough(char *cmdline);
|
||||
|
@ -257,6 +258,7 @@ const clicmd_t cmdTable[] = {
|
|||
"[<index>]", cliProfile),
|
||||
CLI_COMMAND_DEF("rateprofile", "change rate profile",
|
||||
"[<index>]", cliRateProfile),
|
||||
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
|
||||
CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail),
|
||||
CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
|
||||
CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
|
||||
|
@ -927,6 +929,51 @@ static void cliMotorMix(char *cmdline)
|
|||
#endif
|
||||
}
|
||||
|
||||
static void cliRxRange(char *cmdline)
|
||||
{
|
||||
int i, validArgumentCount = 0;
|
||||
char *ptr;
|
||||
|
||||
if (isEmpty(cmdline)) {
|
||||
for (i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
|
||||
rxChannelRangeConfiguration_t *channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i];
|
||||
printf("rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max);
|
||||
}
|
||||
} else if (strcasecmp(cmdline, "reset") == 0) {
|
||||
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
|
||||
} else {
|
||||
ptr = cmdline;
|
||||
i = atoi(ptr);
|
||||
if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) {
|
||||
int rangeMin, rangeMax;
|
||||
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
rangeMin = atoi(++ptr);
|
||||
validArgumentCount++;
|
||||
}
|
||||
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
rangeMax = atoi(++ptr);
|
||||
validArgumentCount++;
|
||||
}
|
||||
|
||||
if (validArgumentCount != 2) {
|
||||
cliShowParseError();
|
||||
} else if (rangeMin < PWM_PULSE_MIN || rangeMin > PWM_PULSE_MAX || rangeMax < PWM_PULSE_MIN || rangeMax > PWM_PULSE_MAX || rangeMin >= rangeMax) {
|
||||
cliShowParseError();
|
||||
} else {
|
||||
rxChannelRangeConfiguration_t *channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i];
|
||||
channelRangeConfiguration->min = rangeMin;
|
||||
channelRangeConfiguration->max = rangeMax;
|
||||
}
|
||||
} else {
|
||||
cliShowArgumentRangeError("channel", 0, NON_AUX_CHANNEL_COUNT - 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef LED_STRIP
|
||||
static void cliLed(char *cmdline)
|
||||
{
|
||||
|
@ -1467,6 +1514,10 @@ static void cliDump(char *cmdline)
|
|||
|
||||
cliAdjustmentRange("");
|
||||
|
||||
printf("\r\n# rxrange\r\n");
|
||||
|
||||
cliRxRange("");
|
||||
|
||||
cliPrint("\r\n# servo\r\n");
|
||||
|
||||
cliServo("");
|
||||
|
|
|
@ -116,6 +116,14 @@ STATIC_UNIT_TESTED void rxUpdateFlightChannelStatus(uint8_t channel, uint16_t pu
|
|||
}
|
||||
}
|
||||
|
||||
void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration) {
|
||||
// set default calibration to full range and 1:1 mapping
|
||||
for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
|
||||
rxChannelRangeConfiguration->min = PWM_RANGE_MIN;
|
||||
rxChannelRangeConfiguration->max = PWM_RANGE_MAX;
|
||||
rxChannelRangeConfiguration++;
|
||||
}
|
||||
}
|
||||
|
||||
void rxInit(rxConfig_t *rxConfig)
|
||||
{
|
||||
|
@ -339,7 +347,20 @@ static uint16_t getRxfailValue(uint8_t channel)
|
|||
|
||||
}
|
||||
|
||||
static void processRxChannels(void)
|
||||
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range)
|
||||
{
|
||||
// Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
|
||||
if (sample == PPM_RCVR_TIMEOUT) {
|
||||
return PPM_RCVR_TIMEOUT;
|
||||
}
|
||||
|
||||
sample = scaleRange(sample, range.min, range.max, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
|
||||
|
||||
return sample;
|
||||
}
|
||||
|
||||
void processRxChannels(void)
|
||||
{
|
||||
uint8_t chan;
|
||||
|
||||
|
@ -361,6 +382,11 @@ static void processRxChannels(void)
|
|||
// sample the channel
|
||||
uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
|
||||
|
||||
// apply the rx calibration
|
||||
if (chan < NON_AUX_CHANNEL_COUNT) {
|
||||
sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[chan]);
|
||||
}
|
||||
|
||||
rxUpdateFlightChannelStatus(chan, sample);
|
||||
|
||||
if (sample < rxConfig->rx_min_usec || sample > rxConfig->rx_max_usec || !rxSignalReceived) {
|
||||
|
|
|
@ -91,6 +91,11 @@ typedef struct rxFailsafeChannelConfiguration_s {
|
|||
uint8_t step;
|
||||
} rxFailsafeChannelConfiguration_t;
|
||||
|
||||
typedef struct rxChannelRangeConfiguration_s {
|
||||
uint16_t min;
|
||||
uint16_t max;
|
||||
} rxChannelRangeConfiguration_t;
|
||||
|
||||
typedef struct rxConfig_s {
|
||||
uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order
|
||||
uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first.
|
||||
|
@ -106,6 +111,7 @@ typedef struct rxConfig_s {
|
|||
uint16_t rx_max_usec;
|
||||
rxFailsafeChannelConfiguration_t failsafe_aux_channel_configurations[MAX_AUX_CHANNEL_COUNT];
|
||||
|
||||
rxChannelRangeConfiguration_t channelRanges[NON_AUX_CHANNEL_COUNT];
|
||||
} rxConfig_t;
|
||||
|
||||
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
|
||||
|
@ -130,3 +136,5 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig);
|
|||
uint8_t serialRxFrameStatus(rxConfig_t *rxConfig);
|
||||
|
||||
void updateRSSI(uint32_t currentTime);
|
||||
void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration);
|
||||
|
||||
|
|
|
@ -45,4 +45,3 @@ typedef struct
|
|||
} TIM_TypeDef;
|
||||
|
||||
typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
|
||||
|
||||
|
|
|
@ -0,0 +1,188 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
extern "C" {
|
||||
#include "platform.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "io/rc_controls.h"
|
||||
#include "common/maths.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
extern "C" {
|
||||
extern uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range);
|
||||
}
|
||||
|
||||
#define RANGE_CONFIGURATION(min, max) (rxChannelRangeConfiguration_t) {min, max}
|
||||
|
||||
TEST(RxChannelRangeTest, TestRxChannelRanges)
|
||||
{
|
||||
// No signal, special condition
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1000, 2000)), 0);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1300, 1700)), 0);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(900, 2100)), 0);
|
||||
|
||||
// Exact mapping
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(1000, 2000)), 1000);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1000, 2000)), 1500);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(2000, RANGE_CONFIGURATION(1000, 2000)), 2000);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1000, 2000)), 750);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1000, 2000)), 2250);
|
||||
|
||||
// Shifted range
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 1900)), 1000);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1400, RANGE_CONFIGURATION(900, 1900)), 1500);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1900, RANGE_CONFIGURATION(900, 1900)), 2000);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 1900)), 750);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(900, 1900)), 2250);
|
||||
|
||||
// Narrower range than expected
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1300, RANGE_CONFIGURATION(1300, 1700)), 1000);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1300, 1700)), 1500);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1700, RANGE_CONFIGURATION(1300, 1700)), 2000);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1300, 1700)), 750);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1300, 1700)), 2250);
|
||||
|
||||
// Wider range than expected
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 2100)), 1000);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(900, 2100)), 1500);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(2100, RANGE_CONFIGURATION(900, 2100)), 2000);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 2100)), 750);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(2700, RANGE_CONFIGURATION(900, 2100)), 2250);
|
||||
|
||||
// extreme out of range
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1000, 2000)), 750);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1300, 1700)), 750);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(900, 2100)), 750);
|
||||
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1000, 2000)), 2250);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1300, 1700)), 2250);
|
||||
EXPECT_EQ(applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(900, 2100)), 2250);
|
||||
}
|
||||
|
||||
|
||||
// stubs
|
||||
extern "C" {
|
||||
|
||||
void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||
{
|
||||
UNUSED(rxRuntimeConfig);
|
||||
UNUSED(callback);
|
||||
}
|
||||
|
||||
bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||
{
|
||||
UNUSED(initialRxConfig);
|
||||
UNUSED(rxRuntimeConfig);
|
||||
UNUSED(callback);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
UNUSED(rxRuntimeConfig);
|
||||
UNUSED(callback);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
UNUSED(rxRuntimeConfig);
|
||||
UNUSED(callback);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
UNUSED(rxRuntimeConfig);
|
||||
UNUSED(callback);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
UNUSED(rxRuntimeConfig);
|
||||
UNUSED(callback);
|
||||
return true;
|
||||
}
|
||||
|
||||
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
|
||||
{
|
||||
UNUSED(modeActivationConditions);
|
||||
}
|
||||
|
||||
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig)
|
||||
{
|
||||
UNUSED(index);
|
||||
UNUSED(auxChannelIndex);
|
||||
UNUSED(adjustmentConfig);
|
||||
}
|
||||
|
||||
void feature(uint32_t)
|
||||
{
|
||||
}
|
||||
|
||||
bool rxMspFrameComplete(void)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
void failsafeReset(void)
|
||||
{
|
||||
}
|
||||
|
||||
bool isPPMDataBeingReceived(void)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
void resetPPMDataReceivedState(void)
|
||||
{
|
||||
}
|
||||
|
||||
void failsafeOnRxCycle(void)
|
||||
{
|
||||
}
|
||||
|
||||
void failsafeOnValidDataReceived(void)
|
||||
{
|
||||
}
|
||||
|
||||
void failsafeOnRxCycleStarted(void)
|
||||
{
|
||||
}
|
||||
|
||||
void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
|
||||
{
|
||||
UNUSED(channel);
|
||||
UNUSED(pulseDuration);
|
||||
}
|
||||
|
||||
}
|
||||
|
Loading…
Reference in New Issue