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

Merge branch 'master' into patch_v3.1.4

Browse source
This commit is contained in:
borisbstyle 2017-02-07 13:47:12 +01:00 • committed by GitHub
commit ecb104b1f1
150 changed files with 3461 additions and 2167 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.travis.yml
View file

@ -82,7 +82,7 @@ install:
- make arm_sdk_install
before_script:
- tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version
- tools/gcc-arm-none-eabi-6_2-2016q4/bin/arm-none-eabi-gcc --version
- clang --version
- clang++ --version

24
Makefile
View file

@ -561,6 +561,7 @@ COMMON_SRC = \
config/config_eeprom.c \
config/feature.c \
config/parameter_group.c \
config/config_streamer.c \
drivers/adc.c \
drivers/buf_writer.c \
drivers/bus_i2c_soft.c \
@ -593,6 +594,7 @@ COMMON_SRC = \
fc/fc_rc.c \
fc/fc_msp.c \
fc/fc_tasks.c \
fc/rc_adjustments.c \
fc/rc_controls.c \
fc/runtime_config.c \
fc/cli.c \
@ -648,6 +650,7 @@ HIGHEND_SRC = \
cms/cms_menu_osd.c \
cms/cms_menu_vtx.c \
common/colorconversion.c \
common/gps_conversion.c \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
drivers/serial_escserial.c \
@ -655,7 +658,6 @@ HIGHEND_SRC = \
drivers/sonar_hcsr04.c \
drivers/vtx_common.c \
flight/navigation.c \
flight/gps_conversion.c \
io/dashboard.c \
io/displayport_max7456.c \
io/displayport_msp.c \
@ -707,25 +709,20 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
drivers/serial.c \
drivers/serial_uart.c \
drivers/sound_beeper.c \
drivers/stack_check.c \
drivers/system.c \
drivers/timer.c \
fc/fc_core.c \
fc/fc_tasks.c \
fc/fc_rc.c \
fc/rc_controls.c \
fc/runtime_config.c \
flight/altitudehold.c \
flight/failsafe.c \
flight/imu.c \
flight/mixer.c \
flight/pid.c \
flight/servos.c \
io/beeper.c \
io/serial.c \
io/statusindicator.c \
rx/ibus.c \
rx/jetiexbus.c \
rx/msp.c \
rx/nrf24_cx10.c \
rx/nrf24_inav.c \
rx/nrf24_h8_3d.c \
@ -746,25 +743,12 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
sensors/gyro.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC) \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
drivers/serial_softserial.c \
io/dashboard.c \
io/displayport_max7456.c \
io/displayport_msp.c \
io/displayport_oled.c \
io/ledstrip.c \
io/osd.c \
telemetry/telemetry.c \
telemetry/crsf.c \
telemetry/frsky.c \
telemetry/hott.c \
telemetry/smartport.c \
telemetry/ltm.c \
telemetry/mavlink.c \
telemetry/esc_telemetry.c \
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/serial_escserial.c \

10
make/tools.mk
View file

@ -14,16 +14,16 @@
##############################
# Set up ARM (STM32) SDK
ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q3
ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-6_2-2016q4
# Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion)
GCC_REQUIRED_VERSION ?= 5.4.1
GCC_REQUIRED_VERSION ?= 6.2.1
## arm_sdk_install : Install Arm SDK
.PHONY: arm_sdk_install
ARM_SDK_URL_BASE := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update/+download/gcc-arm-none-eabi-5_4-2016q3-20160926
ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2016q4/gcc-arm-none-eabi-6_2-2016q4-20161216
# source: https://launchpad.net/gcc-arm-embedded/5.0/
# source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads
ifdef LINUX
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2
endif
@ -33,7 +33,7 @@ ifdef MACOSX
endif
ifdef WINDOWS
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32-zip.zip
endif
ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))

177
src/main/blackbox/blackbox.c
View file

@ -24,6 +24,9 @@
#ifdef BLACKBOX
#include "blackbox.h"
#include "blackbox_io.h"
#include "build/debug.h"
#include "build/version.h"
@ -31,8 +34,10 @@
#include "common/encoding.h"
#include "common/utils.h"
#include "blackbox.h"
#include "blackbox_io.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/compass.h"
@ -43,18 +48,22 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "sensors/sensors.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/compass.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sonar.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#define BLACKBOX_I_INTERVAL 32
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
#define SLOW_FRAME_INTERVAL 4096
@ -232,21 +241,21 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
typedef enum BlackboxState {
BLACKBOX_STATE_DISABLED = 0,
BLACKBOX_STATE_STOPPED,
BLACKBOX_STATE_PREPARE_LOG_FILE,
BLACKBOX_STATE_SEND_HEADER,
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
BLACKBOX_STATE_SEND_GPS_H_HEADER,
BLACKBOX_STATE_SEND_GPS_G_HEADER,
BLACKBOX_STATE_SEND_SLOW_HEADER,
BLACKBOX_STATE_SEND_SYSINFO,
BLACKBOX_STATE_PAUSED,
BLACKBOX_STATE_RUNNING,
BLACKBOX_STATE_SHUTTING_DOWN
BLACKBOX_STATE_STOPPED, //1
BLACKBOX_STATE_PREPARE_LOG_FILE, //2
BLACKBOX_STATE_SEND_HEADER, //3
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4
BLACKBOX_STATE_SEND_GPS_H_HEADER, //5
BLACKBOX_STATE_SEND_GPS_G_HEADER, //6
BLACKBOX_STATE_SEND_SLOW_HEADER, //7
BLACKBOX_STATE_SEND_SYSINFO, //8
BLACKBOX_STATE_PAUSED, //9
BLACKBOX_STATE_RUNNING, //10
BLACKBOX_STATE_SHUTTING_DOWN, //11
BLACKBOX_STATE_START_ERASE, //12
BLACKBOX_STATE_ERASING, //13
} BlackboxState;
#define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER
#define BLACKBOX_LAST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_SYSINFO
typedef struct blackboxMainState_s {
uint32_t time;
@ -761,16 +770,16 @@ void validateBlackboxConfig()
if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
|| blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) {
blackboxConfig()->rate_num = 1;
blackboxConfig()->rate_denom = 1;
blackboxConfigMutable()->rate_num = 1;
blackboxConfigMutable()->rate_denom = 1;
} else {
/* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
* itself more frequently)
*/
div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
blackboxConfig()->rate_num /= div;
blackboxConfig()->rate_denom /= div;
blackboxConfigMutable()->rate_num /= div;
blackboxConfigMutable()->rate_denom /= div;
}
// If we've chosen an unsupported device, change the device to serial
@ -786,7 +795,7 @@ void validateBlackboxConfig()
break;
default:
blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL;
blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL;
}
}
@ -795,47 +804,44 @@ void validateBlackboxConfig()
*/
void startBlackbox(void)
{
if (blackboxState == BLACKBOX_STATE_STOPPED) {
validateBlackboxConfig();
validateBlackboxConfig();
if (!blackboxDeviceOpen()) {
blackboxSetState(BLACKBOX_STATE_DISABLED);
return;
}
memset(&gpsHistory, 0, sizeof(gpsHistory));
blackboxHistory[0] = &blackboxHistoryRing[0];
blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = vbatLatest;
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
/*
* We use conditional tests to decide whether or not certain fields should be logged. Since our headers
* must always agree with the logged data, the results of these tests must not change during logging. So
* cache those now.
*/
blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationProfile()->modeActivationConditions, BOXBLACKBOX);
blackboxIteration = 0;
blackboxPFrameIndex = 0;
blackboxIFrameIndex = 0;
/*
* Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
* it finally plays the beep for this arming event.
*/
blackboxLastArmingBeep = getArmingBeepTimeMicros();
blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status
blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
if (!blackboxDeviceOpen()) {
blackboxSetState(BLACKBOX_STATE_DISABLED);
return;
}
memset(&gpsHistory, 0, sizeof(gpsHistory));
blackboxHistory[0] = &blackboxHistoryRing[0];
blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = vbatLatest;
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
/*
* We use conditional tests to decide whether or not certain fields should be logged. Since our headers
* must always agree with the logged data, the results of these tests must not change during logging. So
* cache those now.
*/
blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX);
blackboxIteration = 0;
blackboxPFrameIndex = 0;
blackboxIFrameIndex = 0;
/*
* Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
* it finally plays the beep for this arming event.
*/
blackboxLastArmingBeep = getArmingBeepTimeMicros();
blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status
blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
}
/**
@ -1460,18 +1466,26 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
void handleBlackbox(timeUs_t currentTimeUs)
{
int i;
if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) {
blackboxReplenishHeaderBudget();
}
static bool erasedOnce = false; //Only allow one erase per FC reboot.
switch (blackboxState) {
case BLACKBOX_STATE_STOPPED:
if (ARMING_FLAG(ARMED)) {
blackboxOpen();
startBlackbox();
}
if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE) && !erasedOnce) {
blackboxSetState(BLACKBOX_STATE_START_ERASE);
erasedOnce = true;
}
break;
case BLACKBOX_STATE_PREPARE_LOG_FILE:
if (blackboxDeviceBeginLog()) {
blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
}
break;
case BLACKBOX_STATE_SEND_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
/*
@ -1492,6 +1506,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
}
break;
case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields),
&blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
@ -1505,6 +1520,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
break;
#ifdef GPS
case BLACKBOX_STATE_SEND_GPS_H_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAY_LENGTH(blackboxGpsHFields),
NULL, NULL)) {
@ -1512,6 +1528,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
}
break;
case BLACKBOX_STATE_SEND_GPS_G_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAY_LENGTH(blackboxGpsGFields),
&blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) {
@ -1520,6 +1537,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
break;
#endif
case BLACKBOX_STATE_SEND_SLOW_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAY_LENGTH(blackboxSlowFields),
NULL, NULL)) {
@ -1527,6 +1545,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
}
break;
case BLACKBOX_STATE_SEND_SYSINFO:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0
//Keep writing chunks of the system info headers until it returns true to signal completion
@ -1556,7 +1575,6 @@ void handleBlackbox(timeUs_t currentTimeUs)
blackboxLogIteration(currentTimeUs);
}
// Keep the logging timers ticking so our log iteration continues to advance
blackboxAdvanceIterationTimers();
break;
@ -1568,7 +1586,6 @@ void handleBlackbox(timeUs_t currentTimeUs)
} else {
blackboxLogIteration(currentTimeUs);
}
blackboxAdvanceIterationTimers();
break;
case BLACKBOX_STATE_SHUTTING_DOWN:
@ -1585,15 +1602,29 @@ void handleBlackbox(timeUs_t currentTimeUs)
blackboxSetState(BLACKBOX_STATE_STOPPED);
}
break;
case BLACKBOX_STATE_START_ERASE:
blackboxEraseAll();
blackboxSetState(BLACKBOX_STATE_ERASING);
beeper(BEEPER_BLACKBOX_ERASE);
break;
case BLACKBOX_STATE_ERASING:
if (isBlackboxErased()) {
//Done eraseing
blackboxSetState(BLACKBOX_STATE_STOPPED);
beeper(BEEPER_BLACKBOX_ERASE);
}
default:
break;
}
// Did we run out of room on the device? Stop!
if (isBlackboxDeviceFull()) {
blackboxSetState(BLACKBOX_STATE_STOPPED);
// ensure we reset the test mode flag if we stop due to full memory card
if (startedLoggingInTestMode) startedLoggingInTestMode = false;
if (blackboxState != BLACKBOX_STATE_ERASING && blackboxState != BLACKBOX_STATE_START_ERASE) {
blackboxSetState(BLACKBOX_STATE_STOPPED);
// ensure we reset the test mode flag if we stop due to full memory card
if (startedLoggingInTestMode) startedLoggingInTestMode = false;
}
} else { // Only log in test mode if there is room!
if(blackboxConfig()->on_motor_test) {

4
src/main/blackbox/blackbox.h
View file

@ -21,6 +21,8 @@
#include "common/time.h"
#include "config/parameter_group.h"
typedef struct blackboxConfig_s {
uint8_t rate_num;
uint8_t rate_denom;
@ -28,6 +30,8 @@ typedef struct blackboxConfig_s {
uint8_t on_motor_test;
} blackboxConfig_t;
PG_DECLARE(blackboxConfig_t, blackboxConfig);
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
void initBlackbox(void);

54
src/main/blackbox/blackbox_io.c
View file

@ -8,6 +8,7 @@
#ifdef BLACKBOX
#include "blackbox.h"
#include "blackbox_io.h"
#include "build/version.h"
@ -16,6 +17,10 @@
#include "common/encoding.h"
#include "common/printf.h"
#include "config/config_profile.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
@ -23,12 +28,10 @@
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/serial.h"
#include "msp/msp_serial.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#define BLACKBOX_SERIAL_PORT_MODE MODE_TX
// How many bytes can we transmit per loop iteration when writing headers?
@ -63,6 +66,14 @@ static struct {
#endif
void blackboxOpen()
{
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
}
void blackboxWrite(uint8_t value)
{
switch (blackboxConfig()->device) {
@ -599,6 +610,43 @@ bool blackboxDeviceOpen(void)
}
}
/**
* Erase all blackbox logs
*/
void blackboxEraseAll(void)
{
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
flashfsEraseCompletely();
break;
#endif
default:
//not supported
break;
}
}
/**
* Check to see if erasing is done
*/
bool isBlackboxErased(void)
{
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
return flashfsIsReady();
break;
#endif
default:
//not supported
return true;
break;
}
}
/**
* Close the Blackbox logging device immediately without attempting to flush any remaining data.
*/

4
src/main/blackbox/blackbox_io.h
View file

@ -49,6 +49,7 @@ typedef enum {
extern int32_t blackboxHeaderBudget;
void blackboxOpen(void);
void blackboxWrite(uint8_t value);
int blackboxPrintf(const char *fmt, ...);
@ -71,6 +72,9 @@ bool blackboxDeviceFlushForce(void);
bool blackboxDeviceOpen(void);
void blackboxDeviceClose(void);
void blackboxEraseAll(void);
bool isBlackboxErased(void);
bool blackboxDeviceBeginLog(void);
bool blackboxDeviceEndLog(bool retainLog);

26
src/main/build/atomic.h
View file

@ -69,25 +69,25 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
__ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 )
// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier.
// Be carefull when using this, you must use some method to prevent optimizer form breaking things
// - lto is used for baseflight compillation, so function call is not memory barrier
// - use ATOMIC_BARRIER or propper volatile to protect used variables
// - gcc 4.8.4 does write all values in registes to memory before 'asm volatile', so this optimization does not help much
// but that can change in future versions
// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier.
// Be careful when using this, you must use some method to prevent optimizer form breaking things
// - lto is used for Cleanflight compilation, so function call is not memory barrier
// - use ATOMIC_BARRIER or volatile to protect used variables
// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
// - gcc 5 and later works as intended, generating quite optimal code
#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
__ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
// ATOMIC_BARRIER
// Create memory barrier
// - at the beginning (all data must be reread from memory)
// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
// ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier
// - at the beginning of containing block (value of parameter must be reread from memory)
// - at exit of block (all exit paths) (parameter value if written into memory, but may be cached in register for subsequent use)
// On gcc 5 and higher, this protects only memory passed as parameter (any type should work)
// this macro can be used only ONCE PER LINE, but multiple uses per block are fine
#if (__GNUC__ > 5)
#if (__GNUC__ > 6)
#warning "Please verify that ATOMIC_BARRIER works as intended"
// increment version number is BARRIER works
// increment version number if BARRIER works
// TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead
// you should check that local variable scope with cleanup spans entire block
#endif
@ -101,10 +101,10 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
// this macro uses local function for cleanup. CLang block can be substituded
#define ATOMIC_BARRIER(data) \
__extension__ void __UNIQL(__barrierEnd)(typeof(data) **__d) { \
__asm__ volatile ("\t# barier(" #data ") end\n" : : "m" (**__d)); \
__asm__ volatile ("\t# barrier(" #data ") end\n" : : "m" (**__d)); \
} \
typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \
__asm__ volatile ("\t# barier (" #data ") start\n" : "=m" (*__UNIQL(__barrier)))
__asm__ volatile ("\t# barrier (" #data ") start\n" : "+m" (*__UNIQL(__barrier)))
// define these wrappers for atomic operations, use gcc buildins

4
src/main/build/version.h
View file

@ -17,8 +17,8 @@
#define FC_FIRMWARE_NAME "Betaflight"
#define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 4 // increment when a bug is fixed
#define FC_VERSION_MINOR 2 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)

15
src/main/cms/cms.c
View file

@ -44,19 +44,24 @@
#include "drivers/system.h"
// For rcData, stopAllMotors, stopPwmAllMotors
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
// For 'ARM' related
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
// For rcData, stopAllMotors, stopPwmAllMotors
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "flight/mixer.h"
// For VISIBLE* (Actually, included by config_master.h)
// For VISIBLE*
#include "io/osd.h"
#include "rx/rx.h"
// DisplayPort management
#ifndef CMS_MAX_DEVICE

9
src/main/cms/cms_menu_blackbox.c
View file

@ -27,10 +27,10 @@
#include "platform.h"
#include "build/version.h"
#ifdef CMS
#include "build/version.h"
#include "drivers/system.h"
#include "cms/cms.h"
@ -40,11 +40,13 @@
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/beeper.h"
#include "blackbox/blackbox_io.h"
@ -62,6 +64,7 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
delay(100);
}
beeper(BEEPER_BLACKBOX_ERASE);
displayClearScreen(pDisplay);
displayResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI?

61
src/main/cms/cms_menu_imu.c
View file

@ -44,8 +44,9 @@
#include "flight/pid.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
//
// PID
@ -104,10 +105,11 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void
static long cmsx_PidRead(void)
{
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
for (uint8_t i = 0; i < 3; i++) {
tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i];
tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i];
tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i];
tempPid[i][0] = pidProfile->P8[i];
tempPid[i][1] = pidProfile->I8[i];
tempPid[i][2] = pidProfile->D8[i];
}
return 0;
@ -125,10 +127,11 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
{
UNUSED(self);
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
for (uint8_t i = 0; i < 3; i++) {
masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0];
masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
pidProfile->P8[i] = tempPid[i][0];
pidProfile->I8[i] = tempPid[i][1];
pidProfile->D8[i] = tempPid[i][2];
}
pidInitConfig(&currentProfile->pidProfile);
@ -233,12 +236,13 @@ static long cmsx_profileOtherOnEnter(void)
{
profileIndexString[1] = '0' + tmpProfileIndex;
cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight;
cmsx_setpointRelaxRatio = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio;
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight;
cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio;
cmsx_angleStrength = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL];
cmsx_horizonStrength = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL];
cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL];
cmsx_angleStrength = pidProfile->P8[PIDLEVEL];
cmsx_horizonStrength = pidProfile->I8[PIDLEVEL];
cmsx_horizonTransition = pidProfile->D8[PIDLEVEL];
return 0;
}
@ -247,13 +251,14 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
{
UNUSED(self);
masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight;
pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidInitConfig(&currentProfile->pidProfile);
masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;
masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition;
pidProfile->P8[PIDLEVEL] = cmsx_angleStrength;
pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength;
pidProfile->D8[PIDLEVEL] = cmsx_horizonTransition;
return 0;
}
@ -311,11 +316,12 @@ static uint16_t cmsx_yaw_p_limit;
static long cmsx_FilterPerProfileRead(void)
{
cmsx_dterm_lpf_hz = masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz;
cmsx_dterm_notch_hz = masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz;
cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff;
cmsx_yaw_lpf_hz = masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz;
cmsx_yaw_p_limit = masterConfig.profile[profileIndex].pidProfile.yaw_p_limit;
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
cmsx_dterm_lpf_hz = pidProfile->dterm_lpf_hz;
cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz;
cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
cmsx_yaw_lpf_hz = pidProfile->yaw_lpf_hz;
cmsx_yaw_p_limit = pidProfile->yaw_p_limit;
return 0;
}
@ -324,11 +330,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
{
UNUSED(self);
masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz;
masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz = cmsx_dterm_notch_hz;
masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz;
masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit;
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
pidProfile->dterm_lpf_hz = cmsx_dterm_lpf_hz;
pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz;
pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
pidProfile->yaw_lpf_hz = cmsx_yaw_lpf_hz;
pidProfile->yaw_p_limit = cmsx_yaw_p_limit;
return 0;
}

