Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

decouple failsafe from the rest of the system.

Browse source 
Note: the pwm_common driver has a dependency on the main failsafe code,
the solution is probably to move some of the code into rx_pwm.
This commit is contained in:
Dominic Clifton 2014-04-21 00:46:16 +01:00
parent aef28c1c08
commit e969d184e6
29 changed files with 224 additions and 119 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -66,6 +66,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
drivers/serial_uart.c \
drivers/sound_beeper.c \
drivers/system_common.c \
flight_common.c \
flight_imu.c \
flight_mixer.c \
gps_common.c \

4
src/build_config.c
View file

@ -12,10 +12,6 @@
#include "drivers/timer_common.h"
#include "drivers/pwm_common.h"
#include "flight_mixer.h"
#include "sensors_common.h"
#include "battery.h"
#include "boardalignment.h"
#include "config.h"
#include "drivers/serial_common.h"
#include "serial_common.h"

15
src/buzzer.c
View file

@ -26,6 +26,13 @@ typedef enum {
FAILSAFE_FIND_ME
} failsafeBuzzerWarnings_e;
failsafe_t* failsafe;
void buzzerInit(failsafe_t *initialFailsafe)
{
failsafe = initialFailsafe;
}
void buzzer(bool warn_vbat)
{
static uint8_t beeperOnBox;
@ -41,19 +48,19 @@ void buzzer(bool warn_vbat)
}
//===================== Beeps for failsafe =====================
if (feature(FEATURE_FAILSAFE)) {
if (shouldFailsafeForceLanding(f.ARMED)) {
if (failsafe->vTable->shouldForceLanding(f.ARMED)) {
warn_failsafe = FAILSAFE_LANDING;
if (shouldFailsafeHaveCausedLandingByNow()) {
if (failsafe->vTable->shouldHaveCausedLandingByNow()) {
warn_failsafe = FAILSAFE_FIND_ME;
}
}
if (hasFailsafeTimerElapsed() && !f.ARMED) {
if (failsafe->vTable->hasTimerElapsed() && !f.ARMED) {
warn_failsafe = FAILSAFE_FIND_ME;
}
if (isFailsafeIdle()) {
if (failsafe->vTable->isIdle()) {
warn_failsafe = FAILSAFE_IDLE; // turn off alarm if TX is okay
}
}

11
src/config.c
View file

@ -24,6 +24,7 @@
#include "rx_common.h"
#include "gps_common.h"
#include "serial_common.h"
#include "failsafe.h"
#include "runtime_config.h"
#include "config.h"
@ -88,6 +89,8 @@ void readEEPROM(void)
setPIDController(cfg.pidController);
gpsSetPIDs();
useFailsafeConfig(&cfg.failsafeConfig);
}
void writeEEPROM(uint8_t b, uint8_t updateProfile)
@ -289,10 +292,10 @@ static void resetConf(void)
cfg.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
cfg.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
cfg.failsafeConfig.failsafe_delay = 10; // 1sec
cfg.failsafeConfig.failsafe_off_delay = 200; // 20sec
cfg.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
cfg.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
// servos
for (i = 0; i < 8; i++) {

3
src/config.h
View file

@ -1,8 +1,5 @@
#pragma once
#define MAX_SUPPORTED_MOTORS 12
#define MAX_SUPPORTED_SERVOS 8
enum {
PIDROLL,
PIDPITCH,

6
src/config_storage.h
View file

@ -37,10 +37,7 @@ typedef struct config_t {
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
// Failsafe related configuration
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t failsafe_detect_threshold; // Update controls channel only if pulse is above failsafe_detect_threshold. below this trigger failsafe.
failsafeConfig_t failsafeConfig;
// mixer-related configuration
int8_t yaw_direction;
@ -103,6 +100,7 @@ typedef struct master_t {
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
uint8_t flaps_speed; // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign

24
src/drivers/pwm_common.c
View file

@ -8,10 +8,10 @@
#include "gpio_common.h"
#include "timer_common.h"
#include "failsafe.h" // FIXME dependency into the main code from a driver
#include "pwm_common.h"
#include "failsafe.h" // FIXME for external global variables
/*
Configuration maps:
@ -151,6 +151,8 @@ static const uint8_t * const hardwareMaps[] = {
#define PWM_TIMER_MHZ 1
#define PWM_BRUSHED_TIMER_MHZ 8
failsafe_t *failsafe;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
@ -271,16 +273,13 @@ static void ppmCallback(uint8_t port, uint16_t capture)
captures[chan] = diff;
if (chan < 4 && diff > failsafeThreshold)
GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK
if (GoodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter
if (GoodPulses == 0x0F) { // If first four channels have good pulses, clear FailSafe counter
GoodPulses = 0;
if (failsafeCnt > 20)
failsafeCnt -= 20;
else
failsafeCnt = 0;
failsafe->vTable->validDataReceived();
}
}
chan++;
failsafeCnt = 0;
failsafe->vTable->reset();
}
}
@ -299,7 +298,7 @@ static void pwmCallback(uint8_t port, uint16_t capture)
pwmPorts[port].state = 0;
pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising);
// reset failsafe
failsafeCnt = 0;
failsafe->vTable->reset();
}
}
@ -313,11 +312,13 @@ static void pwmWriteStandard(uint8_t index, uint16_t value)
*motors[index]->ccr = value;
}
bool pwmInit(drv_pwm_config_t *init)
void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe)
{
int i = 0;
const uint8_t *setup;
failsafe = initialFailsafe;
// to avoid importing cfg/mcfg
failsafeThreshold = init->failsafeThreshold;
@ -394,7 +395,6 @@ bool pwmInit(drv_pwm_config_t *init)
pwmWritePtr = pwmWriteStandard;
if (init->motorPwmRate > 500)
pwmWritePtr = pwmWriteBrushed;
return false;
}
void pwmWriteMotor(uint8_t index, uint16_t value)

1
src/drivers/pwm_common.h
View file

@ -44,7 +44,6 @@ enum {
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmWriteServo(uint8_t index, uint16_t value);
uint16_t pwmRead(uint8_t channel);

116
src/failsafe.c
View file

@ -1,45 +1,73 @@
#include <stdbool.h>
#include <stdint.h>
// FIXME a solution to the dependency problems here is to roll up the failsafe configuration into a structure in failsafe.h and supply it using failsafeInit()
#include "common/axis.h" // FIXME this file should not have this dependency
#include "rx_common.h"
#include "drivers/serial_common.h" // FIXME this file should not have this dependency
#include "serial_common.h" // FIXME this file should not have this dependency
#include "flight_mixer.h" // FIXME this file should not have this dependency
#include "flight_common.h" // FIXME this file should not have this dependency
#include "sensors_common.h" // FIXME this file should not have this dependency
#include "boardalignment.h" // FIXME this file should not have this dependency
#include "battery.h" // FIXME this file should not have this dependency
#include "common/axis.h"
#include "flight_common.h"
#include "runtime_config.h"
#include "config.h"
#include "config_storage.h"
int16_t failsafeCnt = 0;
int16_t failsafeEvents = 0;
#include "failsafe.h"
bool isFailsafeIdle(void)
/*
* Usage:
*
* failsafeInit() and useFailsafeConfig() must be called before the other methods are used.
*
* failsafeInit() and useFailsafeConfig() can be called in any order.
* failsafeInit() should only be called once.
*/
static failsafe_t failsafe;
static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
const failsafeVTable_t failsafeVTable[];
void reset(void)
{
return failsafeCnt == 0;
failsafe.counter = 0;
}
bool hasFailsafeTimerElapsed(void)
/*
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
*/
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
{
return failsafeCnt > (5 * cfg.failsafe_delay);
failsafeConfig = failsafeConfigToUse;
reset();
}
bool shouldFailsafeForceLanding(bool armed)
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig)
{
return hasFailsafeTimerElapsed() && armed;
rxConfig = intialRxConfig;
failsafe.vTable = failsafeVTable;
failsafe.events = 0;
return &failsafe;
}
bool shouldFailsafeHaveCausedLandingByNow(void)
bool isIdle(void)
{
return failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay);
return failsafe.counter == 0;
}
bool hasTimerElapsed(void)
{
return failsafe.counter > (5 * failsafeConfig->failsafe_delay);
}
bool shouldForceLanding(bool armed)
{
return hasTimerElapsed() && armed;
}
bool shouldHaveCausedLandingByNow(void)
{
return failsafe.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
}
void failsafeAvoidRearm(void)
@ -48,29 +76,45 @@ void failsafeAvoidRearm(void)
f.OK_TO_ARM = 0; // to restart accidently by just reconnect to the tx - you will have to switch off first to rearm
}
void onValidDataReceived(void)
{
if (failsafe.counter > 20)
failsafe.counter -= 20;
else
failsafe.counter = 0;
}
void updateFailsafeState(void)
void updateState(void)
{
uint8_t i;
if (!feature(FEATURE_FAILSAFE)) {
return;
}
if (hasTimerElapsed()) {
if (hasFailsafeTimerElapsed()) {
if (shouldFailsafeForceLanding(f.ARMED)) { // Stabilize, and set Throttle to specified level
if (shouldForceLanding(f.ARMED)) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++) {
rcData[i] = mcfg.rxConfig.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[i] = rxConfig->midrc; // after specified guard time after RC signal is lost (in 0.1sec)
}
rcData[THROTTLE] = cfg.failsafe_throttle;
failsafeEvents++;
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
failsafe.events++;
}
if (shouldFailsafeHaveCausedLandingByNow() || !f.ARMED) {
if (shouldHaveCausedLandingByNow() || !f.ARMED) {
failsafeAvoidRearm();
}
}
failsafeCnt++;
failsafe.counter++;
}
const failsafeVTable_t failsafeVTable[] = {
{
reset,
onValidDataReceived,
shouldForceLanding,
hasTimerElapsed,
shouldHaveCausedLandingByNow,
updateState,
isIdle
}
};

33
src/failsafe.h
View file

@ -1,10 +1,29 @@
#pragma once
extern int16_t failsafeCnt;
extern int16_t failsafeEvents;
typedef struct failsafeConfig_s {
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t failsafe_detect_threshold; // Update controls channel only if pulse is above failsafe_detect_threshold. below this trigger failsafe.
} failsafeConfig_t;
typedef struct failsafeVTable_s {
void (*reset)(void);
void (*validDataReceived)(void);
bool (*shouldForceLanding)(bool armed);
bool (*hasTimerElapsed)(void);
bool (*shouldHaveCausedLandingByNow)(void);
void (*updateState)(void);
bool (*isIdle)(void);
} failsafeVTable_t;
typedef struct failsafe_s {
const failsafeVTable_t *vTable;
int16_t counter;
int16_t events;
} failsafe_t;
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
bool isFailsafeIdle(void);
bool hasFailsafeTimerElapsed(void);
bool shouldFailsafeForceLanding(bool armed);
bool shouldFailsafeHaveCausedLandingByNow(void);
void updateFailsafeState(void);

13
src/flight_common.c Normal file
View file

@ -0,0 +1,13 @@
#include "board.h"
#include "mw.h"
#include "runtime_config.h"
#include "flight_common.h"
void mwDisarm(void)
{
if (f.ARMED)
f.ARMED = 0;
}

3
src/flight_common.h
View file

@ -22,3 +22,6 @@ extern int16_t gyroZero[GYRO_INDEX_COUNT]; // see gyro_index_t
extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT];
void mwDisarm(void);

3
src/flight_mixer.h
View file

@ -1,5 +1,8 @@
#pragma once
#define MAX_SUPPORTED_MOTORS 12
#define MAX_SUPPORTED_SERVOS 8
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
typedef enum MultiType
{

16
src/main.c
View file

@ -2,6 +2,7 @@
#include "flight_common.h"
#include "flight_mixer.h"
#include "serial_common.h"
#include "failsafe.h"
#include "mw.h"
#include "gps_common.h"
@ -16,8 +17,14 @@
extern rcReadRawDataPtr rcReadRawFunc;
failsafe_t *failsafe;
void initTelemetry(serialPorts_t *serialPorts);
void serialInit(serialConfig_t *initialSerialConfig);
failsafe_t* failsafeInit(failsafeConfig_t *initialFailsafeConfig, rxConfig_t *intialRxConfig);
void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void buzzerInit(failsafe_t *initialFailsafe);
int main(void)
{
@ -68,7 +75,8 @@ int main(void)
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = mcfg.rxConfig.midrc;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
pwm_params.failsafeThreshold = cfg.failsafeConfig.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;
@ -81,9 +89,11 @@ int main(void)
break;
}
pwmInit(&pwm_params);
failsafe = failsafeInit(&cfg.failsafeConfig, &mcfg.rxConfig);
buzzerInit(failsafe);
pwmInit(&pwm_params, failsafe);
rxInit(&mcfg.rxConfig);
rxInit(&mcfg.rxConfig, failsafe);
if (feature(FEATURE_GPS) && !feature(FEATURE_SERIALRX)) {
gpsInit(mcfg.gps_baudrate);

11
src/mw.c
View file

@ -37,6 +37,8 @@ uint8_t dynP8[3], dynI8[3], dynD8[3];
int16_t axisPID[3];
extern failsafe_t *failsafe;
// **********************
// GPS
// **********************
@ -412,7 +414,9 @@ void loop(void)
rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f);
}
updateFailsafeState();
if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->updateState();
}
// ------------------ STICKS COMMAND HANDLER --------------------
// checking sticks positions
@ -552,8 +556,7 @@ void loop(void)
for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
rcOptions[i] = (auxState & cfg.activate[i]) > 0;
// note: if FAILSAFE is disable, hasFailsafeTimerElapsed() is always false
if ((rcOptions[BOXANGLE] || hasFailsafeTimerElapsed()) && (sensors(SENSOR_ACC))) {
if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
errorAngleI[ROLL] = 0;
@ -561,7 +564,7 @@ void loop(void)
f.ANGLE_MODE = 1;
}
} else {
f.ANGLE_MODE = 0; // failsave support
f.ANGLE_MODE = 0; // failsafe support
}
if (rcOptions[BOXHORIZON]) {

1
src/mw.h
View file

@ -2,6 +2,7 @@
#include "runtime_config.h"
#include "flight_common.h"
#include "failsafe.h"
/* for VBAT monitoring frequency */
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations

6
src/runtime_config.c
View file

@ -27,9 +27,3 @@ uint32_t sensorsMask(void)
{
return enabledSensors;
}
void mwDisarm(void)
{
if (f.ARMED)
f.ARMED = 0;
}

29
src/rx_common.c
View file

@ -6,13 +6,22 @@
#include "platform.h"
#include "rx_common.h"
#include "config.h"
#include "failsafe.h"
#include "rx_pwm.h"
#include "rx_sbus.h"
#include "rx_spektrum.h"
#include "rx_sumd.h"
#include "rx_common.h"
void pwmRxInit(rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *failsafe, rcReadRawDataPtr *callback);
void sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback);
void spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback);
void sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback);
const char rcChannelLetters[] = "AERT1234";
int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
@ -25,7 +34,9 @@ rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) o
rxRuntimeConfig_t rxRuntimeConfig;
void rxInit(rxConfig_t *rxConfig)
void serialRxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe)
{
uint8_t i;
@ -34,24 +45,24 @@ void rxInit(rxConfig_t *rxConfig)
}
if (feature(FEATURE_SERIALRX)) {
serialRxInit(rxConfig);
serialRxInit(rxConfig, failsafe);
} else {
pwmRxInit(&rxRuntimeConfig, &rcReadRawFunc);
pwmRxInit(&rxRuntimeConfig, failsafe, &rcReadRawFunc);
}
}
void serialRxInit(rxConfig_t *rxConfig)
void serialRxInit(rxConfig_t *rxConfig, failsafe_t *failsafe)
{
switch (rxConfig->serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
spektrumInit(rxConfig, &rxRuntimeConfig, failsafe, &rcReadRawFunc);
break;
case SERIALRX_SBUS:
sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
sbusInit(rxConfig, &rxRuntimeConfig, failsafe, &rcReadRawFunc);
break;
case SERIALRX_SUMD:
sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
sumdInit(rxConfig, &rxRuntimeConfig, failsafe, &rcReadRawFunc);
break;
}
}

