Add RSSI PWM on CH2 input.
Also support FrSky 1khz RSSI. See documentation also added in this commit. This commit also cleans up the PWM mapping code. 'mask' didn't need to be a mask and it wasn't possible to add another 'type' since there were only 4 possible values when it was a mask and they were already defined. Combined with switching to using 16 bits instead of 8 for the mapping configurations, it's now possible to have 256 types instead of 4 at the expense of a few bytes of flash. Moved the RSSI calculation into rx_common.c, previously it was in the main loop.
This commit is contained in:
parent
1b80c76e30
commit
1925df26ca
3
Makefile
3
Makefile
|
@ -154,6 +154,7 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \
|
||||||
drivers/sonar_hcsr04.c \
|
drivers/sonar_hcsr04.c \
|
||||||
drivers/pwm_mapping.c \
|
drivers/pwm_mapping.c \
|
||||||
drivers/pwm_output.c \
|
drivers/pwm_output.c \
|
||||||
|
drivers/pwm_rssi.c \
|
||||||
drivers/pwm_rx.c \
|
drivers/pwm_rx.c \
|
||||||
drivers/serial_softserial.c \
|
drivers/serial_softserial.c \
|
||||||
drivers/serial_uart_common.c \
|
drivers/serial_uart_common.c \
|
||||||
|
@ -181,6 +182,7 @@ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \
|
||||||
drivers/gpio_stm32f10x.c \
|
drivers/gpio_stm32f10x.c \
|
||||||
drivers/pwm_mapping.c \
|
drivers/pwm_mapping.c \
|
||||||
drivers/pwm_output.c \
|
drivers/pwm_output.c \
|
||||||
|
drivers/pwm_rssi.c \
|
||||||
drivers/pwm_rx.c \
|
drivers/pwm_rx.c \
|
||||||
drivers/serial_softserial.c \
|
drivers/serial_softserial.c \
|
||||||
drivers/serial_uart_common.c \
|
drivers/serial_uart_common.c \
|
||||||
|
@ -198,6 +200,7 @@ STM32F3DISCOVERY_COMMON_SRC = startup_stm32f30x_md_gcc.S \
|
||||||
drivers/gpio_stm32f30x.c \
|
drivers/gpio_stm32f30x.c \
|
||||||
drivers/pwm_mapping.c \
|
drivers/pwm_mapping.c \
|
||||||
drivers/pwm_output.c \
|
drivers/pwm_output.c \
|
||||||
|
drivers/pwm_rssi.c \
|
||||||
drivers/pwm_rx.c \
|
drivers/pwm_rx.c \
|
||||||
drivers/serial_uart_common.c \
|
drivers/serial_uart_common.c \
|
||||||
drivers/serial_uart_stm32f30x.c \
|
drivers/serial_uart_stm32f30x.c \
|
||||||
|
|
|
@ -0,0 +1,53 @@
|
||||||
|
# RSSI
|
||||||
|
|
||||||
|
RSSI is a measurement of signal strength. RSSI is very handy so you know when you are going out of range or there is interference.
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
## RSSI via PPM
|
||||||
|
|
||||||
|
Configure your receiver to output RSSI on a spare channel, then select the channel used via the cli.
|
||||||
|
|
||||||
|
e.g. if you used channel 1 then you would set:
|
||||||
|
|
||||||
|
```
|
||||||
|
set rssi_channel = 1
|
||||||
|
```
|
||||||
|
|
||||||
|
## RSSI via Parallel PWM channel
|
||||||
|
|
||||||
|
Connect the RSSI signal to any PWM input channel then set the RSSI channel as you would for RSSI via PPM
|
||||||
|
|
||||||
|
## RSSI PWM
|
||||||
|
|
||||||
|
Connect the RSSI PWM signal to the RC2/CH2 input.
|
||||||
|
|
||||||
|
Enable using the RSSI_PWM feature:
|
||||||
|
|
||||||
|
```
|
||||||
|
feature RSSI_PWM
|
||||||
|
```
|
||||||
|
|
||||||
|
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 |
|
|
@ -58,7 +58,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
|
||||||
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) // use the last flash pages for storage
master_t masterConfig; // master config struct with data independent from profiles
|
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) // use the last flash pages for storage
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 = 68;
|
static const uint8_t EEPROM_CONF_VERSION = 69;
|
||||||
|
|
||||||
static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
|
static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
|
||||||
{
|
{
|
||||||
|
@ -206,6 +206,7 @@ static void resetConf(void)
|
||||||
masterConfig.rxConfig.mincheck = 1100;
|
masterConfig.rxConfig.mincheck = 1100;
|
||||||
masterConfig.rxConfig.maxcheck = 1900;
|
masterConfig.rxConfig.maxcheck = 1900;
|
||||||
masterConfig.rxConfig.rssi_channel = 0;
|
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.retarded_arm = 0; // disable arm/disarm on roll left/right
|
||||||
masterConfig.airplaneConfig.flaps_speed = 0;
|
masterConfig.airplaneConfig.flaps_speed = 0;
|
||||||
|
@ -357,6 +358,12 @@ void validateAndFixConfig(void)
|
||||||
featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
|
featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||||
|
if (feature(FEATURE_RSSI_PWM)) {
|
||||||
|
featureClear(FEATURE_RSSI_PWM);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (feature(FEATURE_RX_MSP)) {
|
if (feature(FEATURE_RX_MSP)) {
|
||||||
if (feature(FEATURE_RX_SERIAL)) {
|
if (feature(FEATURE_RX_SERIAL)) {
|
||||||
featureClear(FEATURE_RX_SERIAL);
|
featureClear(FEATURE_RX_SERIAL);
|
||||||
|
|
|
@ -18,6 +18,7 @@ typedef enum {
|
||||||
FEATURE_3D = 1 << 14,
|
FEATURE_3D = 1 << 14,
|
||||||
FEATURE_RX_PARALLEL_PWM = 1 << 15,
|
FEATURE_RX_PARALLEL_PWM = 1 << 15,
|
||||||
FEATURE_RX_MSP = 1 << 16,
|
FEATURE_RX_MSP = 1 << 16,
|
||||||
|
FEATURE_RSSI_PWM = 1 << 17,
|
||||||
} AvailableFeatures;
|
} AvailableFeatures;
|
||||||
|
|
||||||
bool feature(uint32_t mask);
|
bool feature(uint32_t mask);
|
||||||
|
|
|
@ -10,6 +10,7 @@
|
||||||
#include "timer_common.h"
|
#include "timer_common.h"
|
||||||
|
|
||||||
#include "pwm_output.h"
|
#include "pwm_output.h"
|
||||||
|
#include "pwm_rssi.h"
|
||||||
#include "pwm_rx.h"
|
#include "pwm_rx.h"
|
||||||
#include "pwm_mapping.h"
|
#include "pwm_mapping.h"
|
||||||
/*
|
/*
|
||||||
|
@ -47,79 +48,82 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
enum {
|
enum {
|
||||||
TYPE_IP = 0x10,
|
TYPE_IP = 1,
|
||||||
TYPE_IW = 0x20,
|
TYPE_IW,
|
||||||
TYPE_M = 0x40,
|
TYPE_IR,
|
||||||
TYPE_S = 0x80
|
TYPE_M,
|
||||||
|
TYPE_S,
|
||||||
};
|
};
|
||||||
|
|
||||||
static const uint8_t multiPPM[] = {
|
static const uint16_t multiPPM[] = {
|
||||||
PWM1 | TYPE_IP, // PPM input
|
PWM1 | (TYPE_IP << 8), // PPM input
|
||||||
PWM9 | TYPE_M, // Swap to servo if needed
|
PWM2 | (TYPE_IR << 8), // PWM RSSI input
|
||||||
PWM10 | TYPE_M, // Swap to servo if needed
|
PWM9 | (TYPE_M << 8), // Swap to servo if needed
|
||||||
PWM11 | TYPE_M,
|
PWM10 | (TYPE_M << 8), // Swap to servo if needed
|
||||||
PWM12 | TYPE_M,
|
PWM11 | (TYPE_M << 8),
|
||||||
PWM13 | TYPE_M,
|
PWM12 | (TYPE_M << 8),
|
||||||
PWM14 | TYPE_M,
|
PWM13 | (TYPE_M << 8),
|
||||||
PWM5 | TYPE_M, // Swap to servo if needed
|
PWM14 | (TYPE_M << 8),
|
||||||
PWM6 | TYPE_M, // Swap to servo if needed
|
PWM5 | (TYPE_M << 8), // Swap to servo if needed
|
||||||
PWM7 | TYPE_M, // Swap to servo if needed
|
PWM6 | (TYPE_M << 8), // Swap to servo if needed
|
||||||
PWM8 | TYPE_M, // Swap to servo if needed
|
PWM7 | (TYPE_M << 8), // Swap to servo if needed
|
||||||
0xFF
|
PWM8 | (TYPE_M << 8), // Swap to servo if needed
|
||||||
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
static const uint8_t multiPWM[] = {
|
static const uint16_t multiPWM[] = {
|
||||||
PWM1 | TYPE_IW, // input #1
|
PWM1 | (TYPE_IW << 8), // input #1
|
||||||
PWM2 | TYPE_IW,
|
PWM2 | (TYPE_IW << 8),
|
||||||
PWM3 | TYPE_IW,
|
PWM3 | (TYPE_IW << 8),
|
||||||
PWM4 | TYPE_IW,
|
PWM4 | (TYPE_IW << 8),
|
||||||
PWM5 | TYPE_IW,
|
PWM5 | (TYPE_IW << 8),
|
||||||
PWM6 | TYPE_IW,
|
PWM6 | (TYPE_IW << 8),
|
||||||
PWM7 | TYPE_IW,
|
PWM7 | (TYPE_IW << 8),
|
||||||
PWM8 | TYPE_IW, // input #8
|
PWM8 | (TYPE_IW << 8), // input #8
|
||||||
PWM9 | TYPE_M, // motor #1 or servo #1 (swap to servo if needed)
|
PWM9 | (TYPE_M << 8), // motor #1 or servo #1 (swap to servo if needed)
|
||||||
PWM10 | TYPE_M, // motor #2 or servo #2 (swap to servo if needed)
|
PWM10 | (TYPE_M << 8), // motor #2 or servo #2 (swap to servo if needed)
|
||||||
PWM11 | TYPE_M, // motor #1 or #3
|
PWM11 | (TYPE_M << 8), // motor #1 or #3
|
||||||
PWM12 | TYPE_M,
|
PWM12 | (TYPE_M << 8),
|
||||||
PWM13 | TYPE_M,
|
PWM13 | (TYPE_M << 8),
|
||||||
PWM14 | TYPE_M, // motor #4 or #6
|
PWM14 | (TYPE_M << 8), // motor #4 or #6
|
||||||
0xFF
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
static const uint8_t airPPM[] = {
|
static const uint16_t airPPM[] = {
|
||||||
PWM1 | TYPE_IP, // PPM input
|
PWM1 | (TYPE_IP << 8), // PPM input
|
||||||
PWM9 | TYPE_M, // motor #1
|
PWM2 | (TYPE_IR << 8), // PWM RSSI input
|
||||||
PWM10 | TYPE_M, // motor #2
|
PWM9 | (TYPE_M << 8), // motor #1
|
||||||
PWM11 | TYPE_S, // servo #1
|
PWM10 | (TYPE_M << 8), // motor #2
|
||||||
PWM12 | TYPE_S,
|
PWM11 | (TYPE_S << 8), // servo #1
|
||||||
PWM13 | TYPE_S,
|
PWM12 | (TYPE_S << 8),
|
||||||
PWM14 | TYPE_S, // servo #4
|
PWM13 | (TYPE_S << 8),
|
||||||
PWM5 | TYPE_S, // servo #5
|
PWM14 | (TYPE_S << 8), // servo #4
|
||||||
PWM6 | TYPE_S,
|
PWM5 | (TYPE_S << 8), // servo #5
|
||||||
PWM7 | TYPE_S,
|
PWM6 | (TYPE_S << 8),
|
||||||
PWM8 | TYPE_S, // servo #8
|
PWM7 | (TYPE_S << 8),
|
||||||
0xFF
|
PWM8 | (TYPE_S << 8), // servo #8
|
||||||
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
static const uint8_t airPWM[] = {
|
static const uint16_t airPWM[] = {
|
||||||
PWM1 | TYPE_IW, // input #1
|
PWM1 | (TYPE_IW << 8), // input #1
|
||||||
PWM2 | TYPE_IW,
|
PWM2 | (TYPE_IW << 8),
|
||||||
PWM3 | TYPE_IW,
|
PWM3 | (TYPE_IW << 8),
|
||||||
PWM4 | TYPE_IW,
|
PWM4 | (TYPE_IW << 8),
|
||||||
PWM5 | TYPE_IW,
|
PWM5 | (TYPE_IW << 8),
|
||||||
PWM6 | TYPE_IW,
|
PWM6 | (TYPE_IW << 8),
|
||||||
PWM7 | TYPE_IW,
|
PWM7 | (TYPE_IW << 8),
|
||||||
PWM8 | TYPE_IW, // input #8
|
PWM8 | (TYPE_IW << 8), // input #8
|
||||||
PWM9 | TYPE_M, // motor #1
|
PWM9 | (TYPE_M << 8), // motor #1
|
||||||
PWM10 | TYPE_M, // motor #2
|
PWM10 | (TYPE_M << 8), // motor #2
|
||||||
PWM11 | TYPE_S, // servo #1
|
PWM11 | (TYPE_S << 8), // servo #1
|
||||||
PWM12 | TYPE_S,
|
PWM12 | (TYPE_S << 8),
|
||||||
PWM13 | TYPE_S,
|
PWM13 | (TYPE_S << 8),
|
||||||
PWM14 | TYPE_S, // servo #4
|
PWM14 | (TYPE_S << 8), // servo #4
|
||||||
0xFF
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
static const uint8_t * const hardwareMaps[] = {
|
static const uint16_t * const hardwareMaps[] = {
|
||||||
multiPWM,
|
multiPWM,
|
||||||
multiPPM,
|
multiPPM,
|
||||||
airPWM,
|
airPWM,
|
||||||
|
@ -129,7 +133,7 @@ static const uint8_t * const hardwareMaps[] = {
|
||||||
void pwmInit(drv_pwm_config_t *init)
|
void pwmInit(drv_pwm_config_t *init)
|
||||||
{
|
{
|
||||||
int i = 0;
|
int i = 0;
|
||||||
const uint8_t *setup;
|
const uint16_t *setup;
|
||||||
|
|
||||||
int channelIndex = 0;
|
int channelIndex = 0;
|
||||||
int servoIndex = 0;
|
int servoIndex = 0;
|
||||||
|
@ -144,10 +148,10 @@ void pwmInit(drv_pwm_config_t *init)
|
||||||
setup = hardwareMaps[i];
|
setup = hardwareMaps[i];
|
||||||
|
|
||||||
for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
|
for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
|
||||||
uint8_t timerIndex = setup[i] & 0x0F;
|
uint8_t timerIndex = setup[i] & 0x00FF;
|
||||||
uint8_t mask = setup[i] & 0xF0;
|
uint8_t type = (setup[i] & 0xFF00) >> 8;
|
||||||
|
|
||||||
if (setup[i] == 0xFF) // terminator
|
if (setup[i] == 0xFFFF) // terminator
|
||||||
break;
|
break;
|
||||||
|
|
||||||
#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER
|
#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER
|
||||||
|
@ -189,27 +193,30 @@ void pwmInit(drv_pwm_config_t *init)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// hacks to allow current functionality
|
// hacks to allow current functionality
|
||||||
if (mask & TYPE_IW && !init->useParallelPWM)
|
if (type == TYPE_IW && !init->useParallelPWM)
|
||||||
mask = 0;
|
type = 0;
|
||||||
|
|
||||||
if (mask & TYPE_IP && !init->usePPM)
|
if (type == TYPE_IP && !init->usePPM)
|
||||||
mask = 0;
|
type = 0;
|
||||||
|
|
||||||
|
if (type == TYPE_IR && (!init->usePWMRSSI || init->useParallelPWM))
|
||||||
|
type = 0;
|
||||||
|
|
||||||
if (init->useServos && !init->airplane) {
|
if (init->useServos && !init->airplane) {
|
||||||
#if defined(STM32F10X_MD) || defined(CHEBUZZF3)
|
#if defined(STM32F10X_MD) || defined(CHEBUZZF3)
|
||||||
// remap PWM9+10 as servos
|
// remap PWM9+10 as servos
|
||||||
if (timerIndex == PWM9 || timerIndex == PWM10)
|
if (timerIndex == PWM9 || timerIndex == PWM10)
|
||||||
mask = TYPE_S;
|
type = TYPE_S;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if (defined(STM32F303xC) || defined(STM32F3DISCOVERY)) && !defined(CHEBUZZF3)
|
#if (defined(STM32F303xC) || defined(STM32F3DISCOVERY)) && !defined(CHEBUZZF3)
|
||||||
// remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer
|
// remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer
|
||||||
if (init->useSoftSerial) {
|
if (init->useSoftSerial) {
|
||||||
if (timerIndex == PWM5 || timerIndex == PWM6)
|
if (timerIndex == PWM5 || timerIndex == PWM6)
|
||||||
mask = TYPE_S;
|
type = TYPE_S;
|
||||||
} else {
|
} else {
|
||||||
if (timerIndex == PWM9 || timerIndex == PWM10)
|
if (timerIndex == PWM9 || timerIndex == PWM10)
|
||||||
mask = TYPE_S;
|
type = TYPE_S;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
@ -217,24 +224,24 @@ void pwmInit(drv_pwm_config_t *init)
|
||||||
if (init->extraServos && !init->airplane) {
|
if (init->extraServos && !init->airplane) {
|
||||||
// remap PWM5..8 as servos when used in extended servo mode
|
// remap PWM5..8 as servos when used in extended servo mode
|
||||||
if (timerIndex >= PWM5 && timerIndex <= PWM8)
|
if (timerIndex >= PWM5 && timerIndex <= PWM8)
|
||||||
mask = TYPE_S;
|
type = TYPE_S;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mask & TYPE_IP) {
|
if (type == TYPE_IP) {
|
||||||
ppmInConfig(timerIndex);
|
ppmInConfig(timerIndex);
|
||||||
} else if (mask & TYPE_IW) {
|
} else if (type == TYPE_IW) {
|
||||||
pwmInConfig(timerIndex, channelIndex);
|
pwmInConfig(timerIndex, channelIndex);
|
||||||
channelIndex++;
|
channelIndex++;
|
||||||
} else if (mask & TYPE_M) {
|
} else if (type == TYPE_IR) {
|
||||||
|
pwmRSSIInConfig(timerIndex);
|
||||||
|
} else if (type == TYPE_M) {
|
||||||
if (init->motorPwmRate > 500) {
|
if (init->motorPwmRate > 500) {
|
||||||
pwmBrushedMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse);
|
pwmBrushedMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse);
|
||||||
} else {
|
} else {
|
||||||
pwmBrushlessMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse);
|
pwmBrushlessMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse);
|
||||||
}
|
}
|
||||||
motorIndex++;
|
motorIndex++;
|
||||||
} else if (mask & TYPE_S) {
|
} else if (type == TYPE_S) {
|
||||||
pwmServoConfig(&timerHardware[timerIndex], servoIndex, init->servoPwmRate, init->servoCenterPulse);
|
pwmServoConfig(&timerHardware[timerIndex], servoIndex, init->servoPwmRate, init->servoCenterPulse);
|
||||||
servoIndex++;
|
servoIndex++;
|
||||||
}
|
}
|
||||||
|
|
|
@ -21,6 +21,7 @@
|
||||||
typedef struct drv_pwm_config_t {
|
typedef struct drv_pwm_config_t {
|
||||||
bool useParallelPWM;
|
bool useParallelPWM;
|
||||||
bool usePPM;
|
bool usePPM;
|
||||||
|
bool usePWMRSSI;
|
||||||
#ifdef STM32F10X_MD
|
#ifdef STM32F10X_MD
|
||||||
bool useUART2;
|
bool useUART2;
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -0,0 +1,95 @@
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "gpio_common.h"
|
||||||
|
#include "timer_common.h"
|
||||||
|
|
||||||
|
#include "pwm_mapping.h"
|
||||||
|
|
||||||
|
#include "pwm_rssi.h"
|
||||||
|
|
||||||
|
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); // from pwm_output.c
|
||||||
|
|
||||||
|
typedef struct rssiInputPort_s {
|
||||||
|
uint8_t state;
|
||||||
|
captureCompare_t rise;
|
||||||
|
captureCompare_t fall;
|
||||||
|
captureCompare_t capture;
|
||||||
|
|
||||||
|
uint16_t raw;
|
||||||
|
|
||||||
|
const timerHardware_t *timerHardware;
|
||||||
|
} rssiInputPort_t;
|
||||||
|
|
||||||
|
static rssiInputPort_t rssiInputPort;
|
||||||
|
|
||||||
|
static void pwmRssiCallback(uint8_t reference, captureCompare_t capture)
|
||||||
|
{
|
||||||
|
const timerHardware_t *timerHardware = rssiInputPort.timerHardware;
|
||||||
|
|
||||||
|
if (rssiInputPort.state == 0) {
|
||||||
|
rssiInputPort.rise = capture;
|
||||||
|
rssiInputPort.state = 1;
|
||||||
|
pwmICConfig(timerHardware->tim, timerHardware->channel, TIM_ICPolarity_Falling);
|
||||||
|
} else {
|
||||||
|
rssiInputPort.fall = capture;
|
||||||
|
|
||||||
|
// compute and store capture
|
||||||
|
rssiInputPort.raw = rssiInputPort.fall - rssiInputPort.rise;
|
||||||
|
|
||||||
|
// switch state
|
||||||
|
rssiInputPort.state = 0;
|
||||||
|
pwmICConfig(timerHardware->tim, timerHardware->channel, TIM_ICPolarity_Rising);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pwmRSSIGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
|
||||||
|
{
|
||||||
|
gpio_config_t cfg;
|
||||||
|
|
||||||
|
cfg.pin = pin;
|
||||||
|
cfg.mode = mode;
|
||||||
|
cfg.speed = Speed_2MHz;
|
||||||
|
gpioInit(gpio, &cfg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void pwmRSSIICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
||||||
|
{
|
||||||
|
TIM_ICInitTypeDef TIM_ICInitStructure;
|
||||||
|
|
||||||
|
TIM_ICStructInit(&TIM_ICInitStructure);
|
||||||
|
TIM_ICInitStructure.TIM_Channel = channel;
|
||||||
|
TIM_ICInitStructure.TIM_ICPolarity = polarity;
|
||||||
|
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
|
||||||
|
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
|
||||||
|
TIM_ICInitStructure.TIM_ICFilter = 0x00;
|
||||||
|
|
||||||
|
TIM_ICInit(tim, &TIM_ICInitStructure);
|
||||||
|
}
|
||||||
|
|
||||||
|
#define UNUSED_CALLBACK_REFERENCE 0
|
||||||
|
|
||||||
|
void pwmRSSIInConfig(uint8_t timerIndex)
|
||||||
|
{
|
||||||
|
const timerHardware_t *timerHardwarePtr = &(timerHardware[timerIndex]);
|
||||||
|
|
||||||
|
memset(&rssiInputPort, 0, sizeof(rssiInputPort));
|
||||||
|
|
||||||
|
rssiInputPort.timerHardware = timerHardwarePtr;
|
||||||
|
|
||||||
|
pwmRSSIGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
|
||||||
|
pwmRSSIICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
|
||||||
|
|
||||||
|
timerConfigure(timerHardwarePtr, 0xFFFF, 1);
|
||||||
|
configureTimerCaptureCompareInterrupt(timerHardwarePtr, UNUSED_CALLBACK_REFERENCE, pwmRssiCallback);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t pwmRSSIRead(void)
|
||||||
|
{
|
||||||
|
return rssiInputPort.raw;
|
||||||
|
}
|
||||||
|
|
|
@ -0,0 +1,5 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
uint16_t pwmRSSIRead(void);
|
||||||
|
void pwmRSSIInConfig(uint8_t timerIndex);
|
||||||
|
|
|
@ -475,7 +475,7 @@ static void gpsNewData(uint16_t c)
|
||||||
GPS_update = 0;
|
GPS_update = 0;
|
||||||
else
|
else
|
||||||
GPS_update = 1;
|
GPS_update = 1;
|
||||||
#if 1
|
#if 0
|
||||||
debug[3] = GPS_update;
|
debug[3] = GPS_update;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -168,6 +168,7 @@ void init(void)
|
||||||
|
|
||||||
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
|
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
|
||||||
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
|
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
|
||||||
|
pwm_params.usePWMRSSI = feature(FEATURE_RSSI_PWM);
|
||||||
pwm_params.usePPM = feature(FEATURE_RX_PPM);
|
pwm_params.usePPM = feature(FEATURE_RX_PPM);
|
||||||
pwm_params.useServos = isMixerUsingServos();
|
pwm_params.useServos = isMixerUsingServos();
|
||||||
pwm_params.extraServos = currentProfile.gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
|
pwm_params.extraServos = currentProfile.gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
|
||||||
|
|
9
src/mw.c
9
src/mw.c
|
@ -64,8 +64,6 @@ int16_t headFreeModeHold;
|
||||||
|
|
||||||
int16_t telemTemperature1; // gyro sensor temperature
|
int16_t telemTemperature1; // gyro sensor temperature
|
||||||
|
|
||||||
uint16_t rssi; // range: [0;1023]
|
|
||||||
|
|
||||||
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
|
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||||
extern failsafe_t *failsafe;
|
extern failsafe_t *failsafe;
|
||||||
|
|
||||||
|
@ -302,12 +300,7 @@ void loop(void)
|
||||||
mwDisarm();
|
mwDisarm();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read value of AUX channel as rssi
|
updateRSSI();
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (feature(FEATURE_FAILSAFE)) {
|
if (feature(FEATURE_FAILSAFE)) {
|
||||||
|
|
||||||
|
|
|
@ -6,6 +6,8 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "common/maths.h"
|
||||||
|
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
|
|
||||||
#include "drivers/serial_common.h"
|
#include "drivers/serial_common.h"
|
||||||
|
@ -14,6 +16,7 @@
|
||||||
#include "failsafe.h"
|
#include "failsafe.h"
|
||||||
|
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
|
#include "drivers/pwm_rssi.h"
|
||||||
#include "rx_pwm.h"
|
#include "rx_pwm.h"
|
||||||
#include "rx_sbus.h"
|
#include "rx_sbus.h"
|
||||||
#include "rx_spektrum.h"
|
#include "rx_spektrum.h"
|
||||||
|
@ -22,15 +25,20 @@
|
||||||
|
|
||||||
#include "rx_common.h"
|
#include "rx_common.h"
|
||||||
|
|
||||||
|
extern int16_t debug[4];
|
||||||
|
|
||||||
void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
||||||
|
|
||||||
bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
||||||
bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
||||||
bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
||||||
|
|
||||||
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
|
||||||
|
|
||||||
const char rcChannelLetters[] = "AERT1234";
|
const char rcChannelLetters[] = "AERT1234";
|
||||||
|
|
||||||
|
uint16_t rssi; // range: [0;1023]
|
||||||
|
|
||||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||||
|
|
||||||
#define PPM_AND_PWM_SAMPLE_COUNT 4
|
#define PPM_AND_PWM_SAMPLE_COUNT 4
|
||||||
|
@ -275,3 +283,39 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void updateRSSI(void)
|
||||||
|
{
|
||||||
|
if (rxConfig->rssi_channel == 0 && !feature(FEATURE_RSSI_PWM)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
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 1
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -16,6 +16,12 @@ typedef enum {
|
||||||
SERIALRX_PROVIDER_MAX = SERIALRX_SUMD
|
SERIALRX_PROVIDER_MAX = SERIALRX_SUMD
|
||||||
} SerialRXType;
|
} 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 SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1)
|
||||||
|
|
||||||
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12
|
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12
|
||||||
|
@ -39,6 +45,7 @@ typedef struct rxConfig_s {
|
||||||
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;
|
uint8_t rssi_channel;
|
||||||
|
uint8_t rssi_pwm_provider;
|
||||||
} rxConfig_t;
|
} rxConfig_t;
|
||||||
|
|
||||||
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
|
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
|
||||||
|
@ -59,3 +66,5 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime);
|
||||||
|
|
||||||
void parseRcChannels(const char *input, rxConfig_t *rxConfig);
|
void parseRcChannels(const char *input, rxConfig_t *rxConfig);
|
||||||
bool isSerialRxFrameComplete(rxConfig_t *rxConfig);
|
bool isSerialRxFrameComplete(rxConfig_t *rxConfig);
|
||||||
|
|
||||||
|
void updateRSSI(void);
|
||||||
|
|
|
@ -86,7 +86,7 @@ static const char * const featureNames[] = {
|
||||||
"RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
|
"RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
|
||||||
"SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS", "FAILSAFE",
|
"SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS", "FAILSAFE",
|
||||||
"SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", "RX_PARALLEL_PWM",
|
"SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", "RX_PARALLEL_PWM",
|
||||||
"RX_MSP", NULL
|
"RX_MSP", "RSSI_PWM", NULL
|
||||||
};
|
};
|
||||||
|
|
||||||
// sync this with AvailableSensors enum from board.h
|
// sync this with AvailableSensors enum from board.h
|
||||||
|
@ -150,6 +150,7 @@ const clivalue_t valueTable[] = {
|
||||||
{ "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 },
|
{ "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 },
|
{ "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 },
|
||||||
|
|
Loading…
Reference in New Issue