7
src/main/cms/cms_menu_ledstrip.c
View file

@ -22,15 +22,16 @@
#include "platform.h"
#include "build/version.h"
#ifdef CMS
#include "build/version.h"
#include "drivers/system.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "cms/cms.h"
#include "cms/cms_types.h"

9
src/main/cms/cms_menu_misc.c
View file

@ -28,14 +28,15 @@
#include "drivers/system.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_ledstrip.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
//
// Misc
//

7
src/main/cms/cms_menu_osd.c
View file

@ -21,6 +21,8 @@
#include "platform.h"
#if defined(OSD) && defined(CMS)
#include "build/version.h"
#include "cms/cms.h"
@ -28,10 +30,9 @@
#include "cms/cms_menu_osd.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#if defined(OSD) && defined(CMS)
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5};
OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50};

11
src/main/cms/cms_menu_vtx.c
View file

@ -21,6 +21,10 @@
#include "platform.h"
#ifdef CMS
#if defined(VTX) || defined(USE_RTC6705)
#include "build/version.h"
#include "cms/cms.h"
@ -30,12 +34,9 @@
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef CMS
#if defined(VTX) || defined(USE_RTC6705)
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
static bool featureRead = false;
static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel;

0
src/main/flight/gps_conversion.c → src/main/common/gps_conversion.c
View file

0
src/main/flight/gps_conversion.h → src/main/common/gps_conversion.h
View file

4
src/main/common/maths.h
View file

@ -30,6 +30,10 @@
#define M_PIf 3.14159265358979323846f
#define RAD (M_PIf / 180.0f)
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))

471
src/main/config/config_eeprom.c
View file

@ -18,294 +18,285 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <stddef.h>
#include "platform.h"
#include "drivers/system.h"
#include "config/config_master.h"
#include "build/build_config.h"
#include "common/maths.h"
#include "config/config_eeprom.h"
#include "config/config_streamer.h"
#include "config/config_master.h"
#include "config/parameter_group.h"
#if !defined(FLASH_SIZE)
#error "Flash size not defined for target. (specify in KB)"
#endif
#include "drivers/system.h"
#include "fc/config.h"
#ifndef FLASH_PAGE_SIZE
#ifdef STM32F303xC
#define FLASH_PAGE_SIZE ((uint16_t)0x800)
#endif
// declare a dummy PG, since scanEEPROM assumes there is at least one PG
// !!TODO remove once first PG has been created out of masterConfg
typedef struct dummpConfig_s {
uint8_t dummy;
} dummyConfig_t;
PG_DECLARE(dummyConfig_t, dummyConfig);
#define PG_DUMMY_CONFIG 1
PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0);
#ifdef STM32F10X_MD
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
#endif
extern uint8_t __config_start; // configured via linker script when building binaries.
extern uint8_t __config_end;
#ifdef STM32F10X_HD
#define FLASH_PAGE_SIZE ((uint16_t)0x800)
#endif
static uint16_t eepromConfigSize;
#if defined(STM32F745xx)
#define FLASH_PAGE_SIZE ((uint32_t)0x40000)
#endif
typedef enum {
CR_CLASSICATION_SYSTEM = 0,
CR_CLASSICATION_PROFILE1 = 1,
CR_CLASSICATION_PROFILE2 = 2,
CR_CLASSICATION_PROFILE3 = 3,
CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_PROFILE3,
} configRecordFlags_e;
#if defined(STM32F746xx)
#define FLASH_PAGE_SIZE ((uint32_t)0x40000)
#endif
#define CR_CLASSIFICATION_MASK (0x3)
#if defined(STM32F722xx)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
// Header for the saved copy.
typedef struct {
uint8_t format;
} PG_PACKED configHeader_t;
#if defined(STM32F40_41xxx)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
// Header for each stored PG.
typedef struct {
// split up.
uint16_t size;
pgn_t pgn;
uint8_t version;
#if defined (STM32F411xE)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
// lower 2 bits used to indicate system or profile number, see CR_CLASSIFICATION_MASK
uint8_t flags;
#endif
uint8_t pg[];
} PG_PACKED configRecord_t;
#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT)
#ifdef STM32F10X_MD
#define FLASH_PAGE_COUNT 128
#endif
#ifdef STM32F10X_HD
#define FLASH_PAGE_COUNT 128
#endif
#endif
#if defined(FLASH_SIZE)
#if defined(STM32F40_41xxx)
#define FLASH_PAGE_COUNT 4
#elif defined (STM32F411xE)
#define FLASH_PAGE_COUNT 3
#elif defined (STM32F722xx)
#define FLASH_PAGE_COUNT 3
#elif defined (STM32F745xx)
#define FLASH_PAGE_COUNT 4
#elif defined (STM32F746xx)
#define FLASH_PAGE_COUNT 4
#else
#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
#endif
#endif
#if !defined(FLASH_PAGE_SIZE)
#error "Flash page size not defined for target."
#endif
#if !defined(FLASH_PAGE_COUNT)
#error "Flash page count not defined for target."
#endif
#if FLASH_SIZE <= 128
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
#else
#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000
#endif
// use the last flash pages for storage
#ifdef CUSTOM_FLASH_MEMORY_ADDRESS
size_t custom_flash_memory_address = 0;
#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address)
#else
// use the last flash pages for storage
#ifndef CONFIG_START_FLASH_ADDRESS
#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
#endif
#endif
// Footer for the saved copy.
typedef struct {
uint16_t terminator;
} PG_PACKED configFooter_t;
// checksum is appended just after footer. It is not included in footer to make checksum calculation consistent
// Used to check the compiler packing at build time.
typedef struct {
uint8_t byte;
uint32_t word;
} PG_PACKED packingTest_t;
void initEEPROM(void)
{
// Verify that this architecture packs as expected.
BUILD_BUG_ON(offsetof(packingTest_t, byte) != 0);
BUILD_BUG_ON(offsetof(packingTest_t, word) != 1);
BUILD_BUG_ON(sizeof(packingTest_t) != 5);
BUILD_BUG_ON(sizeof(configHeader_t) != 1);
BUILD_BUG_ON(sizeof(configFooter_t) != 2);
BUILD_BUG_ON(sizeof(configRecord_t) != 6);
}
static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length)
{
uint8_t checksum = 0;
const uint8_t *byteOffset;
const uint8_t *p = (const uint8_t *)data;
const uint8_t *pend = p + length;
for (byteOffset = data; byteOffset < (data + length); byteOffset++)
checksum ^= *byteOffset;
return checksum;
for (; p != pend; p++) {
chk ^= *p;
}
return chk;
}
// Scan the EEPROM config. Returns true if the config is valid.
bool isEEPROMContentValid(void)
{
const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS;
uint8_t checksum = 0;
uint8_t chk = 0;
const uint8_t *p = &__config_start;
const configHeader_t *header = (const configHeader_t *)p;
// check version number
if (EEPROM_CONF_VERSION != temp->version)
if (header->format != EEPROM_CONF_VERSION) {
return false;
}
chk = updateChecksum(chk, header, sizeof(*header));
p += sizeof(*header);
// include the transitional masterConfig record
chk = updateChecksum(chk, p, sizeof(masterConfig));
p += sizeof(masterConfig);
// check size and magic numbers
if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
return false;
for (;;) {
const configRecord_t *record = (const configRecord_t *)p;
if (strncasecmp(temp->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)))
return false;
if (record->size == 0) {
// Found the end. Stop scanning.
break;
}
if (p + record->size >= &__config_end
|| record->size < sizeof(*record)) {
// Too big or too small.
return false;
}
// verify integrity of temporary copy
checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
if (checksum != 0)
return false;
chk = updateChecksum(chk, p, record->size);
// looks good, let's roll!
p += record->size;
}
const configFooter_t *footer = (const configFooter_t *)p;
chk = updateChecksum(chk, footer, sizeof(*footer));
p += sizeof(*footer);
chk = ~chk;
const uint8_t checkSum = *p;
p += sizeof(checkSum);
eepromConfigSize = p - &__config_start;
return chk == checkSum;
}
uint16_t getEEPROMConfigSize(void)
{
return eepromConfigSize;
}
// find config record for reg + classification (profile info) in EEPROM
// return NULL when record is not found
// this function assumes that EEPROM content is valid
static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFlags_e classification)
{
const uint8_t *p = &__config_start;
p += sizeof(configHeader_t); // skip header
p += sizeof(master_t); // skip the transitional master_t record
while (true) {
const configRecord_t *record = (const configRecord_t *)p;
if (record->size == 0
|| p + record->size >= &__config_end
|| record->size < sizeof(*record))
break;
if (pgN(reg) == record->pgn
&& (record->flags & CR_CLASSIFICATION_MASK) == classification)
return record;
p += record->size;
}
// record not found
return NULL;
}
// Initialize all PG records from EEPROM.
// This functions processes all PGs sequentially, scanning EEPROM for each one. This is suboptimal,
// but each PG is loaded/initialized exactly once and in defined order.
bool loadEEPROM(void)
{
// read in the transitional masterConfig record
const uint8_t *p = &__config_start;
p += sizeof(configHeader_t); // skip header
masterConfig = *(master_t*)p;
PG_FOREACH(reg) {
configRecordFlags_e cls_start, cls_end;
if (pgIsSystem(reg)) {
cls_start = CR_CLASSICATION_SYSTEM;
cls_end = CR_CLASSICATION_SYSTEM;
} else {
cls_start = CR_CLASSICATION_PROFILE1;
cls_end = CR_CLASSICATION_PROFILE_LAST;
}
for (configRecordFlags_e cls = cls_start; cls <= cls_end; cls++) {
int profileIndex = cls - cls_start;
const configRecord_t *rec = findEEPROM(reg, cls);
if (rec) {
// config from EEPROM is available, use it to initialize PG. pgLoad will handle version mismatch
pgLoad(reg, profileIndex, rec->pg, rec->size - offsetof(configRecord_t, pg), rec->version);
} else {
pgReset(reg, profileIndex);
}
}
}
return true;
}
#if defined(STM32F7)
// FIXME: HAL for now this will only work for F4/F7 as flash layout is different
void writeEEPROM(void)
static bool writeSettingsToEEPROM(void)
{
// Generate compile time error if the config does not fit in the reserved area of flash.
BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
config_streamer_t streamer;
config_streamer_init(&streamer);
HAL_StatusTypeDef status;
uint32_t wordOffset;
int8_t attemptsRemaining = 3;
config_streamer_start(&streamer, (uintptr_t)&__config_start, &__config_end - &__config_start);
uint8_t chk = 0;
suspendRxSignal();
configHeader_t header = {
.format = EEPROM_CONF_VERSION,
};
// prepare checksum/version constants
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.size = sizeof(master_t);
masterConfig.magic_be = 0xBE;
masterConfig.magic_ef = 0xEF;
masterConfig.chk = 0; // erase checksum before recalculating
masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header));
chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header));
// write the transitional masterConfig record
config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig));
chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig));
PG_FOREACH(reg) {
const uint16_t regSize = pgSize(reg);
configRecord_t record = {
.size = sizeof(configRecord_t) + regSize,
.pgn = pgN(reg),
.version = pgVersion(reg),
.flags = 0
};
// write it
/* Unlock the Flash to enable the flash control register access *************/
HAL_FLASH_Unlock();
while (attemptsRemaining--)
{
/* Fill EraseInit structure*/
FLASH_EraseInitTypeDef EraseInitStruct = {0};
EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS;
EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
EraseInitStruct.Sector = (FLASH_SECTOR_TOTAL-1);
EraseInitStruct.NbSectors = 1;
uint32_t SECTORError;
status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
if (status != HAL_OK)
{
continue;
}
else
{
for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4)
{
status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset));
if(status != HAL_OK)
{
break;
}
if (pgIsSystem(reg)) {
// write the only instance
record.flags |= CR_CLASSICATION_SYSTEM;
config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
config_streamer_write(&streamer, reg->address, regSize);
chk = updateChecksum(chk, reg->address, regSize);
} else {
// write one instance for each profile
for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) {
record.flags = 0;
record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
const uint8_t *address = reg->address + (regSize * profileIndex);
config_streamer_write(&streamer, address, regSize);
chk = updateChecksum(chk, address, regSize);
}
}
if (status == HAL_OK) {
break;
}
configFooter_t footer = {
.terminator = 0,
};
config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer));
chk = updateChecksum(chk, (uint8_t *)&footer, sizeof(footer));
// append checksum now
chk = ~chk;
config_streamer_write(&streamer, &chk, sizeof(chk));
config_streamer_flush(&streamer);
bool success = config_streamer_finish(&streamer) == 0;
return success;
}
void writeConfigToEEPROM(void)
{
bool success = false;
// write it
for (int attempt = 0; attempt < 3 && !success; attempt++) {
if (writeSettingsToEEPROM()) {
success = true;
}
}
HAL_FLASH_Lock();
if (success && isEEPROMContentValid()) {
return;
}
// Flash write failed - just die now
if (status != HAL_OK || !isEEPROMContentValid()) {
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
resumeRxSignal();
}
#else
void writeEEPROM(void)
{
// Generate compile time error if the config does not fit in the reserved area of flash.
BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
FLASH_Status status = 0;
uint32_t wordOffset;
int8_t attemptsRemaining = 3;
suspendRxSignal();
// prepare checksum/version constants
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.size = sizeof(master_t);
masterConfig.magic_be = 0xBE;
masterConfig.magic_ef = 0xEF;
masterConfig.chk = 0; // erase checksum before recalculating
masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
// write it
FLASH_Unlock();
while (attemptsRemaining--) {
#if defined(STM32F4)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
#elif defined(STM32F303)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
#elif defined(STM32F10X)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
#endif
for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
if (wordOffset % FLASH_PAGE_SIZE == 0) {
#if defined(STM32F40_41xxx)
status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000
#elif defined (STM32F411xE)
status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000
#else
status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
#endif
if (status != FLASH_COMPLETE) {
break;
}
}
status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset,
*(uint32_t *) ((char *) &masterConfig + wordOffset));
if (status != FLASH_COMPLETE) {
break;
}
}
if (status == FLASH_COMPLETE) {
break;
}
}
FLASH_Lock();
// Flash write failed - just die now
if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
resumeRxSignal();
}
#endif
void readEEPROM(void)
{
// Sanity check
if (!isEEPROMContentValid())
failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
suspendRxSignal();
// Read flash
memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check
masterConfig.current_profile_index = 0;
setProfile(masterConfig.current_profile_index);
validateAndFixConfig();
activateConfig();
resumeRxSignal();
failureMode(FAILURE_FLASH_WRITE_FAILED);
}

9
src/main/config/config_eeprom.h
View file

@ -17,9 +17,12 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#define EEPROM_CONF_VERSION 155
void initEEPROM(void);
void writeEEPROM();
void readEEPROM(void);
bool isEEPROMContentValid(void);
bool loadEEPROM(void);
void writeConfigToEEPROM(void);
uint16_t getEEPROMConfigSize(void);

68
src/main/config/config_master.h
View file