3
src/rx_common.h
View file

@ -74,9 +74,6 @@ extern rxRuntimeConfig_t rxRuntimeConfig;
typedef uint16_t (* rcReadRawDataPtr)(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
void rxInit(rxConfig_t *rxConfig);
void serialRxInit(rxConfig_t *rxConfig);
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
void generatePitchCurve(controlRateConfig_t *controlRateConfig);

4
src/rx_pwm.c
View file

@ -11,6 +11,8 @@
#include "drivers/pwm_common.h"
#include "failsafe.h"
#include "rx_common.h"
#include "rx_pwm.h"
static uint16_t pwmReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
@ -24,7 +26,7 @@ static uint16_t pwmReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC
return data;
}
void pwmRxInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
void pwmRxInit(rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback)
{
// configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled
*callback = pwmReadRawRC;

2
src/rx_pwm.h
View file

@ -1,3 +1 @@
#pragma once
void pwmRxInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);

7
src/rx_sbus.c
View file

@ -28,11 +28,12 @@ static uint16_t sbusReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntime
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
//rxConfig_t *rxConfig;
failsafe_t *failsafe;
void sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
void sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback)
{
int b;
failsafe = initialFailsafe;
//rxConfig = initialRxConfig;
@ -99,7 +100,7 @@ bool sbusFrameComplete(void)
{
if (sbusFrameDone) {
if (!((sbus.in[22] >> 3) & 0x0001)) { // failsave flag
failsafeCnt = 0; // clear FailSafe counter
failsafe->vTable->reset();
sbusChannelData[0] = sbus.msg.chan0;
sbusChannelData[1] = sbus.msg.chan1;
sbusChannelData[2] = sbus.msg.chan2;

1
src/rx_sbus.h
View file

@ -1,4 +1,3 @@
#pragma once
void sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
bool sbusFrameComplete(void);

8
src/rx_spektrum.c
View file

@ -34,8 +34,12 @@ volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
static void spektrumDataReceive(uint16_t c);
static uint16_t spektrumReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
void spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
failsafe_t *failsafe;
void spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback)
{
failsafe = initialFailsafe;
switch (rxConfig->serialrx_type) {
case SERIALRX_SPEKTRUM2048:
// 11 bit frames
@ -74,7 +78,7 @@ static void spektrumDataReceive(uint16_t c)
spekFrame[spekFramePosition] = (uint8_t)c;
if (spekFramePosition == SPEK_FRAME_SIZE - 1) {
rcFrameComplete = true;
failsafeCnt = 0; // clear FailSafe counter
failsafe->vTable->reset();
} else {
spekFramePosition++;
}

1
src/rx_spektrum.h
View file

@ -1,4 +1,3 @@
#pragma once
void spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
bool spektrumFrameComplete(void);

8
src/rx_sumd.c
View file

@ -26,8 +26,12 @@ static uint16_t sumdReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntime
static uint32_t sumdChannelData[SUMD_MAX_CHANNEL];
void sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
failsafe_t *failsafe;
void sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback)
{
failsafe = initialFailsafe;
serialPorts.rcvrport = uartOpen(USART2, sumdDataReceive, 115200, MODE_RX);
if (callback)
*callback = sumdReadRawRC;
@ -74,7 +78,7 @@ bool sumdFrameComplete(void)
if (sumdFrameDone) {
sumdFrameDone = false;
if (sumd[1] == 0x01) {
failsafeCnt = 0;
failsafe->vTable->reset();
if (sumdSize > SUMD_MAX_CHANNEL)
sumdSize = SUMD_MAX_CHANNEL;
for (b = 0; b < sumdSize; b++)

1
src/rx_sumd.h
View file

@ -1,4 +1,3 @@
#pragma once
void sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
bool sumdFrameComplete(void);

8
src/serial_cli.c
View file

@ -181,10 +181,10 @@ const clivalue_t valueTable[] = {
{ "yawrate", VAR_UINT8, &cfg.controlRateConfig.yawRate, 0, 100 },
{ "tparate", VAR_UINT8, &cfg.dynThrPID, 0, 100},
{ "tpa_breakpoint", VAR_UINT16, &cfg.tpaBreakPoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
{ "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8, &cfg.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafe_detect_threshold, 100, PWM_RANGE_MAX },
{ "failsafe_delay", VAR_UINT8, &cfg.failsafeConfig.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8, &cfg.failsafeConfig.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafeConfig.failsafe_detect_threshold, 100, PWM_RANGE_MAX },
{ "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 },

4
src/serial_msp.c
View file

@ -336,7 +336,7 @@ static void evaluateCommand(void)
mcfg.minthrottle = read16();
mcfg.maxthrottle = read16();
mcfg.mincommand = read16();
cfg.failsafe_throttle = read16();
cfg.failsafeConfig.failsafe_throttle = read16();
read16();
read32();
cfg.mag_declination = read16() * 10;
@ -542,7 +542,7 @@ static void evaluateCommand(void)
serialize16(mcfg.minthrottle);
serialize16(mcfg.maxthrottle);
serialize16(mcfg.mincommand);
serialize16(cfg.failsafe_throttle);
serialize16(cfg.failsafeConfig.failsafe_throttle);
serialize16(0); // plog useless shit
serialize32(0); // plog useless shit
serialize16(cfg.mag_declination / 10); // TODO check this shit