Merge branch 'master' into autotune
Conflicts: obj/cleanflight_CHEBUZZF3.hex obj/cleanflight_NAZE.hex obj/cleanflight_OLIMEXINO.hex
This commit is contained in:
commit
d0ec7e25ce
1
Makefile
1
Makefile
|
@ -157,7 +157,6 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \
|
|||
drivers/sonar_hcsr04.c \
|
||||
drivers/pwm_mapping.c \
|
||||
drivers/pwm_output.c \
|
||||
drivers/pwm_rssi.c \
|
||||
drivers/pwm_rx.c \
|
||||
drivers/serial_softserial.c \
|
||||
drivers/serial_uart_common.c \
|
||||
|
|
31
docs/Rssi.md
31
docs/Rssi.md
|
@ -6,7 +6,7 @@ Some receivers have RSSI outputs. 3 types are supported.
|
|||
|
||||
1. RSSI via PPM channel
|
||||
2. RSSI via Parallel PWM channel
|
||||
3. RSSI via PWM with PPM RC that does not have RSSI output - aka RSSI PWM
|
||||
3. RSSI via ADC with PPM RC that has an RSSI output - aka RSSI ADC
|
||||
|
||||
## RSSI via PPM
|
||||
|
||||
|
@ -22,32 +22,17 @@ set rssi_channel = 1
|
|||
|
||||
Connect the RSSI signal to any PWM input channel then set the RSSI channel as you would for RSSI via PPM
|
||||
|
||||
## RSSI PWM
|
||||
## RSSI ADC
|
||||
|
||||
Connect the RSSI PWM signal to the RC2/CH2 input.
|
||||
Connect the RSSI signal to the RC2/CH2 input. The signal must be between 0v and 3.3v to indicate between 0% and 100% RSSI.
|
||||
Use inline resistors to lower voltage if required, inline smoothing capacitors may also help.
|
||||
|
||||
Enable using the RSSI_PWM feature:
|
||||
FrSky D4R-II and X8R supported.
|
||||
|
||||
Enable using the RSSI_ADC feature:
|
||||
|
||||
```
|
||||
feature RSSI_PWM
|
||||
feature RSSI_ADC
|
||||
```
|
||||
|
||||
The feature can not be used when RX_PARALLEL_PWM is enabled.
|
||||
|
||||
|
||||
### RSSI PWM Providers
|
||||
|
||||
When using RSSI PWM it is possible to use standard ~18ms RSSI signals or a faster 1khz/1m RSSI signal.
|
||||
|
||||
The RSSI output on the FrSky X8R (and probably the FrSky X6R) is 1khz.
|
||||
|
||||
To support the 1khz rate enable it via the cli:
|
||||
|
||||
```
|
||||
set rssi_pwm_provider = 1
|
||||
```
|
||||
|
||||
| Value | Meaning |
|
||||
| ----- | ----------- |
|
||||
| 0 | ~18ms pulse |
|
||||
| 1 | 1ms pulse |
|
||||
|
|
File diff suppressed because it is too large
Load Diff
10022
obj/cleanflight_NAZE.hex
10022
obj/cleanflight_NAZE.hex
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
@ -209,7 +209,6 @@ static void resetConf(void)
|
|||
masterConfig.batteryConfig.vbatscale = 110;
|
||||
masterConfig.batteryConfig.vbatmaxcellvoltage = 43;
|
||||
masterConfig.batteryConfig.vbatmincellvoltage = 33;
|
||||
masterConfig.power_adc_channel = 0;
|
||||
|
||||
resetTelemetryConfig(&masterConfig.telemetryConfig);
|
||||
|
||||
|
@ -218,7 +217,6 @@ static void resetConf(void)
|
|||
masterConfig.rxConfig.mincheck = 1100;
|
||||
masterConfig.rxConfig.maxcheck = 1900;
|
||||
masterConfig.rxConfig.rssi_channel = 0;
|
||||
masterConfig.rxConfig.rssi_pwm_provider = RSSI_PWM_PROVIDER_DEFAULT;
|
||||
|
||||
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
|
||||
masterConfig.airplaneConfig.flaps_speed = 0;
|
||||
|
@ -372,8 +370,8 @@ void validateAndFixConfig(void)
|
|||
}
|
||||
|
||||
if (feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||
if (feature(FEATURE_RSSI_PWM)) {
|
||||
featureClear(FEATURE_RSSI_PWM);
|
||||
if (feature(FEATURE_RSSI_ADC)) {
|
||||
featureClear(FEATURE_RSSI_ADC);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -18,7 +18,7 @@ typedef enum {
|
|||
FEATURE_3D = 1 << 14,
|
||||
FEATURE_RX_PARALLEL_PWM = 1 << 15,
|
||||
FEATURE_RX_MSP = 1 << 16,
|
||||
FEATURE_RSSI_PWM = 1 << 17,
|
||||
FEATURE_RSSI_ADC = 1 << 17,
|
||||
} AvailableFeatures;
|
||||
|
||||
bool feature(uint32_t mask);
|
||||
|
|
|
@ -39,8 +39,6 @@ typedef struct master_t {
|
|||
|
||||
batteryConfig_t batteryConfig;
|
||||
|
||||
uint8_t power_adc_channel; // which channel is used for current sensor. Right now, only 2 places are supported: RC_CH2 (unused when in CPPM mode, = 1), RC_CH8 (last channel in PWM mode, = 9)
|
||||
|
||||
rxConfig_t rxConfig;
|
||||
|
||||
uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right
|
||||
|
@ -48,8 +46,6 @@ 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_channel; // Read rssi from channel. 0 to disable, 1 = channel 1, etc.
|
||||
|
||||
// gps-related stuff
|
||||
gpsProvider_e gps_provider;
|
||||
|
||||
|
|
|
@ -2,9 +2,9 @@
|
|||
|
||||
typedef enum {
|
||||
ADC_BATTERY = 0,
|
||||
ADC_EXTERNAL1 = 1,
|
||||
ADC_EXTERNAL2 = 2,
|
||||
ADC_CHANNEL_MAX = ADC_EXTERNAL2
|
||||
ADC_RSSI = 1,
|
||||
ADC_EXTERNAL1 = 2,
|
||||
ADC_CHANNEL_MAX = ADC_EXTERNAL1
|
||||
} AdcChannel;
|
||||
|
||||
#define ADC_CHANNEL_COUNT (ADC_CHANNEL_MAX + 1)
|
||||
|
@ -13,10 +13,11 @@ typedef struct adc_config_t {
|
|||
uint8_t adcChannel; // ADC1_INxx channel number
|
||||
uint8_t dmaIndex; // index into DMA buffer in case of sparse channels
|
||||
bool enabled;
|
||||
uint8_t sampleTime;
|
||||
} adc_config_t;
|
||||
|
||||
typedef struct drv_adc_config_t {
|
||||
uint8_t powerAdcChannel; // which channel used for current monitor, allowed PA1, PB1 (ADC_Channel_1, ADC_Channel_9)
|
||||
bool enableRSSI;
|
||||
} drv_adc_config_t;
|
||||
|
||||
void adcInit(drv_adc_config_t *init);
|
||||
|
|
|
@ -14,8 +14,9 @@
|
|||
// Driver for STM32F103CB onboard ADC
|
||||
// VBAT is connected to PA4 (ADC1_IN4) with 10k:1k divider
|
||||
// rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
|
||||
// Additional channel can be stolen from RC_CH2 (PA1, ADC1_IN1) or
|
||||
// RC_CH8 (PB1, ADC1_IN9) by using set power_adc_channel=1|9
|
||||
//
|
||||
// RSSI ADC uses PA1
|
||||
// An additional ADC source is available on CH8 (PB1, ADC1_IN9)
|
||||
|
||||
extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
|
||||
extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
|
||||
|
@ -33,18 +34,21 @@ void adcInit(drv_adc_config_t *init)
|
|||
adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4;
|
||||
adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
|
||||
adcConfig[ADC_BATTERY].enabled = true;
|
||||
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_28Cycles5;
|
||||
|
||||
// optional ADC5 input on rev.5 hardware
|
||||
if (hse_value == 12000000) {
|
||||
adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5;
|
||||
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
|
||||
adcConfig[ADC_EXTERNAL1].enabled = true;
|
||||
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_28Cycles5;
|
||||
}
|
||||
// another channel can be stolen from PWM for current measurement or other things
|
||||
if (init->powerAdcChannel > 0) {
|
||||
adcConfig[ADC_EXTERNAL2].adcChannel = ADC_Channel_1; // init->powerAdcChannel;
|
||||
adcConfig[ADC_EXTERNAL2].dmaIndex = configuredAdcChannels++;
|
||||
adcConfig[ADC_EXTERNAL2].enabled = true;
|
||||
|
||||
if (init->enableRSSI > 0) {
|
||||
adcConfig[ADC_RSSI].adcChannel = ADC_Channel_1;
|
||||
adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++;
|
||||
adcConfig[ADC_RSSI].enabled = true;
|
||||
adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5;
|
||||
}
|
||||
|
||||
// ADC driver assumes all the GPIO was already placed in 'AIN' mode
|
||||
|
@ -76,7 +80,7 @@ void adcInit(drv_adc_config_t *init)
|
|||
if (!adcConfig[i].enabled) {
|
||||
continue;
|
||||
}
|
||||
ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, ADC_SampleTime_28Cycles5);
|
||||
ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime);
|
||||
}
|
||||
|
||||
ADC_DMACmd(ADC1, ENABLE);
|
||||
|
|
|
@ -26,14 +26,17 @@ void adcInit(drv_adc_config_t *init)
|
|||
|
||||
adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_6;
|
||||
adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount;
|
||||
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_181Cycles5;
|
||||
adcChannelCount++;
|
||||
|
||||
adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_7;
|
||||
adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount;
|
||||
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_181Cycles5;
|
||||
adcChannelCount++;
|
||||
|
||||
adcConfig[ADC_EXTERNAL2].adcChannel = ADC_Channel_8;
|
||||
adcConfig[ADC_EXTERNAL2].dmaIndex = adcChannelCount;
|
||||
adcConfig[ADC_RSSI].adcChannel = ADC_Channel_8;
|
||||
adcConfig[ADC_RSSI].dmaIndex = adcChannelCount;
|
||||
adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5;
|
||||
adcChannelCount++;
|
||||
|
||||
DMA_DeInit(DMA1_Channel1);
|
||||
|
@ -96,7 +99,7 @@ void adcInit(drv_adc_config_t *init)
|
|||
ADC_Init(ADC1, &ADC_InitStructure);
|
||||
|
||||
for (i = 0; i < adcChannelCount; i++)
|
||||
ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, i + 1, ADC_SampleTime_181Cycles5);
|
||||
ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, i + 1, adcConfig[i].sampleTime);
|
||||
|
||||
ADC_Cmd(ADC1, ENABLE);
|
||||
|
||||
|
|
|
@ -50,14 +50,12 @@
|
|||
enum {
|
||||
TYPE_IP = 1,
|
||||
TYPE_IW,
|
||||
TYPE_IR,
|
||||
TYPE_M,
|
||||
TYPE_S,
|
||||
};
|
||||
|
||||
static const uint16_t multiPPM[] = {
|
||||
PWM1 | (TYPE_IP << 8), // PPM input
|
||||
PWM2 | (TYPE_IR << 8), // PWM RSSI input
|
||||
PWM9 | (TYPE_M << 8), // Swap to servo if needed
|
||||
PWM10 | (TYPE_M << 8), // Swap to servo if needed
|
||||
PWM11 | (TYPE_M << 8),
|
||||
|
@ -91,7 +89,6 @@ static const uint16_t multiPWM[] = {
|
|||
|
||||
static const uint16_t airPPM[] = {
|
||||
PWM1 | (TYPE_IP << 8), // PPM input
|
||||
PWM2 | (TYPE_IR << 8), // PWM RSSI input
|
||||
PWM9 | (TYPE_M << 8), // motor #1
|
||||
PWM10 | (TYPE_M << 8), // motor #2
|
||||
PWM11 | (TYPE_S << 8), // servo #1
|
||||
|
@ -186,9 +183,8 @@ void pwmInit(drv_pwm_config_t *init)
|
|||
#endif
|
||||
|
||||
#ifdef STM32F10X_MD
|
||||
// skip ADC for powerMeter if configured
|
||||
// See FIXME where init->adcChannel is initialised
|
||||
if (init->adcChannel && (init->adcChannel == timerIndex))
|
||||
// skip ADC for RSSI
|
||||
if (init->useRSSIADC && timerIndex == PWM2)
|
||||
continue;
|
||||
#endif
|
||||
|
||||
|
@ -199,9 +195,6 @@ void pwmInit(drv_pwm_config_t *init)
|
|||
if (type == TYPE_IP && !init->usePPM)
|
||||
type = 0;
|
||||
|
||||
if (type == TYPE_IR && (!init->usePWMRSSI || init->useParallelPWM))
|
||||
type = 0;
|
||||
|
||||
if (init->useServos && !init->airplane) {
|
||||
#if defined(STM32F10X_MD) || defined(CHEBUZZF3)
|
||||
// remap PWM9+10 as servos
|
||||
|
@ -232,8 +225,6 @@ void pwmInit(drv_pwm_config_t *init)
|
|||
} else if (type == TYPE_IW) {
|
||||
pwmInConfig(timerIndex, channelIndex);
|
||||
channelIndex++;
|
||||
} else if (type == TYPE_IR) {
|
||||
pwmRSSIInConfig(timerIndex);
|
||||
} else if (type == TYPE_M) {
|
||||
if (init->motorPwmRate > 500) {
|
||||
pwmBrushedMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse);
|
||||
|
|
|
@ -21,7 +21,7 @@
|
|||
typedef struct drv_pwm_config_t {
|
||||
bool useParallelPWM;
|
||||
bool usePPM;
|
||||
bool usePWMRSSI;
|
||||
bool useRSSIADC;
|
||||
#ifdef STM32F10X_MD
|
||||
bool useUART2;
|
||||
#endif
|
||||
|
@ -29,7 +29,6 @@ typedef struct drv_pwm_config_t {
|
|||
bool useServos;
|
||||
bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors
|
||||
bool airplane; // fixed wing hardware config, lots of servos etc
|
||||
uint8_t adcChannel; // steal one RC input for current sensor
|
||||
uint16_t motorPwmRate;
|
||||
uint16_t servoPwmRate;
|
||||
uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
|
||||
|
|
22
src/main.c
22
src/main.c
|
@ -104,13 +104,7 @@ void init(void)
|
|||
|
||||
systemInit(masterConfig.emf_avoidance);
|
||||
|
||||
// configure power ADC
|
||||
if (masterConfig.power_adc_channel > 0 && (masterConfig.power_adc_channel == 1 || masterConfig.power_adc_channel == 9))
|
||||
adc_params.powerAdcChannel = masterConfig.power_adc_channel;
|
||||
else {
|
||||
adc_params.powerAdcChannel = 0;
|
||||
masterConfig.power_adc_channel = 0;
|
||||
}
|
||||
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
|
||||
|
||||
adcInit(&adc_params);
|
||||
|
||||
|
@ -168,7 +162,7 @@ void init(void)
|
|||
|
||||
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
|
||||
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
|
||||
pwm_params.usePWMRSSI = feature(FEATURE_RSSI_PWM);
|
||||
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
|
||||
pwm_params.usePPM = feature(FEATURE_RX_PPM);
|
||||
pwm_params.useServos = isMixerUsingServos();
|
||||
pwm_params.extraServos = currentProfile.gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
|
||||
|
@ -181,18 +175,6 @@ void init(void)
|
|||
pwm_params.idlePulse = 0; // brushed motors
|
||||
pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
|
||||
|
||||
switch (masterConfig.power_adc_channel) {
|
||||
case 1:
|
||||
pwm_params.adcChannel = PWM2;
|
||||
break;
|
||||
case 9:
|
||||
pwm_params.adcChannel = PWM8;
|
||||
break;
|
||||
default:
|
||||
pwm_params.adcChannel = 0; // FIXME this is the same as PWM1
|
||||
break;
|
||||
}
|
||||
|
||||
pwmInit(&pwm_params);
|
||||
|
||||
failsafe = failsafeInit(&masterConfig.rxConfig);
|
||||
|
|
2
src/mw.c
2
src/mw.c
|
@ -341,7 +341,7 @@ void loop(void)
|
|||
mwDisarm();
|
||||
}
|
||||
|
||||
updateRSSI();
|
||||
updateRSSI(currentTime);
|
||||
|
||||
if (feature(FEATURE_FAILSAFE)) {
|
||||
|
||||
|
|
|
@ -11,12 +11,12 @@
|
|||
#include "config.h"
|
||||
|
||||
#include "drivers/serial_common.h"
|
||||
#include "drivers/adc_common.h"
|
||||
#include "serial_common.h"
|
||||
|
||||
#include "failsafe.h"
|
||||
|
||||
#include "drivers/pwm_rx.h"
|
||||
#include "drivers/pwm_rssi.h"
|
||||
#include "rx_pwm.h"
|
||||
#include "rx_sbus.h"
|
||||
#include "rx_spektrum.h"
|
||||
|
@ -46,6 +46,7 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
|||
#define PULSE_MIN 750 // minimum PWM pulse width which is considered valid
|
||||
#define PULSE_MAX 2250 // maximum PWM pulse width which is considered valid
|
||||
|
||||
#define DELAY_50_HZ (1000000 / 50)
|
||||
|
||||
static rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
|
||||
|
||||
|
@ -146,7 +147,7 @@ uint8_t calculateChannelRemapping(uint8_t *channelMap, uint8_t channelMapEntryCo
|
|||
}
|
||||
|
||||
static bool rcDataReceived = false;
|
||||
static uint32_t rxTime = 0;
|
||||
static uint32_t rxUpdateAt = 0;
|
||||
|
||||
|
||||
void updateRx(void)
|
||||
|
@ -171,7 +172,7 @@ void updateRx(void)
|
|||
|
||||
bool shouldProcessRx(uint32_t currentTime)
|
||||
{
|
||||
return rcDataReceived || ((int32_t)(currentTime - rxTime) >= 0); // data driven or 50Hz
|
||||
return rcDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
|
||||
}
|
||||
|
||||
static bool isRxDataDriven(void) {
|
||||
|
@ -259,7 +260,7 @@ void processNonDataDrivenRx(void)
|
|||
|
||||
void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
|
||||
{
|
||||
rxTime = currentTime + 20000;
|
||||
rxUpdateAt = currentTime + DELAY_50_HZ;
|
||||
|
||||
if (feature(FEATURE_FAILSAFE)) {
|
||||
failsafe->vTable->incrementCounter();
|
||||
|
@ -283,39 +284,59 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig)
|
|||
}
|
||||
}
|
||||
|
||||
void updateRSSI(void)
|
||||
void updateRSSIPWM(void)
|
||||
{
|
||||
if (rxConfig->rssi_channel == 0 && !feature(FEATURE_RSSI_PWM)) {
|
||||
return;
|
||||
}
|
||||
int16_t pwmRssi = 0;
|
||||
// Read value of AUX channel as rssi
|
||||
pwmRssi = rcData[rxConfig->rssi_channel - 1];
|
||||
|
||||
int16_t rawPwmRssi = 0;
|
||||
if (rxConfig->rssi_channel > 0) {
|
||||
// Read value of AUX channel as rssi
|
||||
rawPwmRssi = rcData[rxConfig->rssi_channel - 1];
|
||||
} else if (feature(FEATURE_RSSI_PWM)) {
|
||||
rawPwmRssi = pwmRSSIRead();
|
||||
|
||||
if (rxConfig->rssi_pwm_provider == RSSI_PWM_PROVIDER_FRSKY_1KHZ) {
|
||||
|
||||
// FrSky X8R has a 1khz RSSI output which is too fast for the IRQ handlers
|
||||
// Values range from 0 to 970 and over 1000 when the transmitter is off.
|
||||
// When the transmitter is OFF the pulse is too short to be detected hence the high value
|
||||
// because the edge detection in the IRQ handler is the detecting the wrong edges.
|
||||
|
||||
if (rawPwmRssi > 1000) {
|
||||
rawPwmRssi = 0;
|
||||
}
|
||||
rawPwmRssi += 1000;
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
debug[3] = rawPwmRssi;
|
||||
#endif
|
||||
|
||||
// Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
|
||||
rssi = (uint16_t)((constrain(rawPwmRssi - 1000, 0, 1000) / 1000.0f) * 1023.0f);
|
||||
rssi = (uint16_t)((constrain(pwmRssi - 1000, 0, 1000) / 1000.0f) * 1023.0f);
|
||||
}
|
||||
|
||||
#define RSSI_ADC_SAMPLE_COUNT 16
|
||||
#define RSSI_SCALE (0xFFF / 100.0f)
|
||||
|
||||
void updateRSSIADC(uint32_t currentTime)
|
||||
{
|
||||
static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT];
|
||||
static uint8_t adcRssiSampleIndex = 0;
|
||||
static uint32_t rssiUpdateAt = 0;
|
||||
|
||||
if ((int32_t)(currentTime - rssiUpdateAt) < 0) {
|
||||
return;
|
||||
}
|
||||
rssiUpdateAt = currentTime + DELAY_50_HZ;
|
||||
|
||||
int16_t adcRssiMean = 0;
|
||||
uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
|
||||
uint8_t rssiPercentage = adcRssiSample / RSSI_SCALE;
|
||||
|
||||
adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT;
|
||||
|
||||
adcRssiSamples[adcRssiSampleIndex] = rssiPercentage;
|
||||
|
||||
uint8_t sampleIndex;
|
||||
|
||||
for (sampleIndex = 0; sampleIndex < RSSI_ADC_SAMPLE_COUNT; sampleIndex++) {
|
||||
adcRssiMean += adcRssiSamples[sampleIndex];
|
||||
}
|
||||
|
||||
adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT;
|
||||
|
||||
rssi = (uint16_t)((constrain(adcRssiMean, 0, 100) / 100.0f) * 1023.0f);
|
||||
}
|
||||
|
||||
void updateRSSI(uint32_t currentTime)
|
||||
{
|
||||
|
||||
if (rxConfig->rssi_channel > 0) {
|
||||
updateRSSIPWM();
|
||||
} else if (feature(FEATURE_RSSI_ADC)) {
|
||||
updateRSSIADC(currentTime);
|
||||
}
|
||||
debug[0] = rssi;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -16,12 +16,6 @@ typedef enum {
|
|||
SERIALRX_PROVIDER_MAX = SERIALRX_SUMD
|
||||
} SerialRXType;
|
||||
|
||||
typedef enum {
|
||||
RSSI_PWM_PROVIDER_DEFAULT,
|
||||
RSSI_PWM_PROVIDER_FRSKY_1KHZ,
|
||||
RSSI_PWM_PROVIDER_MAX = RSSI_PWM_PROVIDER_FRSKY_1KHZ
|
||||
} rssiProvider_e;
|
||||
|
||||
#define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1)
|
||||
|
||||
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12
|
||||
|
@ -45,7 +39,6 @@ typedef struct rxConfig_s {
|
|||
uint16_t mincheck; // minimum rc end
|
||||
uint16_t maxcheck; // maximum rc end
|
||||
uint8_t rssi_channel;
|
||||
uint8_t rssi_pwm_provider;
|
||||
} rxConfig_t;
|
||||
|
||||
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
|
||||
|
@ -67,4 +60,4 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime);
|
|||
void parseRcChannels(const char *input, rxConfig_t *rxConfig);
|
||||
bool isSerialRxFrameComplete(rxConfig_t *rxConfig);
|
||||
|
||||
void updateRSSI(void);
|
||||
void updateRSSI(uint32_t currentTime);
|
||||
|
|
|
@ -86,7 +86,7 @@ static const char * const featureNames[] = {
|
|||
"RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
|
||||
"SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS", "FAILSAFE",
|
||||
"SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", "RX_PARALLEL_PWM",
|
||||
"RX_MSP", "RSSI_PWM", NULL
|
||||
"RX_MSP", "RSSI_ADC", NULL
|
||||
};
|
||||
|
||||
// sync this with AvailableSensors enum from board.h
|
||||
|
@ -150,7 +150,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "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 },
|
||||
{ "rssi_pwm_provider", VAR_INT8, &masterConfig.rxConfig.rssi_pwm_provider, 0, RSSI_PWM_PROVIDER_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 },
|
||||
|
@ -193,8 +192,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "vbatmaxcellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },
|
||||
{ "vbatmincellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 },
|
||||
|
||||
{ "power_adc_channel", VAR_UINT8, &masterConfig.power_adc_channel, 0, 9 },
|
||||
|
||||
{ "align_gyro", VAR_UINT8, &masterConfig.sensorAlignmentConfig.gyro_align, 0, 8 },
|
||||
{ "align_acc", VAR_UINT8, &masterConfig.sensorAlignmentConfig.acc_align, 0, 8 },
|
||||
{ "align_mag", VAR_UINT8, &masterConfig.sensorAlignmentConfig.mag_align, 0, 8 },
|
||||
|
|
Loading…
Reference in New Issue