@ -35,6 +35,7 @@
#include "drivers/flash.h"
#include "drivers/display.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "flight/failsafe.h"
@ -42,10 +43,10 @@
#include "flight/servos.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/gps.h"
#include "io/osd.h"
@ -64,6 +65,7 @@
#include "sensors/battery.h"
#include "sensors/compass.h"
#ifndef USE_PARAMETER_GROUPS
#define motorConfig(x) (&masterConfig.motorConfig)
#define flight3DConfig(x) (&masterConfig.flight3DConfig)
#define servoConfig(x) (&masterConfig.servoConfig)
@ -105,11 +107,71 @@
#define modeActivationProfile(x) (&masterConfig.modeActivationProfile)
#define servoProfile(x) (&masterConfig.servoProfile)
#define customMotorMixer(i) (&masterConfig.customMotorMixer[i])
#define customServoMixer(i) (&masterConfig.customServoMixer[i])
#define customServoMixers(i) (&masterConfig.customServoMixer[i])
#define displayPortProfileMsp(x) (&masterConfig.displayPortProfileMsp)
#define displayPortProfileMax7456(x) (&masterConfig.displayPortProfileMax7456)
#define displayPortProfileOled(x) (&masterConfig.displayPortProfileOled)
#define motorConfigMutable(x) (&masterConfig.motorConfig)
#define flight3DConfigMutable(x) (&masterConfig.flight3DConfig)
#define servoConfigMutable(x) (&masterConfig.servoConfig)
#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig)
#define gimbalConfigMutable(x) (&masterConfig.gimbalConfig)
#define boardAlignmentMutable(x) (&masterConfig.boardAlignment)
#define imuConfigMutable(x) (&masterConfig.imuConfig)
#define gyroConfigMutable(x) (&masterConfig.gyroConfig)
#define compassConfigMutable(x) (&masterConfig.compassConfig)
#define accelerometerConfigMutable(x) (&masterConfig.accelerometerConfig)
#define barometerConfigMutable(x) (&masterConfig.barometerConfig)
#define throttleCorrectionConfigMutable(x) (&masterConfig.throttleCorrectionConfig)
#define batteryConfigMutable(x) (&masterConfig.batteryConfig)
#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig)
#define gpsProfileMutable(x) (&masterConfig.gpsProfile)
#define gpsConfigMutable(x) (&masterConfig.gpsConfig)
#define rxConfigMutable(x) (&masterConfig.rxConfig)
#define armingConfigMutable(x) (&masterConfig.armingConfig)
#define mixerConfigMutable(x) (&masterConfig.mixerConfig)
#define airplaneConfigMutable(x) (&masterConfig.airplaneConfig)
#define failsafeConfigMutable(x) (&masterConfig.failsafeConfig)
#define serialConfigMutable(x) (&masterConfig.serialConfig)
#define telemetryConfigMutable(x) (&masterConfig.telemetryConfig)
#define ibusTelemetryConfigMutable(x) (&masterConfig.telemetryConfig)
#define ppmConfigMutable(x) (&masterConfig.ppmConfig)
#define pwmConfigMutable(x) (&masterConfig.pwmConfig)
#define adcConfigMutable(x) (&masterConfig.adcConfig)
#define beeperConfigMutable(x) (&masterConfig.beeperConfig)
#define sonarConfigMutable(x) (&masterConfig.sonarConfig)
#define ledStripConfigMutable(x) (&masterConfig.ledStripConfig)
#define statusLedConfigMutable(x) (&masterConfig.statusLedConfig)
#define osdProfileMutable(x) (&masterConfig.osdProfile)
#define vcdProfileMutable(x) (&masterConfig.vcdProfile)
#define sdcardConfigMutable(x) (&masterConfig.sdcardConfig)
#define blackboxConfigMutable(x) (&masterConfig.blackboxConfig)
#define flashConfigMutable(x) (&masterConfig.flashConfig)
#define pidConfigMutable(x) (&masterConfig.pidConfig)
#define adjustmentProfileMutable(x) (&masterConfig.adjustmentProfile)
#define modeActivationProfileMutable(x) (&masterConfig.modeActivationProfile)
#define servoProfileMutable(x) (&masterConfig.servoProfile)
#define customMotorMixerMutable(i) (&masterConfig.customMotorMixer[i])
#define customServoMixersMutable(i) (&masterConfig.customServoMixer[i])
#define displayPortProfileMspMutable(x) (&masterConfig.displayPortProfileMsp)
#define displayPortProfileMax7456Mutable(x) (&masterConfig.displayPortProfileMax7456)
#define displayPortProfileOledMutable(x) (&masterConfig.displayPortProfileOled)
#define servoParams(x) (&servoProfile()->servoConf[i])
#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x])
#define rxFailsafeChannelConfigs(x) (&masterConfig.rxConfig.failsafe_channel_configurations[x])
#define osdConfig(x) (&masterConfig.osdProfile)
#define modeActivationConditions(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x])
#define servoParamsMutable(x) (&servoProfile()->servoConf[i])
#define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[i])
#define rxFailsafeChannelConfigsMutable(x) (&masterConfig.rxConfig.>failsafe_channel_configurations[x])
#define osdConfigMutable(x) (&masterConfig.osdProfile)
#define modeActivationConditionsMutable(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x])
#endif
// System-wide
typedef struct master_s {
uint8_t version;
@ -264,7 +326,5 @@ typedef struct master_s {
} master_t;
extern master_t masterConfig;
extern profile_t *currentProfile;
extern controlRateConfig_t *currentControlRateProfile;
void createDefaultConfig(master_t *config);

1
src/main/config/config_profile.h
View file

@ -17,7 +17,6 @@
#pragma once
#include "common/axis.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "flight/pid.h"

40
src/main/config/config_reset.h Normal file
View file

@ -0,0 +1,40 @@
/*
* 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/>.
*/
#pragma once
#ifndef __UNIQL
# define __UNIQL_CONCAT2(x,y) x ## y
# define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y)
# define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__)
#endif
// overwrite _name with data passed as arguments. This version forces GCC to really copy data
// It is not possible to use multiple RESET_CONFIGs on single line (__UNIQL limitation)
#define RESET_CONFIG(_type, _name, ...) \
static const _type __UNIQL(_reset_template_) = { \
__VA_ARGS__ \
}; \
memcpy((_name), &__UNIQL(_reset_template_), sizeof(*(_name))); \
/**/
// overwrite _name with data passed as arguments. GCC is allowed to set structure field-by-field
#define RESET_CONFIG_2(_type, _name, ...) \
*(_name) = (_type) { \
__VA_ARGS__ \
}; \
/**/

263
src/main/config/config_streamer.c Normal file
View file

@ -0,0 +1,263 @@
/*
* 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 <string.h>
#include "platform.h"
#include "drivers/system.h"
#include "config/config_streamer.h"
extern uint8_t __config_start; // configured via linker script when building binaries.
extern uint8_t __config_end;
#if !defined(FLASH_PAGE_SIZE)
// F1
# if defined(STM32F10X_MD)
# define FLASH_PAGE_SIZE (0x400)
# elif defined(STM32F10X_HD)
# define FLASH_PAGE_SIZE (0x800)
// F3
# elif defined(STM32F303xC)
# define FLASH_PAGE_SIZE (0x800)
// F4
# elif defined(STM32F40_41xxx)
# define FLASH_PAGE_SIZE ((uint32_t)0x20000)
# elif defined (STM32F411xE)
# define FLASH_PAGE_SIZE ((uint32_t)0x20000)
# elif defined(STM32F427_437xx)
# define FLASH_PAGE_SIZE ((uint32_t)0x20000) // 128K sectors
// F7
#elif defined(STM32F722xx)
# define FLASH_PAGE_SIZE ((uint32_t)0x20000)
# elif defined(STM32F745xx)
# define FLASH_PAGE_SIZE ((uint32_t)0x40000)
# elif defined(STM32F746xx)
# define FLASH_PAGE_SIZE ((uint32_t)0x40000)
# elif defined(UNIT_TEST)
# define FLASH_PAGE_SIZE (0x400)
# else
# error "Flash page size not defined for target."
# endif
#endif
void config_streamer_init(config_streamer_t *c)
{
memset(c, 0, sizeof(*c));
}
void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
{
// base must start at FLASH_PAGE_SIZE boundary
c->address = base;
c->size = size;
if (!c->unlocked) {
#if defined(STM32F7)
HAL_FLASH_Unlock();
#else
FLASH_Unlock();
#endif
c->unlocked = true;
}
#if defined(STM32F10X)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
#elif defined(STM32F303)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
#elif defined(STM32F4)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
#elif defined(STM32F7)
// NOP
#elif defined(UNIT_TEST)
// NOP
#else
# error "Unsupported CPU"
#endif
c->err = 0;
}
#if defined(STM32F7)
/*
Sector 0 0x08000000 - 0x08007FFF 32 Kbytes
Sector 1 0x08008000 - 0x0800FFFF 32 Kbytes
Sector 2 0x08010000 - 0x08017FFF 32 Kbytes
Sector 3 0x08018000 - 0x0801FFFF 32 Kbytes
Sector 4 0x08020000 - 0x0803FFFF 128 Kbytes
Sector 5 0x08040000 - 0x0807FFFF 256 Kbytes
Sector 6 0x08080000 - 0x080BFFFF 256 Kbytes
Sector 7 0x080C0000 - 0x080FFFFF 256 Kbytes
*/
static uint32_t getFLASHSectorForEEPROM(void)
{
if ((uint32_t)&__config_start <= 0x08007FFF)
return FLASH_SECTOR_0;
if ((uint32_t)&__config_start <= 0x0800FFFF)
return FLASH_SECTOR_1;
if ((uint32_t)&__config_start <= 0x08017FFF)
return FLASH_SECTOR_2;
if ((uint32_t)&__config_start <= 0x0801FFFF)
return FLASH_SECTOR_3;
if ((uint32_t)&__config_start <= 0x0803FFFF)
return FLASH_SECTOR_4;
if ((uint32_t)&__config_start <= 0x0807FFFF)
return FLASH_SECTOR_5;
if ((uint32_t)&__config_start <= 0x080BFFFF)
return FLASH_SECTOR_6;
if ((uint32_t)&__config_start <= 0x080FFFFF)
return FLASH_SECTOR_7;
// Not good
while (1) {
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
}
#elif defined(STM32F4)
/*
Sector 0 0x08000000 - 0x08003FFF 16 Kbytes
Sector 1 0x08004000 - 0x08007FFF 16 Kbytes
Sector 2 0x08008000 - 0x0800BFFF 16 Kbytes
Sector 3 0x0800C000 - 0x0800FFFF 16 Kbytes
Sector 4 0x08010000 - 0x0801FFFF 64 Kbytes
Sector 5 0x08020000 - 0x0803FFFF 128 Kbytes
Sector 6 0x08040000 - 0x0805FFFF 128 Kbytes
Sector 7 0x08060000 - 0x0807FFFF 128 Kbytes
Sector 8 0x08080000 - 0x0809FFFF 128 Kbytes
Sector 9 0x080A0000 - 0x080BFFFF 128 Kbytes
Sector 10 0x080C0000 - 0x080DFFFF 128 Kbytes
Sector 11 0x080E0000 - 0x080FFFFF 128 Kbytes
*/
static uint32_t getFLASHSectorForEEPROM(void)
{
if ((uint32_t)&__config_start <= 0x08003FFF)
return FLASH_Sector_0;
if ((uint32_t)&__config_start <= 0x08007FFF)
return FLASH_Sector_1;
if ((uint32_t)&__config_start <= 0x0800BFFF)
return FLASH_Sector_2;
if ((uint32_t)&__config_start <= 0x0800FFFF)
return FLASH_Sector_3;
if ((uint32_t)&__config_start <= 0x0801FFFF)
return FLASH_Sector_4;
if ((uint32_t)&__config_start <= 0x0803FFFF)
return FLASH_Sector_5;
if ((uint32_t)&__config_start <= 0x0805FFFF)
return FLASH_Sector_6;
if ((uint32_t)&__config_start <= 0x0807FFFF)
return FLASH_Sector_7;
if ((uint32_t)&__config_start <= 0x0809FFFF)
return FLASH_Sector_8;
if ((uint32_t)&__config_start <= 0x080DFFFF)
return FLASH_Sector_9;
if ((uint32_t)&__config_start <= 0x080BFFFF)
return FLASH_Sector_10;
if ((uint32_t)&__config_start <= 0x080FFFFF)
return FLASH_Sector_11;
// Not good
while (1) {
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
}
#endif
static int write_word(config_streamer_t *c, uint32_t value)
{
if (c->err != 0) {
return c->err;
}
#if defined(STM32F7)
if (c->address % FLASH_PAGE_SIZE == 0) {
FLASH_EraseInitTypeDef EraseInitStruct = {
.TypeErase = FLASH_TYPEERASE_SECTORS,
.VoltageRange = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V
.NbSectors = 1
};
EraseInitStruct.Sector = getFLASHSectorForEEPROM();
uint32_t SECTORError;
const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
if (status != HAL_OK){
return -1;
}
}
const HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value);
if (status != HAL_OK) {
return -2;
}
#else
if (c->address % FLASH_PAGE_SIZE == 0) {
#if defined(STM32F4)
const FLASH_Status status = FLASH_EraseSector(getFLASHSectorForEEPROM(), VoltageRange_3); //0x08080000 to 0x080A0000
#else
const FLASH_Status status = FLASH_ErasePage(c->address);
#endif
if (status != FLASH_COMPLETE) {
return -1;
}
}
const FLASH_Status status = FLASH_ProgramWord(c->address, value);
if (status != FLASH_COMPLETE) {
return -2;
}
#endif
c->address += sizeof(value);
return 0;
}
int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size)
{
for (const uint8_t *pat = p; pat != (uint8_t*)p + size; pat++) {
c->buffer.b[c->at++] = *pat;
if (c->at == sizeof(c->buffer)) {
c->err = write_word(c, c->buffer.w);
c->at = 0;
}
}
return c->err;
}
int config_streamer_status(config_streamer_t *c)
{
return c->err;
}
int config_streamer_flush(config_streamer_t *c)
{
if (c->at != 0) {
memset(c->buffer.b + c->at, 0, sizeof(c->buffer) - c->at);
c->err = write_word(c, c->buffer.w);
c->at = 0;
}
return c-> err;
}
int config_streamer_finish(config_streamer_t *c)
{
if (c->unlocked) {
#if defined(STM32F7)
HAL_FLASH_Lock();
#else
FLASH_Lock();
#endif
c->unlocked = false;
}
return c->err;
}

45
src/main/config/config_streamer.h Normal file
View file

@ -0,0 +1,45 @@
/*
* 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/>.
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
// Streams data out to the EEPROM, padding to the write size as
// needed, and updating the checksum as it goes.
typedef struct config_streamer_s {
uintptr_t address;
int size;
union {
uint8_t b[4];
uint32_t w;
} buffer;
int at;
int err;
bool unlocked;
} config_streamer_t;
void config_streamer_init(config_streamer_t *c);
void config_streamer_start(config_streamer_t *c, uintptr_t base, int size);
int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size);
int config_streamer_flush(config_streamer_t *c);
int config_streamer_finish(config_streamer_t *c);
int config_streamer_status(config_streamer_t *c);

3
src/main/config/feature.c
View file

@ -21,8 +21,9 @@
#include "platform.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
static uint32_t activeFeaturesLatch = 0;

10
src/main/config/feature.h
View file

@ -17,6 +17,14 @@
#pragma once
#include "config/parameter_group.h"
typedef struct featureConfig_s {
uint32_t enabledFeatures;
} featureConfig_t;
PG_DECLARE(featureConfig_t, featureConfig);
void latchActiveFeatures(void);
bool featureConfigured(uint32_t mask);
bool feature(uint32_t mask);
@ -27,4 +35,4 @@ uint32_t featureMask(void);
void intFeatureClearAll(uint32_t *features);
void intFeatureSet(uint32_t mask, uint32_t *features);
void intFeatureClear(uint32_t mask, uint32_t *features);
void intFeatureClear(uint32_t mask, uint32_t *features);

3
src/main/config/parameter_group.c
View file

@ -19,6 +19,8 @@
#include <string.h>
#include <stdint.h>
#include "platform.h"
#include "parameter_group.h"
#include "common/maths.h"
@ -122,4 +124,3 @@ void pgActivateProfile(int profileIndex)
}
}
}

69
src/main/config/parameter_group.h
View file

@ -20,6 +20,8 @@
#include <stdint.h>
#include <stdbool.h>
#include "build/build_config.h"
typedef uint16_t pgn_t;
// parameter group registry flags
@ -54,6 +56,7 @@ static inline uint16_t pgN(const pgRegistry_t* reg) {return reg->pgn & PGR_PGN_M
static inline uint8_t pgVersion(const pgRegistry_t* reg) {return reg->pgn >> 12;}
static inline uint16_t pgSize(const pgRegistry_t* reg) {return reg->size & PGR_SIZE_MASK;}
static inline uint16_t pgIsSystem(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == 0;}
static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == PGR_SIZE_PROFILE_FLAG;}
#define PG_PACKED __attribute__((packed))
@ -97,28 +100,47 @@ extern const uint8_t __pg_resetdata_end[];
} while(0) \
/**/
#ifdef USE_PARAMETER_GROUPS
// Declare system config
#define PG_DECLARE(_type, _name) \
extern _type _name ## _System; \
static inline _type* _name(void) { return &_name ## _System; } \
static inline const _type* _name(void) { return &_name ## _System; }\
static inline _type* _name ## Mutable(void) { return &_name ## _System; }\
struct _dummy \
/**/
// Declare system config array
#define PG_DECLARE_ARR(_type, _size, _name) \
#define PG_DECLARE_ARRAY(_type, _size, _name) \
extern _type _name ## _SystemArray[_size]; \
static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \
static inline const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
static inline _type* _name ## Mutable(int _index) { return &_name ## _SystemArray[_index]; } \
static inline _type (* _name ## _array(void))[_size] { return &_name ## _SystemArray; } \
struct _dummy \
/**/
// Declare profile config
#define PG_DECLARE_PROFILE(_type, _name) \
extern _type *_name ## _ProfileCurrent; \
static inline _type* _name(void) { return _name ## _ProfileCurrent; } \
static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \
static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \
struct _dummy \
/**/
#else
#define PG_DECLARE(_type, _name) \
extern _type _name ## _System
#define PG_DECLARE_ARRAY(_type, _size, _name) \
extern _type _name ## _SystemArray[_size]
// Declare profile config
#define PG_DECLARE_PROFILE(_type, _name) \
extern _type *_name ## _ProfileCurrent
#endif // USE_PARAMETER_GROUPS
#ifdef USE_PARAMETER_GROUPS
// Register system config
#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \
_type _name ## _System; \
@ -148,7 +170,7 @@ extern const uint8_t __pg_resetdata_end[];
/**/
// Register system config array
#define PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, _reset) \
#define PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, _reset) \
_type _name ## _SystemArray[_size]; \
extern const pgRegistry_t _name ##_Registry; \
const pgRegistry_t _name ## _Registry PG_REGISTER_ATTRIBUTES = { \
@ -160,20 +182,20 @@ extern const uint8_t __pg_resetdata_end[];
} \
/**/
#define PG_REGISTER_ARR(_type, _size, _name, _pgn, _version) \
PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
#define PG_REGISTER_ARRAY(_type, _size, _name, _pgn, _version) \
PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
/**/
#define PG_REGISTER_ARR_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \
#define PG_REGISTER_ARRAY_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \
extern void pgResetFn_ ## _name(_type *); \
PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
/**/
#if 0
// ARRAY reset mechanism is not implemented yet, only few places in code would benefit from it - See pgResetInstance
#define PG_REGISTER_ARR_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \
#define PG_REGISTER_ARRAY_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \
extern const _type pgResetTemplate_ ## _name; \
PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
/**/
#endif
@ -221,6 +243,29 @@ extern const uint8_t __pg_resetdata_end[];
__VA_ARGS__ \
} \
/**/
#else
#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \
_type _name ## _System
#define PG_REGISTER(_type, _name, _pgn, _version) \
PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \
/**/
#define PG_REGISTER_WITH_RESET_FN(_type, _name, _pgn, _version) \
extern void pgResetFn_ ## _name(_type *); \
PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name }) \
/**/
#define PG_REGISTER_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \
extern const _type pgResetTemplate_ ## _name; \
PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
/**/
#define PG_RESET_TEMPLATE(_type, _name, ...) \
const _type pgResetTemplate_ ## _name PG_RESETDATA_ATTRIBUTES = { \
__VA_ARGS__ \
} \
/**/
#endif
const pgRegistry_t* pgFind(pgn_t pgn);

6
src/main/config/parameter_group_ids.h
View file

@ -15,8 +15,12 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef USE_PARAMETER_GROUPS
#include "config/config_master.h"
#endif
// FC configuration
#define PG_FAILSAFE_CONFIG 1 // strruct OK
#define PG_FAILSAFE_CONFIG 1 // struct OK
#define PG_BOARD_ALIGNMENT 2 // struct OK
#define PG_GIMBAL_CONFIG 3 // struct OK
#define PG_MOTOR_MIXER 4 // two structs mixerConfig_t servoMixerConfig_t

236
src/main/drivers/accgyro_mpu.c
View file

@ -48,16 +48,7 @@
#include "accgyro_mpu.h"
mpuResetFuncPtr mpuReset;
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
static void mpu6050FindRevision(gyroDev_t *gyro);
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
#endif
mpuResetFnPtr mpuResetFn;
#ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE
@ -71,110 +62,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
#define MPU_INQUIRY_MASK 0x7E
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
{
bool ack;
uint8_t sig;
uint8_t inquiryResult;
// MPU datasheet specifies 30ms.
delay(35);
#ifndef USE_I2C
ack = false;
sig = 0;
#else
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif
if (ack) {
gyro->mpuConfiguration.read = mpuReadRegisterI2C;
gyro->mpuConfiguration.write = mpuWriteRegisterI2C;
} else {
#ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor);
#endif
return &gyro->mpuDetectionResult;
}
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
// If an MPU3050 is connected sig will contain 0.
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &gyro->mpuDetectionResult;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return &gyro->mpuDetectionResult;
}
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
#ifdef USE_GYRO_SPI_MPU6500
uint8_t mpu6500Sensor = mpu6500SpiDetect();
if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu6500ReadRegister;
gyro->mpuConfiguration.write = mpu6500WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiDetect()) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = icm20689ReadRegister;
gyro->mpuConfiguration.write = icm20689WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu6000ReadRegister;
gyro->mpuConfiguration.write = mpu6000WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu9250ReadRegister;
gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
gyro->mpuConfiguration.write = mpu9250WriteRegister;
gyro->mpuConfiguration.reset = mpu9250ResetGyro;
return true;
}
#endif
UNUSED(gyro);
return false;
}
#endif
static void mpu6050FindRevision(gyroDev_t *gyro)
{
bool ack;
@ -188,7 +75,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
ack = gyro->mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer);
ack = gyro->mpuConfiguration.readFn(MPU_RA_XA_OFFS_H, 6, readBuffer);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
@ -202,7 +89,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} else {
ack = gyro->mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId);
ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F;
if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE);
@ -289,7 +176,7 @@ bool mpuAccRead(accDev_t *acc)
{
uint8_t data[6];
bool ack = acc->mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
bool ack = acc->mpuConfiguration.readFn(MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) {
return false;
}
@ -312,7 +199,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
const bool ack = gyro->mpuConfiguration.read(gyro->mpuConfiguration.gyroReadXRegister, 6, data);
const bool ack = gyro->mpuConfiguration.readFn(gyro->mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) {
return false;
}
@ -324,11 +211,6 @@ bool mpuGyroRead(gyroDev_t *gyro)
return true;
}
void mpuGyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit(gyro);
}
bool mpuCheckDataReady(gyroDev_t* gyro)
{
bool ret;
@ -340,3 +222,111 @@ bool mpuCheckDataReady(gyroDev_t* gyro)
}
return ret;
}
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6000ReadRegister;
gyro->mpuConfiguration.writeFn = mpu6000WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6500
uint8_t mpu6500Sensor = mpu6500SpiDetect();
if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6500ReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu9250ReadRegister;
gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister;
gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister;
gyro->mpuConfiguration.writeFn = mpu9250WriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250ResetGyro;
return true;
}
#endif
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiDetect()) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = icm20689ReadRegister;
gyro->mpuConfiguration.writeFn = icm20689WriteRegister;
return true;
}
#endif
UNUSED(gyro);
return false;
}
#endif
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
{
// MPU datasheet specifies 30ms.
delay(35);
#ifndef USE_I2C
uint8_t sig = 0;
bool ack = false;
#else
uint8_t sig;
bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif
if (ack) {
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
} else {
#ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor);
#endif
return &gyro->mpuDetectionResult;
}
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
// If an MPU3050 is connected sig will contain 0.
uint8_t inquiryResult;
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &gyro->mpuDetectionResult;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return &gyro->mpuDetectionResult;
}
void mpuGyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit(gyro);
}

22
src/main/drivers/accgyro_mpu.h
View file

@ -124,18 +124,18 @@
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFuncPtr)(void);
typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFnPtr)(void);
extern mpuResetFuncPtr mpuReset;
extern mpuResetFnPtr mpuResetFn;
typedef struct mpuConfiguration_s {
mpuReadRegisterFunc read;
mpuWriteRegisterFunc write;
mpuReadRegisterFunc slowread;
mpuWriteRegisterFunc verifywrite;
mpuResetFuncPtr reset;
mpuReadRegisterFnPtr readFn;
mpuWriteRegisterFnPtr writeFn;
mpuReadRegisterFnPtr slowreadFn;
mpuWriteRegisterFnPtr verifywriteFn;
mpuResetFnPtr resetFn;
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t;
@ -178,7 +178,7 @@ typedef enum {
ICM_20689_SPI,
ICM_20608_SPI,
ICM_20602_SPI
} detectedMPUSensor_e;
} mpuSensor_e;
typedef enum {
MPU_HALF_RESOLUTION,
@ -186,7 +186,7 @@ typedef enum {
} mpu6050Resolution_e;
typedef struct mpuDetectionResult_s {
detectedMPUSensor_e sensor;
mpuSensor_e sensor;
mpu6050Resolution_e resolution;
} mpuDetectionResult_t;

12
src/main/drivers/accgyro_mpu3050.c
View file

@ -53,21 +53,21 @@ static void mpu3050Init(gyroDev_t *gyro)
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = gyro->mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(FAILURE_ACC_INIT);
gyro->mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.write(MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{
UNUSED(gyro);
uint8_t buf[2];
if (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) {
return false;
}

16
src/main/drivers/accgyro_mpu6050.c
View file

@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff.
// Accel scale 8g (4096 LSB/g)
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG,
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
}

22
src/main/drivers/accgyro_mpu6500.c
View file

@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
#ifdef USE_MPU9250_MAG
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
#else
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
#endif
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
#endif
delay(15);
}

22
src/main/drivers/accgyro_spi_icm20689.c
View file

@ -130,32 +130,32 @@ void icm20689GyroInit(gyroDev_t *gyro)
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03);
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
// gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
// gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
// delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
// gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
// gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);

2
src/main/drivers/bus_spi_hal.c
View file

@ -72,7 +72,7 @@
static spiDevice_t spiHardwareMap[] = {
{ .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER },
{ .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER },
{ .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF5_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
{ .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
{ .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER }
};

2
src/main/drivers/light_led.c
View file

@ -25,7 +25,7 @@
static IO_t leds[LED_NUMBER];
static uint8_t ledPolarity = 0;
void ledInit(statusLedConfig_t *statusLedConfig)
void ledInit(const statusLedConfig_t *statusLedConfig)
{
LED0_OFF;
LED1_OFF;

5
src/main/drivers/light_led.h
View file

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/io_types.h"
#define LED_NUMBER 3
@ -26,6 +27,8 @@ typedef struct statusLedConfig_s {
uint8_t polarity;
} statusLedConfig_t;
PG_DECLARE(statusLedConfig_t, statusLedConfig);
// Helpful macros
#ifdef LED0
# define LED0_TOGGLE ledToggle(0)
@ -57,6 +60,6 @@ typedef struct statusLedConfig_s {
# define LED2_ON do {} while(0)
#endif
void ledInit(statusLedConfig_t *statusLedConfig);
void ledInit(const statusLedConfig_t *statusLedConfig);
void ledToggle(int led);
void ledSet(int led, bool state);

14
src/main/drivers/pwm_output.c
View file

@ -83,7 +83,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
#endif
}
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion)
{
#if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
@ -91,7 +91,8 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
#endif
configTimeBase(timerHardware->tim, period, mhz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
#if defined(USE_HAL_DRIVER)
HAL_TIM_PWM_Start(Handle, timerHardware->channel);
@ -259,7 +260,8 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
#ifdef USE_DSHOT
if (isDigital) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
motors[motorIndex].enabled = true;
continue;
}
@ -274,9 +276,9 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse);
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion);
} else {
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0);
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion);
}
bool timerAlreadyUsed = false;
@ -348,7 +350,7 @@ void servoInit(const servoConfig_t *servoConfig)
break;
}
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
servos[servoIndex].enabled = true;
}
}

2
src/main/drivers/pwm_output.h
View file

@ -119,7 +119,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
#ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDigital(uint8_t index, uint16_t value);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
#endif

8
src/main/drivers/pwm_output_stm32f3xx.c
View file

@ -105,7 +105,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
}
}
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
DMA_InitTypeDef DMA_InitStructure;
@ -139,14 +139,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
} else {
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
}
TIM_OCInitStructure.TIM_Pulse = 0;

8
src/main/drivers/pwm_output_stm32f4xx.c
View file

@ -102,7 +102,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
}
}
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
DMA_InitTypeDef DMA_InitStructure;
@ -136,14 +136,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
} else {
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
}
TIM_OCInitStructure.TIM_Pulse = 0;

12
src/main/drivers/pwm_output_stm32f7xx.c
View file

@ -96,7 +96,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
UNUSED(motorCount);
}
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
motor->timerHardware = timerHardware;
@ -174,9 +174,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
/* PWM1 Mode configuration: Channel1 */
TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
} else {
TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
}
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
TIM_OCInitStructure.Pulse = 0;

3
src/main/drivers/system.h
View file

@ -17,6 +17,9 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
void systemInit(void);
void delayMicroseconds(uint32_t us);
void delay(uint32_t ms);

8
src/main/drivers/system_stm32f4xx.c
View file

@ -31,8 +31,8 @@ void SetSysClock(void);
void systemReset(void)
{
if (mpuReset) {
mpuReset();
if (mpuResetFn) {
mpuResetFn();
}
__disable_irq();
@ -41,8 +41,8 @@ void systemReset(void)
void systemResetToBootloader(void)
{
if (mpuReset) {
mpuReset();
if (mpuResetFn) {
mpuResetFn();
}
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX

8
src/main/drivers/system_stm32f7xx.c
View file

@ -33,8 +33,8 @@ void SystemClock_Config(void);
void systemReset(void)
{
if (mpuReset) {
mpuReset();
if (mpuResetFn) {
mpuResetFn();
}
__disable_irq();
@ -43,8 +43,8 @@ void systemReset(void)
void systemResetToBootloader(void)
{
if (mpuReset) {
mpuReset();
if (mpuResetFn) {
mpuResetFn();
}
(*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot

630
src/main/fc/cli.c
View file

File diff suppressed because it is too large Load diff

169
src/main/fc/config.c
View file

@ -29,12 +29,18 @@
#include "cms/cms.h"
#include "common/color.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/color.h"
#include "common/filter.h"
#include "common/maths.h"
#include "config/config_eeprom.h"
#include "config/config_master.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/io.h"
@ -45,6 +51,7 @@
#include "drivers/rx_pwm.h"
#include "drivers/rx_spi.h"
#include "drivers/sdcard.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
#include "drivers/sound_beeper.h"
#include "drivers/system.h"
@ -56,42 +63,37 @@
#include "fc/fc_rc.h"
#include "fc/runtime_config.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/ledstrip.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/motors.h"
#include "io/osd.h"
#include "io/serial.h"
#include "io/servos.h"
#include "io/vtx.h"
#include "rx/rx.h"
#include "rx/rx_spi.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "flight/navigation.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifndef DEFAULT_RX_FEATURE
#define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
#endif
@ -544,7 +546,7 @@ uint8_t getCurrentProfile(void)
return masterConfig.current_profile_index;
}
void setProfile(uint8_t profileIndex)
static void setProfile(uint8_t profileIndex)
{
currentProfile = &masterConfig.profile[profileIndex];
currentControlRateProfileIndex = currentProfile->activeRateProfile;
@ -866,11 +868,14 @@ void createDefaultConfig(master_t *config)
}
}
static void resetConf(void)
void resetConfigs(void)
{
createDefaultConfig(&masterConfig);
pgResetAll(MAX_PROFILE_COUNT);
pgActivateProfile(0);
setProfile(0);
setControlRateProfile(0);
#ifdef LED_STRIP
reevaluateLedConfig();
@ -883,32 +888,19 @@ void activateConfig(void)
resetAdjustmentStates();
useRcControlsConfig(
modeActivationProfile()->modeActivationConditions,
&masterConfig.motorConfig,
&currentProfile->pidProfile
);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
#endif
useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &currentProfile->pidProfile);
useAdjustmentConfig(&currentProfile->pidProfile);
#ifdef GPS
gpsUseProfile(&masterConfig.gpsProfile);
gpsUsePIDs(&currentProfile->pidProfile);
#endif
useFailsafeConfig(&masterConfig.failsafeConfig);
setAccelerationTrims(&accelerometerConfig()->accZero);
failsafeReset();
setAccelerationTrims(&accelerometerConfigMutable()->accZero);
setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
mixerUseConfigs(
&masterConfig.flight3DConfig,
&masterConfig.motorConfig,
&masterConfig.mixerConfig,
&masterConfig.airplaneConfig,
&masterConfig.rxConfig
);
mixerUseConfigs(&masterConfig.airplaneConfig);
#ifdef USE_SERVOS
servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoProfile.servoConf, &masterConfig.gimbalConfig);
@ -921,22 +913,13 @@ void activateConfig(void)
throttleCorrectionConfig()->throttle_correction_angle
);
configureAltitudeHold(
&currentProfile->pidProfile,
&masterConfig.barometerConfig,
&masterConfig.rcControlsConfig,
&masterConfig.motorConfig
);
#ifdef BARO
useBarometerConfig(&masterConfig.barometerConfig);
#endif
configureAltitudeHold(&currentProfile->pidProfile);
}
void validateAndFixConfig(void)
{
if ((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
motorConfig()->mincommand = 1000;
if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){
motorConfigMutable()->mincommand = 1000;
}
if ((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
@ -1053,18 +1036,18 @@ void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
gyroConfig()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
}
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfig()->gyro_soft_notch_hz_2 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
}
float samplingTime = 0.000125f;
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfig()->gyro_sync_denom = 1;
gyroConfig()->gyro_use_32khz = false;
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfigMutable()->gyro_sync_denom = 1;
gyroConfigMutable()->gyro_use_32khz = false;
samplingTime = 0.001f;
}
@ -1072,18 +1055,18 @@ void validateAndFixGyroConfig(void)
samplingTime = 0.00003125;
// F1 and F3 can't handle high sample speed.
#if defined(STM32F1)
gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
#elif defined(STM32F3)
gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
#endif
} else {
#if defined(STM32F1)
gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
#endif
}
#if !defined(GYRO_USES_SPI) || !defined(USE_MPU_DATA_READY_SIGNAL)
gyroConfig()->gyro_isr_update = false;
gyroConfigMutable()->gyro_isr_update = false;
#endif
// check for looptime restrictions based on motor protocol. Motor times have safety margin
@ -1113,7 +1096,7 @@ void validateAndFixGyroConfig(void)
if (pidLooptime < motorUpdateRestriction) {
const uint8_t maxPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
pidConfig()->pid_process_denom = MIN(pidConfig()->pid_process_denom, maxPidProcessDenom);
pidConfigMutable()->pid_process_denom = MIN(pidConfigMutable()->pid_process_denom, maxPidProcessDenom);
}
// Prevent overriding the max rate of motors
@ -1121,25 +1104,57 @@ void validateAndFixGyroConfig(void)
uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
if(motorConfig()->motorPwmRate > maxEscRate)
motorConfig()->motorPwmRate = maxEscRate;
motorConfigMutable()->motorPwmRate = maxEscRate;
}
}
void readEEPROM(void)
{
suspendRxSignal();
// Sanity check, read flash
if (!loadEEPROM()) {
failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
}
// pgActivateProfile(getCurrentProfile());
// setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex);
if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check
masterConfig.current_profile_index = 0;
}
setProfile(masterConfig.current_profile_index);
validateAndFixConfig();
activateConfig();
resumeRxSignal();
}
void writeEEPROM(void)
{
suspendRxSignal();
writeConfigToEEPROM();
resumeRxSignal();
}
void resetEEPROM(void)
{
resetConfigs();
writeEEPROM();
}
void ensureEEPROMContainsValidData(void)
{
if (isEEPROMContentValid()) {
return;
}
resetEEPROM();
}
void resetEEPROM(void)
{
resetConf();
writeEEPROM();
}
void saveConfigAndNotify(void)
{
writeEEPROM();

22
src/main/fc/config.h
View file

@ -17,8 +17,11 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "config/parameter_group.h"
#if FLASH_SIZE <= 128
#define MAX_PROFILE_COUNT 2
#else
@ -58,6 +61,16 @@ typedef enum {
FEATURE_ESC_SENSOR = 1 << 27,
} features_e;
typedef struct systemConfig_s {
uint8_t debug_mode;
} systemConfig_t;
//!!TODOPG_DECLARE(systemConfig_t, systemConfig);
struct profile_s;
extern struct profile_s *currentProfile;
struct controlRateConfig_s;
extern struct controlRateConfig_s *currentControlRateProfile;
void beeperOffSet(uint32_t mask);
void beeperOffSetAll(uint8_t beeperCount);
void beeperOffClear(uint32_t mask);
@ -69,7 +82,10 @@ void setPreferredBeeperOffMask(uint32_t mask);
void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex);
void initEEPROM(void);
void resetEEPROM(void);
void readEEPROM(void);
void writeEEPROM();
void ensureEEPROMContainsValidData(void);
void saveConfigAndNotify(void);
@ -79,14 +95,16 @@ void activateConfig(void);
uint8_t getCurrentProfile(void);
void changeProfile(uint8_t profileIndex);
void setProfile(uint8_t profileIndex);
struct profile_s;
void resetProfile(struct profile_s *profile);
uint8_t getCurrentControlRateProfile(void);
void changeControlRateProfile(uint8_t profileIndex);
bool canSoftwareSerialBeUsed(void);
uint16_t getCurrentMinthrottle(void);
struct master_s;
void resetConfigs(void);
struct master_s;
void targetConfiguration(struct master_s *config);
void targetValidateConfiguration(struct master_s *config);

43
src/main/fc/fc_core.c
View file

@ -24,20 +24,26 @@
#include "blackbox/blackbox.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/utils.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/light_led.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
@ -59,15 +65,15 @@
#include "scheduler/scheduler.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "flight/imu.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
// June 2013 V2.2-dev
@ -95,8 +101,8 @@ static bool armingCalibrationWasInitialised;
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveConfigAndNotify();
}
@ -183,15 +189,6 @@ void mwArm(void)
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
startBlackbox();
}
#endif
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
@ -291,7 +288,7 @@ void processRx(timeUs_t currentTimeUs)
failsafeUpdateState();
}
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
const throttleStatus_e throttleStatus = calculateThrottleStatus();
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
@ -357,7 +354,7 @@ void processRx(timeUs_t currentTimeUs)
}
}
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch);
processRcStickPositions(throttleStatus);
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
@ -367,7 +364,7 @@ void processRx(timeUs_t currentTimeUs)
if (!cliMode) {
updateAdjustmentStates(adjustmentProfile()->adjustmentRanges);
processRcAdjustments(currentControlRateProfile, rxConfig());
processRcAdjustments(currentControlRateProfile);
}
bool canUseHorizonMode = true;
@ -488,7 +485,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold(&masterConfig.airplaneConfig);
applyAltHold();
}
}
#endif

29
src/main/fc/fc_init.c
View file

@ -28,6 +28,12 @@
#include "common/maths.h"
#include "common/printf.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
@ -115,10 +121,6 @@
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
@ -257,16 +259,16 @@ void init(void)
timerInit(); // timer must be initialized before any channel is allocated
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
serialInit(feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
serialInit(feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
serialInit(feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer);
@ -401,12 +403,7 @@ void init(void)
}
#endif
#ifdef SONAR
const sonarConfig_t *sonarConfig = sonarConfig();
#else
const void *sonarConfig = NULL;
#endif
if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) {
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
@ -446,7 +443,7 @@ void init(void)
cliInit(serialConfig());
#endif
failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
failsafeInit();
rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
@ -554,7 +551,7 @@ void init(void)
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(batteryConfig());
batteryInit();
#ifdef USE_DASHBOARD
if (feature(FEATURE_DASHBOARD)) {

347
src/main/fc/fc_msp.c
View file

@ -34,20 +34,27 @@
#include "common/maths.h"
#include "common/streambuf.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/serial.h"
#include "drivers/bus_i2c.h"
#include "drivers/io.h"
#include "drivers/compass.h"
#include "drivers/flash.h"
#include "drivers/sdcard.h"
#include "drivers/vcd.h"
#include "drivers/io.h"
#include "drivers/max7456.h"
#include "drivers/vtx_soft_spi_rtc6705.h"
#include "drivers/pwm_output.h"
#include "drivers/sdcard.h"
#include "drivers/serial.h"
#include "drivers/serial_escserial.h"
#include "drivers/system.h"
#include "drivers/vcd.h"
#include "drivers/vtx_common.h"
#include "drivers/vtx_soft_spi_rtc6705.h"
#include "fc/config.h"
#include "fc/fc_core.h"
@ -56,17 +63,25 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/flashfs.h"
#include "io/gps.h"
#include "io/gimbal.h"
#include "io/serial.h"
#include "io/ledstrip.h"
#include "io/flashfs.h"
#include "io/transponder_ir.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/motors.h"
#include "io/serial.h"
#include "io/serial_4way.h"
#include "io/servos.h"
#include "io/transponder_ir.h"
#include "msp/msp.h"
#include "msp/msp_protocol.h"
@ -77,36 +92,22 @@
#include "scheduler/scheduler.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "flight/altitudehold.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif
extern uint16_t cycleTime; // FIXME dependency on mw.c
extern void resetProfile(profile_t *profile);
static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
@ -150,6 +151,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE;", 31 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -378,6 +380,7 @@ void initActiveBoxIds(void)
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE;
}
#endif
@ -442,6 +445,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) << BOXBLACKBOXERASE |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX;
@ -675,25 +679,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_SERVO_CONFIGURATIONS:
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
sbufWriteU16(dst, servoProfile()->servoConf[i].min);
sbufWriteU16(dst, servoProfile()->servoConf[i].max);
sbufWriteU16(dst, servoProfile()->servoConf[i].middle);
sbufWriteU8(dst, servoProfile()->servoConf[i].rate);
sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin);
sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax);
sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel);
sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources);
sbufWriteU16(dst, servoParams(i)->min);
sbufWriteU16(dst, servoParams(i)->max);
sbufWriteU16(dst, servoParams(i)->middle);
sbufWriteU8(dst, servoParams(i)->rate);
sbufWriteU8(dst, servoParams(i)->angleAtMin);
sbufWriteU8(dst, servoParams(i)->angleAtMax);
sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
sbufWriteU32(dst, servoParams(i)->reversedSources);
}
break;
case MSP_SERVO_MIX_RULES:
for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, customServoMixer(i)->targetChannel);
sbufWriteU8(dst, customServoMixer(i)->inputSource);
sbufWriteU8(dst, customServoMixer(i)->rate);
sbufWriteU8(dst, customServoMixer(i)->speed);
sbufWriteU8(dst, customServoMixer(i)->min);
sbufWriteU8(dst, customServoMixer(i)->max);
sbufWriteU8(dst, customServoMixer(i)->box);
sbufWriteU8(dst, customServoMixers(i)->targetChannel);
sbufWriteU8(dst, customServoMixers(i)->inputSource);
sbufWriteU8(dst, customServoMixers(i)->rate);
sbufWriteU8(dst, customServoMixers(i)->speed);
sbufWriteU8(dst, customServoMixers(i)->min);
sbufWriteU8(dst, customServoMixers(i)->max);
sbufWriteU8(dst, customServoMixers(i)->box);
}
break;
#endif
@ -800,7 +804,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_ADJUSTMENT_RANGES:
for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
const adjustmentRange_t *adjRange = adjustmentRanges(i);
sbufWriteU8(dst, adjRange->adjustmentIndex);
sbufWriteU8(dst, adjRange->auxChannelIndex);
sbufWriteU8(dst, adjRange->range.startStep);
@ -967,8 +971,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RXFAIL_CONFIG:
for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
sbufWriteU8(dst, rxConfig()->failsafe_channel_configurations[i].mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step));
sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
}
break;
@ -1014,7 +1018,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#ifdef LED_STRIP
case MSP_LED_COLORS:
for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &ledStripConfig()->colors[i];
const hsvColor_t *color = &ledStripConfig()->colors[i];
sbufWriteU16(dst, color->h);
sbufWriteU8(dst, color->s);
sbufWriteU8(dst, color->v);
@ -1023,7 +1027,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_LED_STRIP_CONFIG:
for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
sbufWriteU32(dst, *ledConfig);
}
break;
@ -1091,13 +1095,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, osdProfile()->units);
sbufWriteU8(dst, osdProfile()->rssi_alarm);
sbufWriteU16(dst, osdProfile()->cap_alarm);
sbufWriteU16(dst, osdProfile()->time_alarm);
sbufWriteU16(dst, osdProfile()->alt_alarm);
sbufWriteU8(dst, osdConfig()->units);
sbufWriteU8(dst, osdConfig()->rssi_alarm);
sbufWriteU16(dst, osdConfig()->cap_alarm);
sbufWriteU16(dst, osdConfig()->time_alarm);
sbufWriteU16(dst, osdConfig()->alt_alarm);
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
sbufWriteU16(dst, osdProfile()->item_pos[i]);
sbufWriteU16(dst, osdConfig()->item_pos[i]);
}
#else
sbufWriteU8(dst, 0); // OSD not supported
@ -1146,6 +1150,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
//!!TODO gyro_isr_update to be added pending decision
//sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
sbufWriteU8(dst, motorConfig()->motorPwmInversion);
break;
case MSP_FILTER_CONFIG :
@ -1316,12 +1321,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#endif
break;
case MSP_SET_ACC_TRIM:
accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src);
accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src);
accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src);
break;
case MSP_SET_ARMING_CONFIG:
armingConfig()->auto_disarm_delay = sbufReadU8(src);
armingConfig()->disarm_kill_switch = sbufReadU8(src);
armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
armingConfigMutable()->disarm_kill_switch = sbufReadU8(src);
break;
case MSP_SET_LOOP_TIME:
@ -1343,7 +1348,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_MODE_RANGE:
i = sbufReadU8(src);
if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
i = sbufReadU8(src);
const box_t *box = findBoxByPermenantId(i);
if (box) {
@ -1352,7 +1357,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
mac->range.startStep = sbufReadU8(src);
mac->range.endStep = sbufReadU8(src);
useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile);
useRcControlsConfig(modeActivationConditions(0), &currentProfile->pidProfile);
} else {
return MSP_RESULT_ERROR;
}
@ -1364,7 +1369,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_ADJUSTMENT_RANGE:
i = sbufReadU8(src);
if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
i = sbufReadU8(src);
if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
adjRange->adjustmentIndex = i;
@ -1407,32 +1412,32 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_MISC:
rxConfig()->midrc = sbufReadU16(src);
motorConfig()->minthrottle = sbufReadU16(src);
motorConfig()->maxthrottle = sbufReadU16(src);
motorConfig()->mincommand = sbufReadU16(src);
rxConfigMutable()->midrc = sbufReadU16(src);
motorConfigMutable()->minthrottle = sbufReadU16(src);
motorConfigMutable()->maxthrottle = sbufReadU16(src);
motorConfigMutable()->mincommand = sbufReadU16(src);
failsafeConfig()->failsafe_throttle = sbufReadU16(src);
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
#ifdef GPS
gpsConfig()->provider = sbufReadU8(src); // gps_type
gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
sbufReadU8(src); // gps_baudrate
gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
#else
sbufReadU8(src); // gps_type
sbufReadU8(src); // gps_baudrate
sbufReadU8(src); // gps_ubx_sbas
#endif
batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src);
rxConfig()->rssi_channel = sbufReadU8(src);
batteryConfigMutable()->multiwiiCurrentMeterOutput = sbufReadU8(src);
rxConfigMutable()->rssi_channel = sbufReadU8(src);
sbufReadU8(src);
compassConfig()->mag_declination = sbufReadU16(src) * 10;
compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
break;
case MSP_SET_MOTOR:
@ -1450,14 +1455,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= MAX_SUPPORTED_SERVOS) {
return MSP_RESULT_ERROR;
} else {
servoProfile()->servoConf[i].min = sbufReadU16(src);
servoProfile()->servoConf[i].max = sbufReadU16(src);
servoProfile()->servoConf[i].middle = sbufReadU16(src);
servoProfile()->servoConf[i].rate = sbufReadU8(src);
servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src);
servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src);
servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src);
servoProfile()->servoConf[i].reversedSources = sbufReadU32(src);
servoParamsMutable(i)->min = sbufReadU16(src);
servoParamsMutable(i)->max = sbufReadU16(src);
servoParamsMutable(i)->middle = sbufReadU16(src);
servoParamsMutable(i)->rate = sbufReadU8(src);
servoParamsMutable(i)->angleAtMin = sbufReadU8(src);
servoParamsMutable(i)->angleAtMax = sbufReadU8(src);
servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
servoParamsMutable(i)->reversedSources = sbufReadU32(src);
}
#endif
break;
@ -1468,76 +1473,80 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= MAX_SERVO_RULES) {
return MSP_RESULT_ERROR;
} else {
customServoMixer(i)->targetChannel = sbufReadU8(src);
customServoMixer(i)->inputSource = sbufReadU8(src);
customServoMixer(i)->rate = sbufReadU8(src);
customServoMixer(i)->speed = sbufReadU8(src);
customServoMixer(i)->min = sbufReadU8(src);
customServoMixer(i)->max = sbufReadU8(src);
customServoMixer(i)->box = sbufReadU8(src);
customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
customServoMixersMutable(i)->inputSource = sbufReadU8(src);
customServoMixersMutable(i)->rate = sbufReadU8(src);
customServoMixersMutable(i)->speed = sbufReadU8(src);
customServoMixersMutable(i)->min = sbufReadU8(src);
customServoMixersMutable(i)->max = sbufReadU8(src);
customServoMixersMutable(i)->box = sbufReadU8(src);
loadCustomServoMixer();
}
#endif
break;
case MSP_SET_3D:
flight3DConfig()->deadband3d_low = sbufReadU16(src);
flight3DConfig()->deadband3d_high = sbufReadU16(src);
flight3DConfig()->neutral3d = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
flight3DConfigMutable()->neutral3d = sbufReadU16(src);
break;
case MSP_SET_RC_DEADBAND:
rcControlsConfig()->deadband = sbufReadU8(src);
rcControlsConfig()->yaw_deadband = sbufReadU8(src);
rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
rcControlsConfigMutable()->deadband = sbufReadU8(src);
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
break;
case MSP_SET_RESET_CURR_PID:
resetProfile(currentProfile);
break;
case MSP_SET_SENSOR_ALIGNMENT:
gyroConfig()->gyro_align = sbufReadU8(src);
accelerometerConfig()->acc_align = sbufReadU8(src);
compassConfig()->mag_align = sbufReadU8(src);
gyroConfigMutable()->gyro_align = sbufReadU8(src);
accelerometerConfigMutable()->acc_align = sbufReadU8(src);
compassConfigMutable()->mag_align = sbufReadU8(src);
break;
case MSP_SET_ADVANCED_CONFIG:
gyroConfig()->gyro_sync_denom = sbufReadU8(src);
pidConfig()->pid_process_denom = sbufReadU8(src);
motorConfig()->useUnsyncedPwm = sbufReadU8(src);
gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
pidConfigMutable()->pid_process_denom = sbufReadU8(src);
motorConfigMutable()->useUnsyncedPwm = sbufReadU8(src);
#ifdef USE_DSHOT
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
#else
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
#endif
motorConfig()->motorPwmRate = sbufReadU16(src);
if (dataSize > 7) {
motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
motorConfigMutable()->motorPwmRate = sbufReadU16(src);
if (sbufBytesRemaining(src) >= 2) {
motorConfigMutable()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
}
if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_use_32khz = sbufReadU8(src);
gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
}
//!!TODO gyro_isr_update to be added pending decision
/*if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_isr_update = sbufReadU8(src);
gyroConfigMutable()->gyro_isr_update = sbufReadU8(src);
}*/
validateAndFixGyroConfig();
if (sbufBytesRemaining(src)) {
motorConfigMutable()->motorPwmInversion = sbufReadU8(src);
}
break;
case MSP_SET_FILTER_CONFIG:
gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src);
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src);
currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src);
if (dataSize > 5) {
gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src);
}
if (dataSize > 13) {
gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
validateAndFixGyroConfig();
@ -1567,9 +1576,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_SENSOR_CONFIG:
accelerometerConfig()->acc_hardware = sbufReadU8(src);
barometerConfig()->baro_hardware = sbufReadU8(src);
compassConfig()->mag_hardware = sbufReadU8(src);
accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
barometerConfigMutable()->baro_hardware = sbufReadU8(src);
compassConfigMutable()->mag_hardware = sbufReadU8(src);
break;
case MSP_RESET_CONF:
@ -1601,9 +1610,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_BLACKBOX_CONFIG:
// Don't allow config to be updated while Blackbox is logging
if (blackboxMayEditConfig()) {
blackboxConfig()->device = sbufReadU8(src);
blackboxConfig()->rate_num = sbufReadU8(src);
blackboxConfig()->rate_denom = sbufReadU8(src);
blackboxConfigMutable()->device = sbufReadU8(src);
blackboxConfigMutable()->rate_num = sbufReadU8(src);
blackboxConfigMutable()->rate_denom = sbufReadU8(src);
}
break;
#endif
@ -1759,85 +1768,85 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_BOARD_ALIGNMENT:
boardAlignment()->rollDegrees = sbufReadU16(src);
boardAlignment()->pitchDegrees = sbufReadU16(src);
boardAlignment()->yawDegrees = sbufReadU16(src);
boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
break;
case MSP_SET_VOLTAGE_METER_CONFIG:
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
if (dataSize > 4) {
batteryConfig()->batteryMeterType = sbufReadU8(src);
batteryConfigMutable()->batteryMeterType = sbufReadU8(src);
}
break;
case MSP_SET_CURRENT_METER_CONFIG:
batteryConfig()->currentMeterScale = sbufReadU16(src);
batteryConfig()->currentMeterOffset = sbufReadU16(src);
batteryConfig()->currentMeterType = sbufReadU8(src);
batteryConfig()->batteryCapacity = sbufReadU16(src);
batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
batteryConfigMutable()->currentMeterType = sbufReadU8(src);
batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
break;
#ifndef USE_QUAD_MIXER_ONLY
case MSP_SET_MIXER:
mixerConfig()->mixerMode = sbufReadU8(src);
mixerConfigMutable()->mixerMode = sbufReadU8(src);
break;
#endif
case MSP_SET_RX_CONFIG:
rxConfig()->serialrx_provider = sbufReadU8(src);
rxConfig()->maxcheck = sbufReadU16(src);
rxConfig()->midrc = sbufReadU16(src);
rxConfig()->mincheck = sbufReadU16(src);
rxConfig()->spektrum_sat_bind = sbufReadU8(src);
rxConfigMutable()->serialrx_provider = sbufReadU8(src);
rxConfigMutable()->maxcheck = sbufReadU16(src);
rxConfigMutable()->midrc = sbufReadU16(src);
rxConfigMutable()->mincheck = sbufReadU16(src);
rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
if (dataSize > 8) {
rxConfig()->rx_min_usec = sbufReadU16(src);
rxConfig()->rx_max_usec = sbufReadU16(src);
rxConfigMutable()->rx_min_usec = sbufReadU16(src);
rxConfigMutable()->rx_max_usec = sbufReadU16(src);
}
if (dataSize > 12) {
rxConfig()->rcInterpolation = sbufReadU8(src);
rxConfig()->rcInterpolationInterval = sbufReadU8(src);
rxConfig()->airModeActivateThreshold = sbufReadU16(src);
rxConfigMutable()->rcInterpolation = sbufReadU8(src);
rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
rxConfigMutable()->airModeActivateThreshold = sbufReadU16(src);
}
if (dataSize > 16) {
rxConfig()->rx_spi_protocol = sbufReadU8(src);
rxConfig()->rx_spi_id = sbufReadU32(src);
rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src);
rxConfigMutable()->rx_spi_protocol = sbufReadU8(src);
rxConfigMutable()->rx_spi_id = sbufReadU32(src);
rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
}
if (dataSize > 22) {
rxConfig()->fpvCamAngleDegrees = sbufReadU8(src);
rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src);
}
break;
case MSP_SET_FAILSAFE_CONFIG:
failsafeConfig()->failsafe_delay = sbufReadU8(src);
failsafeConfig()->failsafe_off_delay = sbufReadU8(src);
failsafeConfig()->failsafe_throttle = sbufReadU16(src);
failsafeConfig()->failsafe_kill_switch = sbufReadU8(src);
failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src);
failsafeConfig()->failsafe_procedure = sbufReadU8(src);
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src);
failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
break;
case MSP_SET_RXFAIL_CONFIG:
i = sbufReadU8(src);
if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
rxConfig()->failsafe_channel_configurations[i].mode = sbufReadU8(src);
rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
rxConfigMutable()->failsafe_channel_configurations[i].mode = sbufReadU8(src);
rxConfigMutable()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
} else {
return MSP_RESULT_ERROR;
}
break;
case MSP_SET_RSSI_CONFIG:
rxConfig()->rssi_channel = sbufReadU8(src);
rxConfigMutable()->rssi_channel = sbufReadU8(src);
break;
case MSP_SET_RX_MAP:
for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
rxConfig()->rcmap[i] = sbufReadU8(src);
rxConfigMutable()->rcmap[i] = sbufReadU8(src);
}
break;
@ -1845,20 +1854,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef USE_QUAD_MIXER_ONLY
sbufReadU8(src); // mixerMode ignored
#else
mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode
mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode
#endif
featureClearAll();
featureSet(sbufReadU32(src)); // features bitmap
rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type
rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type
boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll
boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll
boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch
boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw
batteryConfig()->currentMeterScale = sbufReadU16(src);
batteryConfig()->currentMeterOffset = sbufReadU16(src);
batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
break;
case MSP_SET_CF_SERIAL_CONFIG:
@ -1892,7 +1901,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef LED_STRIP
case MSP_SET_LED_COLORS:
for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &ledStripConfig()->colors[i];
hsvColor_t *color = &ledStripConfigMutable()->colors[i];
color->h = sbufReadU16(src);
color->s = sbufReadU8(src);
color->v = sbufReadU8(src);
@ -1905,7 +1914,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
return MSP_RESULT_ERROR;
}
ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i];
*ledConfig = sbufReadU32(src);
reevaluateLedConfig();
}

19
src/main/fc/fc_tasks.c
View file

@ -27,6 +27,11 @@
#include "common/color.h"
#include "common/utils.h"
#include "config/feature.h"
#include "config/config_profile.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
@ -43,8 +48,10 @@
#include "fc/cli.h"
#include "fc/fc_dispatch.h"
#include "flight/pid.h"
#include "flight/altitudehold.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/dashboard.h"
@ -72,10 +79,6 @@
#include "telemetry/telemetry.h"
#include "config/feature.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
#endif
@ -94,7 +97,7 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
accUpdate(&accelerometerConfig()->accelerometerTrims);
accUpdate(&accelerometerConfigMutable()->accelerometerTrims);
}
static void taskHandleSerial(timeUs_t currentTimeUs)
@ -128,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs)
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTimeUs;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
updateCurrentMeter(ibatTimeSinceLastServiced);
}
}
}
@ -201,7 +204,7 @@ static void taskTelemetry(timeUs_t currentTimeUs)
telemetryCheckState();
if (!cliMode && feature(FEATURE_TELEMETRY)) {
telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
telemetryProcess(currentTimeUs);
}
}
#endif

456
src/main/fc/rc_adjustments.c Normal file
View file

@ -0,0 +1,456 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "blackbox/blackbox.h"
#include "build/build_config.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "config/feature.h"
#include "config/config_master.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/rc_curves.h"
#include "fc/config.h"
#include "rx/rx.h"
static pidProfile_t *pidProfile;
static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
{
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newValue = newValue;
eventData.floatFlag = false;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
#if 0
static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue)
{
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newFloatValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newFloatValue = newFloatValue;
eventData.floatFlag = true;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
#endif
static uint8_t adjustmentStateMask = 0;
#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
// sync with adjustmentFunction_e
static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
{
.adjustmentFunction = ADJUSTMENT_RC_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .selectConfig = { .switchPositions = 3 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}
};
#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig)
{
adjustmentState_t *adjustmentState = &adjustmentStates[index];
if (adjustmentState->config == adjustmentConfig) {
// already configured
return;
}
adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
adjustmentState->config = adjustmentConfig;
adjustmentState->timeoutAt = 0;
MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
}
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
{
int newValue;
if (delta > 0) {
beeperConfirmationBeeps(2);
} else {
beeperConfirmationBeeps(1);
}
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
break;
case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->thrExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
break;
case ADJUSTMENT_RC_RATE_YAW:
newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcYawRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
break;
default:
break;
};
}
static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
applied = true;
}
break;
}
if (applied) {
beeperConfirmationBeeps(position + 1);
}
}
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(controlRateConfig_t *controlRateConfig)
{
const uint32_t now = millis();
const bool canUseRxData = rxIsReceivingSignal();
for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
if (!adjustmentState->config) {
continue;
}
const uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
if (adjustmentFunction == ADJUSTMENT_NONE) {
continue;
}
const int32_t signedDiff = now - adjustmentState->timeoutAt;
const bool canResetReadyStates = signedDiff >= 0L;
if (canResetReadyStates) {
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
}
if (!canUseRxData) {
continue;
}
const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
int delta;
if (rcData[channelIndex] > rxConfig()->midrc + 200) {
delta = adjustmentState->config->data.stepConfig.step;
} else if (rcData[channelIndex] < rxConfig()->midrc - 200) {
delta = 0 - adjustmentState->config->data.stepConfig.step;
} else {
// returning the switch to the middle immediately resets the ready state
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
continue;
}
if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
continue;
}
applyStepAdjustment(controlRateConfig,adjustmentFunction,delta);
pidInitConfig(pidProfile);
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
applySelectAdjustment(adjustmentFunction, position);
}
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
}
}
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
{
for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
adjustmentRange_t *adjustmentRange = &adjustmentRanges[index];
if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
}
}
}
void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}
void useAdjustmentConfig(pidProfile_t *pidProfileToUse)
{
pidProfile = pidProfileToUse;
}

116
src/main/fc/rc_adjustments.h Normal file
View file

@ -0,0 +1,116 @@
/*
* 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/>.
*/
#pragma once
#include <stdbool.h>
#include "config/parameter_group.h"
#include "fc/rc_controls.h"
typedef enum {
ADJUSTMENT_NONE = 0,
ADJUSTMENT_RC_RATE,
ADJUSTMENT_RC_EXPO,
ADJUSTMENT_THROTTLE_EXPO,
ADJUSTMENT_PITCH_ROLL_RATE,
ADJUSTMENT_YAW_RATE,
ADJUSTMENT_PITCH_ROLL_P,
ADJUSTMENT_PITCH_ROLL_I,
ADJUSTMENT_PITCH_ROLL_D,
ADJUSTMENT_YAW_P,
ADJUSTMENT_YAW_I,
ADJUSTMENT_YAW_D,
ADJUSTMENT_RATE_PROFILE,
ADJUSTMENT_PITCH_RATE,
ADJUSTMENT_ROLL_RATE,
ADJUSTMENT_PITCH_P,
ADJUSTMENT_PITCH_I,
ADJUSTMENT_PITCH_D,
ADJUSTMENT_ROLL_P,
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
ADJUSTMENT_RC_RATE_YAW,
ADJUSTMENT_D_SETPOINT,
ADJUSTMENT_D_SETPOINT_TRANSITION,
ADJUSTMENT_FUNCTION_COUNT
} adjustmentFunction_e;
typedef enum {
ADJUSTMENT_MODE_STEP,
ADJUSTMENT_MODE_SELECT
} adjustmentMode_e;
typedef struct adjustmentStepConfig_s {
uint8_t step;
} adjustmentStepConfig_t;
typedef struct adjustmentSelectConfig_s {
uint8_t switchPositions;
} adjustmentSelectConfig_t;
typedef union adjustmentConfig_u {
adjustmentStepConfig_t stepConfig;
adjustmentSelectConfig_t selectConfig;
} adjustmentData_t;
typedef struct adjustmentConfig_s {
uint8_t adjustmentFunction;
uint8_t mode;
adjustmentData_t data;
} adjustmentConfig_t;
typedef struct adjustmentRange_s {
// when aux channel is in range...
uint8_t auxChannelIndex;
channelRange_t range;
// ..then apply the adjustment function to the auxSwitchChannel ...
uint8_t adjustmentFunction;
uint8_t auxSwitchChannelIndex;
// ... via slot
uint8_t adjustmentIndex;
} adjustmentRange_t;
#define ADJUSTMENT_INDEX_OFFSET 1
typedef struct adjustmentState_s {
uint8_t auxChannelIndex;
const adjustmentConfig_t *config;
uint32_t timeoutAt;
} adjustmentState_t;
#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
#endif
#define MAX_ADJUSTMENT_RANGE_COUNT 15
PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges);
typedef struct adjustmentProfile_s {
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
} adjustmentProfile_t;
void resetAdjustmentStates(void);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
struct controlRateConfig_s;
void processRcAdjustments(struct controlRateConfig_s *controlRateConfig);
struct pidProfile_s;
void useAdjustmentConfig(struct pidProfile_s *pidProfileToUse);

427
src/main/fc/rc_controls.c
View file

@ -31,6 +31,8 @@
#include "common/maths.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
@ -59,7 +61,6 @@
#include "flight/failsafe.h"
static motorConfig_t *motorConfig;
static pidProfile_t *pidProfile;
// true if arming is done via the sticks (as opposed to a switch)
@ -73,37 +74,6 @@ bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
#ifndef BLACKBOX
#define UNUSED(x) (void)(x)
UNUSED(adjustmentFunction);
UNUSED(newValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newValue = newValue;
eventData.floatFlag = false;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) {
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newFloatValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newFloatValue = newFloatValue;
eventData.floatFlag = true;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;
@ -114,20 +84,20 @@ bool areSticksInApModePosition(uint16_t ap_mode)
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
}
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
throttleStatus_e calculateThrottleStatus(void)
{
if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle)))
if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)))
return THROTTLE_LOW;
} else {
if (rcData[THROTTLE] < rxConfig->mincheck)
if (rcData[THROTTLE] < rxConfig()->mincheck)
return THROTTLE_LOW;
}
return THROTTLE_HIGH;
}
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
void processRcStickPositions(throttleStatus_e throttleStatus)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
@ -139,9 +109,9 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
// checking sticks positions
for (i = 0; i < 4; i++) {
stTmp >>= 2;
if (rcData[i] > rxConfig->mincheck)
if (rcData[i] > rxConfig()->mincheck)
stTmp |= 0x80; // check for MIN
if (rcData[i] < rxConfig->maxcheck)
if (rcData[i] < rxConfig()->maxcheck)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) {
@ -168,7 +138,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (disarm_kill_switch) {
if (armingConfig()->disarm_kill_switch) {
mwDisarm();
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm();
@ -317,12 +287,12 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
}
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
{
uint8_t index;
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
return true;
@ -357,386 +327,13 @@ void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
}
}
uint8_t adjustmentStateMask = 0;
#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
// sync with adjustmentFunction_e
static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
{
.adjustmentFunction = ADJUSTMENT_RC_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .selectConfig = { .switchPositions = 3 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}
};
#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) {
adjustmentState_t *adjustmentState = &adjustmentStates[index];
if (adjustmentState->config == adjustmentConfig) {
// already configured
return;
}
adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
adjustmentState->config = adjustmentConfig;
adjustmentState->timeoutAt = 0;
MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
}
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue;
if (delta > 0) {
beeperConfirmationBeeps(2);
} else {
beeperConfirmationBeeps(1);
}
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
break;
case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->thrExpo8 = newValue;
generateThrottleCurve();
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
break;
case ADJUSTMENT_RC_RATE_YAW:
newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcYawRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
break;
default:
break;
};
}
static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
applied = true;
}
break;
}
if (applied) {
beeperConfirmationBeeps(position + 1);
}
}
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
{
uint8_t adjustmentIndex;
uint32_t now = millis();
bool canUseRxData = rxIsReceivingSignal();
for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
if (!adjustmentState->config) {
continue;
}
uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
if (adjustmentFunction == ADJUSTMENT_NONE) {
continue;
}
int32_t signedDiff = now - adjustmentState->timeoutAt;
bool canResetReadyStates = signedDiff >= 0L;
if (canResetReadyStates) {
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
}
if (!canUseRxData) {
continue;
}
uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
int delta;
if (rcData[channelIndex] > rxConfig->midrc + 200) {
delta = adjustmentState->config->data.stepConfig.step;
} else if (rcData[channelIndex] < rxConfig->midrc - 200) {
delta = 0 - adjustmentState->config->data.stepConfig.step;
} else {
// returning the switch to the middle immediately resets the ready state
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
continue;
}
if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
continue;
}
applyStepAdjustment(controlRateConfig,adjustmentFunction,delta);
pidInitConfig(pidProfile);
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
applySelectAdjustment(adjustmentFunction, position);
}
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
}
}
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
{
uint8_t index;
for (index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
adjustmentRange_t *adjustmentRange = &adjustmentRanges[index];
if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
}
}
}
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return MIN(ABS(rcData[axis] - midrc), 500);
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse)
void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse)
{
motorConfig = motorConfigToUse;
pidProfile = pidProfileToUse;
isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);
}
void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}

119
src/main/fc/rc_controls.h
View file

@ -19,6 +19,8 @@
#include <stdbool.h>
#include "config/parameter_group.h"
typedef enum {
BOXARM = 0,
BOXANGLE,
@ -51,6 +53,7 @@ typedef enum {
BOXAIRMODE,
BOX3DDISABLESWITCH,
BOXFPVANGLEMIX,
BOXBLACKBOXERASE,
CHECKBOX_ITEM_COUNT
} boxId_e;
@ -140,6 +143,8 @@ typedef struct modeActivationCondition_s {
channelRange_t range;
} modeActivationCondition_t;
PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
typedef struct modeActivationProfile_s {
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
} modeActivationProfile_t;
@ -158,6 +163,8 @@ typedef struct controlRateConfig_s {
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
} controlRateConfig_t;
//!!TODO PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);
extern int16_t rcCommand[4];
typedef struct rcControlsConfig_s {
@ -168,117 +175,39 @@ typedef struct rcControlsConfig_s {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
} rcControlsConfig_t;
PG_DECLARE(rcControlsConfig_t, rcControlsConfig);
typedef struct flight3DConfig_s {
uint16_t deadband3d_low; // min 3d value
uint16_t deadband3d_high; // max 3d value
uint16_t neutral3d; // center 3d value
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC
} flight3DConfig_t;
PG_DECLARE(flight3DConfig_t, flight3DConfig);
typedef struct armingConfig_s {
uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
} armingConfig_t;
PG_DECLARE(armingConfig_t, armingConfig);
bool areUsingSticksToArm(void);
bool areSticksInApModePosition(uint16_t ap_mode);
struct rxConfig_s;
throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
throttleStatus_e calculateThrottleStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus);
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range);
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
typedef enum {
ADJUSTMENT_NONE = 0,
ADJUSTMENT_RC_RATE,
ADJUSTMENT_RC_EXPO,
ADJUSTMENT_THROTTLE_EXPO,
ADJUSTMENT_PITCH_ROLL_RATE,
ADJUSTMENT_YAW_RATE,
ADJUSTMENT_PITCH_ROLL_P,
ADJUSTMENT_PITCH_ROLL_I,
ADJUSTMENT_PITCH_ROLL_D,
ADJUSTMENT_YAW_P,
ADJUSTMENT_YAW_I,
ADJUSTMENT_YAW_D,
ADJUSTMENT_RATE_PROFILE,
ADJUSTMENT_PITCH_RATE,
ADJUSTMENT_ROLL_RATE,
ADJUSTMENT_PITCH_P,
ADJUSTMENT_PITCH_I,
ADJUSTMENT_PITCH_D,
ADJUSTMENT_ROLL_P,
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
ADJUSTMENT_RC_RATE_YAW,
ADJUSTMENT_D_SETPOINT,
ADJUSTMENT_D_SETPOINT_TRANSITION,
ADJUSTMENT_FUNCTION_COUNT
} adjustmentFunction_e;
typedef enum {
ADJUSTMENT_MODE_STEP,
ADJUSTMENT_MODE_SELECT
} adjustmentMode_e;
typedef struct adjustmentStepConfig_s {
uint8_t step;
} adjustmentStepConfig_t;
typedef struct adjustmentSelectConfig_s {
uint8_t switchPositions;
} adjustmentSelectConfig_t;
typedef union adjustmentConfig_u {
adjustmentStepConfig_t stepConfig;
adjustmentSelectConfig_t selectConfig;
} adjustmentData_t;
typedef struct adjustmentConfig_s {
uint8_t adjustmentFunction;
uint8_t mode;
adjustmentData_t data;
} adjustmentConfig_t;
typedef struct adjustmentRange_s {
// when aux channel is in range...
uint8_t auxChannelIndex;
channelRange_t range;
// ..then apply the adjustment function to the auxSwitchChannel ...
uint8_t adjustmentFunction;
uint8_t auxSwitchChannelIndex;
// ... via slot
uint8_t adjustmentIndex;
} adjustmentRange_t;
#define ADJUSTMENT_INDEX_OFFSET 1
typedef struct adjustmentState_s {
uint8_t auxChannelIndex;
const adjustmentConfig_t *config;
uint32_t timeoutAt;
} adjustmentState_t;
#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
#endif
#define MAX_ADJUSTMENT_RANGE_COUNT 15
typedef struct adjustmentProfile_s {
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
} adjustmentProfile_t;
bool isAirmodeActive(void);
void resetAdjustmentStates(void);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig);
bool isUsingSticksForArming(void);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
struct pidProfile_s;
struct motorConfig_s;
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse);
void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse);

60
src/main/fc/rc_curves.c Normal file
View file

@ -0,0 +1,60 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "config/feature.h"
#include "io/motors.h"
#include "fc/config.h"
#include "fc/rc_curves.h"
#include "fc/rc_controls.h"
#include "rx/rx.h"
#define THROTTLE_LOOKUP_LENGTH 12
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, const motorConfig_t *motorConfig)
{
uint8_t i;
uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : motorConfig->minthrottle);
for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
int16_t tmp = 10 * i - controlRateConfig->thrMid8;
uint8_t y = 1;
if (tmp > 0)
y = 100 - controlRateConfig->thrMid8;
if (tmp < 0)
y = controlRateConfig->thrMid8;
lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
lookupThrottleRC[i] = minThrottle + (int32_t) (motorConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
}
}
int16_t rcLookupThrottle(int32_t tmp)
{
const int32_t tmp2 = tmp / 100;
// [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
}

25
src/main/fc/rc_curves.h Normal file
View file

@ -0,0 +1,25 @@
/*
* 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/>.
*/
#pragma once
struct controlRateConfig_s;
struct motorConfig_s;
void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig);
int16_t rcLookupThrottle(int32_t tmp);

60
src/main/flight/altitudehold.c
View file

@ -21,25 +21,26 @@
#include <stdlib.h>
#include <math.h>
#include "platform.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/maths.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "rx/rx.h"
#include "fc/rc_controls.h"
#include "io/motors.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "fc/runtime_config.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
int32_t setVelocity = 0;
@ -50,22 +51,11 @@ int32_t AltHold;
int32_t vario = 0; // variometer in cm/s
static barometerConfig_t *barometerConfig;
static pidProfile_t *pidProfile;
static rcControlsConfig_t *rcControlsConfig;
static motorConfig_t *motorConfig;
void configureAltitudeHold(
pidProfile_t *initialPidProfile,
barometerConfig_t *intialBarometerConfig,
rcControlsConfig_t *initialRcControlsConfig,
motorConfig_t *initialMotorConfig
)
void configureAltitudeHold(pidProfile_t *initialPidProfile)
{
pidProfile = initialPidProfile;
barometerConfig = intialBarometerConfig;
rcControlsConfig = initialRcControlsConfig;
motorConfig = initialMotorConfig;
}
#if defined(BARO) || defined(SONAR)
@ -82,22 +72,22 @@ static void applyMultirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
// multirotor alt hold
if (rcControlsConfig->alt_hold_fast_change) {
if (rcControlsConfig()->alt_hold_fast_change) {
// rapid alt changes
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig->minthrottle, motorConfig->maxthrottle);
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig()->minthrottle, motorConfig()->maxthrottle);
}
} else {
// slow alt changes, mostly used for aerial photography
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
@ -107,23 +97,23 @@ static void applyMultirotorAltHold(void)
velocityControl = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig->minthrottle, motorConfig->maxthrottle);
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig()->minthrottle, motorConfig()->maxthrottle);
}
}
static void applyFixedWingAltHold(airplaneConfig_t *airplaneConfig)
static void applyFixedWingAltHold(void)
{
// handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor
rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig->fixedwing_althold_dir;
rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig()->fixedwing_althold_dir;
}
void applyAltHold(airplaneConfig_t *airplaneConfig)
void applyAltHold(void)
{
if (STATE(FIXED_WING)) {
applyFixedWingAltHold(airplaneConfig);
applyFixedWingAltHold();
} else {
applyMultirotorAltHold();
}
@ -272,7 +262,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#ifdef DEBUG_ALT_HOLD
@ -308,7 +298,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel);
vel = vel * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario

8
src/main/flight/altitudehold.h
View file

@ -23,13 +23,9 @@ extern int32_t vario;
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
struct pidProfile_s;
struct barometerConfig_s;
struct rcControlsConfig_s;
struct motorConfig_s;
void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct motorConfig_s *initialMotorConfig);
void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
struct airplaneConfig_s;
void applyAltHold(struct airplaneConfig_s *airplaneConfig);
void applyAltHold(void);
void updateAltHoldState(void);
void updateSonarAltHoldState(void);

50
src/main/flight/failsafe.c
View file

@ -24,6 +24,9 @@
#include "common/axis.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"
@ -40,9 +43,9 @@
/*
* Usage:
*
* failsafeInit() and useFailsafeConfig() must be called before the other methods are used.
* failsafeInit() and resetFailsafe() must be called before the other methods are used.
*
* failsafeInit() and useFailsafeConfig() can be called in any order.
* failsafeInit() and resetFailsafe() can be called in any order.
* failsafeInit() should only be called once.
*
* enable() should be called after system initialisation.
@ -50,15 +53,12 @@
static failsafeState_t failsafeState;
static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC
static void failsafeReset(void)
/*
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
*/
void failsafeReset(void)
{
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig->failsafe_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.validRxDataReceivedAt = 0;
failsafeState.validRxDataFailedAt = 0;
failsafeState.throttleLowPeriod = 0;
@ -69,20 +69,8 @@ static void failsafeReset(void)
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
/*
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
*/
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
void failsafeInit(void)
{
failsafeConfig = failsafeConfigToUse;
failsafeReset();
}
void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle)
{
rxConfig = intialRxConfig;
deadband3dThrottle = deadband3d_throttle;
failsafeState.events = 0;
failsafeState.monitoring = false;
@ -119,7 +107,7 @@ static void failsafeActivate(void)
failsafeState.active = true;
failsafeState.phase = FAILSAFE_LANDING;
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.events++;
}
@ -127,9 +115,9 @@ static void failsafeActivate(void)
static void failsafeApplyControlInput(void)
{
for (int i = 0; i < 3; i++) {
rcData[i] = rxConfig->midrc;
rcData[i] = rxConfig()->midrc;
}
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
rcData[THROTTLE] = failsafeConfig()->failsafe_throttle;
}
bool failsafeIsReceivingRxData(void)
@ -189,11 +177,11 @@ void failsafeUpdateState(void)
case FAILSAFE_IDLE:
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
if (THROTTLE_HIGH == calculateThrottleStatus()) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) {
if (failsafeSwitchIsOn && failsafeConfig()->failsafe_kill_switch) {
// KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
@ -226,7 +214,7 @@ void failsafeUpdateState(void)
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
} else {
switch (failsafeConfig->failsafe_procedure) {
switch (failsafeConfig()->failsafe_procedure) {
default:
case FAILSAFE_PROCEDURE_AUTO_LANDING:
// Stabilize, and set Throttle to specified level
@ -288,7 +276,7 @@ void failsafeUpdateState(void)
// Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
// This is to prevent that JustDisarm is activated on the next iteration.
// Because that would have the effect of shutting down failsafe handling on intermittent connections.
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);

9
src/main/flight/failsafe.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "config/parameter_group.h"
#define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5)
#define MILLIS_PER_TENTH_SECOND 100
#define MILLIS_PER_SECOND 1000
@ -36,6 +38,8 @@ typedef struct failsafeConfig_s {
uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it
} failsafeConfig_t;
PG_DECLARE(failsafeConfig_t, failsafeConfig);
typedef enum {
FAILSAFE_IDLE = 0,
FAILSAFE_RX_LOSS_DETECTED,
@ -70,9 +74,8 @@ typedef struct failsafeState_s {
failsafeRxLinkState_e rxLinkState;
} failsafeState_t;
struct rxConfig_s;
void failsafeInit(struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle);
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
void failsafeInit(void);
void failsafeReset(void);
void failsafeStartMonitoring(void);
void failsafeUpdateState(void);

3
src/main/flight/imu.c
View file

@ -30,6 +30,9 @@
#include "common/axis.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "sensors/sensors.h"

11
src/main/flight/imu.h
View file

@ -21,6 +21,8 @@
#include "common/maths.h"
#include "common/time.h"
#include "config/parameter_group.h"
#include "sensors/acceleration.h"
// Exported symbols
@ -30,11 +32,6 @@ extern float accVelScale;
extern int32_t accSum[XYZ_AXIS_COUNT];
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
typedef union {
int16_t raw[XYZ_AXIS_COUNT];
struct {
@ -57,6 +54,8 @@ typedef struct throttleCorrectionConfig_s {
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
} throttleCorrectionConfig_t;
PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
typedef struct imuConfig_s {
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
@ -65,6 +64,8 @@ typedef struct imuConfig_s {
accDeadband_t accDeadband;
} imuConfig_t;
PG_DECLARE(imuConfig_t, imuConfig);
typedef struct imuRuntimeConfig_s {
float dcm_ki;
float dcm_kp;

68
src/main/flight/mixer.c
View file

@ -28,6 +28,10 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/pwm_output.h"
@ -46,9 +50,6 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "config/feature.h"
#include "config/config_master.h"
#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
// (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977
@ -61,11 +62,7 @@ static float motorMixRange;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static mixerConfig_t *mixerConfig;
static flight3DConfig_t *flight3DConfig;
static motorConfig_t *motorConfig;
static airplaneConfig_t *airplaneConfig;
rxConfig_t *rxConfig;
mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -253,7 +250,7 @@ float getMotorMixRange()
bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
switch(motorConfig->motorPwmProtocol) {
switch(motorConfig()->motorPwmProtocol) {
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
@ -273,39 +270,30 @@ void initEscEndpoints(void) {
if (isMotorProtocolDshot()) {
disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D))
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent);
else
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent);
motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
} else
#endif
{
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
motorOutputLow = motorConfig->minthrottle;
motorOutputHigh = motorConfig->maxthrottle;
deadbandMotor3dHigh = flight3DConfig->deadband3d_high;
deadbandMotor3dLow = flight3DConfig->deadband3d_low;
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle;
motorOutputHigh = motorConfig()->maxthrottle;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
}
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck);
rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck);
rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
}
void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse,
motorConfig_t *motorConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
rxConfig_t *rxConfigToUse)
void mixerUseConfigs(airplaneConfig_t *airplaneConfigToUse)
{
flight3DConfig = flight3DConfigToUse;
motorConfig = motorConfigToUse;
mixerConfig = mixerConfigToUse;
airplaneConfig = airplaneConfigToUse;
rxConfig = rxConfigToUse;
}
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
@ -440,25 +428,25 @@ void mixTable(pidProfile_t *pidProfile)
// Find min and max throttle based on condition.
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = motorOutputLow;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
} else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling
motorOutputMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
} else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = motorOutputLow;
throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle;
throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else { // Deadband handling from positive to negative
@ -468,7 +456,7 @@ void mixTable(pidProfile_t *pidProfile)
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
}
} else {
throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
currentThrottleInputRange = rcCommandThrottleRange;
motorOutputMin = motorOutputLow;
motorOutputMax = motorOutputHigh;
@ -484,7 +472,7 @@ void mixTable(pidProfile_t *pidProfile)
}
// Calculate voltage compensation
const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;
const float vbatCompensationFactor = pidProfile->vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f;
// Find roll/pitch/yaw desired output
float motorMix[MAX_SUPPORTED_MOTORS];
@ -493,7 +481,7 @@ void mixTable(pidProfile_t *pidProfile)
motorMix[i] =
scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
scaledAxisPIDf[ROLL] * currentMixer[i].roll +
scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction);
scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig()->yaw_motor_direction);
if (vbatCompensationFactor > 1.0f) {
motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
@ -541,7 +529,7 @@ void mixTable(pidProfile_t *pidProfile)
// Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
if (((rcData[THROTTLE]) < rxConfig()->mincheck)) {
motor[i] = disarmMotorOutput;
}
}

35
src/main/flight/mixer.h
View file

@ -17,6 +17,9 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/io_types.h"
#define MAX_SUPPORTED_MOTORS 12
#define QUAD_MOTOR_COUNT 4
@ -85,6 +88,8 @@ typedef struct motorMixer_s {
float yaw;
} motorMixer_t;
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer);
// Custom mixer configuration
typedef struct mixer_s {
uint8_t motorCount;
@ -97,12 +102,21 @@ typedef struct mixerConfig_s {
int8_t yaw_motor_direction;
} mixerConfig_t;
typedef struct flight3DConfig_s {
uint16_t deadband3d_low; // min 3d value
uint16_t deadband3d_high; // max 3d value
uint16_t neutral3d; // center 3d value
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC
} flight3DConfig_t;
PG_DECLARE(mixerConfig_t, mixerConfig);
typedef struct motorConfig_s {
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
uint8_t motorPwmProtocol; // Pwm Protocol
uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation
uint8_t useUnsyncedPwm;
float digitalIdleOffsetPercent;
ioTag_t ioTags[MAX_SUPPORTED_MOTORS];
} motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig);
typedef struct airplaneConfig_s {
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
@ -113,19 +127,12 @@ typedef struct airplaneConfig_s {
extern const mixer_t mixers[];
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct motorConfig_s;
struct rxConfig_s;
uint8_t getMotorCount();
float getMotorMixRange();
void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse,
struct motorConfig_s *motorConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse);
void mixerUseConfigs(airplaneConfig_t *airplaneConfigToUse);
void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);

19
src/main/flight/navigation.c
View file

@ -27,30 +27,33 @@
#include "build/debug.h"
#include "common/axis.h"
#include "common/gps_conversion.h"
#include "common/maths.h"
#include "common/time.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/acceleration.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "io/gps.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "flight/gps_conversion.h"
#include "flight/imu.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
extern int16_t magHold;

3
src/main/flight/pid.c
View file

@ -28,6 +28,9 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"

4
src/main/flight/pid.h
View file

@ -85,10 +85,14 @@ typedef struct pidProfile_s {
float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
} pidProfile_t;
PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
typedef struct pidConfig_s {
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
} pidConfig_t;
PG_DECLARE(pidConfig_t, pidConfig);
union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);

30
src/main/flight/servos.c
View file

@ -28,6 +28,9 @@
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/pwm_output.h"
#include "drivers/system.h"
@ -47,7 +50,6 @@
#include "config/feature.h"
extern mixerMode_e currentMixerMode;
extern rxConfig_t *rxConfig;
static servoMixerConfig_t *servoMixerConfig;
@ -276,7 +278,7 @@ void writeServos(void)
case MIXER_TRI:
case MIXER_CUSTOM_TRI:
if (servoMixerConfig->tri_unarmed_servo) {
if (servoMixerConfig()->tri_unarmed_servo) {
// if unarmed flag set, we always move servo
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
} else {
@ -346,7 +348,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING;
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) {
input[INPUT_STABILIZED_YAW] *= -1;
}
}
@ -362,14 +364,14 @@ STATIC_UNIT_TESTED void servoMixer(void)
// 2000 - 1500 = +500
// 1500 - 1500 = 0
// 1000 - 1500 = -500
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc;
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig()->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig()->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig()->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig()->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig()->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig()->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig()->midrc;
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servo[i] = 0;
@ -440,7 +442,7 @@ void servoTable(void)
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) {
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} else {
@ -471,10 +473,10 @@ void filterServos(void)
uint32_t startTime = micros();
#endif
if (servoMixerConfig->servo_lowpass_enable) {
if (servoMixerConfig()->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime);
biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig()->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}

12
src/main/flight/servos.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "config/parameter_group.h"
#define MAX_SUPPORTED_SERVOS 8
// These must be consecutive, see 'reversedSources'
@ -87,6 +89,8 @@ typedef struct servoMixer_s {
#define MAX_SERVO_SPEED UINT8_MAX
#define MAX_SERVO_BOXES 3
PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);
// Custom mixer configuration
typedef struct mixerRules_s {
uint8_t servoRuleCount;
@ -94,6 +98,7 @@ typedef struct mixerRules_s {
} mixerRules_t;
typedef struct servoParam_s {
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
int16_t min; // servo min
int16_t max; // servo max
int16_t middle; // servo middle
@ -101,8 +106,9 @@ typedef struct servoParam_s {
uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value.
uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value.
int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
} __attribute__ ((__packed__)) servoParam_t;
} servoParam_t;
PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams);
typedef struct servoMixerConfig_s{
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
@ -110,6 +116,8 @@ typedef struct servoMixerConfig_s{
int8_t servo_lowpass_enable; // enable/disable lowpass filter
} servoMixerConfig_t;
//!!TODO PG_DECLARE(servoConfig_t, servoConfig);
typedef struct servoProfile_s {
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
} servoProfile_t;

6
src/main/io/beeper.c
View file

@ -171,9 +171,9 @@ typedef struct beeperTableEntry_s {
{ BEEPER_ENTRY(BEEPER_ARMED, 15, beep_armedBeep, "ARMED") },
{ BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 16, NULL, "SYSTEM_INIT") },
{ BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") },
{ BEEPER_ENTRY(BEEPER_ALL, 18, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 19, NULL, "PREFERRED") },
{ BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 18, beep_2shortBeeps, "BLACKBOX_ERASE") },
{ BEEPER_ENTRY(BEEPER_ALL, 19, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 20, NULL, "PREFERRED") },
};
static const beeperTableEntry_t *currentBeeperEntry = NULL;

2
src/main/io/beeper.h
View file

@ -41,7 +41,7 @@ typedef enum {
BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased)
BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on
BEEPER_USB, // Some boards have beeper powered USB connected
BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes
BEEPER_ALL, // Turn ON or OFF all beeper conditions
BEEPER_PREFERENCE // Save preferred beeper configuration
// BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum

3
src/main/io/displayport_max7456.c
View file

@ -24,7 +24,8 @@
#include "common/utils.h"
#include "config/config_master.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/display.h"
#include "drivers/max7456.h"

4
src/main/io/gimbal.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "config/parameter_group.h"
typedef enum {
GIMBAL_MODE_NORMAL = 0,
GIMBAL_MODE_MIXTILT = 1
@ -27,3 +29,5 @@ typedef enum {
typedef struct gimbalConfig_s {
uint8_t mode;
} gimbalConfig_t;
PG_DECLARE(gimbalConfig_t, gimbalConfig);

21
src/main/io/gps.c
View file

@ -29,27 +29,32 @@
#include "build/build_config.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/gps_conversion.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/light_led.h"
#include "sensors/sensors.h"
#include "drivers/light_led.h"
#include "drivers/system.h"
#include "io/serial.h"
#include "io/dashboard.h"
#include "io/gps.h"
#include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "io/serial.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "config/feature.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "sensors/sensors.h"
#define LOG_ERROR '?'
#define LOG_IGNORED '!'

4
src/main/io/gps.h
View file

@ -19,6 +19,8 @@
#include "common/time.h"
#include "config/parameter_group.h"
#define LAT 0
#define LON 1
@ -68,6 +70,8 @@ typedef struct gpsConfig_s {
gpsAutoBaud_e autoBaud;
} gpsConfig_t;
PG_DECLARE(gpsConfig_t, gpsConfig);
typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t dddmm;
int16_t mmmm;

12
src/main/io/ledstrip.c
View file

@ -31,6 +31,9 @@
#include "common/maths.h"
#include "common/typeconversion.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/system.h"
#include "drivers/serial.h"
@ -44,6 +47,11 @@
#include "common/axis.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
@ -73,10 +81,6 @@
#include "telemetry/telemetry.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
/*
PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0);
PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0);

3
src/main/io/ledstrip.h
View file

@ -19,6 +19,7 @@
#include "common/color.h"
#include "common/time.h"
#include "config/parameter_group.h"
#include "drivers/io_types.h"
#define LED_MAX_STRIP_LENGTH 32
@ -147,6 +148,8 @@ typedef struct ledStripConfig_s {
ioTag_t ioTag;
} ledStripConfig_t;
PG_DECLARE(ledStripConfig_t, ledStripConfig);
ledConfig_t *ledConfigs;
hsvColor_t *colors;
modeColorIndexes_t *modeColors;

11
src/main/io/motors.h
View file

@ -20,13 +20,4 @@
#include "drivers/io_types.h"
#include "flight/mixer.h"
typedef struct motorConfig_s {
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
uint8_t motorPwmProtocol; // Pwm Protocol
uint8_t useUnsyncedPwm;
float digitalIdleOffsetPercent;
ioTag_t ioTags[MAX_SUPPORTED_MOTORS];
} motorConfig_t;

62
src/main/io/osd.c
View file

@ -24,6 +24,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <ctype.h>
@ -31,12 +32,19 @@
#ifdef OSD
#include "blackbox/blackbox_io.h"
#include "build/debug.h"
#include "build/version.h"
#include "common/printf.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/max7456_symbols.h"
#include "drivers/display.h"
#include "drivers/system.h"
@ -48,20 +56,15 @@
#include "cms/cms_types.h"
#include "cms/cms_menu_osd.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif
@ -610,6 +613,47 @@ static void osdUpdateStats(void)
stats.max_altitude = baro.BaroAlt;
}
static void osdGetBlackboxStatusString(char * buff, uint8_t len)
{
bool storageDeviceIsWorking = false;
uint32_t storageUsed = 0;
uint32_t storageTotal = 0;
switch (blackboxConfig()->device)
{
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY);
if (storageDeviceIsWorking) {
storageTotal = sdcard_getMetadata()->numBlocks / 2000;
storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
}
break;
#endif
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
storageDeviceIsWorking = flashfsIsReady();
if (storageDeviceIsWorking) {
const flashGeometry_t *geometry = flashfsGetGeometry();
storageTotal = geometry->totalSize / 1024;
storageUsed = flashfsGetOffset() / 1024;
}
break;
#endif
default:
storageDeviceIsWorking = true;
}
if (storageDeviceIsWorking) {
uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
snprintf(buff, len, "%d%%", storageUsedPercent);
} else {
snprintf(buff, len, "FAULT");
}
}
static void osdShowStats(void)
{
uint8_t top = 2;
@ -650,6 +694,12 @@ static void osdShowStats(void)
sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
displayWrite(osdDisplayPort, 22, top++, buff);
if (feature(FEATURE_BLACKBOX) && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
displayWrite(osdDisplayPort, 2, top, "BLACKBOX :");
osdGetBlackboxStatusString(buff, 10);
displayWrite(osdDisplayPort, 22, top++, buff);
}
refreshTimeout = 60 * REFRESH_1S;
}

4
src/main/io/osd.h
View file

@ -18,6 +18,7 @@
#pragma once
#include "common/time.h"
#include "config/parameter_group.h"
#define VISIBLE_FLAG 0x0800
#define VISIBLE(x) (x & VISIBLE_FLAG)
@ -67,6 +68,9 @@ typedef struct osd_profile_s {
osd_unit_e units;
} osd_profile_t;
// !!TODO change to osdConfig_t
PG_DECLARE(osd_profile_t, osdConfig);
struct displayPort_s;
void osdInit(struct displayPort_s *osdDisplayPort);
void osdResetConfig(osd_profile_t *osdProfile);

42
src/main/io/serial.c
View file

@ -25,6 +25,9 @@
#include "common/utils.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)
@ -42,7 +45,8 @@
#endif
#include "io/serial.h"
#include "fc/cli.h" // for cliEnter()
#include "fc/cli.h"
#include "msp/msp_serial.h"
@ -50,7 +54,6 @@
#include "telemetry/telemetry.h"
#endif
static serialConfig_t *serialConfig;
static serialPortUsage_t serialPortUsageList[SERIAL_PORT_COUNT];
const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
@ -157,7 +160,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function)
{
while (findSerialPortConfigState.lastIndex < SERIAL_PORT_COUNT) {
serialPortConfig_t *candidate = &serialConfig->portConfigs[findSerialPortConfigState.lastIndex++];
serialPortConfig_t *candidate = &serialConfigMutable()->portConfigs[findSerialPortConfigState.lastIndex++];
if (candidate->functionMask & function) {
return candidate;
@ -170,7 +173,7 @@ typedef struct findSharedSerialPortState_s {
uint8_t lastIndex;
} findSharedSerialPortState_t;
portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function)
portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function)
{
if (!portConfig || (portConfig->functionMask & function) == 0) {
return PORTSHARING_UNUSED;
@ -178,7 +181,7 @@ portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFun
return portConfig->functionMask == function ? PORTSHARING_NOT_SHARED : PORTSHARING_SHARED;
}
bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction)
bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction)
{
return (portConfig) && (portConfig->functionMask & sharedWithFunction) && (portConfig->functionMask & functionMask);
}
@ -195,10 +198,10 @@ serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e s
serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction)
{
while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) {
serialPortConfig_t *candidate = &serialConfig->portConfigs[findSharedSerialPortState.lastIndex++];
const serialPortConfig_t *candidate = &serialConfig()->portConfigs[findSharedSerialPortState.lastIndex++];
if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) {
serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier);
const serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier);
if (!serialPortUsage) {
continue;
}
@ -215,7 +218,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX)
#endif
bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck)
{
UNUSED(serialConfigToCheck);
/*
@ -229,9 +232,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
*/
uint8_t mspPortCount = 0;
uint8_t index;
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index];
for (int index = 0; index < SERIAL_PORT_COUNT; index++) {
const serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index];
if (portConfig->functionMask & FUNCTION_MSP) {
mspPortCount++;
@ -265,9 +267,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier)
{
uint8_t index;
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
serialPortConfig_t *candidate = &serialConfig->portConfigs[index];
for (int index = 0; index < SERIAL_PORT_COUNT; index++) {
serialPortConfig_t *candidate = &serialConfigMutable()->portConfigs[index];
if (candidate->identifier == identifier) {
return candidate;
}
@ -394,16 +395,12 @@ void closeSerialPort(serialPort_t *serialPort)
serialPortUsage->serialPort = NULL;
}
void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable)
void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable)
{
uint8_t index;
serialConfig = initialSerialConfig;
serialPortCount = SERIAL_PORT_COUNT;
memset(&serialPortUsageList, 0, sizeof(serialPortUsageList));
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
for (int index = 0; index < SERIAL_PORT_COUNT; index++) {
serialPortUsageList[index].identifier = serialPortIdentifiers[index];
if (serialPortToDisable != SERIAL_PORT_NONE) {
@ -469,7 +466,7 @@ void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar)
cliEnter(serialPort);
}
#endif
if (receivedChar == serialConfig->reboot_character) {
if (receivedChar == serialConfig()->reboot_character) {
systemResetToBootloader();
}
}
@ -486,8 +483,7 @@ static void nopConsumer(uint8_t data)
arbitrary serial passthrough "proxy". Optional callbacks can be given to allow
for specialized data processing.
*/
void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
*leftC, serialConsumer *rightC)
void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC)
{
waitForSerialPortToFinishTransmitting(left);
waitForSerialPortToFinishTransmitting(right);

14
src/main/io/serial.h
View file

@ -17,6 +17,10 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "config/parameter_group.h"
#include "drivers/serial.h"
typedef enum {
@ -111,23 +115,25 @@ typedef struct serialConfig_s {
uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else.
} serialConfig_t;
PG_DECLARE(serialConfig_t, serialConfig);
typedef void serialConsumer(uint8_t);
//
// configuration
//
void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable);
void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable);
void serialRemovePort(serialPortIdentifier_e identifier);
uint8_t serialGetAvailablePortCount(void);
bool serialIsPortAvailable(serialPortIdentifier_e identifier);
bool isSerialConfigValid(serialConfig_t *serialConfig);
bool isSerialConfigValid(const serialConfig_t *serialConfig);
serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier);
bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier);
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function);
serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function);
portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function);
bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction);
portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function);
bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction);
serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier);
int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier);

2
src/main/io/servos.h
View file

@ -26,3 +26,5 @@ typedef struct servoConfig_s {
uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz)
ioTag_t ioTags[MAX_SUPPORTED_SERVOS];
} servoConfig_t;
PG_DECLARE(servoConfig_t, servoConfig);

6
src/main/io/vtx.c
View file

@ -26,10 +26,14 @@
#include "io/osd.h"
//External dependencies
#include "config/config_master.h"
#include "config/config_eeprom.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/vtx_rtc6705.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"

16
src/main/io/vtx_smartaudio.c
View file

@ -19,32 +19,36 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <ctype.h>
#include "platform.h"
#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
#include "build/build_config.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "string.h"
#include "common/printf.h"
#include "common/utils.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/vtx_common.h"
#include "io/serial.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx_string.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
#include "config/config_master.h"
#include "build/build_config.h"
#include "io/serial.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx_string.h"
//#define SMARTAUDIO_DPRINTF
//#define SMARTAUDIO_DEBUG_MONITOR

2
src/main/msp/msp_protocol.h
View file

@ -59,7 +59,7 @@
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 31 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_MINOR 32 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_LENGTH 2

5
src/main/rx/jetiexbus.c
View file

@ -407,10 +407,8 @@ static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uin
-----------------------------------------------
*/
void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig)
void initJetiExBusTelemetry(void)
{
UNUSED(initialTelemetryConfig);
// Init Ex Bus Frame header
jetiExBusTelemetryFrame[EXBUS_HEADER_SYNC] = 0x3B; // Startbytes
jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01;
@ -427,7 +425,6 @@ void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig)
jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00
}
void createExTelemetrieTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor)
{
uint8_t labelLength = strlen(sensor->label);

47
src/main/rx/rx.c
View file

@ -30,6 +30,8 @@
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/adc.h"
#include "drivers/rx_pwm.h"
@ -90,7 +92,6 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
rxRuntimeConfig_t rxRuntimeConfig;
static const rxConfig_t *rxConfig;
static uint8_t rcSampleIndex = 0;
static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
@ -108,7 +109,7 @@ static uint8_t nullFrameStatus(void)
void useRxConfig(const rxConfig_t *rxConfigToUse)
{
rxConfig = rxConfigToUse;
(void)(rxConfigToUse);
}
#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
@ -126,8 +127,8 @@ STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void)
STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration)
{
return pulseDuration >= rxConfig->rx_min_usec &&
pulseDuration <= rxConfig->rx_max_usec;
return pulseDuration >= rxConfig()->rx_min_usec &&
pulseDuration <= rxConfig()->rx_max_usec;
}
// pulse duration is in micro seconds (usec)
@ -204,20 +205,20 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
}
#endif
void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions)
void rxInit(const rxConfig_t *initialRxConfig, const modeActivationCondition_t *modeActivationConditions)
{
useRxConfig(rxConfig);
useRxConfig(initialRxConfig);
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
rcSampleIndex = 0;
needRxSignalMaxDelayUs = DELAY_10_HZ;
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcData[i] = rxConfig->midrc;
rcData[i] = rxConfig()->midrc;
rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
}
rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec;
rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
// Initialize ARM switch to OFF position when arming via switch is defined
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
@ -237,7 +238,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct
#ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) {
const bool enabled = serialRxInit(rxConfig, &rxRuntimeConfig);
const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig);
if (!enabled) {
featureClear(FEATURE_RX_SERIAL);
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
@ -248,14 +249,14 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct
#ifdef USE_RX_MSP
if (feature(FEATURE_RX_MSP)) {
rxMspInit(rxConfig, &rxRuntimeConfig);
rxMspInit(rxConfig(), &rxRuntimeConfig);
needRxSignalMaxDelayUs = DELAY_5_HZ;
}
#endif
#ifdef USE_RX_SPI
if (feature(FEATURE_RX_SPI)) {
const bool enabled = rxSpiInit(rxConfig, &rxRuntimeConfig);
const bool enabled = rxSpiInit(rxConfig(), &rxRuntimeConfig);
if (!enabled) {
featureClear(FEATURE_RX_SPI);
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
@ -266,7 +267,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct
#if defined(USE_PWM) || defined(USE_PPM)
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxPwmInit(rxConfig, &rxRuntimeConfig);
rxPwmInit(rxConfig(), &rxRuntimeConfig);
}
#endif
}
@ -376,7 +377,7 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample)
static uint16_t getRxfailValue(uint8_t channel)
{
const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel];
const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig()->failsafe_channel_configurations[channel];
switch(channelFailsafeConfiguration->mode) {
case RX_FAILSAFE_MODE_AUTO:
@ -384,12 +385,12 @@ static uint16_t getRxfailValue(uint8_t channel)
case ROLL:
case PITCH:
case YAW:
return rxConfig->midrc;
return rxConfig()->midrc;
case THROTTLE:
if (feature(FEATURE_3D))
return rxConfig->midrc;
return rxConfig()->midrc;
else
return rxConfig->rx_min_usec;
return rxConfig()->rx_min_usec;
}
/* no break */
@ -420,7 +421,7 @@ static uint8_t getRxChannelCount(void) {
static uint8_t maxChannelsAllowed;
if (!maxChannelsAllowed) {
uint8_t maxChannels = rxConfig->max_aux_channel + NON_AUX_CHANNEL_COUNT;
uint8_t maxChannels = rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT;
if (maxChannels > rxRuntimeConfig.channelCount) {
maxChannelsAllowed = rxRuntimeConfig.channelCount;
} else {
@ -436,14 +437,14 @@ static void readRxChannelsApplyRanges(void)
const int channelCount = getRxChannelCount();
for (int channel = 0; channel < channelCount; channel++) {
const uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
// sample the channel
uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel);
// apply the rx calibration
if (channel < NON_AUX_CHANNEL_COUNT) {
sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[channel]);
sample = applyRxChannelRangeConfiguraton(sample, rxConfig()->channelRanges[channel]);
}
rcRaw[channel] = sample;
@ -548,10 +549,10 @@ static void updateRSSIPWM(void)
{
int16_t pwmRssi = 0;
// Read value of AUX channel as rssi
pwmRssi = rcData[rxConfig->rssi_channel - 1];
pwmRssi = rcData[rxConfig()->rssi_channel - 1];
// RSSI_Invert option
if (rxConfig->rssi_ppm_invert) {
if (rxConfig()->rssi_ppm_invert) {
pwmRssi = ((2000 - pwmRssi) + 1000);
}
@ -578,7 +579,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs)
int16_t adcRssiMean = 0;
uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
uint8_t rssiPercentage = adcRssiSample / rxConfig->rssi_scale;
uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale;
adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT;
@ -599,7 +600,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs)
void updateRSSI(timeUs_t currentTimeUs)
{
if (rxConfig->rssi_channel > 0) {
if (rxConfig()->rssi_channel > 0) {
updateRSSIPWM();
} else if (feature(FEATURE_RSSI_ADC)) {
updateRSSIADC(currentTimeUs);

9
src/main/rx/rx.h
View file

@ -18,6 +18,7 @@
#pragma once
#include "common/time.h"
#include "config/parameter_group.h"
#define STICK_CHANNEL_COUNT 4
@ -105,11 +106,17 @@ typedef struct rxFailsafeChannelConfiguration_s {
uint8_t step;
} rxFailsafeChannelConfiguration_t;
//!!TODO change to rxFailsafeChannelConfig_t
PG_DECLARE_ARRAY(rxFailsafeChannelConfiguration_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs);
typedef struct rxChannelRangeConfiguration_s {
uint16_t min;
uint16_t max;
} rxChannelRangeConfiguration_t;
//!!TODO change to rxChannelRangeConfig_t
PG_DECLARE_ARRAY(rxChannelRangeConfiguration_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs);
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.
@ -139,6 +146,8 @@ typedef struct rxConfig_s {
rxChannelRangeConfiguration_t channelRanges[NON_AUX_CHANNEL_COUNT];
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
struct rxRuntimeConfig_s;

7
src/main/sensors/acceleration.c
View file

@ -27,6 +27,9 @@
#include "common/axis.h"
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/accgyro.h"
#include "drivers/accgyro_adxl345.h"
#include "drivers/accgyro_bma280.h"
@ -243,13 +246,13 @@ retry:
return true;
}
bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval)
bool accInit(uint32_t gyroSamplingInverval)
{
memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings
acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration;
acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult;
if (!accDetect(&acc.dev, accelerometerConfig->acc_hardware)) {
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
return false;
}
acc.dev.acc_1G = 256; // set default

5
src/main/sensors/acceleration.h
View file

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
@ -66,7 +67,9 @@ typedef struct accelerometerConfig_s {
rollAndPitchTrims_t accelerometerTrims;
} accelerometerConfig_t;
bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime);
PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
bool accInit(uint32_t gyroTargetLooptime);
bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);

16
src/main/sensors/barometer.c
View file

@ -23,6 +23,9 @@
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/barometer.h"
#include "drivers/barometer_bmp085.h"
#include "drivers/barometer_bmp280.h"
@ -51,8 +54,6 @@ static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
static uint32_t baroPressureSum = 0;
static const barometerConfig_t *barometerConfig;
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
{
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
@ -119,11 +120,6 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
return true;
}
void useBarometerConfig(const barometerConfig_t *barometerConfigToUse)
{
barometerConfig = barometerConfigToUse;
}
bool isBaroCalibrationComplete(void)
{
return calibratingB == 0;
@ -160,7 +156,7 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
return newPressureReading;
}
#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1)
#define PRESSURE_SAMPLE_COUNT (barometerConfig()->baro_sample_count - 1)
static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading)
{
@ -213,7 +209,7 @@ uint32_t baroUpdate(void)
baro.dev.get_up();
baro.dev.start_ut();
baro.dev.calculate(&baroPressure, &baroTemperature);
baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure);
state = BAROMETER_NEEDS_SAMPLES;
return baro.dev.ut_delay;
break;
@ -228,7 +224,7 @@ int32_t baroCalculateAltitude(void)
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp -= baroGroundAltitude;
baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise
baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise
return baro.BaroAlt;
}

4
src/main/sensors/barometer.h
View file

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/barometer.h"
typedef enum {
@ -37,6 +38,8 @@ typedef struct barometerConfig_s {
float baro_cf_alt; // apply CF to use ACC for height estimation
} barometerConfig_t;
PG_DECLARE(barometerConfig_t, barometerConfig);
typedef struct baro_s {
baroDev_t dev;
int32_t BaroAlt;
@ -46,7 +49,6 @@ typedef struct baro_s {
extern baro_t baro;
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse);
void useBarometerConfig(const barometerConfig_t *barometerConfigToUse);
bool isBaroCalibrationComplete(void);
void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
uint32_t baroUpdate(void);

72
src/main/sensors/battery.c
View file

@ -22,26 +22,23 @@
#include "build/debug.h"
#include "common/maths.h"
#include "common/filter.h"
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/adc.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "config/feature.h"
#include "io/beeper.h"
#include "sensors/battery.h"
#include "sensors/esc_sensor.h"
#include "fc/rc_controls.h"
#include "io/beeper.h"
#include "rx/rx.h"
#include "common/utils.h"
#define VBAT_LPF_FREQ 0.4f
#define IBAT_LPF_FREQ 0.4f
@ -61,7 +58,7 @@ uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered)
uint16_t vbatLatest = 0; // most recent unsmoothed value
int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A)
int32_t amperageLatest = 0; // most recent value
int32_t amperageLatest = 0; // most recent value
int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start
@ -72,7 +69,7 @@ static uint16_t batteryAdcToVoltage(uint16_t src)
{
// calculate battery voltage based on ADC reading
// result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier);
return ((((uint32_t)src * batteryConfig()->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig()->vbatresdivval))/batteryConfig()->vbatresdivmultiplier);
}
static void updateBatteryVoltage(void)
@ -86,7 +83,7 @@ static void updateBatteryVoltage(void)
}
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
if (feature(FEATURE_ESC_SENSOR) && batteryConfig()->batteryMeterType == BATTERY_SENSOR_ESC) {
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0;
if (debugMode == DEBUG_BATTERY) {
@ -134,20 +131,20 @@ void updateBattery(void)
uint16_t vBatMeasured = vbatLatest;
/* battery has just been connected*/
if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
vBatState = BATTERY_OK;
unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1;
unsigned cells = (vBatMeasured / batteryConfig()->vbatmaxcellvoltage) + 1;
if (cells > 8) {
// something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
cells = 8;
}
batteryCellCount = cells;
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */
} else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) {
batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage;
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */
} else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) {
vBatState = BATTERY_NOT_PRESENT;
batteryCellCount = 0;
batteryWarningVoltage = 0;
@ -159,16 +156,16 @@ void updateBattery(void)
debug[3] = batteryCellCount;
}
if (batteryConfig->useVBatAlerts) {
if (batteryConfig()->useVBatAlerts) {
switch(vBatState) {
case BATTERY_OK:
if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) {
if (vbat <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
vBatState = BATTERY_WARNING;
}
break;
case BATTERY_WARNING:
if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) {
if (vbat <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) {
vBatState = BATTERY_CRITICAL;
} else if (vbat > batteryWarningVoltage) {
vBatState = BATTERY_OK;
@ -206,9 +203,8 @@ const char * getBatteryStateString(void)
return batteryStateStrings[getBatteryState()];
}
void batteryInit(batteryConfig_t *initialBatteryConfig)
void batteryInit(void)
{
batteryConfig = initialBatteryConfig;
vBatState = BATTERY_NOT_PRESENT;
consumptionState = BATTERY_OK;
batteryCellCount = 0;
@ -221,9 +217,9 @@ static int32_t currentSensorToCentiamps(uint16_t src)
int32_t millivolts;
millivolts = ((uint32_t)src * ADCVREF) / 4096;
millivolts -= batteryConfig->currentMeterOffset;
millivolts -= batteryConfig()->currentMeterOffset;
return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps
return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps
}
static void updateBatteryCurrent(void)
@ -251,10 +247,10 @@ static void updateCurrentDrawn(int32_t lastUpdateAt)
void updateConsumptionWarning(void)
{
if (batteryConfig->useConsumptionAlerts && batteryConfig->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) {
if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) {
if (calculateBatteryPercentage() == 0) {
vBatState = BATTERY_CRITICAL;
} else if (calculateBatteryPercentage() <= batteryConfig->consumptionWarningPercentage) {
} else if (calculateBatteryPercentage() <= batteryConfig()->consumptionWarningPercentage) {
consumptionState = BATTERY_WARNING;
} else {
consumptionState = BATTERY_OK;
@ -264,35 +260,29 @@ void updateConsumptionWarning(void)
}
}
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
void updateCurrentMeter(int32_t lastUpdateAt)
{
if (getBatteryState() != BATTERY_NOT_PRESENT) {
switch(batteryConfig->currentMeterType) {
switch(batteryConfig()->currentMeterType) {
case CURRENT_SENSOR_ADC:
updateBatteryCurrent();
updateCurrentDrawn(lastUpdateAt);
updateConsumptionWarning();
break;
case CURRENT_SENSOR_VIRTUAL:
amperageLatest = (int32_t)batteryConfig->currentMeterOffset;
amperageLatest = (int32_t)batteryConfig()->currentMeterOffset;
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
throttleStatus_e throttleStatus = calculateThrottleStatus();
int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
throttleOffset = 0;
}
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000;
amperageLatest += throttleFactor * (int32_t)batteryConfig()->currentMeterScale / 1000;
}
amperage = amperageLatest;
updateCurrentDrawn(lastUpdateAt);
updateConsumptionWarning();
break;
case CURRENT_SENSOR_ESC:
#ifdef USE_ESC_SENSOR
@ -328,7 +318,7 @@ float calculateVbatPidCompensation(void) {
float batteryScaler = 1.0f;
if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
// Up to 33% PID gain. Should be fine for 4,2to 3,3 difference
batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f);
batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f);
}
return batteryScaler;
}
@ -337,11 +327,11 @@ uint8_t calculateBatteryPercentage(void)
{
uint8_t batteryPercentage = 0;
if (batteryCellCount > 0) {
uint16_t batteryCapacity = batteryConfig->batteryCapacity;
uint16_t batteryCapacity = batteryConfig()->batteryCapacity;
if (batteryCapacity > 0) {
batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100);
} else {
batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100);
batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
}
}

9
src/main/sensors/battery.h
View file

@ -17,7 +17,7 @@
#pragma once
#include "common/maths.h" // for fix12_t
#include "config/parameter_group.h"
#ifndef VBAT_SCALE_DEFAULT
#define VBAT_SCALE_DEFAULT 110
@ -63,6 +63,8 @@ typedef struct batteryConfig_s {
uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning
} batteryConfig_t;
PG_DECLARE(batteryConfig_t, batteryConfig);
typedef enum {
BATTERY_OK = 0,
BATTERY_WARNING,
@ -81,11 +83,10 @@ extern int32_t mAhDrawn;
batteryState_e getBatteryState(void);
const char * getBatteryStateString(void);
void updateBattery(void);
void batteryInit(batteryConfig_t *initialBatteryConfig);
batteryConfig_t *batteryConfig;
void batteryInit(void);
struct rxConfig_s;
void updateCurrentMeter(int32_t lastUpdateAt, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void updateCurrentMeter(int32_t lastUpdateAt);
int32_t currentMeterToCentiamps(uint16_t src);
float calculateVbatPidCompensation(void);

Some files were not shown because too many files have changed in this diff Show more