Merge branch 'master' into patch_v3.1.6

Makefile

@@ -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 \
@@ -594,6 +595,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 \
@@ -634,10 +636,6 @@ COMMON_SRC = \
             sensors/compass.c \
             sensors/gyro.c \
             sensors/initialisation.c \
-            $(CMSIS_SRC) \
-            $(DEVICE_STDPERIPH_SRC)
-
-HIGHEND_SRC = \
             blackbox/blackbox.c \
             blackbox/blackbox_io.c \
             cms/cms.c \
@@ -649,13 +647,13 @@ 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 \
             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 \
@@ -677,7 +675,10 @@ HIGHEND_SRC = \
             sensors/esc_sensor.c \
             io/vtx_string.c \
             io/vtx_smartaudio.c \
-            io/vtx_tramp.c
+            io/vtx_tramp.c \
+            $(CMSIS_SRC) \
+            $(DEVICE_STDPERIPH_SRC)
+
 
 SPEED_OPTIMISED_SRC := ""
 SIZE_OPTIMISED_SRC  := ""
@@ -707,25 +708,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 +742,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 \
@@ -888,13 +871,8 @@ TARGET_SRC += \
             io/flashfs.c
 endif
 
-ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS) $(F4_TARGETS) $(F3_TARGETS)))
-TARGET_SRC += $(HIGHEND_SRC)
-else ifneq ($(filter HIGHEND,$(FEATURES)),)
-TARGET_SRC += $(HIGHEND_SRC)
-endif
-
 TARGET_SRC += $(COMMON_SRC)
+
 #excludes
 ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS)))
 TARGET_SRC   := $(filter-out ${F7EXCLUDES}, $(TARGET_SRC))

reference/spektrum/spektrumTelemetrySensors.h

@@ -1,822 +0,0 @@
-//////////////////////////////////////////////////////////////////
-//
-//	Copyright 2013 by Horizon Hobby, Inc.  
-//	Released to Public Domain
-//
-//	This header file may be incorporated into non-Horizon
-//	products.
-//
-//////////////////////////////////////////////////////////////////
-
-#ifndef TELEMETRY_H
-#define	TELEMETRY_H
-
-//////////////////////////////////////////////////////////////////
-//
-//	Assigned I2C Addresses and Device Types
-//
-//////////////////////////////////////////////////////////////////
-
-#define	TELE_DEVICE_NODATA	(0x00)	// No data in packet
-#define	TELE_DEVICE_VOLTAGE	(0x01)	// High-Voltage sensor (INTERNAL)
-#define	TELE_DEVICE_TEMPERATURE	(0x02)	// Temperature Sensor (INTERNAL)
-#define	TELE_DEVICE_RSV_03	(0x03)	// Reserved
-#define	TELE_DEVICE_RSV_04	(0x04)	// Reserved
-#define	TELE_DEVICE_RSV_05	(0x05)	// Reserved
-#define	TELE_DEVICE_RSV_06	(0x06)	// Reserved
-#define	TELE_DEVICE_RSV_07	(0x07)	// Reserved
-#define	TELE_DEVICE_RSV_08	(0x08)	// Reserved
-#define	TELE_DEVICE_RSV_09	(0x09)	// Reserved
-#define	TELE_DEVICE_PBOX	(0x0A)	// PowerBox
-#define	TELE_DEVICE_LAPTIMER	(0x0B)	// Lap Timer
-#define	TELE_DEVICE_TEXTGEN	(0x0C)	// Text Generator
-#define	TELE_DEVICE_AIRSPEED	(0x11)	// Air Speed
-#define	TELE_DEVICE_ALTITUDE	(0x12)	// Altitude
-#define	TELE_DEVICE_GMETER	(0x14)	// GForce
-#define	TELE_DEVICE_JETCAT	(0x15)	// JetCat interface
-#define	TELE_DEVICE_GPS_LOC	(0x16)	// GPS Location Data
-#define	TELE_DEVICE_GPS_STATS	(0x17)	// GPS Status
-#define	TELE_DEVICE_RX_MAH	(0x18)	// Receiver Pack Capacity (Dual)
-#define	TELE_DEVICE_JETCAT_2	(0x19)	// JetCat interface, msg 2
-#define	TELE_DEVICE_GYRO	(0x1A)	// 3-axis gyro
-#define	TELE_DEVICE_ATTMAG	(0x1B)	// Attitude and Magnetic Compass
-#define	TELE_DEVICE_AS3X_LEGACYGAIN	(0x1F)	// Active AS3X Gains for legacy mode
-#define	TELE_DEVICE_ESC	(0x20)	// ESC
-#define	TELE_DEVICE_FUEL	(0x22)	// Fuel Flow Meter
-#define	TELE_DEVICE_ALPHA6	(0x24)	// Alpha6 Stabilizer
-//	DO NOT USE	(0x30)	// Reserved for internal use
-//	DO NOT USE	(0x32)	// Reserved for internal use
-#define	TELE_DEVICE_MAH	(0x34)	// Battery Gauge (mAh) (Dual)
-#define	TELE_DEVICE_DIGITAL_AIR	(0x36)	// Digital Inputs & Tank Pressure
-#define	TELE_DEVICE_STRAIN	(0x38)	// Thrust/Strain Gauge
-#define	TELE_DEVICE_LIPOMON	(0x3A)	// 6S Cell Monitor (LiPo taps)
-#define	TELE_DEVICE_LIPOMON_14	(0x3F)	// 14S Cell Monitor (LiPo taps)
-#define	TELE_DEVICE_VARIO_S	(0x40)	// Vario
-#define	TELE_DEVICE_RSV_43	(0x43)	// Reserved
-#define	TELE_DEVICE_USER_16SU	(0x50)	// User-Def, STRU_TELE_USER_16SU
-#define	TELE_DEVICE_USER_16SU32U	(0x52)	// User-Def, STRU_TELE_USER_16SU32U
-#define	TELE_DEVICE_USER_16SU32S	(0x54)	// User-Def, STRU_TELE_USER_16SU32S
-#define	TELE_DEVICE_USER_16U32SU	(0x56)	// User-Def, STRU_TELE_USER_16U32SU
-#define	TELE_DEVICE_RSV_60	(0x60)	// Reserved
-#define	TELE_DEVICE_RSV_68	(0x68)	// Reserved
-#define	TELE_DEVICE_RSV_69	(0x69)	// Reserved
-#define	TELE_DEVICE_RSV_6A	(0x6A)	// Reserved
-#define	TELE_DEVICE_RSV_6B	(0x6B)	// Reserved
-#define	TELE_DEVICE_RSV_6C	(0x6C)	// Reserved
-#define	TELE_DEVICE_RSV_6D	(0x6D)	// Reserved
-#define	TELE_DEVICE_RSV_6E	(0x6E)	// Reserved
-#define	TELE_DEVICE_RSV_6F	(0x6F)	// Reserved
-#define	TELE_DEVICE_RSV_70	(0x70)	// Reserved
-#define	TELE_DEVICE_RTC	(0x7C)	// Pseudo-device giving timestamp
-#define	TELE_DEVICE_FRAMEDATA	(0x7D)	// Transmitter frame data
-#define	TELE_DEVICE_RPM	(0x7E)	// RPM sensor
-#define	TELE_DEVICE_QOS	(0x7F)	// RxV + flight log data
-#define	TELE_DEVICE_MAX	(0x7F)	// Last address available
-
-#define	TELE_DEVICE_SHORTRANGE	(0x80)	// Data is from a TM1100
-
-
-//////////////////////////////////////////////////////////////////
-//
-//	Message Data Structures
-//
-//////////////////////////////////////////////////////////////////
-
-
-//////////////////////////////////////////////////////////////////
-//
-//		Electronic Speed Control
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;	// Source device = 0x20
-	UINT8	sID;	// Secondary ID
-	UINT16	RPM;	// RPM, 10RPM (0-655340 RPM).     0xFFFF --> "No data"
-	UINT16	voltsInput;	// Volts, 0.01v (0-655.34V).      0xFFFF --> "No data"
-	UINT16	tempFET;	// Temperature, 0.1C (0-999.8C)   0xFFFF --> "No data"
-	UINT16	currentMotor;	// Current, 10mA (0-655.34A).     0xFFFF --> "No data"
-	UINT16	tempBEC;	// Temperature, 0.1C (0-999.8C)   0x7FFF --> "No data"
-	UINT8	currentBEC;	// BEC Current, 100mA (0-25.4A).  0xFF ----> "No data"
-	UINT8	voltsBEC;	// BEC Volts, 0.05V (0-12.70V).   0xFF ----> "No data"
-	UINT8	throttle;	// 0.5% (0-127%).                 0xFF ----> "No data"
-	UINT8	powerOut;	// Power Output, 0.5% (0-127%).   0xFF ----> "No data"} STRU_TELE_ESC;
-
-//////////////////////////////////////////////////////////////////
-//
-//		LAP TIMER
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;	// Source device = 0x0B
-	UINT8	sID;	// Secondary ID
-	UINT8	lapNumber;	// Lap last finished
-	UINT8	gateNumber;	// Last gate passed
-	UINT32	lastLapTime;	// Time of lap in 1ms increments (NOT duration)
-	UINT32	gateTime;	// Duration between last 2 gates
-	UINT8	unused[4];
-} STRU_TELE_LAPTIMER;
-
-//////////////////////////////////////////////////////////////////
-//
-//		TEXT GENERATOR
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;	// Source device = 0x0C
-	UINT8	sID;	// Secondary ID
-	UINT8	lineNumber;	// Line number to display (0 = title, 1-8 for general, 
-				//	255 = Erase all text on screen)
-	char	text[13];	// 0-terminated text
-} STRU_TELE_TEXTGEN;
-
-//////////////////////////////////////////////////////////////////
-//
-//		(Liquid) Fuel Flow/Capacity (Two Tanks/Engines)
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x22
-	UINT8	sID;			// Secondary ID
-	UINT16	fuelConsumed_A;			// Integrated fuel consumption, 0.1mL
-	UINT16	flowRate_A;			// Instantaneous consumption, 0.01mL/min
-	UINT16	temp_A;			// Temperature, 0.1C (0-655.34C)
-	UINT16	fuelConsumed_B;			// Integrated fuel consumption, 0.1mL
-	UINT16	flowRate_B;			// Instantaneous consumption, 0.01mL/min
-	UINT16	temp_B;			// Temperature, 0.1C (0-655.34C)
-	UINT16	spare;			// Not used
-} STRU_TELE_FUEL;
-
-//////////////////////////////////////////////////////////////////
-//
-//		Battery Current/Capacity (Dual Batteries)
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x34
-	UINT8	sID;			// Secondary ID
-	INT16	current_A;			// Instantaneous current, 0.1A (0-3276.8A)
-	INT16	chargeUsed_A;			// Integrated mAh used, 1mAh (0-32.766Ah)
-	UINT16	temp_A;			// Temperature, 0.1C (0-150.0C, 
-						// 0x7FFF indicates not populated)
-	INT16	current_B;			// Instantaneous current, 0.1A (0-6553.4A)
-	INT16	chargeUsed_B;			// Integrated mAh used, 1mAh (0-65.534Ah)
-	UINT16	temp_B;			// Temperature, 0.1C (0-150.0C, 
-						// 0x7FFF indicates not populated)
-	UINT16	spare;			// Not used
-} STRU_TELE_MAH;
-
-//////////////////////////////////////////////////////////////////
-//
-//		Digital Input Status (Retract Status) and Tank Pressure
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x36
-	UINT8	sID;			// Secondary ID
-	UINT16	digital;			// Digital inputs (bit per input)
-	UINT16	pressure;			// Tank pressure, 0.1PSI (0-6553.4PSI)
-} STRU_TELE_DIGITAL_AIR;
-
-//////////////////////////////////////////////////////////////////
-//
-//		Thrust/Strain Gauge
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x38
-	UINT8	sID;			// Secondary ID
-	UINT16	strain_A,			// Strain sensor A
-			strain_B,			// Strain sensor B
-			strain_C,			// Strain sensor D
-			strain_D;			// Strain sensor C
-} STRU_TELE_STRAIN;
-
-//////////////////////////////////////////////////////////////////
-//
-//		THIRD-PARTY 16-BIT DATA SIGNED/UNSIGNED
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x50
-	UINT8	sID;			// Secondary ID
-	INT16	sField1,			// Signed 16-bit data fields
-			sField2,
-			sField3;
-	UINT16	uField1, 			// Unsigned 16-bit data fields
-			uField2,
-			uField3,
-			uField4;
-} STRU_TELE_USER_16SU;
-
-//////////////////////////////////////////////////////////////////
-//
-//		THIRD-PARTY 16-BIT SIGNED/UNSIGNED AND 32-BIT UNSIGNED
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x52
-	UINT8	sID;			// Secondary ID
-	INT16	sField1,			// Signed 16-bit data fields
-			sField2;
-	UINT16	uField1, 			// Unsigned 16-bit data fields
-			uField2,
-			uField3;
-	UINT32	u32Field; 			// Unsigned 32-bit data field
-} STRU_TELE_USER_16SU32U;
-
-//////////////////////////////////////////////////////////////////
-//
-//		THIRD-PARTY 16-BIT SIGNED/UNSIGNED AND 32-BIT SIGNED
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x54
-	UINT8	sID;			// Secondary ID
-	INT16	sField1,			// Signed 16-bit data fields
-			sField2;
-	UINT16	uField1, 			// Unsigned 16-bit data fields
-			uField2,
-			uField3;
-	INT32	u32Field; 			// Signed 32-bit data field
-} STRU_TELE_USER_16U32SU;
-
-//////////////////////////////////////////////////////////////////
-//
-//		THIRD-PARTY 16-BIT UNSIGNED AND 32-BIT SIGNED/UNSIGNED
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x56
-	UINT8	sID;			// Secondary ID
-	UINT16	uField1; 			// Unsigned 16-bit data field
-	INT32	u32Field; 			// Signed 32-bit data field
-	INT32	u32Field1, 			// Signed 32-bit data fields
-			u32Field2;
- } STRU_TELE_USER_16U32SU;
-
-//////////////////////////////////////////////////////////////////
-//
-//		POWERBOX
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x0A
-	UINT8	sID;			// Secondary ID
-	UINT16	volt1;			// Volts, 0v01v
-	UINT16	volt2;			// Volts, 0.01v
-	UINT16	capacity1;			// mAh, 1mAh
-	UINT16	capacity2;			// mAh, 1mAh
-	UINT16	spare16_1;
-	UINT16	spare16_2;
-	UINT8	spare;
-	UINT8	alarms;			// Alarm bitmask (see below)
-} STRU_TELE_POWERBOX;
-
-#define	TELE_PBOX_ALARM_VOLTAGE_1		(0x01)
-#define	TELE_PBOX_ALARM_VOLTAGE_2		(0x02)
-#define	TELE_PBOX_ALARM_CAPACITY_1	(0x04)
-#define	TELE_PBOX_ALARM_CAPACITY_2	(0x08)
-//#define	TELE_PBOX_ALARM_RPM		(0x10)
-//#define	TELE_PBOX_ALARM_TEMPERATURE	(0x20)
-#define	TELE_PBOX_ALARM_RESERVED_1	(0x40)
-#define	TELE_PBOX_ALARM_RESERVED_2	(0x80)
-
-//////////////////////////////////////////////////////////////////
-//
-//		DUAL ENERGY
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	id;			// Source device = 0x18
-	UINT8	sID;			// Secondary ID
-	INT16	current_A;			// Instantaneous current, 0.01A (0-328.7A)
-	INT16	chargeUsed_A;			// Integrated mAh used, 0.1mAh (0-3276.6mAh)
-	UINT16	volts_A;			// Voltage, 0.01VC (0-16.00V)
-	INT16	current_B;			// Instantaneous current, 0.1A (0-3276.8A)
-	INT16	chargeUsed_B;			// Integrated mAh used, 1mAh (0-32.766Ah)
-	UINT16	volts_B;			// Voltage, 0.01VC (0-16.00V)
-	UINT16	spare;			// Not used
-} STRU_TELE_ENERGY_DUAL;
-
-//////////////////////////////////////////////////////////////////
-//
-//		HIGH-CURRENT
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x03
-	UINT8	sID;			// Secondary ID
-	INT16	current,			// Range: +/- 150A     
-						// Resolution: 300A/2048 = 0.196791 A/tick
-			dummy;			// TBD
-} STRU_TELE_IHIGH;
-
-#define	IHIGH_RESOLUTION_FACTOR		((FP32)(0.196791))
-
-//////////////////////////////////////////////////////////////////
-//
-//		VARIO-S
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x40
-	UINT8	sID;			// Secondary ID
-	INT16	altitude;			// .1m increments
-	INT16	delta_0250ms,			// delta last 250ms, 0.1m/s increments
-			delta_0500ms,			// delta last 500ms, 0.1m/s increments
-			delta_1000ms,			// delta last 1.0 seconds
-			delta_1500ms,			// delta last 1.5 seconds
-			delta_2000ms,			// delta last 2.0 seconds
-			delta_3000ms;			// delta last 3.0 seconds
-} STRU_TELE_VARIO_S;
-
-//////////////////////////////////////////////////////////////////
-//
-//		ALTIMETER
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;
-	UINT8	sID;			// Secondary ID
-	INT16	altitude;			// .1m increments
-	INT16	maxAltitude;			// .1m increments
-} STRU_TELE_ALT;
-
-//////////////////////////////////////////////////////////////////
-//
-//		AIRSPEED
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;
-	UINT8	sID;			// Secondary ID
-	UINT16	airspeed;			// 1 km/h increments
-	UINT16	maxAirspeed;			// 1 km/h increments
-} STRU_TELE_SPEED;
-
-//////////////////////////////////////////////////////////////////
-//
-//		GFORCE
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x14
-	UINT8	sID;			// Secondary ID
-	INT16	GForceX;			// force is reported as .01G increments
-	INT16	GForceY;			// Range = +/-4000 (+/- 40G) in Pro model
-	INT16	GForceZ;			// Range = +/-800 (+/- 8G) in Standard model
-	INT16	maxGForceX;			// abs(max G X-axis)   FORE/AFT
-	INT16	maxGForceY;			// abs (max G Y-axis)  LEFT/RIGHT
-	INT16	maxGForceZ;			// max G Z-axis        WING SPAR LOAD
-	INT16	minGForceZ;			// min G Z-axis        WING SPAR LOAD
-} STRU_TELE_G_METER;
-
-//////////////////////////////////////////////////////////////////
-//
-//		JETCAT/TURBINE
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x15
-	UINT8	sID;			// Secondary ID
-	UINT8	status;			// See table below
-	UINT8	throttle;			// (BCD) xx Percent
-	UINT16	packVoltage;			// (BCD) xx.yy
-	UINT16	pumpVoltage;			// (BCD) xx.yy
-	UINT32	RPM;			// (BCD) 
-	UINT16	EGT;			// (BCD) Temperature, Celsius
-	UINT8	offCondition;			// (BCD) See table below
-	UINT8	spare;
-} STRU_TELE_JETCAT;	
-
-enum JETCAT_ECU_TURBINE_STATE {		// ECU Status definitions
-		JETCAT_ECU_STATE_OFF = 0x00,
-		JETCAT_ECU_STATE_WAIT_for_RPM = 0x01, // (Stby/Start)
-		JETCAT_ECU_STATE_Ignite = 0x02,
-		JETCAT_ECU_STATE_Accelerate = 0x03,
-		JETCAT_ECU_STATE_Stabilise = 0x04,
-		JETCAT_ECU_STATE_Learn_HI = 0x05,
-		JETCAT_ECU_STATE_Learn_LO = 0x06,
-		JETCAT_ECU_STATE_UNDEFINED = 0x07,
-		JETCAT_ECU_STATE_Slow_Down = 0x08,
-		JETCAT_ECU_STATE_Manual = 0x09,
-		JETCAT_ECU_STATE_AutoOff = 0x10,
-		JETCAT_ECU_STATE_Run = 0x11, // (reg.)
-		JETCAT_ECU_STATE_Accleleration_delay = 0x12,
-		JETCAT_ECU_STATE_SpeedReg = 0x13, // (Speed Ctrl)
-		JETCAT_ECU_STATE_Two_Shaft_Regulate = 0x14, // (only for secondary shaft)
-		JETCAT_ECU_STATE_PreHeat1 = 0x15,
-		JETCAT_ECU_STATE_PreHeat2 = 0x16,
-		JETCAT_ECU_STATE_MainFStart = 0x17,
-		JETCAT_ECU_STATE_NotUsed = 0x18,
-		JETCAT_ECU_STATE_KeroFullOn = 0x19,
-		// undefined states 0x1A-0x1F
-		EVOJET_ECU_STATE_off = 0x20,
-		EVOJET_ECU_STATE_ignt = 0x21,
-		EVOJET_ECU_STATE_acce = 0x22,
-		EVOJET_ECU_STATE_run = 0x23,
-		EVOJET_ECU_STATE_cal = 0x24,
-		EVOJET_ECU_STATE_cool = 0x25,
-		EVOJET_ECU_STATE_fire = 0x26,
-		EVOJET_ECU_STATE_glow = 0x27,
-		EVOJET_ECU_STATE_heat = 0x28,
-		EVOJET_ECU_STATE_idle = 0x29,
-		EVOJET_ECU_STATE_lock = 0x2A,
-		EVOJET_ECU_STATE_rel = 0x2B,
-		EVOJET_ECU_STATE_spin = 0x2C,
-		EVOJET_ECU_STATE_stop = 0x2D,
-		// undefined states 0x2E-0x2F
-		HORNET_ECU_STATE_OFF = 0x30,
-		HORNET_ECU_STATE_SLOWDOWN = 0x31,
-		HORNET_ECU_STATE_COOL_DOWN = 0x32,
-		HORNET_ECU_STATE_AUTO = 0x33,
-		HORNET_ECU_STATE_AUTO_HC = 0x34,
-		HORNET_ECU_STATE_BURNER_ON = 0x35,
-		HORNET_ECU_STATE_CAL_IDLE = 0x36,
-		HORNET_ECU_STATE_CALIBRATE = 0x37,
-		HORNET_ECU_STATE_DEV_DELAY = 0x38,
-		HORNET_ECU_STATE_EMERGENCY = 0x39,
-		HORNET_ECU_STATE_FUEL_HEAT = 0x3A,
-		HORNET_ECU_STATE_FUEL_IGNITE = 0x3B,
-		HORNET_ECU_STATE_GO_IDLE = 0x3C,
-		HORNET_ECU_STATE_PROP_IGNITE = 0x3D,
-		HORNET_ECU_STATE_RAMP_DELAY = 0x3E,
-		HORNET_ECU_STATE_RAMP_UP = 0x3F,
-		HORNET_ECU_STATE_STANDBY = 0x40,
-		HORNET_ECU_STATE_STEADY = 0x41,
-		HORNET_ECU_STATE_WAIT_ACC = 0x42,
-		HORNET_ECU_STATE_ERROR = 0x43,
-		// undefined states 0x44-0x4F
-		XICOY_ECU_STATE_Temp_High = 0x50,
-		XICOY_ECU_STATE_Trim_Low = 0x51,
-		XICOY_ECU_STATE_Set_Idle = 0x52,
-		XICOY_ECU_STATE_Ready = 0x53,
-		XICOY_ECU_STATE_Ignition = 0x54,
-		XICOY_ECU_STATE_Fuel_Ramp = 0x55,
-		XICOY_ECU_STATE_Glow_Test = 0x56,
-		XICOY_ECU_STATE_Running = 0x57,
-		XICOY_ECU_STATE_Stop = 0x58,
-		XICOY_ECU_STATE_Flameout = 0x59,
-		XICOY_ECU_STATE_Speed_Low = 0x5A,
-		XICOY_ECU_STATE_Cooling = 0x5B,
-		XICOY_ECU_STATE_Igniter_Bad = 0x5C,
-		XICOY_ECU_STATE_Starter_F = 0x5D,
-		XICOY_ECU_STATE_Weak_Fuel = 0x5E,
-		XICOY_ECU_STATE_Start_On = 0x5F,
-		XICOY_ECU_STATE_Pre_Heat = 0x60,
-		XICOY_ECU_STATE_Battery = 0x61,
-		XICOY_ECU_STATE_Time_Out = 0x62,
-		XICOY_ECU_STATE_Overload = 0x63,
-		XICOY_ECU_STATE_Igniter_Fail = 0x64,
-		XICOY_ECU_STATE_Burner_On = 0x65,
-		XICOY_ECU_STATE_Starting = 0x66,
-		XICOY_ECU_STATE_SwitchOver = 0x67,
-		XICOY_ECU_STATE_Cal_Pump = 0x68,
-		XICOY_ECU_STATE_Pump_Limit = 0x69,
-		XICOY_ECU_STATE_No_Engine = 0x6A,
-		XICOY_ECU_STATE_Pwr_Boost = 0x6B,
-		XICOY_ECU_STATE_Run_Idle = 0x6C,
-		XICOY_ECU_STATE_Run_Max = 0x6D,
-
-		TURBINE_ECU_MAX_STATE = 0x74
-};
-
-enum JETCAT_ECU_OFF_CONDITIONS {		// ECU off conditions. Valid only when the ECUStatus = JETCAT_ECU_STATE_OFF
-		JETCAT_ECU_OFF_No_Off_Condition_defined = 0,
-		JETCAT_ECU_OFF_Shut_down_via_RC,
-		JETCAT_ECU_OFF_Overtemperature,
-		JETCAT_ECU_OFF_Ignition_timeout,
-		JETCAT_ECU_OFF_Acceleration_time_out,
-		JETCAT_ECU_OFF_Acceleration_too_slow,
-		JETCAT_ECU_OFF_Over_RPM,
-		JETCAT_ECU_OFF_Low_Rpm_Off,
-		JETCAT_ECU_OFF_Low_Battery,
-		JETCAT_ECU_OFF_Auto_Off,
-		JETCAT_ECU_OFF_Low_temperature_Off,
-		JETCAT_ECU_OFF_Hi_Temp_Off,
-		JETCAT_ECU_OFF_Glow_Plug_defective,
-		JETCAT_ECU_OFF_Watch_Dog_Timer,
-		JETCAT_ECU_OFF_Fail_Safe_Off,
-		JETCAT_ECU_OFF_Manual_Off, // (via GSU)
-		JETCAT_ECU_OFF_Power_fail, // (Battery fail)
-		JETCAT_ECU_OFF_Temp_Sensor_fail, // (only during startup)
-		JETCAT_ECU_OFF_Fuel_fail,
-		JETCAT_ECU_OFF_Prop_fail,
-		JETCAT_ECU_OFF_2nd_Engine_fail,
-		JETCAT_ECU_OFF_2nd_Engine_Diff_Too_High,
-		JETCAT_ECU_OFF_2nd_Engine_No_Comm,
-		JETCAT_ECU_MAX_OFF_COND
-};
-
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x19
-	UINT8	sID;			// Secondary ID
-	UINT16	FuelFlowRateMLMin;			// (BCD) mL per Minute
-	UINT32	RestFuelVolumeInTankML;		// (BCD) mL remaining in tank
-	// 8 bytes left
-} STRU_TELE_JETCAT2;
-
-//////////////////////////////////////////////////////////////////
-//
-//		GPS
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x16
-	UINT8	sID;			// Secondary ID
-	UINT16	altitudeLow;			// BCD, meters, format 3.1 (Low bits of alt)
-	UINT32	latitude;			// BCD, format 4.4, 
-						// Degrees * 100 + minutes, < 100 degrees
-	UINT32	longitude;			// BCD, format 4.4, 
-						// Degrees * 100 + minutes, flag --> > 99deg
-	UINT16	course;			// BCD, 3.1
-	UINT8	HDOP;			// BCD, format 1.1
-	UINT8	GPSflags;			// see definitions below
-} STRU_TELE_GPS_LOC;
-
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x17
-	UINT8	sID;			// Secondary ID
-	UINT16	speed;			// BCD, knots, format 3.1
-	UINT32	UTC;			// BCD, format HH:MM:SS.S, format 6.1
-	UINT8	numSats;			// BCD, 0-99
-	UINT8	altitudeHigh;			// BCD, meters, format 2.0 (High bits alt)
-} STRU_TELE_GPS_STAT;
-
-// GPS flags definitions:
-#define	GPS_INFO_FLAGS_IS_NORTH_BIT	(0)
-#define	GPS_INFO_FLAGS_IS_NORTH		(1 << GPS_INFO_FLAGS_IS_NORTH_BIT)
-#define	GPS_INFO_FLAGS_IS_EAST_BIT	(1)
-#define	GPS_INFO_FLAGS_IS_EAST		(1 << GPS_INFO_FLAGS_IS_EAST_BIT)
-#define	GPS_INFO_FLAGS_LONG_GREATER_99_BIT	(2)
-#define	GPS_INFO_FLAGS_LONG_GREATER_99 	(1 << GPS_INFO_FLAGS_LONG_GREATER_99_BIT)
-#define	GPS_INFO_FLAGS_GPS_FIX_VALID_BIT	(3)
-#define	GPS_INFO_FLAGS_GPS_FIX_VALID	(1 << GPS_INFO_FLAGS_GPS_FIX_VALID_BIT)
-#define	GPS_INFO_FLAGS_GPS_DATA_RECEIVED_BIT	(4)
-#define	GPS_INFO_FLAGS_GPS_DATA_RECEIVED	(1 << GPS_INFO_FLAGS_GPS_DATA_RECEIVED_BIT)
-#define	GPS_INFO_FLAGS_3D_FIX_BIT		(5)
-#define	GPS_INFO_FLAGS_3D_FIX		(1 << GPS_INFO_FLAGS_3D_FIX_BIT)
-#define	GPS_INFO_FLAGS_NEGATIVE_ALT_BIT	(7)
-#define	GPS_INFO_FLAGS_NEGATIVE_ALT	(1 << GPS_INFO_FLAGS_NEGATIVE_ALT_BIT)
-
-//////////////////////////////////////////////////////////////////
-//
-//		GYRO
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8		identifier;		// Source device = 0x1A
-	UINT8		sID;			// Secondary ID
-	INT16		gyroX;			// Rotation rates of the body - Rate 
-							// is about the X Axis which is
-							// defined out the nose of the
-							// vehicle.
-	INT16		gyroY;			// Units are 0.1 deg/sec  - Rate is
-							// about the Y Axis which is defined
-							// out the right wing of the vehicle.
-	INT16		gyroZ;			// Rate is about the Z axis which is
-							// defined down from the vehicle.
-	INT16		maxGyroX;		// Max rates (absolute value)
-	INT16		maxGyroY;
-	INT16		maxGyroZ;
-} STRU_TELE_GYRO;
-
-//////////////////////////////////////////////////////////////////
-//
-//		Alpha6 Stabilizer
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8		identifier;		// Source device = 0x24
-	UINT8		sID;			// Secondary ID
-	UINT16		volts;			// 0.01V increments
-	UINT8		state_FM;		// Flight Mode and System State 
-							// (see below)
-	UINT8		gainRoll,		// Roll Gain,  high bit --> 
-							//    Heading Hold
-				gainPitch,		// Pitch Gain
-				gainYaw;		// Yaw Gain
-	INT16		attRoll,		// Roll Attitude, 0.1degree, RHR
-				attPitch,		// Pitch Attitude
-				attYaw;		// Yaw Attitude
-	UINT16		spare;
-} STRU_TELE_ALPHA6;
-
-#define	GBOX_STATE_BOOT		(0x00)		// Alpha6 State - Boot
-#define	GBOX_STATE_INIT		(0x01)		// Init
-#define	GBOX_STATE_READY		(0x02)		// Ready
-#define	GBOX_STATE_SENSORFAULT	(0x03)		// Sensor Fault
-#define	GBOX_STATE_POWERFAULT	(0x04)		// Power Fault
-#define	GBOX_STATE_MASK		(0x0F)
-
-#define	GBOX_FMODE_FM0		(0x00)		// FM0 through FM4
-#define	GBOX_FMODE_FM1		(0x10)
-#define	GBOX_FMODE_FM2		(0x20)
-#define	GBOX_FMODE_FM3		(0x30)
-#define	GBOX_FMODE_FM4		(0x40)
-#define	GBOX_FMODE_PANIC		(0x50)
-#define	GBOX_FMODE_MASK		(0xF0)
-
-//////////////////////////////////////////////////////////////////
-//
-//		6S LiPo Cell Monitor
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8		identifier;		// Source device = 0x3A
-	UINT8		sID;			// Secondary ID
-	UINT16		cell[6];		// Voltage across cell 1, .01V steps
-							// 0x7FFF --> cell not present
-	UINT16		temp;			// Temperature, 0.1C (0-655.34C)
-} STRU_TELE_LIPOMON;
-
-//////////////////////////////////////////////////////////////////
-//
-//		14S LiPo Cell Monitor
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8		identifier;		// Source device = 0x3F
-	UINT8		sID;			// Secondary ID
-	UINT8		cell[14];		// Voltage across cell 1, .01V steps,
-							// excess of 2.56V (ie, 3.00V would
-							// report 300-256 = 44)
-							// 0xFF --> cell not present
-} STRU_TELE_LIPOMON_14;
-
-//////////////////////////////////////////////////////////////////
-//
-//		ATTITUDE & MAG COMPASS
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8		identifier;		// Source device = 0x1B
-	UINT8		sID;			// Secondary ID
-	INT16		attRoll;		// Attitude, 3 axes.  Roll is a
-							// rotation about the X Axis of 
-							// the vehicle using the RHR.
-	INT16		attPitch;		// Units are 0.1 deg - Pitch is a
-							// rotation about the Y Axis of the
-							// vehicle using the RHR.
-	INT16		attYaw;		// Yaw is a rotation about the Z 
-							// Axis of the vehicle using the RHR.
-	INT16		magX;			// Magnetic Compass, 3 axes
-	INT16		magY;			// Units are TBD
-	INT16		magZ;			// 
-} STRU_TELE_ATTMAG;
-
-//////////////////////////////////////////////////////////////////
-//
-//				Real-Time Clock
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8		identifier;		// Source device = 0x7C
-	UINT8		sID;			// Secondary ID
-	UINT8		spare[6];
-	UINT64		UTC64;			// Linux 64-bit time_t for 
-							// post-2038 date compatibility
-} STRU_TELE_RTC;
-
-//////////////////////////////////////////////////////////////////
-//
-//		RPM/Volts/Temperature
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x7E
-	UINT8	sID;			// Secondary ID
-	UINT16	microseconds;			// microseconds between pulse leading edges
-	UINT16	volts;			// 0.01V increments
-	INT16	temperature;			// degrees F
-	INT8	dBm_A,			// Average signal for A antenna in dBm
-			dBm_B;			// Average signal for B antenna in dBm.  
-						// If only 1 antenna, set B = A
-} STRU_TELE_RPM;
-
-//////////////////////////////////////////////////////////////////
-//
-//		QoS DATA
-//
-//////////////////////////////////////////////////////////////////
-//
-//	NOTE:  AR6410-series send:
-//			id = 7F
-//			sID = 0
-//			A = 0
-//			B = 0
-//			L = 0
-//			R = 0
-//			F = fades
-//			H = holds
-//			rxV = 0xFFFF
-//
-typedef struct
-{
-	UINT8	identifier;			// Source device = 0x7F
-	UINT8	sID;			// Secondary ID
-	UINT16	A;
-	UINT16	B;
-	UINT16	L;
-	UINT16	R;
-	UINT16	F;
-	UINT16	H;
-	UINT16	rxVoltage;			// Volts, 0.01V increments
-} STRU_TELE_QOS;
-
-//////////////////////////////////////////////////////////////////
-//
-//		UNION OF ALL DEVICE MESSAGES
-//
-//////////////////////////////////////////////////////////////////
-//
-typedef union
-{
-	UINT16		raw[8];
-	STRU_TELE_RTC	rtc;
-	STRU_TELE_QOS	qos;
-	STRU_TELE_RPM	rpm;
-	STRU_TELE_FRAMEDATA	frame;
-	STRU_TELE_ALT	alt;
-	STRU_TELE_SPEED	speed;
-	STRU_TELE_ENERGY_DUAL	eDual;
-	STRU_TELE_VARIO_S	varioS;
-	STRU_TELE_G_METER	accel;
-	STRU_TELE_JETCAT	jetcat;
-	STRU_TELE_JETCAT2	jetcat2;
-	STRU_TELE_GPS_LOC	gpsloc;
-	STRU_TELE_GPS_STAT	gpsstat;
-	STRU_TELE_GYRO	gyro;
-	STRU_TELE_ATTMAG	attMag;
-	STRU_TELE_POWERBOX	powerBox;
-	STRU_TELE_ESC	escGeneric;
-	STRU_TELE_LAPTIMER	lapTimer;
-	STRU_TELE_TEXTGEN	textgen;
-	STRU_TELE_FUEL	fuel;
-	STRU_TELE_MAH	mAh;
-	STRU_TELE_DIGITAL_AIR	digAir;
-	STRU_TELE_STRAIN	strain;
-	STRU_TELE_LIPOMON	lipomon;
-	STRU_TELE_LIPOMON_14	lipomon14;
-	STRU_TELE_USER_16SU	user_16SU;
-	STRU_TELE_USER_16SU32U	user_16SU32U;
-	STRU_TELE_USER_16SU32S	user_16SU32S;
-	STRU_TELE_USER_16U32SU	user_16U32SU;
-} UN_TELEMETRY;			// All telemetry messages
-

src/main/blackbox/blackbox.c

@@ -24,15 +24,22 @@
 
 #ifdef BLACKBOX
 
+#include "blackbox.h"
+#include "blackbox_io.h"
+
 #include "build/debug.h"
 #include "build/version.h"
 
 #include "common/axis.h"
 #include "common/encoding.h"
+#include "common/maths.h"
 #include "common/utils.h"
 
-#include "blackbox.h"
+#include "config/config_master.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 +50,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 +243,22 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
 
 typedef enum BlackboxState {
     BLACKBOX_STATE_DISABLED = 0,
-    BLACKBOX_STATE_STOPPED,
+    BLACKBOX_STATE_STOPPED, //1
-    BLACKBOX_STATE_PREPARE_LOG_FILE,
+    BLACKBOX_STATE_PREPARE_LOG_FILE, //2
-    BLACKBOX_STATE_SEND_HEADER,
+    BLACKBOX_STATE_SEND_HEADER, //3
-    BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
+    BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4
-    BLACKBOX_STATE_SEND_GPS_H_HEADER,
+    BLACKBOX_STATE_SEND_GPS_H_HEADER, //5
-    BLACKBOX_STATE_SEND_GPS_G_HEADER,
+    BLACKBOX_STATE_SEND_GPS_G_HEADER, //6
-    BLACKBOX_STATE_SEND_SLOW_HEADER,
+    BLACKBOX_STATE_SEND_SLOW_HEADER, //7
-    BLACKBOX_STATE_SEND_SYSINFO,
+    BLACKBOX_STATE_SEND_SYSINFO, //8
-    BLACKBOX_STATE_PAUSED,
+    BLACKBOX_STATE_PAUSED, //9
-    BLACKBOX_STATE_RUNNING,
+    BLACKBOX_STATE_RUNNING, //10
-    BLACKBOX_STATE_SHUTTING_DOWN
+    BLACKBOX_STATE_SHUTTING_DOWN, //11
+    BLACKBOX_STATE_START_ERASE, //12
+    BLACKBOX_STATE_ERASING, //13
+    BLACKBOX_STATE_ERASED //14
 } 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 +773,16 @@ void validateBlackboxConfig()
 
     if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
             || blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) {
-        blackboxConfig()->rate_num = 1;
+        blackboxConfigMutable()->rate_num = 1;
-        blackboxConfig()->rate_denom = 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;
+        blackboxConfigMutable()->rate_num /= div;
-        blackboxConfig()->rate_denom /= div;
+        blackboxConfigMutable()->rate_denom /= div;
     }
 
     // If we've chosen an unsupported device, change the device to serial
@@ -786,7 +798,7 @@ void validateBlackboxConfig()
         break;
 
         default:
-            blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL;
+            blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL;
     }
 }
 
@@ -795,47 +807,44 @@ void validateBlackboxConfig()
  */
 void startBlackbox(void)
 {
-    if (blackboxState == BLACKBOX_STATE_STOPPED) {
+    validateBlackboxConfig();
-        validateBlackboxConfig();
 
-        if (!blackboxDeviceOpen()) {
+    if (!blackboxDeviceOpen()) {
-            blackboxSetState(BLACKBOX_STATE_DISABLED);
+        blackboxSetState(BLACKBOX_STATE_DISABLED);
-            return;
+        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);
     }
+
+    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);
 }
 
 /**
@@ -1175,7 +1184,7 @@ static bool blackboxWriteSysinfo()
         BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s",                    "Cleanflight");
         BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
         BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s",                 buildDate, buildTime);
-        BLACKBOX_PRINT_HEADER_LINE("Craft name:%s",                       masterConfig.name);
+        BLACKBOX_PRINT_HEADER_LINE("Craft name:%s",                       systemConfig()->name);
         BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d",                    blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
         BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d",                      motorConfig()->minthrottle);
         BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d",                      motorConfig()->maxthrottle);
@@ -1283,12 +1292,12 @@ static bool blackboxWriteSysinfo()
         BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d",        rxConfig()->rcInterpolationInterval);
         BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d",        rxConfig()->airModeActivateThreshold);
         BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d",                rxConfig()->serialrx_provider);
-        BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d",                motorConfig()->useUnsyncedPwm);
+        BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d",                motorConfig()->dev.useUnsyncedPwm);
-        BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d",                motorConfig()->motorPwmProtocol);
+        BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d",                motorConfig()->dev.motorPwmProtocol);
-        BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d",                   motorConfig()->motorPwmRate);
+        BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d",                   motorConfig()->dev.motorPwmRate);
         BLACKBOX_PRINT_HEADER_LINE("digitalIdleOffset:%d",          (int)(motorConfig()->digitalIdleOffsetPercent * 100.0f));
-        BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d",                       masterConfig.debug_mode);
+        BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d",                       systemConfig()->debug_mode);
-        BLACKBOX_PRINT_HEADER_LINE("features:%d",                         masterConfig.enabledFeatures);
+        BLACKBOX_PRINT_HEADER_LINE("features:%d",                         featureConfig()->enabledFeatures);
 
         default:
             return true;
@@ -1461,17 +1470,23 @@ void handleBlackbox(timeUs_t currentTimeUs)
 {
     int i;
   
-    if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) {
-        blackboxReplenishHeaderBudget();
-    }
-
     switch (blackboxState) {
+        case BLACKBOX_STATE_STOPPED:
+            if (ARMING_FLAG(ARMED)) {
+                blackboxOpen();  
+                startBlackbox();
+            }
+            if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
+                blackboxSetState(BLACKBOX_STATE_START_ERASE);
+            }
+        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 +1507,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 +1521,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 +1529,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 +1538,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 +1546,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 +1576,6 @@ void handleBlackbox(timeUs_t currentTimeUs)
 
                 blackboxLogIteration(currentTimeUs);
             }
-
             // Keep the logging timers ticking so our log iteration continues to advance
             blackboxAdvanceIterationTimers();
         break;
@@ -1568,7 +1587,6 @@ void handleBlackbox(timeUs_t currentTimeUs)
             } else {
                 blackboxLogIteration(currentTimeUs);
             }
-
             blackboxAdvanceIterationTimers();
         break;
         case BLACKBOX_STATE_SHUTTING_DOWN:
@@ -1585,15 +1603,36 @@ 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_ERASED);
+                beeper(BEEPER_BLACKBOX_ERASE);
+            }
+        break;  
+        case BLACKBOX_STATE_ERASED:
+	        if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
+                blackboxSetState(BLACKBOX_STATE_STOPPED);
+            }
+        break;
         default:
         break;
     }
 
     // Did we run out of room on the device? Stop!
     if (isBlackboxDeviceFull()) {
-        blackboxSetState(BLACKBOX_STATE_STOPPED);
+        if (blackboxState != BLACKBOX_STATE_ERASING 
-        // ensure we reset the test mode flag if we stop due to full memory card
+            && blackboxState != BLACKBOX_STATE_START_ERASE
-        if (startedLoggingInTestMode) startedLoggingInTestMode = false;
+            && blackboxState != BLACKBOX_STATE_ERASED) {
+            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) {

src/main/blackbox/blackbox.h

@@ -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);

src/main/blackbox/blackbox_io.c

@@ -8,14 +8,20 @@
 
 #ifdef BLACKBOX
 
+#include "blackbox.h"
 #include "blackbox_io.h"
 
 #include "build/version.h"
 #include "build/build_config.h"
 
 #include "common/encoding.h"
+#include "common/maths.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 +29,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 +67,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 +611,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.
  */

src/main/blackbox/blackbox_io.h

@@ -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);
 

src/main/build/atomic.h

@@ -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.
+// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier.
-// Be carefull when using this, you must use some method to prevent optimizer form breaking things
+// Be careful 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
+// - lto is used for Cleanflight compilation, so function call is not memory barrier
-// - use ATOMIC_BARRIER or propper volatile to protect used variables
+// - use ATOMIC_BARRIER or 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
+// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
-//    but that can change in future versions
+// - 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 the beginning of containing block (value of parameter 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)
+// - at exit of block (all exit paths) (parameter value if written into memory, 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
+// 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__ > 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

src/main/build/version.h

@@ -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_MINOR            2  // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL      6  // increment when a bug is fixed
+#define FC_VERSION_PATCH_LEVEL      0  // increment when a bug is fixed
 
 #define STR_HELPER(x) #x
 #define STR(x) STR_HELPER(x)

src/main/cms/cms.c

@@ -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 "flight/mixer.h"
-#include "config/config_profile.h"
-#include "config/config_master.h"
-#include "config/feature.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

src/main/cms/cms_menu_blackbox.c

@@ -27,11 +27,12 @@
 
 #include "platform.h"
 
-#include "build/version.h"
-
 #ifdef CMS
 
-#include "drivers/system.h"
+#include "build/version.h"
+
+#include "blackbox/blackbox.h"
+#include "blackbox/blackbox_io.h"
 
 #include "cms/cms.h"
 #include "cms/cms_types.h"
@@ -40,13 +41,16 @@
 #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 "drivers/system.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/flashfs.h"
+#include "io/beeper.h"
 
-#include "blackbox/blackbox_io.h"
 
 #ifdef USE_FLASHFS
 static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
@@ -62,6 +66,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?
 
@@ -78,6 +83,7 @@ static const char * const cmsx_BlackboxDeviceNames[] = {
 static bool featureRead = false;
 
 static uint8_t cmsx_FeatureBlackbox;
+static uint8_t blackboxConfig_rate_denom;
 
 static uint8_t cmsx_BlackboxDevice;
 static OSD_TAB_t cmsx_BlackboxDeviceTable = { &cmsx_BlackboxDevice, 2, cmsx_BlackboxDeviceNames };
@@ -167,7 +173,7 @@ static long cmsx_Blackbox_onEnter(void)
         cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0;
         featureRead = true;
     }
-
+    blackboxConfig_rate_denom = blackboxConfig()->rate_denom;
     return 0;
 }
 
@@ -176,10 +182,10 @@ static long cmsx_Blackbox_onExit(const OSD_Entry *self)
     UNUSED(self);
 
     if (blackboxMayEditConfig()) {
-        blackboxConfig()->device = cmsx_BlackboxDevice;
+        blackboxConfigMutable()->device = cmsx_BlackboxDevice;
         validateBlackboxConfig();
     }
-
+    blackboxConfigMutable()->rate_denom = blackboxConfig_rate_denom;
     return 0;
 }
 
@@ -203,7 +209,7 @@ static OSD_Entry cmsx_menuBlackboxEntries[] =
     { "(STATUS)",    OME_String,  NULL,            &cmsx_BlackboxStatus,                                      0 },
     { "(USED)",      OME_String,  NULL,            &cmsx_BlackboxDeviceStorageUsed,                           0 },
     { "(FREE)",      OME_String,  NULL,            &cmsx_BlackboxDeviceStorageFree,                           0 },
-    { "RATE DENOM",  OME_UINT8,   NULL,            &(OSD_UINT8_t){ &blackboxConfig()->rate_denom, 1, 32, 1 }, 0 },
+    { "RATE DENOM",  OME_UINT8,   NULL,            &(OSD_UINT8_t){ &blackboxConfig_rate_denom, 1, 32, 1 },    0 },
 
 #ifdef USE_FLASHFS
     { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL,                                                      0 },

src/main/cms/cms_menu_imu.c

@@ -43,9 +43,11 @@
 
 #include "flight/pid.h"
 
-#include "config/config_profile.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"
 
 //
 // PID
@@ -104,10 +106,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][0] = pidProfile->P8[i];
-        tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i];
+        tempPid[i][1] = pidProfile->I8[i];
-        tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i];
+        tempPid[i][2] = pidProfile->D8[i];
     }
 
     return 0;
@@ -125,10 +128,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];
+        pidProfile->P8[i] = tempPid[i][0];
-        masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
+        pidProfile->I8[i] = tempPid[i][1];
-        masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
+        pidProfile->D8[i] = tempPid[i][2];
     }
     pidInitConfig(&currentProfile->pidProfile);
 
@@ -233,12 +237,13 @@ static long cmsx_profileOtherOnEnter(void)
 {
     profileIndexString[1] = '0' + tmpProfileIndex;
 
-    cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight;
+    const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
-    cmsx_setpointRelaxRatio  = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio;
+    cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight;
+    cmsx_setpointRelaxRatio  = pidProfile->setpointRelaxRatio;
 
-    cmsx_angleStrength =     masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL];
+    cmsx_angleStrength =     pidProfile->P8[PIDLEVEL];
-    cmsx_horizonStrength =   masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL];
+    cmsx_horizonStrength =   pidProfile->I8[PIDLEVEL];
-    cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL];
+    cmsx_horizonTransition = pidProfile->D8[PIDLEVEL];
 
     return 0;
 }
@@ -247,13 +252,14 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
 {
     UNUSED(self);
 
-    masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
+    pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
-    masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
+    pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight;
+    pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
     pidInitConfig(&currentProfile->pidProfile);
 
-    masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
+    pidProfile->P8[PIDLEVEL] = cmsx_angleStrength;
-    masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;
+    pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength;
-    masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition;
+    pidProfile->D8[PIDLEVEL] = cmsx_horizonTransition;
 
     return 0;
 }
@@ -280,15 +286,45 @@ static CMS_Menu cmsx_menuProfileOther = {
     .entries = cmsx_menuProfileOtherEntries,
 };
 
+static uint8_t gyroConfig_gyro_soft_lpf_hz;
+static uint16_t gyroConfig_gyro_soft_notch_hz_1;
+static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
+static uint16_t gyroConfig_gyro_soft_notch_hz_2;
+static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
+
+static long cmsx_menuGyro_onEnter(void)
+{
+    gyroConfig_gyro_soft_lpf_hz =  gyroConfig()->gyro_soft_lpf_hz;
+    gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
+    gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
+    gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
+    gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
+
+    return 0;
+}
+
+static long cmsx_menuGyro_onExit(const OSD_Entry *self)
+{
+    UNUSED(self);
+
+    gyroConfigMutable()->gyro_soft_lpf_hz =  gyroConfig_gyro_soft_lpf_hz;
+    gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
+    gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
+    gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
+    gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
+
+    return 0;
+}
+
 static OSD_Entry cmsx_menuFilterGlobalEntries[] =
 {
     { "-- FILTER GLB  --", OME_Label, NULL, NULL, 0 },
 
-    { "GYRO LPF",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &gyroConfig()->gyro_soft_lpf_hz,         0, 255, 1 }, 0 },
+    { "GYRO LPF",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &gyroConfig_gyro_soft_lpf_hz,         0, 255, 1 }, 0 },
-    { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_1,     0, 500, 1 }, 0 },
+    { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1,     0, 500, 1 }, 0 },
-    { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
+    { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
-    { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_2,     0, 500, 1 }, 0 },
+    { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2,     0, 500, 1 }, 0 },
-    { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
+    { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
 
     { "BACK", OME_Back, NULL, NULL, 0 },
     { NULL, OME_END, NULL, NULL, 0 }
@@ -297,8 +333,8 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
 static CMS_Menu cmsx_menuFilterGlobal = {
     .GUARD_text = "XFLTGLB",
     .GUARD_type = OME_MENU,
-    .onEnter = NULL,
+    .onEnter = cmsx_menuGyro_onEnter,
-    .onExit = NULL,
+    .onExit = cmsx_menuGyro_onExit,
     .onGlobalExit = NULL,
     .entries = cmsx_menuFilterGlobalEntries,
 };
@@ -311,11 +347,12 @@ static uint16_t cmsx_yaw_p_limit;
 
 static long cmsx_FilterPerProfileRead(void)
 {
-    cmsx_dterm_lpf_hz =       masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz;
+    const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
-    cmsx_dterm_notch_hz =     masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz;
+    cmsx_dterm_lpf_hz =       pidProfile->dterm_lpf_hz;
-    cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff;
+    cmsx_dterm_notch_hz =     pidProfile->dterm_notch_hz;
-    cmsx_yaw_lpf_hz =         masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz;
+    cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
-    cmsx_yaw_p_limit =        masterConfig.profile[profileIndex].pidProfile.yaw_p_limit;
+    cmsx_yaw_lpf_hz =         pidProfile->yaw_lpf_hz;
+    cmsx_yaw_p_limit =        pidProfile->yaw_p_limit;
 
     return 0;
 }
@@ -324,11 +361,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
 {
     UNUSED(self);
 
-    masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz =       cmsx_dterm_lpf_hz;
+    pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
-    masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz =     cmsx_dterm_notch_hz;
+    pidProfile->dterm_lpf_hz =       cmsx_dterm_lpf_hz;
-    masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
+    pidProfile->dterm_notch_hz =     cmsx_dterm_notch_hz;
-    masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz =         cmsx_yaw_lpf_hz;
+    pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
-    masterConfig.profile[profileIndex].pidProfile.yaw_p_limit =        cmsx_yaw_p_limit;
+    pidProfile->yaw_lpf_hz =         cmsx_yaw_lpf_hz;
+    pidProfile->yaw_p_limit =        cmsx_yaw_p_limit;
 
     return 0;
 }

src/main/cms/cms_menu_ledstrip.c

@@ -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"

src/main/cms/cms_menu_misc.c

@@ -22,20 +22,29 @@
 
 #include "platform.h"
 
-#include "build/version.h"
-
 #ifdef CMS
 
-#include "drivers/system.h"
+#include "build/version.h"
 
-#include "config/config_profile.h"
+#include "drivers/system.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 "common/utils.h"
+
+#include "config/config_profile.h"
+#include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "flight/mixer.h"
+
+#include "rx/rx.h"
+
+#include "sensors/battery.h"
+
 //
 // Misc
 //
@@ -78,14 +87,35 @@ CMS_Menu cmsx_menuRcPreview = {
     .entries = cmsx_menuRcEntries
 };
 
+static uint16_t motorConfig_minthrottle;
+static uint8_t batteryConfig_vbatscale;
+static uint8_t batteryConfig_vbatmaxcellvoltage;
+
+static long cmsx_menuMiscOnEnter(void)
+{
+    motorConfig_minthrottle = motorConfig()->minthrottle;
+    batteryConfig_vbatscale = batteryConfig()->vbatscale;
+    batteryConfig_vbatmaxcellvoltage = batteryConfig()->vbatmaxcellvoltage;
+    return 0;
+}
+
+static long cmsx_menuMiscOnExit(const OSD_Entry *self)
+{
+    UNUSED(self);
+
+    motorConfigMutable()->minthrottle = motorConfig_minthrottle;
+    batteryConfigMutable()->vbatscale = batteryConfig_vbatscale;
+    batteryConfigMutable()->vbatmaxcellvoltage = batteryConfig_vbatmaxcellvoltage;
+    return 0;
+}
 
 static OSD_Entry menuMiscEntries[]=
 {
     { "-- MISC --", OME_Label, NULL, NULL, 0 },
 
-    { "MIN THR",    OME_UINT16,  NULL,          &(OSD_UINT16_t){ &motorConfig()->minthrottle,         1000, 2000, 1 }, 0 },
+    { "MIN THR",    OME_UINT16,  NULL,          &(OSD_UINT16_t){ &motorConfig_minthrottle,         1000, 2000, 1 }, 0 },
-    { "VBAT SCALE", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &batteryConfig()->vbatscale,             1, 250, 1 }, 0 },
+    { "VBAT SCALE", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &batteryConfig_vbatscale,             1, 250, 1 }, 0 },
-    { "VBAT CLMAX", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage,   10,  50, 1 }, 0 },
+    { "VBAT CLMAX", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &batteryConfig_vbatmaxcellvoltage,   10,  50, 1 }, 0 },
     { "RC PREV",    OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0},
 
     { "BACK", OME_Back, NULL, NULL, 0},
@@ -95,8 +125,8 @@ static OSD_Entry menuMiscEntries[]=
 CMS_Menu cmsx_menuMisc = {
     .GUARD_text = "XMISC",
     .GUARD_type = OME_MENU,
-    .onEnter = NULL,
+    .onEnter = cmsx_menuMiscOnEnter,
-    .onExit = NULL,
+    .onExit = cmsx_menuMiscOnExit,
     .onGlobalExit = NULL,
     .entries = menuMiscEntries
 };

src/main/cms/cms_menu_osd.c

@@ -17,34 +17,60 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 #include <ctype.h>
 
 #include "platform.h"
 
+#if defined(OSD) && defined(CMS)
+
 #include "build/version.h"
 
 #include "cms/cms.h"
 #include "cms/cms_types.h"
 #include "cms/cms_menu_osd.h"
 
+#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"
 
-#if defined(OSD) && defined(CMS)
+#include "io/osd.h"
 
-OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5};
+static uint8_t osdConfig_rssi_alarm;
-OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50};
+static uint16_t osdConfig_cap_alarm;
-OSD_UINT16_t enryAlarmFlyTime = {&osdProfile()->time_alarm, 1, 200, 1};
+static uint16_t osdConfig_time_alarm;
-OSD_UINT16_t entryAlarmAltitude = {&osdProfile()->alt_alarm, 1, 200, 1};
+static uint16_t osdConfig_alt_alarm;
+
+static long cmsx_menuAlarmsOnEnter(void)
+{
+    osdConfig_rssi_alarm = osdConfig()->rssi_alarm;
+    osdConfig_cap_alarm = osdConfig()->cap_alarm;
+    osdConfig_time_alarm = osdConfig()->time_alarm;
+    osdConfig_alt_alarm = osdConfig()->alt_alarm;
+    return 0;
+}
+
+static long cmsx_menuAlarmsOnExit(const OSD_Entry *self)
+{
+    UNUSED(self);
+
+    osdConfigMutable()->rssi_alarm = osdConfig_rssi_alarm;
+    osdConfigMutable()->cap_alarm = osdConfig_cap_alarm;
+    osdConfigMutable()->time_alarm = osdConfig_time_alarm;
+    osdConfigMutable()->alt_alarm = osdConfig_alt_alarm;
+    return 0;
+}
 
 OSD_Entry cmsx_menuAlarmsEntries[] =
 {
     {"--- ALARMS ---", OME_Label, NULL, NULL, 0},
-    {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0},
+    {"RSSI",     OME_UINT8,  NULL, &(OSD_UINT8_t){&osdConfig_rssi_alarm, 5, 90, 5}, 0},
-    {"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0},
+    {"MAIN BAT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_cap_alarm, 50, 30000, 50}, 0},
-    {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0},
+    {"FLY TIME", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_time_alarm, 1, 200, 1}, 0},
-    {"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0},
+    {"MAX ALT",  OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_alt_alarm, 1, 200, 1}, 0},
     {"BACK", OME_Back, NULL, NULL, 0},
     {NULL, OME_END, NULL, NULL, 0}
 };
@@ -52,38 +78,54 @@ OSD_Entry cmsx_menuAlarmsEntries[] =
 CMS_Menu cmsx_menuAlarms = {
     .GUARD_text = "MENUALARMS",
     .GUARD_type = OME_MENU,
-    .onEnter = NULL,
+    .onEnter = cmsx_menuAlarmsOnEnter,
-    .onExit = NULL,
+    .onExit = cmsx_menuAlarmsOnExit,
     .onGlobalExit = NULL,
     .entries = cmsx_menuAlarmsEntries,
 };
 
+static uint16_t osdConfig_item_pos[OSD_ITEM_COUNT];
+
+static long menuOsdActiveElemsOnEnter(void)
+{
+    memcpy(&osdConfig_item_pos[0], &osdConfig()->item_pos[0], sizeof(uint16_t) * OSD_ITEM_COUNT);
+    return 0;
+}
+
+static long menuOsdActiveElemsOnExit(const OSD_Entry *self)
+{
+    UNUSED(self);
+
+    memcpy(&osdConfigMutable()->item_pos[0], &osdConfig_item_pos[0], sizeof(uint16_t) * OSD_ITEM_COUNT);
+    return 0;
+}
+
 OSD_Entry menuOsdActiveElemsEntries[] =
 {
     {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0},
-    {"RSSI", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_RSSI_VALUE], 0},
+    {"RSSI", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_RSSI_VALUE], 0},
-    {"MAIN BATTERY", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
+    {"MAIN BATTERY", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
-    {"HORIZON", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], 0},
+    {"HORIZON", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ARTIFICIAL_HORIZON], 0},
-    {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_HORIZON_SIDEBARS], 0},
+    {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_HORIZON_SIDEBARS], 0},
-    {"UPTIME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ONTIME], 0},
+    {"UPTIME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ONTIME], 0},
-    {"FLY TIME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_FLYTIME], 0},
+    {"FLY TIME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_FLYTIME], 0},
-    {"FLY MODE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_FLYMODE], 0},
+    {"FLY MODE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_FLYMODE], 0},
-    {"NAME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CRAFT_NAME], 0},
+    {"NAME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_CRAFT_NAME], 0},
-    {"THROTTLE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_THROTTLE_POS], 0},
+    {"THROTTLE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_THROTTLE_POS], 0},
 #ifdef VTX
-    {"VTX CHAN", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_VTX_CHANNEL], 0},
+    {"VTX CHAN", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_VTX_CHANNEL], 0},
 #endif // VTX
-    {"CURRENT (A)", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CURRENT_DRAW], 0},
+    {"CURRENT (A)", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_CURRENT_DRAW], 0},
-    {"USED MAH", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAH_DRAWN], 0},
+    {"USED MAH", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_MAH_DRAWN], 0},
 #ifdef GPS
-    {"GPS SPEED", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SPEED], 0},
+    {"GPS SPEED", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_GPS_SPEED], 0},
-    {"GPS SATS.", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SATS], 0},
+    {"GPS SATS.", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_GPS_SATS], 0},
 #endif // GPS
-    {"ALTITUDE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ALTITUDE], 0},
+    {"ALTITUDE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ALTITUDE], 0},
-    {"POWER", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_POWER], 0},
+    {"POWER", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_POWER], 0},
-    {"ROLL PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ROLL_PIDS], 0},
+    {"ROLL PID", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_PIDS], 0},
-    {"PITCH PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_PITCH_PIDS], 0},
+    {"PITCH PID", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PITCH_PIDS], 0},
-    {"YAW PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_YAW_PIDS], 0},
+    {"YAW PID", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_YAW_PIDS], 0},
     {"BACK", OME_Back, NULL, NULL, 0},
     {NULL, OME_END, NULL, NULL, 0}
 };
@@ -91,8 +133,8 @@ OSD_Entry menuOsdActiveElemsEntries[] =
 CMS_Menu menuOsdActiveElems = {
     .GUARD_text = "MENUOSDACT",
     .GUARD_type = OME_MENU,
-    .onEnter = NULL,
+    .onEnter = menuOsdActiveElemsOnEnter,
-    .onExit = NULL,
+    .onExit = menuOsdActiveElemsOnExit,
     .onGlobalExit = NULL,
     .entries = menuOsdActiveElemsEntries
 };

src/main/cms/cms_menu_vtx.c

@@ -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"
-
+#include "config/parameter_group.h"
-#ifdef CMS
+#include "config/parameter_group_ids.h"
-
-#if defined(VTX) || defined(USE_RTC6705)
 
 static bool featureRead = false;
 static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel;

src/main/common/maths.h

@@ -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))

src/main/config/config_eeprom.c

@@ -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)
+#include "drivers/system.h"
-#error "Flash size not defined for target. (specify in KB)"
-#endif
 
+#include "fc/config.h"
 
-#ifndef FLASH_PAGE_SIZE
+// declare a dummy PG, since scanEEPROM assumes there is at least one PG
-    #ifdef STM32F303xC
+// !!TODO remove once first PG has been created out of masterConfg
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
+typedef struct dummpConfig_s {
-    #endif
+    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
+extern uint8_t __config_start;   // configured via linker script when building binaries.
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x400)
+extern uint8_t __config_end;
-    #endif
 
-    #ifdef STM32F10X_HD
+static uint16_t eepromConfigSize;
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
-    #endif
 
-    #if defined(STM32F745xx)
+typedef enum {
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x40000)
+    CR_CLASSICATION_SYSTEM   = 0,
-    #endif
+    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 CR_CLASSIFICATION_MASK (0x3)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x40000)
-    #endif
 
-    #if defined(STM32F722xx)
+// Header for the saved copy.
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
+typedef struct {
-    #endif
+    uint8_t format;
+} PG_PACKED configHeader_t;
 
-    #if defined(STM32F40_41xxx)
+// Header for each stored PG.
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
+typedef struct {
-    #endif
+    // split up.
+    uint16_t size;
+    pgn_t pgn;
+    uint8_t version;
 
-    #if defined (STM32F411xE)
+    // lower 2 bits used to indicate system or profile number, see CR_CLASSIFICATION_MASK
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
+    uint8_t flags;
-    #endif
 
-#endif
+    uint8_t pg[];
+} PG_PACKED configRecord_t;
 
-#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT)
+// Footer for the saved copy.
-    #ifdef STM32F10X_MD
+typedef struct {
-        #define FLASH_PAGE_COUNT 128
+    uint16_t terminator;
-    #endif
+} PG_PACKED configFooter_t;
-
+// checksum is appended just after footer. It is not included in footer to make checksum calculation consistent
-    #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
 
+// 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 *p = (const uint8_t *)data;
-    const uint8_t *byteOffset;
+    const uint8_t *pend = p + length;
 
-    for (byteOffset = data; byteOffset < (data + length); byteOffset++)
+    for (; p != pend; p++) {
-        checksum ^= *byteOffset;
+        chk ^= *p;
-    return checksum;
+    }
+    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 chk = 0;
-    uint8_t checksum = 0;
+    const uint8_t *p = &__config_start;
+    const configHeader_t *header = (const configHeader_t *)p;
 
-    // check version number
+    if (header->format != EEPROM_CONF_VERSION) {
-    if (EEPROM_CONF_VERSION != temp->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
+    for (;;) {
-    if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
+        const configRecord_t *record = (const configRecord_t *)p;
-        return false;
 
-    if (strncasecmp(temp->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)))
+        if (record->size == 0) {
-        return false;
+            // 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
+        chk = updateChecksum(chk, p, record->size);
-    checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
-    if (checksum != 0)
-        return false;
 
-    // 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)
+static bool writeSettingsToEEPROM(void)
-
-// FIXME: HAL for now this will only work for F4/F7 as flash layout is different
-void writeEEPROM(void)
 {
-    // Generate compile time error if the config does not fit in the reserved area of flash.
+    config_streamer_t streamer;
-    BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
+    config_streamer_init(&streamer);
 
-    HAL_StatusTypeDef status;
+    config_streamer_start(&streamer, (uintptr_t)&__config_start, &__config_end - &__config_start);
-    uint32_t wordOffset;
+    uint8_t chk = 0;
-    int8_t attemptsRemaining = 3;
 
-    suspendRxSignal();
+    configHeader_t header = {
+        .format = EEPROM_CONF_VERSION,
+    };
 
-    // prepare checksum/version constants
+    config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header));
-    masterConfig.version = EEPROM_CONF_VERSION;
+    chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header));
-    masterConfig.size = sizeof(master_t);
+    // write the transitional masterConfig record
-    masterConfig.magic_be = 0xBE;
+    config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig));
-    masterConfig.magic_ef = 0xEF;
+    chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig));
-    masterConfig.chk = 0; // erase checksum before recalculating
+    PG_FOREACH(reg) {
-    masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
+        const uint16_t regSize = pgSize(reg);
+        configRecord_t record = {
+            .size = sizeof(configRecord_t) + regSize,
+            .pgn = pgN(reg),
+            .version = pgVersion(reg),
+            .flags = 0
+        };
 
-    // write it
+        if (pgIsSystem(reg)) {
-    /* Unlock the Flash to enable the flash control register access *************/
+            // write the only instance
-    HAL_FLASH_Unlock();
+            record.flags |= CR_CLASSICATION_SYSTEM;
-    while (attemptsRemaining--)
+            config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
-    {
+            chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
-        /* Fill EraseInit structure*/
+            config_streamer_write(&streamer, reg->address, regSize);
-        FLASH_EraseInitTypeDef EraseInitStruct = {0};
+            chk = updateChecksum(chk, reg->address, regSize);
-        EraseInitStruct.TypeErase     = FLASH_TYPEERASE_SECTORS;
+        } else {
-        EraseInitStruct.VoltageRange  = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
+            // write one instance for each profile
-        EraseInitStruct.Sector        = (FLASH_SECTOR_TOTAL-1);
+            for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) {
-        EraseInitStruct.NbSectors     = 1;
+                record.flags = 0;
-        uint32_t SECTORError;
+
-        status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
+                record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
-        if (status != HAL_OK)
+                config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
-        {
+                chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
-            continue;
+                const uint8_t *address = reg->address + (regSize * profileIndex);
-        }
+                config_streamer_write(&streamer, address, regSize);
-        else
+                chk = updateChecksum(chk, address, regSize);
-        {
-            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 (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);
-        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();
 }

src/main/config/config_eeprom.h

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

src/main/config/config_master.h

@@ -20,6 +20,7 @@
 #include <stdlib.h>
 
 #include "config/config_profile.h"
+#include "config/feature.h"
 
 #include "blackbox/blackbox.h"
 
@@ -36,17 +37,20 @@
 #include "drivers/display.h"
 #include "drivers/serial.h"
 
+#include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
+#include "fc/fc_core.h"
 
+#include "flight/altitudehold.h"
 #include "flight/failsafe.h"
 #include "flight/mixer.h"
 #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"
@@ -65,6 +69,9 @@
 #include "sensors/battery.h"
 #include "sensors/compass.h"
 
+#ifndef USE_PARAMETER_GROUPS
+#define featureConfig(x) (&masterConfig.featureConfig)
+#define systemConfig(x) (&masterConfig.systemConfig)
 #define motorConfig(x) (&masterConfig.motorConfig)
 #define flight3DConfig(x) (&masterConfig.flight3DConfig)
 #define servoConfig(x) (&masterConfig.servoConfig)
@@ -94,11 +101,11 @@
 #define ppmConfig(x) (&masterConfig.ppmConfig)
 #define pwmConfig(x) (&masterConfig.pwmConfig)
 #define adcConfig(x) (&masterConfig.adcConfig)
-#define beeperConfig(x) (&masterConfig.beeperConfig)
+#define beeperDevConfig(x) (&masterConfig.beeperDevConfig)
 #define sonarConfig(x) (&masterConfig.sonarConfig)
 #define ledStripConfig(x) (&masterConfig.ledStripConfig)
 #define statusLedConfig(x) (&masterConfig.statusLedConfig)
-#define osdProfile(x) (&masterConfig.osdProfile)
+#define osdConfig(x) (&masterConfig.osdProfile)
 #define vcdProfile(x) (&masterConfig.vcdProfile)
 #define sdcardConfig(x) (&masterConfig.sdcardConfig)
 #define blackboxConfig(x) (&masterConfig.blackboxConfig)
@@ -108,18 +115,82 @@
 #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 featureConfigMutable(x) (&masterConfig.featureConfig)
+#define systemConfigMutable(x) (&masterConfig.systemConfig)
+#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 beeperDevConfigMutable(x) (&masterConfig.beeperDevConfig)
+#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[x])
+#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[x])
+#define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[x])
+#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;
     uint16_t size;
     uint8_t magic_be;                       // magic number, should be 0xBE
 
-    uint32_t enabledFeatures;
+    featureConfig_t featureConfig;
+
+    systemConfig_t systemConfig;
 
     // motor/esc/servo related stuff
     motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
@@ -128,7 +199,6 @@ typedef struct master_s {
 
 #ifdef USE_SERVOS
     servoConfig_t servoConfig;
-    servoMixerConfig_t servoMixerConfig;
     servoMixer_t customServoMixer[MAX_SERVO_RULES];
     // Servo-related stuff
     servoProfile_t servoProfile;
@@ -144,7 +214,6 @@ typedef struct master_s {
 
     pidConfig_t pidConfig;
 
-    uint8_t debug_mode;                     // Processing denominator for PID controller vs gyro sampling rate
     uint8_t task_statistics;
 
     gyroConfig_t gyroConfig;
@@ -194,7 +263,7 @@ typedef struct master_s {
 #endif
 
 #ifdef BEEPER
-    beeperConfig_t beeperConfig;
+    beeperDevConfig_t beeperDevConfig;
 #endif
 
 #ifdef SONAR
@@ -258,7 +327,6 @@ typedef struct master_s {
     uint32_t beeper_off_flags;
     uint32_t preferred_beeper_off_flags;
 
-    char name[MAX_NAME_LENGTH + 1];
     char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER)];
 
     uint8_t magic_ef;                       // magic number, should be 0xEF
@@ -270,7 +338,5 @@ typedef struct master_s {
 } master_t;
 
 extern master_t masterConfig;
-extern profile_t *currentProfile;
-extern controlRateConfig_t *currentControlRateProfile;
 
 void createDefaultConfig(master_t *config);

src/main/config/config_profile.h

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

src/main/config/config_reset.h

@@ -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__                                       \
+    };                                                    \
+    /**/

src/main/config/config_streamer.c

@@ -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;
+}

src/main/config/config_streamer.h

@@ -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);

src/main/config/feature.c

@@ -23,6 +23,8 @@
 
 #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;
@@ -44,12 +46,12 @@ void intFeatureClearAll(uint32_t *features)
 
 void latchActiveFeatures()
 {
-    activeFeaturesLatch = masterConfig.enabledFeatures;
+    activeFeaturesLatch = featureConfig()->enabledFeatures;
 }
 
 bool featureConfigured(uint32_t mask)
 {
-    return masterConfig.enabledFeatures & mask;
+    return featureConfig()->enabledFeatures & mask;
 }
 
 bool feature(uint32_t mask)
@@ -59,22 +61,22 @@ bool feature(uint32_t mask)
 
 void featureSet(uint32_t mask)
 {
-    intFeatureSet(mask, &masterConfig.enabledFeatures);
+    intFeatureSet(mask, &featureConfigMutable()->enabledFeatures);
 }
 
 void featureClear(uint32_t mask)
 {
-    intFeatureClear(mask, &masterConfig.enabledFeatures);
+    intFeatureClear(mask, &featureConfigMutable()->enabledFeatures);
 }
 
 void featureClearAll()
 {
-    intFeatureClearAll(&masterConfig.enabledFeatures);
+    intFeatureClearAll(&featureConfigMutable()->enabledFeatures);
 }
 
 uint32_t featureMask(void)
 {
-    return masterConfig.enabledFeatures;
+    return featureConfig()->enabledFeatures;
 }
 
 

src/main/config/feature.h

@@ -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);

src/main/config/parameter_group.c

@@ -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)
         }
     }
 }
-

src/main/config/parameter_group.h

@@ -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 const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
-    static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \
+    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)            \
+#define PG_REGISTER_ARRAY(_type, _size, _name, _pgn, _version)            \
-    PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
+    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);
 

src/main/config/parameter_group_ids.h

@@ -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
@@ -51,25 +55,38 @@
 #define PG_NAVIGATION_CONFIG 34 // structs OK
 #define PG_ACCELEROMETER_CONFIG 35 // no accelerometerConfig_t in betaflight
 #define PG_RATE_PROFILE_SELECTION 36 // part of profile in betaflight
-#define PG_ADJUSTMENT_PROFILE 37 // array needs to be made into struct
+//#define PG_ADJUSTMENT_PROFILE 37 // array needs to be made into struct
+#define PG_ADJUSTMENT_RANGE_CONFIG 37
 #define PG_BAROMETER_CONFIG 38 // structs OK
 #define PG_THROTTLE_CORRECTION_CONFIG 39
-#define PG_COMPASS_CONFIGURATION 40 // structs OK
+#define PG_COMPASS_CONFIG 40 // structs OK
 #define PG_MODE_ACTIVATION_PROFILE 41 // array needs to be made into struct
-#define PG_SERVO_PROFILE 42
+//#define PG_SERVO_PROFILE 42
-#define PG_FAILSAFE_CHANNEL_CONFIG 43 // structs OK
+#define PG_SERVO_PARAMS 42
-#define PG_CHANNEL_RANGE_CONFIG 44 // structs OK
+//#define PG_FAILSAFE_CHANNEL_CONFIG 43 // structs OK
+#define PG_RX_FAILSAFE_CHANNEL_CONFIG 43
+//#define PG_CHANNEL_RANGE_CONFIG 44 // structs OK
+#define PG_RX_CHANNEL_RANGE_CONFIG 44
 #define PG_MODE_COLOR_CONFIG 45  // part of led strip, structs OK
 #define PG_SPECIAL_COLOR_CONFIG 46  // part of led strip, structs OK
 #define PG_PILOT_CONFIG 47 // does not exist in betaflight
 #define PG_MSP_SERVER_CONFIG 48 // does not exist in betaflight
 #define PG_VOLTAGE_METER_CONFIG 49 // Cleanflight has voltageMeterConfig_t, betaflight has batteryConfig_t
 #define PG_AMPERAGE_METER_CONFIG 50 // Cleanflight has amperageMeterConfig_t, betaflight has batteryConfig_t
+#define PG_SERVO_CONFIG 52
 
 // Driver configuration
 #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight
 #define PG_DRIVER_FLASHCHIP_CONFIG 101 // does not exist in betaflight
 
+// betaflight specific parameter group ids start at 500
+#define PG_BETAFLIGHT_START 500
+#define PG_MODE_ACTIVATION_OPERATOR_CONFIG 500
+#define PG_OSD_CONFIG 501
+#define PG_BEEPER_CONFIG 5002
+#define PG_BETAFLIGHT_END 1002
+
+
 // OSD configuration (subject to change)
 #define PG_OSD_FONT_CONFIG 2047
 #define PG_OSD_VIDEO_CONFIG 2046

src/main/drivers/accgyro_fake.c

@@ -20,6 +20,8 @@
 
 #include "platform.h"
 
+#ifdef USE_FAKE_GYRO
+
 #include "common/axis.h"
 #include "common/utils.h"
 
@@ -27,8 +29,6 @@
 #include "accgyro_fake.h"
 
 
-#ifdef USE_FAKE_GYRO
-
 static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
 
 static void fakeGyroInit(gyroDev_t *gyro)

src/main/drivers/accgyro_mma845x.c

@@ -83,12 +83,10 @@ static uint8_t device_id;
 
 static inline void mma8451ConfigureInterrupt(void)
 {
-#ifdef NAZE
+#ifdef MMA8451_INT_PIN
-    // PA5 - ACC_INT2 output on NAZE rev3/4 hardware
+    IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0);
-    // NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
+    // TODO - maybe pullup / pulldown ?
-    // OLIMEXINO - The PA5 pin is wired up to LED1, if you need to use an mma8452 on an Olimexino use a different pin and provide support in code.
+    IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING);   
-    IOInit(IOGetByTag(IO_TAG(PA5)), OWNER_MPU_EXTI, 0);
-    IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING);   // TODO - maybe pullup / pulldown ?
 #endif
 
     i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)

src/main/drivers/accgyro_mpu.c

@@ -48,16 +48,7 @@
 #include "accgyro_mpu.h"
 
 
-mpuResetFuncPtr mpuReset;
+mpuResetFnPtr mpuResetFn;
-
-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
 
 #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);
+}

src/main/drivers/accgyro_mpu.h

@@ -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 (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data);
-typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
+typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data);
-typedef void(*mpuResetFuncPtr)(void);
+typedef void(*mpuResetFnPtr)(void);
 
-extern mpuResetFuncPtr mpuReset;
+extern mpuResetFnPtr mpuResetFn;
 
 typedef struct mpuConfiguration_s {
-    mpuReadRegisterFunc read;
+    mpuReadRegisterFnPtr readFn;
-    mpuWriteRegisterFunc write;
+    mpuWriteRegisterFnPtr writeFn;
-    mpuReadRegisterFunc slowread;
+    mpuReadRegisterFnPtr slowreadFn;
-    mpuWriteRegisterFunc verifywrite;
+    mpuWriteRegisterFnPtr verifywriteFn;
-    mpuResetFuncPtr reset;
+    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;
 

src/main/drivers/accgyro_mpu3050.c

@@ -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.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
-    gyro->mpuConfiguration.write(MPU3050_INT_CFG, 0);
+    gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0);
-    gyro->mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
+    gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET);
-    gyro->mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
+    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;
     }
 

src/main/drivers/accgyro_mpu6050.c

@@ -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.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.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_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.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.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_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
 }
 

src/main/drivers/accgyro_mpu6500.c

@@ -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);
 }

src/main/drivers/accgyro_spi_icm20689.c

@@ -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.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.write(MPU_RA_INT_PIN_CFG, 0x10);  // 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);

src/main/drivers/adc.h

@@ -47,5 +47,5 @@ typedef struct adcConfig_s {
     adcChannelConfig_t external1;
 } adcConfig_t;
 
-void adcInit(adcConfig_t *config);
+void adcInit(const adcConfig_t *config);
 uint16_t adcGetChannel(uint8_t channel);

src/main/drivers/adc_stm32f10x.c

@@ -77,7 +77,7 @@ const adcTagMap_t adcTagMap[] = {
 // NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
 //
 
-void adcInit(adcConfig_t *config)
+void adcInit(const adcConfig_t *config)
 {
 
     uint8_t configuredAdcChannels = 0;

src/main/drivers/adc_stm32f30x.c

@@ -104,7 +104,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
     return ADCINVALID;
 }
 
-void adcInit(adcConfig_t *config)
+void adcInit(const adcConfig_t *config)
 {
     ADC_InitTypeDef ADC_InitStructure;
     DMA_InitTypeDef DMA_InitStructure;

src/main/drivers/adc_stm32f4xx.c

@@ -87,7 +87,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
     return ADCINVALID;
 }
 
-void adcInit(adcConfig_t *config)
+void adcInit(const adcConfig_t *config)
 {
     ADC_InitTypeDef ADC_InitStructure;
     DMA_InitTypeDef DMA_InitStructure;

src/main/drivers/adc_stm32f7xx.c

@@ -83,7 +83,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
     return ADCINVALID;
 }
 
-void adcInit(adcConfig_t *config)
+void adcInit(const adcConfig_t *config)
 {
     uint8_t i;
     uint8_t configuredAdcChannels = 0;

src/main/drivers/bus_spi_hal.c

@@ -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 }
 };
 

src/main/drivers/compass_hmc5883l.c

@@ -119,11 +119,9 @@
 
 static float magGain[3] = { 1.0f, 1.0f, 1.0f };
 
-static const hmc5883Config_t *hmc5883Config = NULL;
-
 #ifdef USE_MAG_DATA_READY_SIGNAL
 
-static IO_t intIO;
+static IO_t hmc5883InterruptIO;
 static extiCallbackRec_t hmc5883_extiCallbackRec;
 
 static void hmc5883_extiHandler(extiCallbackRec_t* cb)
@@ -150,20 +148,19 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void)
 {
 #ifdef USE_MAG_DATA_READY_SIGNAL
 
-    if (!(hmc5883Config->intTag)) {
+    if (!(hmc5883InterruptIO)) {
         return;
     }
-    intIO = IOGetByTag(hmc5883Config->intTag);
 #ifdef ENSURE_MAG_DATA_READY_IS_HIGH
-    uint8_t status = IORead(intIO);
+    uint8_t status = IORead(hmc5883InterruptIO);
     if (!status) {
         return;
     }
 #endif
 
     EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler);
-    EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
+    EXTIConfig(hmc5883InterruptIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
-    EXTIEnable(intIO, true);
+    EXTIEnable(hmc5883InterruptIO, true);
 #endif
 }
 
@@ -257,9 +254,13 @@ static bool hmc5883lInit(void)
     return true;
 }
 
-bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
+bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag)
 {
-    hmc5883Config = hmc5883ConfigToUse;
+#ifdef USE_MAG_DATA_READY_SIGNAL
+    hmc5883InterruptIO = IOGetByTag(interruptTag);
+#else
+    UNUSED(interruptTag);
+#endif
 
     uint8_t sig = 0;
     bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);

src/main/drivers/compass_hmc5883l.h

@@ -19,8 +19,4 @@
 
 #include "io_types.h"
 
-typedef struct hmc5883Config_s {
+bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag);
-    ioTag_t intTag;
-} hmc5883Config_t;
-
-bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);

src/main/drivers/flash_m25p16.c

@@ -208,7 +208,7 @@ static bool m25p16_readIdentification()
  * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
  * m25p16_getGeometry().
  */
-bool m25p16_init(flashConfig_t *flashConfig)
+bool m25p16_init(const flashConfig_t *flashConfig)
 {
     /*
         if we have already detected a flash device we can simply exit

src/main/drivers/flash_m25p16.h

@@ -22,7 +22,7 @@
 
 #define M25P16_PAGESIZE 256
 
-bool m25p16_init(flashConfig_t *flashConfig);
+bool m25p16_init(const flashConfig_t *flashConfig);
 
 void m25p16_eraseSector(uint32_t address);
 void m25p16_eraseCompletely();

src/main/drivers/light_led.c

@@ -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;

src/main/drivers/light_led.h

@@ -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);

src/main/drivers/pwm_output.c

@@ -23,8 +23,8 @@
 #include "platform.h"
 
 #include "io.h"
-#include "timer.h"
 #include "pwm_output.h"
+#include "timer.h"
 
 #define MULTISHOT_5US_PW    (MULTISHOT_TIMER_MHZ * 5)
 #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
@@ -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);
@@ -193,7 +194,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount)
     pwmCompleteWritePtr(motorCount);
 }
 
-void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
+void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
 {
     memset(motors, 0, sizeof(motors));
     
@@ -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;
@@ -323,7 +325,7 @@ void pwmWriteServo(uint8_t index, uint16_t value)
     }
 }
 
-void servoInit(const servoConfig_t *servoConfig)
+void servoDevInit(const servoDevConfig_t *servoConfig)
 {
     for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
         const ioTag_t tag = servoConfig->ioTags[servoIndex];
@@ -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;
     }
 }

src/main/drivers/pwm_output.h

@@ -17,10 +17,17 @@
 
 #pragma once
 
-#include "io/motors.h"
+#include "dma.h"
-#include "io/servos.h"
+#include "io_types.h"
-#include "drivers/timer.h"
+#include "timer.h"
-#include "drivers/dma.h"
+
+#define MAX_SUPPORTED_MOTORS 12
+
+#if defined(USE_QUAD_MIXER_ONLY)
+#define MAX_SUPPORTED_SERVOS 1
+#else
+#define MAX_SUPPORTED_SERVOS 8
+#endif
 
 typedef enum {
     PWM_TYPE_STANDARD = 0,
@@ -111,15 +118,31 @@ typedef struct {
     IO_t io;
 } pwmOutputPort_t;
 
-void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount);
+typedef struct motorDevConfig_s {
-void servoInit(const servoConfig_t *servoConfig);
+    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;
+    ioTag_t  ioTags[MAX_SUPPORTED_MOTORS];
+} motorDevConfig_t;
+
+void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount);
+
+typedef struct servoDevConfig_s {
+    // PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
+    uint16_t servoCenterPulse;              // This is the value for servos when they should be in the middle. e.g. 1500.
+    uint16_t servoPwmRate;                  // The update rate of servo outputs (50-498Hz)
+    ioTag_t  ioTags[MAX_SUPPORTED_SERVOS];
+} servoDevConfig_t;
+
+void servoDevInit(const servoDevConfig_t *servoDevConfig);
 
 void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
 
 #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
 

src/main/drivers/pwm_output_stm32f3xx.c

@@ -20,6 +20,8 @@
 
 #include "platform.h"
 
+#ifdef USE_DSHOT
+
 #include "build/debug.h"
 
 #include "io.h"
@@ -30,8 +32,6 @@
 #include "system.h"
 #include "rcc.h"
 
-#ifdef USE_DSHOT
-
 static uint8_t dmaMotorTimerCount = 0;
 static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@@ -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;
 

src/main/drivers/pwm_output_stm32f4xx.c

@@ -20,6 +20,8 @@
 
 #include "platform.h"
 
+#ifdef USE_DSHOT
+
 #include "io.h"
 #include "timer.h"
 #include "timer_stm32f4xx.h"
@@ -29,8 +31,6 @@
 #include "system.h"
 #include "rcc.h"
 
-#ifdef USE_DSHOT
-
 static uint8_t dmaMotorTimerCount = 0;
 static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@@ -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_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;
 

src/main/drivers/pwm_output_stm32f7xx.c

@@ -20,6 +20,8 @@
 
 #include "platform.h"
 
+#ifdef USE_DSHOT
+
 #include "io.h"
 #include "timer.h"
 #include "pwm_output.h"
@@ -28,8 +30,6 @@
 #include "system.h"
 #include "rcc.h"
 
-#ifdef USE_DSHOT
-
 static uint8_t dmaMotorTimerCount = 0;
 static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@@ -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;
+    if (output & TIMER_OUTPUT_N_CHANNEL) {
-    TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
+        TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
-    TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+        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;
 

src/main/drivers/rx_pwm.c

@@ -36,6 +36,8 @@
 #include "pwm_output.h"
 #include "rx_pwm.h"
 
+#include "flight/mixer.h" //!!TODO remove dependency on this
+
 #define DEBUG_PPM_ISR
 
 #define PPM_CAPTURE_COUNT 12

src/main/drivers/sound_beeper.c

@@ -49,7 +49,7 @@ void systemBeepToggle(void)
 #endif
 }
 
-void beeperInit(const beeperConfig_t *config)
+void beeperInit(const beeperDevConfig_t *config)
 {
 #ifndef BEEPER
     UNUSED(config);

src/main/drivers/sound_beeper.h

@@ -29,13 +29,13 @@
 #define BEEP_ON     do {} while(0)
 #endif
 
-typedef struct beeperConfig_s {
+typedef struct beeperDevConfig_s {
     ioTag_t ioTag;
     uint8_t isInverted;
     uint8_t isOpenDrain;
-} beeperConfig_t;
+} beeperDevConfig_t;
 
 void systemBeep(bool on);
 void systemBeepToggle(void);
-void beeperInit(const beeperConfig_t *beeperConfig);
+void beeperInit(const beeperDevConfig_t *beeperDevConfig);
 

src/main/drivers/system.h

@@ -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);

src/main/drivers/system_stm32f4xx.c

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

src/main/drivers/system_stm32f7xx.c

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

src/main/drivers/vtx_rtc6705.c

@@ -87,7 +87,7 @@
 #define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO,   RTC6705_CS_PIN)
 #define ENABLE_RTC6705  GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
 
-#if defined(SPRACINGF3NEO)
+#ifdef RTC6705_POWER_PIN
 static IO_t vtxPowerPin        = IO_NONE;
 #endif
 

src/main/fc/cli.h

@@ -20,7 +20,7 @@
 extern uint8_t cliMode;
 
 struct serialConfig_s;
-void cliInit(struct serialConfig_s *serialConfig);
+void cliInit(const struct serialConfig_s *serialConfig);
 void cliProcess(void);
 struct serialPort_s;
 void cliEnter(struct serialPort_s *serialPort);

src/main/fc/config.c

@@ -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 "flight/altitudehold.h"
-#include "sensors/gyro.h"
+#include "flight/failsafe.h"
-#include "sensors/compass.h"
+#include "flight/imu.h"
-#include "sensors/acceleration.h"
+#include "flight/mixer.h"
-#include "sensors/barometer.h"
+#include "flight/navigation.h"
-#include "sensors/battery.h"
+#include "flight/pid.h"
-#include "sensors/boardalignment.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
@@ -115,6 +117,16 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
     accelerometerTrims->values.yaw = 0;
 }
 
+static void resetCompassConfig(compassConfig_t* compassConfig)
+{
+    compassConfig->mag_align = ALIGN_DEFAULT;
+#ifdef MAG_INT_EXTI
+    compassConfig->interruptTag = IO_TAG(MAG_INT_EXTI);
+#else
+    compassConfig->interruptTag = IO_TAG_NONE;
+#endif
+}
+
 static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
 {
     controlRateConfig->rcRate8 = 100;
@@ -239,13 +251,15 @@ void resetLedStripConfig(ledStripConfig_t *ledStripConfig)
 #ifdef USE_SERVOS
 void resetServoConfig(servoConfig_t *servoConfig)
 {
-    servoConfig->servoCenterPulse = 1500;
+    servoConfig->dev.servoCenterPulse = 1500;
-    servoConfig->servoPwmRate = 50;
+    servoConfig->dev.servoPwmRate = 50;
+    servoConfig->tri_unarmed_servo = 1;
+    servoConfig->servo_lowpass_freq = 0;
 
     int servoIndex = 0;
     for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) {
         if (timerHardware[i].usageFlags & TIM_USE_SERVO) {
-            servoConfig->ioTags[servoIndex] = timerHardware[i].tag;
+            servoConfig->dev.ioTags[servoIndex] = timerHardware[i].tag;
             servoIndex++;
         }
     }
@@ -256,22 +270,22 @@ void resetMotorConfig(motorConfig_t *motorConfig)
 {
 #ifdef BRUSHED_MOTORS
     motorConfig->minthrottle = 1000;
-    motorConfig->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
+    motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
-    motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED;
+    motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
-    motorConfig->useUnsyncedPwm = true;
+    motorConfig->dev.useUnsyncedPwm = true;
 #else
 #ifdef BRUSHED_ESC_AUTODETECT
     if (hardwareMotorType == MOTOR_BRUSHED) {
         motorConfig->minthrottle = 1000;
-        motorConfig->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
+        motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
-        motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED;
+        motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
-        motorConfig->useUnsyncedPwm = true;
+        motorConfig->dev.useUnsyncedPwm = true;
     } else
 #endif
     {
         motorConfig->minthrottle = 1070;
-        motorConfig->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
+        motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
-        motorConfig->motorPwmProtocol = PWM_TYPE_ONESHOT125;
+        motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125;
     }
 #endif
     motorConfig->maxthrottle = 2000;
@@ -281,7 +295,7 @@ void resetMotorConfig(motorConfig_t *motorConfig)
     int motorIndex = 0;
     for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) {
         if (timerHardware[i].usageFlags & TIM_USE_MOTOR) {
-            motorConfig->ioTags[motorIndex] = timerHardware[i].tag;
+            motorConfig->dev.ioTags[motorIndex] = timerHardware[i].tag;
             motorIndex++;
         }
     }
@@ -338,16 +352,16 @@ void resetAdcConfig(adcConfig_t *adcConfig)
 
 
 #ifdef BEEPER
-void resetBeeperConfig(beeperConfig_t *beeperConfig)
+void resetBeeperConfig(beeperDevConfig_t *beeperDevConfig)
 {
 #ifdef BEEPER_INVERTED
-    beeperConfig->isOpenDrain = false;
+    beeperDevConfig->isOpenDrain = false;
-    beeperConfig->isInverted = true;
+    beeperDevConfig->isInverted = true;
 #else
-    beeperConfig->isOpenDrain = true;
+    beeperDevConfig->isOpenDrain = true;
-    beeperConfig->isInverted = false;
+    beeperDevConfig->isInverted = false;
 #endif
-    beeperConfig->ioTag = IO_TAG(BEEPER);
+    beeperDevConfig->ioTag = IO_TAG(BEEPER);
 }
 #endif
 
@@ -644,15 +658,6 @@ void resetMixerConfig(mixerConfig_t *mixerConfig)
     mixerConfig->yaw_motor_direction = 1;
 }
 
-#ifdef USE_SERVOS
-void resetServoMixerConfig(servoMixerConfig_t *servoMixerConfig)
-{
-    servoMixerConfig->tri_unarmed_servo = 1;
-    servoMixerConfig->servo_lowpass_freq = 400;
-    servoMixerConfig->servo_lowpass_enable = 0;
-}
-#endif
-
 #ifdef USE_MAX7456
 void resetMax7456Config(vcdProfile_t *pVcdProfile)
 {
@@ -713,7 +718,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;
@@ -748,10 +753,11 @@ void createDefaultConfig(master_t *config)
     // Clear all configuration
     memset(config, 0, sizeof(master_t));
 
-    uint32_t *featuresPtr = &config->enabledFeatures;
+    uint32_t *featuresPtr = &config->featureConfig.enabledFeatures;
 
     intFeatureClearAll(featuresPtr);
     intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , featuresPtr);
+
 #ifdef DEFAULT_FEATURES
     intFeatureSet(DEFAULT_FEATURES, featuresPtr);
 #endif
@@ -768,7 +774,6 @@ void createDefaultConfig(master_t *config)
 #endif
 
 #ifdef OSD
-    intFeatureSet(FEATURE_OSD, featuresPtr);
     osdResetConfig(&config->osdProfile);
 #endif
 
@@ -802,14 +807,15 @@ void createDefaultConfig(master_t *config)
     config->gyroConfig.gyro_soft_notch_hz_2 = 200;
     config->gyroConfig.gyro_soft_notch_cutoff_2 = 100;
 
-    config->debug_mode = DEBUG_MODE;
+    config->systemConfig.debug_mode = DEBUG_MODE;
     config->task_statistics = true;
 
     resetAccelerometerTrims(&config->accelerometerConfig.accZero);
 
     config->gyroConfig.gyro_align = ALIGN_DEFAULT;
     config->accelerometerConfig.acc_align = ALIGN_DEFAULT;
-    config->compassConfig.mag_align = ALIGN_DEFAULT;
+    
+    resetCompassConfig(&config->compassConfig);
 
     config->boardAlignment.rollDegrees = 0;
     config->boardAlignment.pitchDegrees = 0;
@@ -839,7 +845,7 @@ void createDefaultConfig(master_t *config)
 #endif
 
 #ifdef BEEPER
-    resetBeeperConfig(&config->beeperConfig);
+    resetBeeperConfig(&config->beeperDevConfig);
 #endif
 
 #ifdef SONAR
@@ -868,9 +874,9 @@ void createDefaultConfig(master_t *config)
     config->rxConfig.rx_max_usec = 2115;         // any of first 4 channels above this value will trigger rx loss detection
 
     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
-        rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &config->rxConfig.failsafe_channel_configurations[i];
+        rxFailsafeChannelConfig_t *channelFailsafeConfig = &config->rxConfig.failsafe_channel_configurations[i];
-        channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD;
+        channelFailsafeConfig->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD;
-        channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.midrc);
+        channelFailsafeConfig->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.midrc);
     }
 
     config->rxConfig.rssi_channel = 0;
@@ -900,7 +906,6 @@ void createDefaultConfig(master_t *config)
     resetMixerConfig(&config->mixerConfig);
     resetMotorConfig(&config->motorConfig);
 #ifdef USE_SERVOS
-    resetServoMixerConfig(&config->servoMixerConfig);
     resetServoConfig(&config->servoConfig);
 #endif
     resetFlight3DConfig(&config->flight3DConfig);
@@ -1041,11 +1046,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();
@@ -1058,63 +1066,34 @@ void activateConfig(void)
 
     resetAdjustmentStates();
 
-    useRcControlsConfig(
+    useRcControlsConfig(modeActivationConditions(0), &currentProfile->pidProfile);
-        modeActivationProfile()->modeActivationConditions,
+    useAdjustmentConfig(&currentProfile->pidProfile);
-        &masterConfig.motorConfig,
-        &currentProfile->pidProfile
-    );
-
-#ifdef TELEMETRY
-    telemetryUseConfig(&masterConfig.telemetryConfig);
-#endif
 
 #ifdef GPS
-    gpsUseProfile(&masterConfig.gpsProfile);
     gpsUsePIDs(&currentProfile->pidProfile);
 #endif
 
-    useFailsafeConfig(&masterConfig.failsafeConfig);
+    failsafeReset();
-    setAccelerationTrims(&accelerometerConfig()->accZero);
+    setAccelerationTrims(&accelerometerConfigMutable()->accZero);
     setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
 
-    mixerUseConfigs(
-        &masterConfig.flight3DConfig,
-        &masterConfig.motorConfig,
-        &masterConfig.mixerConfig,
-        &masterConfig.airplaneConfig,
-        &masterConfig.rxConfig
-    );
-
 #ifdef USE_SERVOS
-    servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoProfile.servoConf, &masterConfig.gimbalConfig, &masterConfig.channelForwardingConfig);
+    servoUseConfigs(masterConfig.servoProfile.servoConf, &masterConfig.channelForwardingConfig);
 #endif
 
-    imuConfigure(
+    imuConfigure(throttleCorrectionConfig()->throttle_correction_angle);
-        &masterConfig.imuConfig,
-        &currentProfile->pidProfile,
-        throttleCorrectionConfig()->throttle_correction_angle
-    );
 
-    configureAltitudeHold(
+    configureAltitudeHold(&currentProfile->pidProfile);
-        &currentProfile->pidProfile,
-        &masterConfig.barometerConfig,
-        &masterConfig.rcControlsConfig,
-        &masterConfig.motorConfig
-    );
-
-#ifdef BARO
-    useBarometerConfig(&masterConfig.barometerConfig);
-#endif
 }
 
 void validateAndFixConfig(void)
 {
-    if ((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
+    if((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){
-        motorConfig()->mincommand = 1000;
+        motorConfigMutable()->mincommand = 1000;
     }
 
-    if ((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
+    if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
-        motorConfig()->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
+        motorConfigMutable()->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
     }
 
     if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
@@ -1147,12 +1126,6 @@ void validateAndFixConfig(void)
             featureClear(FEATURE_CURRENT_METER);
         }
 #endif
-
-#if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
-        // led strip needs the same ports
-        featureClear(FEATURE_LED_STRIP);
-#endif
-
         // software serial needs free PWM ports
         featureClear(FEATURE_SOFTSERIAL);
     }
@@ -1172,42 +1145,6 @@ void validateAndFixConfig(void)
     }
 #endif
 
-#if defined(NAZE) && defined(SONAR)
-    if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
-        featureClear(FEATURE_CURRENT_METER);
-    }
-#endif
-
-#if defined(OLIMEXINO) && defined(SONAR)
-    if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
-        featureClear(FEATURE_CURRENT_METER);
-    }
-#endif
-
-#if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3)
-    if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DASHBOARD)) {
-        featureClear(FEATURE_DASHBOARD);
-    }
-#endif
-
-#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
-    // shared pin
-    if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
-        featureClear(FEATURE_SONAR);
-        featureClear(FEATURE_SOFTSERIAL);
-        featureClear(FEATURE_RSSI_ADC);
-    }
-#endif
-
-#if defined(COLIBRI_RACE)
-    serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
-    if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
-        featureClear(FEATURE_RX_PARALLEL_PWM);
-        featureClear(FEATURE_RX_MSP);
-        featureSet(FEATURE_RX_PPM);
-    }
-#endif
-
     useRxConfig(&masterConfig.rxConfig);
 
     serialConfig_t *serialConfig = &masterConfig.serialConfig;
@@ -1227,18 +1164,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
+        pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
-        gyroConfig()->gyro_sync_denom = 1;
+        gyroConfigMutable()->gyro_sync_denom = 1;
-        gyroConfig()->gyro_use_32khz = false;
+        gyroConfigMutable()->gyro_use_32khz = false;
         samplingTime = 0.001f;
     }
 
@@ -1246,24 +1183,24 @@ 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, 3);
+        gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
 #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
     const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
     float motorUpdateRestriction;
-    switch(motorConfig()->motorPwmProtocol) {
+    switch(motorConfig()->dev.motorPwmProtocol) {
         case (PWM_TYPE_STANDARD):
             motorUpdateRestriction = 1.0f/BRUSHLESS_MOTORS_PWM_RATE;
             break;
@@ -1287,33 +1224,65 @@ 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
-    if (motorConfig()->useUnsyncedPwm && (motorConfig()->motorPwmProtocol <= PWM_TYPE_BRUSHED) && motorConfig()->motorPwmProtocol != PWM_TYPE_STANDARD) {
+    if (motorConfig()->dev.useUnsyncedPwm && (motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD) {
         uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
 
-        if(motorConfig()->motorPwmRate > maxEscRate)
+        if(motorConfig()->dev.motorPwmRate > maxEscRate)
-            motorConfig()->motorPwmRate = maxEscRate;
+            motorConfigMutable()->dev.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();

src/main/fc/config.h

@@ -17,8 +17,18 @@
 
 #pragma once
 
+#include <stdint.h>
 #include <stdbool.h>
 
+#include "config/parameter_group.h"
+
+#include "drivers/adc.h"
+#include "drivers/flash.h"
+#include "drivers/rx_pwm.h"
+#include "drivers/sdcard.h"
+#include "drivers/sound_beeper.h"
+#include "drivers/vcd.h"
+
 #if FLASH_SIZE <= 128
 #define MAX_PROFILE_COUNT 2
 #else
@@ -58,6 +68,33 @@ typedef enum {
     FEATURE_ESC_SENSOR = 1 << 27,
 } features_e;
 
+typedef struct systemConfig_s {
+    uint8_t debug_mode;
+    char name[MAX_NAME_LENGTH + 1];
+} systemConfig_t;
+
+PG_DECLARE(systemConfig_t, systemConfig);
+PG_DECLARE(adcConfig_t, adcConfig);
+PG_DECLARE(beeperDevConfig_t, beeperDevConfig);
+PG_DECLARE(flashConfig_t, flashConfig);
+PG_DECLARE(ppmConfig_t, ppmConfig);
+PG_DECLARE(pwmConfig_t, pwmConfig);
+PG_DECLARE(vcdProfile_t, vcdProfile);
+PG_DECLARE(sdcardConfig_t, sdcardConfig);
+
+
+/*typedef struct beeperConfig_s {
+    uint32_t beeper_off_flags;
+    uint32_t preferred_beeper_off_flags;
+} beeperConfig_t;
+PG_DECLARE(beeperConfig_t, beeperConfig);
+*/
+
+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 +106,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 +119,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);

src/main/fc/fc_core.c

@@ -24,30 +24,39 @@
 
 #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/cli.h"
 #include "fc/config.h"
+#include "fc/fc_core.h"
+#include "fc/fc_rc.h"
+#include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
-#include "fc/cli.h"
-#include "fc/fc_rc.h"
 
 #include "msp/msp_serial.h"
 
 #include "io/beeper.h"
+#include "io/gps.h"
 #include "io/motors.h"
 #include "io/servos.h"
 #include "io/serial.h"
@@ -59,15 +68,16 @@
 
 #include "scheduler/scheduler.h"
 
-#include "flight/mixer.h"
+#include "telemetry/telemetry.h"
-#include "flight/servos.h"
+
-#include "flight/pid.h"
+#include "flight/altitudehold.h"
-#include "flight/failsafe.h"
+#include "flight/failsafe.h"
-#include "flight/altitudehold.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
+#include "flight/servos.h"
 
-#include "config/config_profile.h"
-#include "config/config_master.h"
-#include "config/feature.h"
 
 // June 2013     V2.2-dev
 
@@ -87,7 +97,6 @@ int16_t headFreeModeHold;
 
 uint8_t motorControlEnable = false;
 
-int16_t telemTemperature1;      // gyro sensor temperature
 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 
 bool isRXDataNew;
@@ -95,8 +104,8 @@ static bool armingCalibrationWasInitialised;
 
 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 {
-    accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
+    accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
-    accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
+    accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
 
     saveConfigAndNotify();
 }
@@ -183,15 +192,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 +291,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,17 +357,17 @@ void processRx(timeUs_t currentTimeUs)
         }
     }
 
-    processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch);
+    processRcStickPositions(throttleStatus);
 
     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
         updateInflightCalibrationState();
     }
 
-    updateActivatedModes(modeActivationProfile()->modeActivationConditions);
+    updateActivatedModes();
 
     if (!cliMode) {
-        updateAdjustmentStates(adjustmentProfile()->adjustmentRanges);
+        updateAdjustmentStates();
-        processRcAdjustments(currentControlRateProfile, rxConfig());
+        processRcAdjustments(currentControlRateProfile);
     }
 
     bool canUseHorizonMode = true;
@@ -473,8 +473,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 
     // Read out gyro temperature if used for telemmetry
-    if (feature(FEATURE_TELEMETRY) && gyro.dev.temperature) {
+    if (feature(FEATURE_TELEMETRY)) {
-        gyro.dev.temperature(&gyro.dev, &telemTemperature1);
+        gyroReadTemperature();
     }
 
 #ifdef MAG
@@ -488,7 +488,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
@@ -500,7 +500,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
     if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
 #ifndef USE_QUAD_MIXER_ONLY
 #ifdef USE_SERVOS
-                && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoMixerConfig()->tri_unarmed_servo)
+                && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
 #endif
                 && mixerConfig()->mixerMode != MIXER_AIRPLANE
                 && mixerConfig()->mixerMode != MIXER_FLYING_WING
@@ -531,6 +531,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
     if (!cliMode && feature(FEATURE_BLACKBOX)) {
         handleBlackbox(currentTimeUs);
     }
+#else
+    UNUSED(currentTimeUs);
 #endif
 
 #ifdef TRANSPONDER
@@ -558,8 +560,6 @@ static void subTaskMotorUpdate(void)
 #ifdef USE_SERVOS
     // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
     if (isMixerUsingServos()) {
-        servoTable();
-        filterServos();
         writeServos();
     }
 #endif

src/main/fc/fc_core.h

@@ -18,11 +18,19 @@
 #pragma once
 
 #include "common/time.h"
+#include "config/parameter_group.h"
 
 extern int16_t magHold;
 extern bool isRXDataNew;
 extern int16_t headFreeModeHold;
 
+typedef struct throttleCorrectionConfig_s {
+    uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
+    uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
+} throttleCorrectionConfig_t;
+
+PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
+
 union rollAndPitchTrims_u;
 void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
 void handleInflightCalibrationStickPosition();

src/main/fc/fc_init.c

@@ -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"
 
@@ -96,29 +102,26 @@
 
 #include "scheduler/scheduler.h"
 
-#include "sensors/sensors.h"
-#include "sensors/sonar.h"
-#include "sensors/barometer.h"
-#include "sensors/compass.h"
 #include "sensors/acceleration.h"
-#include "sensors/gyro.h"
+#include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
+#include "sensors/compass.h"
+#include "sensors/esc_sensor.h"
+#include "sensors/gyro.h"
 #include "sensors/initialisation.h"
+#include "sensors/sensors.h"
+#include "sensors/sonar.h"
 
 #include "telemetry/telemetry.h"
-#include "sensors/esc_sensor.h"
 
-#include "flight/pid.h"
+#include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
-#include "flight/failsafe.h"
 #include "flight/navigation.h"
+#include "flight/pid.h"
+#include "flight/servos.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"
@@ -186,7 +189,7 @@ void init(void)
 
     //i2cSetOverclock(masterConfig.i2c_overclock);
 
-    debugMode = masterConfig.debug_mode;
+    debugMode = systemConfig()->debug_mode;
 
     // Latch active features to be used for feature() in the remainder of init().
     latchActiveFeatures();
@@ -241,13 +244,13 @@ void init(void)
 #ifdef SPEKTRUM_BIND
     if (feature(FEATURE_RX_SERIAL)) {
         switch (rxConfig()->serialrx_provider) {
-            case SERIALRX_SPEKTRUM1024:
+        case SERIALRX_SPEKTRUM1024:
-            case SERIALRX_SPEKTRUM2048:
+        case SERIALRX_SPEKTRUM2048:
-                // Spektrum satellite binding if enabled on startup.
+            // Spektrum satellite binding if enabled on startup.
-                // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
+            // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
-                // The rest of Spektrum initialization will happen later - via spektrumInit()
+            // The rest of Spektrum initialization will happen later - via spektrumInit()
-                spektrumBind(rxConfig());
+            spektrumBind(rxConfigMutable());
-                break;
+            break;
         }
     }
 #endif
@@ -257,21 +260,21 @@ 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);
+    mixerInit(mixerConfig()->mixerMode);
 #ifdef USE_SERVOS
-    servoMixerInit(masterConfig.customServoMixer);
+    servosInit();
 #endif
 
     uint16_t idlePulse = motorConfig()->mincommand;
@@ -279,25 +282,25 @@ void init(void)
         idlePulse = flight3DConfig()->neutral3d;
     }
 
-    if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
+    if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
         featureClear(FEATURE_3D);
         idlePulse = 0; // brushed motors
     }
 
     mixerConfigureOutput();
-    motorInit(motorConfig(), idlePulse, getMotorCount());
+    motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
 
 #ifdef USE_SERVOS
     servoConfigureOutput();
     if (isMixerUsingServos()) {
         //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
-        servoInit(servoConfig());
+        servoDevInit(&servoConfig()->dev);
     }
 #endif
 
 #if defined(USE_PWM) || defined(USE_PPM)
     if (feature(FEATURE_RX_PPM)) {
-        ppmRxInit(ppmConfig(), motorConfig()->motorPwmProtocol);
+        ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol);
     } else if (feature(FEATURE_RX_PARALLEL_PWM)) {
         pwmRxInit(pwmConfig());
     }
@@ -306,7 +309,7 @@ void init(void)
     systemState |= SYSTEM_STATE_MOTORS_READY;
 
 #ifdef BEEPER
-    beeperInit(beeperConfig());
+    beeperInit(beeperDevConfig());
 #endif
 /* temp until PGs are implemented. */
 #ifdef USE_INVERTER
@@ -362,9 +365,9 @@ void init(void)
 
 #ifdef USE_ADC
     /* these can be removed from features! */
-    adcConfig()->vbat.enabled = feature(FEATURE_VBAT);
+    adcConfigMutable()->vbat.enabled = feature(FEATURE_VBAT);
-    adcConfig()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
+    adcConfigMutable()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
-    adcConfig()->rssi.enabled = feature(FEATURE_RSSI_ADC);
+    adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC);
     adcInit(adcConfig());
 #endif
 
@@ -376,7 +379,7 @@ void init(void)
 
 #ifdef USE_DASHBOARD
     if (feature(FEATURE_DASHBOARD)) {
-        dashboardInit(rxConfig());
+        dashboardInit();
     }
 #endif
 
@@ -393,7 +396,7 @@ void init(void)
     if (feature(FEATURE_OSD)) {
 #ifdef USE_MAX7456
         // if there is a max7456 chip for the OSD then use it, otherwise use MSP
-        displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile(), displayPortProfileMax7456());
+        displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile());
 #else
         displayPort_t *osdDisplayPort = displayPortMspInit(displayPortProfileMax7456());
 #endif
@@ -401,12 +404,7 @@ void init(void)
     }
 #endif
 
-#ifdef SONAR
+    if (!sensorsAutodetect()) {
-    const sonarConfig_t *sonarConfig = sonarConfig();
-#else
-    const void *sonarConfig = NULL;
-#endif
-    if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
     }
@@ -439,32 +437,26 @@ void init(void)
     mspSerialInit();
 
 #if defined(USE_MSP_DISPLAYPORT) && defined(CMS)
-    cmsDisplayPortRegister(displayPortMspInit(displayPortProfileMsp()));
+    cmsDisplayPortRegister(displayPortMspInit());
 #endif
 
 #ifdef USE_CLI
     cliInit(serialConfig());
 #endif
 
-    failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
+    failsafeInit();
 
-    rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
+    rxInit(rxConfig(), modeActivationConditions(0));
 
 #ifdef GPS
     if (feature(FEATURE_GPS)) {
-        gpsInit(
+        gpsInit();
-            serialConfig(),
+        navigationInit(&currentProfile->pidProfile);
-            gpsConfig()
-        );
-        navigationInit(
-            gpsProfile(),
-            &currentProfile->pidProfile
-        );
     }
 #endif
 
 #ifdef LED_STRIP
-    ledStripInit(ledStripConfig());
+    ledStripInit();
 
     if (feature(FEATURE_LED_STRIP)) {
         ledStripEnable();
@@ -554,7 +546,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)) {

src/main/fc/fc_msp.c

@@ -17,7 +17,6 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-#include <stdlib.h>
 #include <string.h>
 #include <math.h>
 
@@ -34,79 +33,82 @@
 #include "common/maths.h"
 #include "common/streambuf.h"
 
-#include "drivers/system.h"
+#include "config/config_master.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/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/io.h"
-#include "drivers/vcd.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"
 #include "fc/fc_msp.h"
 #include "fc/fc_rc.h"
+#include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "io/beeper.h"
+#include "flight/altitudehold.h"
-#include "io/motors.h"
+#include "flight/failsafe.h"
-#include "io/servos.h"
+#include "flight/imu.h"
-#include "io/gps.h"
+#include "flight/mixer.h"
-#include "io/gimbal.h"
+#include "flight/navigation.h"
-#include "io/serial.h"
+#include "flight/pid.h"
-#include "io/ledstrip.h"
+#include "flight/servos.h"
-#include "io/flashfs.h"
+
-#include "io/transponder_ir.h"
 #include "io/asyncfatfs/asyncfatfs.h"
+#include "io/beeper.h"
+#include "io/flashfs.h"
+#include "io/gimbal.h"
+#include "io/gps.h"
+#include "io/ledstrip.h"
+#include "io/motors.h"
+#include "io/osd.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"
 #include "msp/msp_serial.h"
 
-#include "rx/rx.h"
 #include "rx/msp.h"
+#include "rx/rx.h"
 
 #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 +152,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 (>30s);", 31 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -254,14 +257,6 @@ static void mspRebootFn(serialPort_t *serialPort)
     while (true) ;
 }
 
-static void serializeNames(sbuf_t *dst, const char *s)
-{
-    const char *c;
-    for (c = s; *c; c++) {
-        sbufWriteU8(dst, *c);
-    }
-}
-
 static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
 {
     for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
@@ -286,20 +281,19 @@ static const box_t *findBoxByPermenantId(uint8_t permenantId)
 
 static void serializeBoxNamesReply(sbuf_t *dst)
 {
-    int activeBoxId, flag = 1, count = 0, len;
+    int flag = 1, count = 0;
 
 reset:
     // in first run of the loop, we grab total size of junk to be sent
     // then come back and actually send it
     for (int i = 0; i < activeBoxIdCount; i++) {
-        activeBoxId = activeBoxIds[i];
+        const int activeBoxId = activeBoxIds[i];
-
         const box_t *box = findBoxByActiveBoxId(activeBoxId);
         if (!box) {
             continue;
         }
 
-        len = strlen(box->boxName);
+        const int len = strlen(box->boxName);
         if (flag) {
             count += len;
         } else {
@@ -378,6 +372,9 @@ void initActiveBoxIds(void)
 #ifdef BLACKBOX
     if (feature(FEATURE_BLACKBOX)) {
         activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
+#ifdef USE_FLASHFS
+        activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE;
+#endif
     }
 #endif
 
@@ -442,6 +439,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;
@@ -573,9 +571,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_FC_VARIANT:
-        for (int i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
+        sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
-            sbufWriteU8(dst, flightControllerIdentifier[i]);
-        }
         break;
 
     case MSP_FC_VERSION:
@@ -585,9 +581,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_BOARD_INFO:
-        for (int i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
+        sbufWriteData(dst, boardIdentifier, BOARD_IDENTIFIER_LENGTH);
-            sbufWriteU8(dst, boardIdentifier[i]);
-        }
 #ifdef USE_HARDWARE_REVISION_DETECTION
         sbufWriteU16(dst, hardwareRevision);
 #else
@@ -596,15 +590,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_BUILD_INFO:
-        for (int i = 0; i < BUILD_DATE_LENGTH; i++) {
+        sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
-            sbufWriteU8(dst, buildDate[i]);
+        sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
-        }
+        sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
-        for (int i = 0; i < BUILD_TIME_LENGTH; i++) {
-            sbufWriteU8(dst, buildTime[i]);
-        }
-        for (int i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
-            sbufWriteU8(dst, shortGitRevision[i]);
-        }
         break;
 
     // DEPRECATED - Use MSP_API_VERSION
@@ -632,9 +620,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 
     case MSP_NAME:
         {
-            const int nameLen = strlen(masterConfig.name);
+            const int nameLen = strlen(systemConfig()->name);
             for (int i = 0; i < nameLen; i++) {
-                sbufWriteU8(dst, masterConfig.name[i]);
+                sbufWriteU8(dst, systemConfig()->name[i]);
             }
         }
         break;
@@ -661,7 +649,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
                 sbufWriteU16(dst, acc.accSmooth[i] / scale);
             }
             for (int i = 0; i < 3; i++) {
-                sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
+                sbufWriteU16(dst, gyroRateDps(i));
             }
             for (int i = 0; i < 3; i++) {
                 sbufWriteU16(dst, mag.magADC[i]);
@@ -675,25 +663,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, servoParams(i)->min);
-            sbufWriteU16(dst, servoProfile()->servoConf[i].max);
+            sbufWriteU16(dst, servoParams(i)->max);
-            sbufWriteU16(dst, servoProfile()->servoConf[i].middle);
+            sbufWriteU16(dst, servoParams(i)->middle);
-            sbufWriteU8(dst, servoProfile()->servoConf[i].rate);
+            sbufWriteU8(dst, servoParams(i)->rate);
-            sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin);
+            sbufWriteU8(dst, servoParams(i)->angleAtMin);
-            sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax);
+            sbufWriteU8(dst, servoParams(i)->angleAtMax);
-            sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel);
+            sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
-            sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources);
+            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, customServoMixers(i)->targetChannel);
-            sbufWriteU8(dst, customServoMixer(i)->inputSource);
+            sbufWriteU8(dst, customServoMixers(i)->inputSource);
-            sbufWriteU8(dst, customServoMixer(i)->rate);
+            sbufWriteU8(dst, customServoMixers(i)->rate);
-            sbufWriteU8(dst, customServoMixer(i)->speed);
+            sbufWriteU8(dst, customServoMixers(i)->speed);
-            sbufWriteU8(dst, customServoMixer(i)->min);
+            sbufWriteU8(dst, customServoMixers(i)->min);
-            sbufWriteU8(dst, customServoMixer(i)->max);
+            sbufWriteU8(dst, customServoMixers(i)->max);
-            sbufWriteU8(dst, customServoMixer(i)->box);
+            sbufWriteU8(dst, customServoMixers(i)->box);
         }
         break;
 #endif
@@ -780,7 +768,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_PIDNAMES:
-        serializeNames(dst, pidnames);
+        for (const char *c = pidnames; *c; c++) {
+            sbufWriteU8(dst, *c);
+        }
         break;
 
     case MSP_PID_CONTROLLER:
@@ -789,7 +779,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 
     case MSP_MODE_RANGES:
         for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
-            modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
+            const modeActivationCondition_t *mac = modeActivationConditions(i);
             const box_t *box = &boxes[mac->modeId];
             sbufWriteU8(dst, box->permanentId);
             sbufWriteU8(dst, mac->auxChannelIndex);
@@ -800,7 +790,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 +957,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);
+            sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
-            sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step));
+            sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
         }
         break;
 
@@ -977,9 +967,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_RX_MAP:
-        for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
+        sbufWriteData(dst, rxConfig()->rcmap, MAX_MAPPABLE_RX_INPUTS);
-            sbufWriteU8(dst, rxConfig()->rcmap[i]);
-        }
         break;
 
     case MSP_BF_CONFIG:
@@ -1014,7 +1002,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 +1011,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 +1079,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 #else
         sbufWriteU8(dst, 0);
 #endif
-        sbufWriteU8(dst, osdProfile()->units);
+        sbufWriteU8(dst, osdConfig()->units);
-        sbufWriteU8(dst, osdProfile()->rssi_alarm);
+        sbufWriteU8(dst, osdConfig()->rssi_alarm);
-        sbufWriteU16(dst, osdProfile()->cap_alarm);
+        sbufWriteU16(dst, osdConfig()->cap_alarm);
-        sbufWriteU16(dst, osdProfile()->time_alarm);
+        sbufWriteU16(dst, osdConfig()->time_alarm);
-        sbufWriteU16(dst, osdProfile()->alt_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
@@ -1105,9 +1093,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_BF_BUILD_INFO:
-        for (int i = 0; i < 11; i++) {
+        sbufWriteData(dst, buildDate, 11); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
-            sbufWriteU8(dst, buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
-        }
         sbufWriteU32(dst, 0); // future exp
         sbufWriteU32(dst, 0); // future exp
         break;
@@ -1139,13 +1125,14 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
             sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
             sbufWriteU8(dst, pidConfig()->pid_process_denom);
         }
-        sbufWriteU8(dst, motorConfig()->useUnsyncedPwm);
+        sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
-        sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
+        sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
-        sbufWriteU16(dst, motorConfig()->motorPwmRate);
+        sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
         sbufWriteU16(dst, (uint16_t)lrintf(motorConfig()->digitalIdleOffsetPercent * 100));
         sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
         //!!TODO gyro_isr_update to be added pending decision
         //sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
+        sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
         break;
 
     case MSP_FILTER_CONFIG :
@@ -1316,12 +1303,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 #endif
         break;
     case MSP_SET_ACC_TRIM:
-        accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src);
+        accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
-        accelerometerConfig()->accelerometerTrims.values.roll  = sbufReadU16(src);
+        accelerometerConfigMutable()->accelerometerTrims.values.roll  = sbufReadU16(src);
         break;
     case MSP_SET_ARMING_CONFIG:
-        armingConfig()->auto_disarm_delay = sbufReadU8(src);
+        armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
-        armingConfig()->disarm_kill_switch = sbufReadU8(src);
+        armingConfigMutable()->disarm_kill_switch = sbufReadU8(src);
         break;
 
     case MSP_SET_LOOP_TIME:
@@ -1343,7 +1330,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 +1339,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 +1351,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 +1394,32 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_MISC:
-        rxConfig()->midrc = sbufReadU16(src);
+        rxConfigMutable()->midrc = sbufReadU16(src);
-        motorConfig()->minthrottle = sbufReadU16(src);
+        motorConfigMutable()->minthrottle = sbufReadU16(src);
-        motorConfig()->maxthrottle = sbufReadU16(src);
+        motorConfigMutable()->maxthrottle = sbufReadU16(src);
-        motorConfig()->mincommand = 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);
+        batteryConfigMutable()->multiwiiCurrentMeterOutput = sbufReadU8(src);
-        rxConfig()->rssi_channel = 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
+        batteryConfigMutable()->vbatscale = sbufReadU8(src);           // actual vbatscale as intended
-        batteryConfig()->vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
+        batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
-        batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
+        batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
-        batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
+        batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
         break;
 
     case MSP_SET_MOTOR:
@@ -1450,14 +1437,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);
+            servoParamsMutable(i)->min = sbufReadU16(src);
-            servoProfile()->servoConf[i].max = sbufReadU16(src);
+            servoParamsMutable(i)->max = sbufReadU16(src);
-            servoProfile()->servoConf[i].middle = sbufReadU16(src);
+            servoParamsMutable(i)->middle = sbufReadU16(src);
-            servoProfile()->servoConf[i].rate = sbufReadU8(src);
+            servoParamsMutable(i)->rate = sbufReadU8(src);
-            servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src);
+            servoParamsMutable(i)->angleAtMin = sbufReadU8(src);
-            servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src);
+            servoParamsMutable(i)->angleAtMax = sbufReadU8(src);
-            servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src);
+            servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
-            servoProfile()->servoConf[i].reversedSources = sbufReadU32(src);
+            servoParamsMutable(i)->reversedSources = sbufReadU32(src);
         }
 #endif
         break;
@@ -1468,76 +1455,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);
+            customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
-            customServoMixer(i)->inputSource = sbufReadU8(src);
+            customServoMixersMutable(i)->inputSource = sbufReadU8(src);
-            customServoMixer(i)->rate = sbufReadU8(src);
+            customServoMixersMutable(i)->rate = sbufReadU8(src);
-            customServoMixer(i)->speed = sbufReadU8(src);
+            customServoMixersMutable(i)->speed = sbufReadU8(src);
-            customServoMixer(i)->min = sbufReadU8(src);
+            customServoMixersMutable(i)->min = sbufReadU8(src);
-            customServoMixer(i)->max = sbufReadU8(src);
+            customServoMixersMutable(i)->max = sbufReadU8(src);
-            customServoMixer(i)->box = sbufReadU8(src);
+            customServoMixersMutable(i)->box = sbufReadU8(src);
             loadCustomServoMixer();
         }
 #endif
         break;
 
     case MSP_SET_3D:
-        flight3DConfig()->deadband3d_low = sbufReadU16(src);
+        flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
-        flight3DConfig()->deadband3d_high = sbufReadU16(src);
+        flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
-        flight3DConfig()->neutral3d = sbufReadU16(src);
+        flight3DConfigMutable()->neutral3d = sbufReadU16(src);
         break;
 
     case MSP_SET_RC_DEADBAND:
-        rcControlsConfig()->deadband = sbufReadU8(src);
+        rcControlsConfigMutable()->deadband = sbufReadU8(src);
-        rcControlsConfig()->yaw_deadband = sbufReadU8(src);
+        rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
-        rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
+        rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
-        flight3DConfig()->deadband3d_throttle = sbufReadU16(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);
+        gyroConfigMutable()->gyro_align = sbufReadU8(src);
-        accelerometerConfig()->acc_align = sbufReadU8(src);
+        accelerometerConfigMutable()->acc_align = sbufReadU8(src);
-        compassConfig()->mag_align = sbufReadU8(src);
+        compassConfigMutable()->mag_align = sbufReadU8(src);
         break;
 
     case MSP_SET_ADVANCED_CONFIG:
-        gyroConfig()->gyro_sync_denom = sbufReadU8(src);
+        gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
-        pidConfig()->pid_process_denom = sbufReadU8(src);
+        pidConfigMutable()->pid_process_denom = sbufReadU8(src);
-        motorConfig()->useUnsyncedPwm = sbufReadU8(src);
+        motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
 #ifdef USE_DSHOT
-        motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
+        motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
 #else
-        motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
+        motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
 #endif
-        motorConfig()->motorPwmRate = sbufReadU16(src);
+        motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
-        if (dataSize > 7) {
+        if (sbufBytesRemaining(src) >= 2) {
-            motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
+            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()->dev.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);
+            gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
-            gyroConfig()->gyro_soft_notch_cutoff_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);
+            gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
-            gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
+            gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
         }
         // reinitialize the gyro filters with the new values
         validateAndFixGyroConfig();
@@ -1567,9 +1558,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_SENSOR_CONFIG:
-        accelerometerConfig()->acc_hardware = sbufReadU8(src);
+        accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
-        barometerConfig()->baro_hardware = sbufReadU8(src);
+        barometerConfigMutable()->baro_hardware = sbufReadU8(src);
-        compassConfig()->mag_hardware = sbufReadU8(src);
+        compassConfigMutable()->mag_hardware = sbufReadU8(src);
         break;
 
     case MSP_RESET_CONF:
@@ -1601,9 +1592,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);
+            blackboxConfigMutable()->device = sbufReadU8(src);
-            blackboxConfig()->rate_num = sbufReadU8(src);
+            blackboxConfigMutable()->rate_num = sbufReadU8(src);
-            blackboxConfig()->rate_denom = sbufReadU8(src);
+            blackboxConfigMutable()->rate_denom = sbufReadU8(src);
         }
         break;
 #endif
@@ -1627,20 +1618,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             // set all the other settings
             if ((int8_t)addr == -1) {
 #ifdef USE_MAX7456
-                vcdProfile()->video_system = sbufReadU8(src);
+                vcdProfileMutable()->video_system = sbufReadU8(src);
 #else
                 sbufReadU8(src); // Skip video system
 #endif
-                osdProfile()->units = sbufReadU8(src);
+                osdConfigMutable()->units = sbufReadU8(src);
-                osdProfile()->rssi_alarm = sbufReadU8(src);
+                osdConfigMutable()->rssi_alarm = sbufReadU8(src);
-                osdProfile()->cap_alarm = sbufReadU16(src);
+                osdConfigMutable()->cap_alarm = sbufReadU16(src);
-                osdProfile()->time_alarm = sbufReadU16(src);
+                osdConfigMutable()->time_alarm = sbufReadU16(src);
-                osdProfile()->alt_alarm = sbufReadU16(src);
+                osdConfigMutable()->alt_alarm = sbufReadU16(src);
             } else {
                 // set a position setting
                 const uint16_t pos  = sbufReadU16(src);
                 if (addr < OSD_ITEM_COUNT) {
-                    osdProfile()->item_pos[addr] = pos;
+                    osdConfigMutable()->item_pos[addr] = pos;
                 }
             }
         }
@@ -1759,85 +1750,85 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_BOARD_ALIGNMENT:
-        boardAlignment()->rollDegrees = sbufReadU16(src);
+        boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
-        boardAlignment()->pitchDegrees = sbufReadU16(src);
+        boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
-        boardAlignment()->yawDegrees = sbufReadU16(src);
+        boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
         break;
 
     case MSP_SET_VOLTAGE_METER_CONFIG:
-        batteryConfig()->vbatscale = sbufReadU8(src);           // actual vbatscale as intended
+        batteryConfigMutable()->vbatscale = sbufReadU8(src);           // actual vbatscale as intended
-        batteryConfig()->vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
+        batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
-        batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
+        batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
-        batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
+        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);
+        batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
-        batteryConfig()->currentMeterOffset = sbufReadU16(src);
+        batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
-        batteryConfig()->currentMeterType = sbufReadU8(src);
+        batteryConfigMutable()->currentMeterType = sbufReadU8(src);
-        batteryConfig()->batteryCapacity = sbufReadU16(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);
+        rxConfigMutable()->serialrx_provider = sbufReadU8(src);
-        rxConfig()->maxcheck = sbufReadU16(src);
+        rxConfigMutable()->maxcheck = sbufReadU16(src);
-        rxConfig()->midrc = sbufReadU16(src);
+        rxConfigMutable()->midrc = sbufReadU16(src);
-        rxConfig()->mincheck = sbufReadU16(src);
+        rxConfigMutable()->mincheck = sbufReadU16(src);
-        rxConfig()->spektrum_sat_bind = sbufReadU8(src);
+        rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
         if (dataSize > 8) {
-            rxConfig()->rx_min_usec = sbufReadU16(src);
+            rxConfigMutable()->rx_min_usec = sbufReadU16(src);
-            rxConfig()->rx_max_usec = sbufReadU16(src);
+            rxConfigMutable()->rx_max_usec = sbufReadU16(src);
         }
         if (dataSize > 12) {
-            rxConfig()->rcInterpolation = sbufReadU8(src);
+            rxConfigMutable()->rcInterpolation = sbufReadU8(src);
-            rxConfig()->rcInterpolationInterval = sbufReadU8(src);
+            rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
-            rxConfig()->airModeActivateThreshold = sbufReadU16(src);
+            rxConfigMutable()->airModeActivateThreshold = sbufReadU16(src);
         }
         if (dataSize > 16) {
-            rxConfig()->rx_spi_protocol = sbufReadU8(src);
+            rxConfigMutable()->rx_spi_protocol = sbufReadU8(src);
-            rxConfig()->rx_spi_id = sbufReadU32(src);
+            rxConfigMutable()->rx_spi_id = sbufReadU32(src);
-            rxConfig()->rx_spi_rf_channel_count = sbufReadU8(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);
+        failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
-        failsafeConfig()->failsafe_off_delay = sbufReadU8(src);
+        failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
-        failsafeConfig()->failsafe_throttle = sbufReadU16(src);
+        failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
-        failsafeConfig()->failsafe_kill_switch = sbufReadU8(src);
+        failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src);
-        failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src);
+        failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
-        failsafeConfig()->failsafe_procedure = sbufReadU8(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);
+            rxConfigMutable()->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].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 +1836,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
+        boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll
-        boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
+        boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch
-        boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
+        boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw
 
-        batteryConfig()->currentMeterScale = sbufReadU16(src);
+        batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
-        batteryConfig()->currentMeterOffset = sbufReadU16(src);
+        batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
         break;
 
     case MSP_SET_CF_SERIAL_CONFIG:
@@ -1892,7 +1883,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 +1896,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();
         }
@@ -1924,9 +1915,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 #endif
 
     case MSP_SET_NAME:
-        memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
+        memset(systemConfigMutable()->name, 0, ARRAYLEN(systemConfig()->name));
         for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
-            masterConfig.name[i] = sbufReadU8(src);
+            systemConfigMutable()->name[i] = sbufReadU8(src);
         }
         break;
 

src/main/fc/fc_tasks.c

@@ -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

src/main/fc/rc_adjustments.c

@@ -0,0 +1,457 @@
+/*
+ * 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/fc_rc.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;
+            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)
+{
+    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 resetAdjustmentStates(void)
+{
+    memset(adjustmentStates, 0, sizeof(adjustmentStates));
+}
+
+void updateAdjustmentStates(void)
+{
+    for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
+        const adjustmentRange_t * const 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 useAdjustmentConfig(pidProfile_t *pidProfileToUse)
+{
+    pidProfile = pidProfileToUse;
+}

src/main/fc/rc_adjustments.h

@@ -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(void);
+struct controlRateConfig_s;
+void processRcAdjustments(struct controlRateConfig_s *controlRateConfig);
+struct pidProfile_s;
+void useAdjustmentConfig(struct pidProfile_s *pidProfileToUse);

src/main/fc/rc_controls.c

@@ -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;
@@ -332,24 +302,22 @@ bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationC
     return false;
 }
 
-bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) {
+bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
     if (!IS_RANGE_USABLE(range)) {
         return false;
     }
 
-    uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
+    const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
     return (channelValue >= 900 + (range->startStep * 25) &&
             channelValue < 900 + (range->endStep * 25));
 }
 
-void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
+void updateActivatedModes(void)
 {
     rcModeActivationMask = 0;
 
-    uint8_t index;
+    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-
+        const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index);
-    for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
 
         if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
             ACTIVATE_RC_MODE(modeActivationCondition->modeId);
@@ -357,386 +325,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));
-}
-

src/main/fc/rc_controls.h

@@ -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,9 @@ typedef struct controlRateConfig_s {
     uint16_t tpa_breakpoint;                // Breakpoint where TPA is activated
 } controlRateConfig_t;
 
+#define MAX_CONTROL_RATE_PROFILE_COUNT 3
+PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);
+
 extern int16_t rcCommand[4];
 
 typedef struct rcControlsConfig_s {
@@ -168,117 +176,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(void);
-throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
+void processRcStickPositions(throttleStatus_e throttleStatus);
-void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
 
-bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range);
+bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
-void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
+void updateActivatedModes(void);
-
-
-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(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse);
-void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse);

src/main/flight/altitudehold.c

@@ -21,25 +21,27 @@
 #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 "config/parameter_group.h"
-#include "sensors/sonar.h"
+#include "config/parameter_group_ids.h"
+
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "flight/altitudehold.h"
+#include "flight/imu.h"
+#include "flight/pid.h"
 
 #include "rx/rx.h"
 
-#include "fc/rc_controls.h"
+#include "sensors/barometer.h"
-#include "io/motors.h"
+#include "sensors/sonar.h"
-
-#include "flight/pid.h"
-#include "flight/imu.h"
-
-#include "fc/runtime_config.h"
 
 
 int32_t setVelocity = 0;
@@ -50,22 +52,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(
+void configureAltitudeHold(pidProfile_t *initialPidProfile)
-        pidProfile_t *initialPidProfile,
-        barometerConfig_t *intialBarometerConfig,
-        rcControlsConfig_t *initialRcControlsConfig,
-        motorConfig_t *initialMotorConfig
-)
 {
     pidProfile = initialPidProfile;
-    barometerConfig = intialBarometerConfig;
-    rcControlsConfig = initialRcControlsConfig;
-    motorConfig = initialMotorConfig;
 }
 
 #if defined(BARO) || defined(SONAR)
@@ -82,12 +73,12 @@ 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;
@@ -97,7 +88,7 @@ static void applyMultirotorAltHold(void)
         }
     } 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;
@@ -111,19 +102,19 @@ static void applyMultirotorAltHold(void)
     }
 }
 
-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 +263,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 +299,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

src/main/flight/altitudehold.h

@@ -17,19 +17,23 @@
 
 #pragma once
 
+#include "common/time.h"
+
 extern int32_t AltHold;
 extern int32_t vario;
 
+typedef struct airplaneConfig_s {
+    int8_t fixedwing_althold_dir;           // +1 or -1 for pitch/althold gain. later check if need more than just sign
+} airplaneConfig_t;
+
+PG_DECLARE(airplaneConfig_t, airplaneConfig);
+
 void calculateEstimatedAltitude(timeUs_t currentTimeUs);
 
 struct pidProfile_s;
-struct barometerConfig_s;
+void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
-struct rcControlsConfig_s;
-struct motorConfig_s;
-void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct motorConfig_s *initialMotorConfig);
 
-struct airplaneConfig_s;
+void applyAltHold(void);
-void applyAltHold(struct airplaneConfig_s *airplaneConfig);
 void updateAltHoldState(void);
 void updateSonarAltHoldState(void);
 

src/main/flight/failsafe.c

@@ -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;
+/*
-
+ * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
-static rxConfig_t *rxConfig;
+ */
-
+void failsafeReset(void)
-static uint16_t deadband3dThrottle;           // default throttle deadband from MIDRC
-
-static 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;
 }
 
-/*
+void failsafeInit(void)
- * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
- */
-void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
 {
-    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)) {
+                    if (THROTTLE_HIGH == calculateThrottleStatus()) {
-                        failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
+                        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);

src/main/flight/failsafe.h

@@ -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(void);
-void failsafeInit(struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle);
+void failsafeReset(void);
-void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
 
 void failsafeStartMonitoring(void);
 void failsafeUpdateState(void);

src/main/flight/imu.c

@@ -21,37 +21,40 @@
 #include <stdint.h>
 #include <math.h>
 
-#include "common/maths.h"
-
 #include "platform.h"
 
 #include "build/build_config.h"
 #include "build/debug.h"
 
 #include "common/axis.h"
+#include "common/maths.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
 
 #include "drivers/system.h"
 
-#include "sensors/sensors.h"
+#include "fc/runtime_config.h"
-#include "sensors/gyro.h"
-#include "sensors/compass.h"
-#include "sensors/acceleration.h"
-#include "sensors/barometer.h"
-#include "sensors/sonar.h"
 
+#include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
-#include "flight/imu.h"
 
 #include "io/gps.h"
 
-#include "fc/runtime_config.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/compass.h"
+#include "sensors/gyro.h"
+#include "sensors/sensors.h"
+#include "sensors/sonar.h"
 
 
 // the limit (in degrees/second) beyond which we stop integrating
 // omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
 // which results in false gyro drift. See
 // http://gentlenav.googlecode.com/files/fastRotations.pdf
+
 #define SPIN_RATE_LIMIT 20
 
 int32_t accSum[XYZ_AXIS_COUNT];
@@ -60,14 +63,13 @@ uint32_t accTimeSum = 0;        // keep track for integration of acc
 int accSumCount = 0;
 float accVelScale;
 
-float throttleAngleScale;
+static float throttleAngleScale;
-float fc_acc;
+static float fc_acc;
-float smallAngleCosZ = 0;
+static float smallAngleCosZ = 0;
 
-float magneticDeclination = 0.0f;       // calculated at startup from config
+static float magneticDeclination = 0.0f;       // calculated at startup from config
 
 static imuRuntimeConfig_t imuRuntimeConfig;
-static pidProfile_t *pidProfile;
 
 STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
 static float rMat[3][3];
@@ -100,18 +102,26 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
     rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
 }
 
-void imuConfigure(
+/*
-    imuConfig_t *imuConfig,
+* Calculate RC time constant used in the accZ lpf.
-    pidProfile_t *initialPidProfile,
+*/
-    uint16_t throttle_correction_angle
+static float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
-)
 {
-    imuRuntimeConfig.dcm_kp = imuConfig->dcm_kp / 10000.0f;
+    return 0.5f / (M_PIf * accz_lpf_cutoff);
-    imuRuntimeConfig.dcm_ki = imuConfig->dcm_ki / 10000.0f;
+}
-    imuRuntimeConfig.acc_unarmedcal = imuConfig->acc_unarmedcal;
+
-    imuRuntimeConfig.small_angle = imuConfig->small_angle;
+static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
+{
+    return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
+}
+
+void imuConfigure(uint16_t throttle_correction_angle)
+{
+    imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f;
+    imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f;
+    imuRuntimeConfig.acc_unarmedcal = imuConfig()->acc_unarmedcal;
+    imuRuntimeConfig.small_angle = imuConfig()->small_angle;
 
-    pidProfile = initialPidProfile;
     fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 }
@@ -124,19 +134,6 @@ void imuInit(void)
     imuComputeRotationMatrix();
 }
 
-float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
-{
-    return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
-}
-
-/*
-* Calculate RC time constant used in the accZ lpf.
-*/
-float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
-{
-    return 0.5f / (M_PIf * accz_lpf_cutoff);
-}
-
 void imuResetAccelerationSum(void)
 {
     accSum[0] = 0;
@@ -146,14 +143,12 @@ void imuResetAccelerationSum(void)
     accTimeSum = 0;
 }
 
-void imuTransformVectorBodyToEarth(t_fp_vector * v)
+static void imuTransformVectorBodyToEarth(t_fp_vector * v)
 {
-    float x,y,z;
-
     /* From body frame to earth frame */
-    x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
+    const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
-    y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
+    const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
-    z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
+    const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
 
     v->V.X = x;
     v->V.Y = -y;
@@ -161,16 +156,15 @@ void imuTransformVectorBodyToEarth(t_fp_vector * v)
 }
 
 // rotate acc into Earth frame and calculate acceleration in it
-void imuCalculateAcceleration(uint32_t deltaT)
+static void imuCalculateAcceleration(uint32_t deltaT)
 {
     static int32_t accZoffset = 0;
     static float accz_smooth = 0;
-    float dT;
-    t_fp_vector accel_ned;
 
     // deltaT is measured in us ticks
-    dT = (float)deltaT * 1e-6f;
+    const float dT = (float)deltaT * 1e-6f;
 
+    t_fp_vector accel_ned;
     accel_ned.V.X = acc.accSmooth[X];
     accel_ned.V.Y = acc.accSmooth[Y];
     accel_ned.V.Z = acc.accSmooth[Z];
@@ -183,8 +177,9 @@ void imuCalculateAcceleration(uint32_t deltaT)
             accZoffset += accel_ned.V.Z;
         }
         accel_ned.V.Z -= accZoffset / 64;  // compensate for gravitation on z-axis
-    } else
+    } else {
         accel_ned.V.Z -= acc.dev.acc_1G;
+    }
 
     accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
 
@@ -224,15 +219,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                 bool useYaw, float yawError)
 {
     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
-    float recipNorm;
-    float hx, hy, bx;
-    float ex = 0, ey = 0, ez = 0;
-    float qa, qb, qc;
 
     // Calculate general spin rate (rad/s)
-    float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
+    const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
 
     // Use raw heading error (from GPS or whatever else)
+    float ez = 0;
     if (useYaw) {
         while (yawError >  M_PIf) yawError -= (2.0f * M_PIf);
         while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
@@ -241,7 +233,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     }
 
     // Use measured magnetic field vector
-    recipNorm = sq(mx) + sq(my) + sq(mz);
+    float ex = 0, ey = 0;
+    float recipNorm = sq(mx) + sq(my) + sq(mz);
     if (useMag && recipNorm > 0.01f) {
         // Normalise magnetometer measurement
         recipNorm = invSqrt(recipNorm);
@@ -254,12 +247,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 
         // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
         // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
-        hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
+        const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
-        hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
+        const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
-        bx = sqrtf(hx * hx + hy * hy);
+        const float bx = sqrtf(hx * hx + hy * hy);
 
         // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
-        float ez_ef = -(hy * bx);
+        const float ez_ef = -(hy * bx);
 
         // Rotate mag error vector back to BF and accumulate
         ex += rMat[2][0] * ez_ef;
@@ -286,7 +279,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     if(imuRuntimeConfig.dcm_ki > 0.0f) {
         // Stop integrating if spinning beyond the certain limit
         if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
-            float dcmKiGain = imuRuntimeConfig.dcm_ki;
+            const float dcmKiGain = imuRuntimeConfig.dcm_ki;
             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
             integralFBy += dcmKiGain * ey * dt;
             integralFBz += dcmKiGain * ez * dt;
@@ -299,7 +292,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     }
 
     // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
-    float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
+    const float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
 
     // Apply proportional and integral feedback
     gx += dcmKpGain * ex + integralFBx;
@@ -311,9 +304,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     gy *= (0.5f * dt);
     gz *= (0.5f * dt);
 
-    qa = q0;
+    const float qa = q0;
-    qb = q1;
+    const float qb = q1;
-    qc = q2;
+    const float qc = q2;
     q0 += (-qb * gx - qc * gy - q3 * gz);
     q1 += (qa * gx + qc * gz - q3 * gy);
     q2 += (qa * gy - qb * gz + q3 * gx);
@@ -350,10 +343,8 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 
 static bool imuIsAccelerometerHealthy(void)
 {
-    int32_t axis;
     int32_t accMagnitude = 0;
-
+    for (int axis = 0; axis < 3; axis++) {
-    for (axis = 0; axis < 3; axis++) {
         accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis];
     }
 

src/main/flight/imu.h

@@ -18,10 +18,9 @@
 #pragma once
 
 #include "common/axis.h"
-#include "common/maths.h"
 #include "common/time.h"
 
-#include "sensors/acceleration.h"
+#include "config/parameter_group.h"
 
 // Exported symbols
 extern uint32_t accTimeSum;
@@ -30,11 +29,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 {
@@ -52,11 +46,6 @@ typedef struct accDeadband_s {
     uint8_t z;                  // set the acc deadband for z-Axis, this ignores small accelerations
 } accDeadband_t;
 
-typedef struct throttleCorrectionConfig_s {
-    uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
-    uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
-} throttleCorrectionConfig_t;
-
 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 +54,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;
@@ -73,38 +64,12 @@ typedef struct imuRuntimeConfig_s {
     accDeadband_t accDeadband;
 } imuRuntimeConfig_t;
 
-typedef enum {
+void imuConfigure(uint16_t throttle_correction_angle);
-    ACCPROC_READ = 0,
-    ACCPROC_CHUNK_1,
-    ACCPROC_CHUNK_2,
-    ACCPROC_CHUNK_3,
-    ACCPROC_CHUNK_4,
-    ACCPROC_CHUNK_5,
-    ACCPROC_CHUNK_6,
-    ACCPROC_CHUNK_7,
-    ACCPROC_COPY
-} accProcessorState_e;
-
-typedef struct accProcessor_s {
-    accProcessorState_e state;
-} accProcessor_t;
-
-struct pidProfile_s;
-void imuConfigure(
-    imuConfig_t *imuConfig,
-    struct pidProfile_s *initialPidProfile,
-    uint16_t throttle_correction_angle
-);
 
 float getCosTiltAngle(void);
 void calculateEstimatedAltitude(timeUs_t currentTimeUs);
 void imuUpdateAttitude(timeUs_t currentTimeUs);
-float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
-float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
-
-union u_fp_vector;
-int16_t imuCalculateHeading(union u_fp_vector *vec);
 
 void imuResetAccelerationSum(void);
 void imuInit(void);

src/main/flight/mixer.c

@@ -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
@@ -62,12 +63,6 @@ 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];
 
@@ -236,8 +231,6 @@ const mixer_t mixers[] = {
 };
 #endif
 
-static motorMixer_t *customMixers;
-
 static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
 uint16_t motorOutputHigh, motorOutputLow;
 static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
@@ -254,7 +247,7 @@ float getMotorMixRange()
 
 bool isMotorProtocolDshot(void) {
 #ifdef USE_DSHOT
-    switch(motorConfig->motorPwmProtocol) {
+    switch(motorConfig()->dev.motorPwmProtocol) {
     case PWM_TYPE_DSHOT1200:
     case PWM_TYPE_DSHOT600:
     case PWM_TYPE_DSHOT300:
@@ -274,47 +267,31 @@ 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;
+        disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
-        motorOutputLow = motorConfig->minthrottle;
+        motorOutputLow = motorConfig()->minthrottle;
-        motorOutputHigh = motorConfig->maxthrottle;
+        motorOutputHigh = motorConfig()->maxthrottle;
-        deadbandMotor3dHigh = flight3DConfig->deadband3d_high;
+        deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
-        deadbandMotor3dLow = flight3DConfig->deadband3d_low;
+        deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
     }
 
-    rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck);
+    rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck);
-    rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle;
+    rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle;
-    rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
+    rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
 }
 
-void mixerUseConfigs(
+void mixerInit(mixerMode_e mixerMode)
-        flight3DConfig_t *flight3DConfigToUse,
-        motorConfig_t *motorConfigToUse,
-        mixerConfig_t *mixerConfigToUse,
-        airplaneConfig_t *airplaneConfigToUse,
-        rxConfig_t *rxConfigToUse)
-{
-    flight3DConfig = flight3DConfigToUse;
-    motorConfig = motorConfigToUse;
-    mixerConfig = mixerConfigToUse;
-    airplaneConfig = airplaneConfigToUse;
-    rxConfig = rxConfigToUse;
-}
-
-void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
 {
     currentMixerMode = mixerMode;
 
-    customMixers = initialCustomMixers;
-
     initEscEndpoints();
 }
 
@@ -328,9 +305,9 @@ void mixerConfigureOutput(void)
         // load custom mixer into currentMixer
         for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
             // check if done
-            if (customMixers[i].throttle == 0.0f)
+            if (customMotorMixer(i)->throttle == 0.0f)
                 break;
-            currentMixer[i] = customMixers[i];
+            currentMixer[i] = *customMotorMixer(i);
             motorCount++;
         }
     } else {
@@ -441,25 +418,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
@@ -469,7 +446,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;
@@ -485,7 +462,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];
@@ -494,7 +471,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
@@ -542,7 +519,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;
             }
         }

src/main/flight/mixer.h

@@ -17,7 +17,9 @@
 
 #pragma once
 
-#define MAX_SUPPORTED_MOTORS 12
+#include "config/parameter_group.h"
+#include "drivers/io_types.h"
+#include "drivers/pwm_output.h"
 
 #define QUAD_MOTOR_COUNT 4
 
@@ -85,6 +87,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,38 +101,30 @@ typedef struct mixerConfig_s {
     int8_t yaw_motor_direction;
 } mixerConfig_t;
 
-typedef struct flight3DConfig_s {
+PG_DECLARE(mixerConfig_t, mixerConfig);
-    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;
 
-typedef struct airplaneConfig_s {
+typedef struct motorConfig_s {
-    int8_t fixedwing_althold_dir;           // +1 or -1 for pitch/althold gain. later check if need more than just sign
+    motorDevConfig_t dev;
-} airplaneConfig_t;
+    float    digitalIdleOffsetPercent;
+    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
+} motorConfig_t;
+
+PG_DECLARE(motorConfig_t, motorConfig);
 
 #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
 
 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 mixerLoadMix(int index, motorMixer_t *customMixers);
-void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
+void mixerInit(mixerMode_e mixerMode);
 
 void mixerConfigureOutput(void);
 

src/main/flight/navigation.c

@@ -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 "flight/imu.h"
-#include "sensors/boardalignment.h"
+#include "flight/navigation.h"
-#include "sensors/acceleration.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;
 
@@ -72,17 +75,9 @@ static int16_t nav[2];
 static int16_t nav_rated[2];               // Adding a rate controller to the navigation to make it smoother
 navigationMode_e nav_mode = NAV_MODE_NONE;    // Navigation mode
 
-static gpsProfile_t *gpsProfile;
-
-void gpsUseProfile(gpsProfile_t *gpsProfileToUse)
-{
-    gpsProfile = gpsProfileToUse;
-}
-
 // When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
-void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile)
+void navigationInit(pidProfile_t *pidProfile)
 {
-    gpsUseProfile(initialGpsProfile);
     gpsUsePIDs(pidProfile);
 }
 
@@ -163,7 +158,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
 
     // Low pass filter cut frequency for derivative calculation
     // Set to  "1 / ( 2 * PI * gps_lpf )
-    float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf));
+    float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile()->gps_lpf));
     // discrete low pass filter, cuts out the
     // high frequency noise that can drive the controller crazy
     pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);
@@ -327,13 +322,13 @@ void onGpsNewData(void)
             break;
 
         case NAV_MODE_WP:
-            speed = GPS_calc_desired_speed(gpsProfile->nav_speed_max, NAV_SLOW_NAV);    // slow navigation
+            speed = GPS_calc_desired_speed(gpsProfile()->nav_speed_max, NAV_SLOW_NAV);    // slow navigation
             // use error as the desired rate towards the target
             // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
             GPS_calc_nav_rate(speed);
 
             // Tail control
-            if (gpsProfile->nav_controls_heading) {
+            if (gpsProfile()->nav_controls_heading) {
                 if (NAV_TAIL_FIRST) {
                     magHold = wrap_18000(nav_bearing - 18000) / 100;
                 } else {
@@ -341,7 +336,7 @@ void onGpsNewData(void)
                 }
             }
             // Are we there yet ?(within x meters of the destination)
-            if ((wp_distance <= gpsProfile->gps_wp_radius) || check_missed_wp()) {      // if yes switch to poshold mode
+            if ((wp_distance <= gpsProfile()->gps_wp_radius) || check_missed_wp()) {      // if yes switch to poshold mode
                 nav_mode = NAV_MODE_POSHOLD;
                 if (NAV_SET_TAKEOFF_HEADING) {
                     magHold = nav_takeoff_bearing;
@@ -431,7 +426,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
     nav_bearing = target_bearing;
     GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
     original_target_bearing = target_bearing;
-    waypoint_speed_gov = gpsProfile->nav_speed_min;
+    waypoint_speed_gov = gpsProfile()->nav_speed_min;
 }
 
 ////////////////////////////////////////////////////////////////////////////////////
@@ -610,7 +605,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
         max_speed = MIN(max_speed, wp_distance / 2);
     } else {
         max_speed = MIN(max_speed, wp_distance);
-        max_speed = MAX(max_speed, gpsProfile->nav_speed_min);      // go at least 100cm/s
+        max_speed = MAX(max_speed, gpsProfile()->nav_speed_min);      // go at least 100cm/s
     }
 
     // limit the ramp up of the speed
@@ -647,9 +642,9 @@ void updateGpsStateForHomeAndHoldMode(void)
 {
     float sin_yaw_y = sin_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
     float cos_yaw_x = cos_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
-    if (gpsProfile->nav_slew_rate) {
+    if (gpsProfile()->nav_slew_rate) {
-        nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8
+        nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile()->nav_slew_rate, gpsProfile()->nav_slew_rate); // TODO check this on uint8
-        nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate);
+        nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile()->nav_slew_rate, gpsProfile()->nav_slew_rate);
         GPS_angle[AI_ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
         GPS_angle[AI_PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
     } else {
@@ -693,7 +688,7 @@ void updateGpsWaypointsAndMode(void)
             // process HOLD mode
             //
 
-            if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile->ap_mode)) {
+            if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile()->ap_mode)) {
                 if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
 
                     // Transition to HOLD mode

src/main/flight/navigation.h

@@ -36,6 +36,8 @@ typedef struct gpsProfile_s {
     uint16_t ap_mode;                       // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
 } gpsProfile_t;
 
+PG_DECLARE(gpsProfile_t, gpsProfile);
+
 extern int16_t GPS_angle[ANGLE_INDEX_COUNT];                // it's the angles that must be applied for GPS correction
 
 extern int32_t GPS_home[2];
@@ -47,11 +49,10 @@ extern int16_t GPS_directionToHome;        // direction to home or hol point in
 extern navigationMode_e nav_mode;          // Navigation mode
 
 struct pidProfile_s;
-void navigationInit(gpsProfile_t *initialGpsProfile, struct pidProfile_s *pidProfile);
+void navigationInit(struct pidProfile_s *pidProfile);
 void GPS_reset_home_position(void);
 void GPS_reset_nav(void);
 void GPS_set_next_wp(int32_t* lat, int32_t* lon);
-void gpsUseProfile(gpsProfile_t *gpsProfileToUse);
 void gpsUsePIDs(struct pidProfile_s *pidProfile);
 void updateGpsStateForHomeAndHoldMode(void);
 void updateGpsWaypointsAndMode(void);

src/main/flight/pid.c

@@ -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"
 

src/main/flight/pid.h

@@ -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);
 

src/main/flight/servos.c

@@ -27,6 +27,10 @@
 #include "build/build_config.h"
 
 #include "common/filter.h"
+#include "common/maths.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
 
 #include "drivers/pwm_output.h"
 #include "drivers/system.h"
@@ -34,11 +38,11 @@
 #include "rx/rx.h"
 
 #include "io/gimbal.h"
-#include "io/servos.h"
 
+#include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
-#include "flight/imu.h"
+#include "flight/servos.h"
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
@@ -47,13 +51,9 @@
 #include "config/feature.h"
 
 extern mixerMode_e currentMixerMode;
-extern rxConfig_t *rxConfig;
-
-static servoMixerConfig_t *servoMixerConfig;
 
 static uint8_t servoRuleCount = 0;
 static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
-static gimbalConfig_t *gimbalConfig;
 int16_t servo[MAX_SUPPORTED_SERVOS];
 static int useServo;
 static servoParam_t *servoConf;
@@ -111,6 +111,12 @@ static const servoMixer_t servoMixerGimbal[] = {
 };
 
 
+// Custom mixer configuration
+typedef struct mixerRules_s {
+    uint8_t servoRuleCount;
+    const servoMixer_t *rule;
+} mixerRules_t;
+
 const mixerRules_t servoMixers[] = {
     { 0, NULL },                // entry 0
     { COUNT_SERVO_RULES(servoMixerTri), servoMixerTri },       // MULTITYPE_TRI
@@ -141,19 +147,15 @@ const mixerRules_t servoMixers[] = {
     { 0, NULL },
 };
 
-static servoMixer_t *customServoMixers;
+void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse)
-
-void servoUseConfigs(servoMixerConfig_t *servoMixerConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse)
 {
-    servoMixerConfig = servoMixerConfigToUse;
     servoConf = servoParamsToUse;
-    gimbalConfig = gimbalConfigToUse;
     channelForwardingConfig = channelForwardingConfigToUse;
 }
 
 int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
 {
-    uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
+    const uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
 
     if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) {
         return rcData[channelToForwardFrom];
@@ -162,7 +164,6 @@ int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
     return servoConf[servoIndex].middle;
 }
 
-
 int servoDirection(int servoIndex, int inputSource)
 {
     // determine the direction (reversed or not) from the direction bitfield of the servo
@@ -172,10 +173,8 @@ int servoDirection(int servoIndex, int inputSource)
         return 1;
 }
 
-void servoMixerInit(servoMixer_t *initialCustomServoMixers)
+void servosInit(void)
 {
-    customServoMixers = initialCustomServoMixers;
-
     // enable servos for mixes that require them. note, this shifts motor counts.
     useServo = mixers[currentMixerMode].useServo;
     // if we want camstab/trig, that also enables servos, even if mixer doesn't
@@ -197,10 +196,10 @@ void loadCustomServoMixer(void)
     // load custom mixer into currentServoMixer
     for (uint8_t i = 0; i < MAX_SERVO_RULES; i++) {
         // check if done
-        if (customServoMixers[i].rate == 0)
+        if (customServoMixers(i)->rate == 0)
             break;
 
-        currentServoMixer[i] = customServoMixers[i];
+        currentServoMixer[i] = *customServoMixers(i);
         servoRuleCount++;
     }
 }
@@ -266,10 +265,15 @@ static void updateGimbalServos(uint8_t firstServoIndex)
     pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
 }
 
+static void servoTable(void);
+static void filterServos(void);
+
 void writeServos(void)
 {
-    uint8_t servoIndex = 0;
+    servoTable();
+    filterServos();
 
+    uint8_t servoIndex = 0;
     switch (currentMixerMode) {
     case MIXER_BICOPTER:
         pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
@@ -278,7 +282,7 @@ void writeServos(void)
 
     case MIXER_TRI:
     case MIXER_CUSTOM_TRI:
-        if (servoMixerConfig->tri_unarmed_servo) {
+        if (servoConfig()->tri_unarmed_servo) {
             // if unarmed flag set, we always move servo
             pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
         } else {
@@ -348,7 +352,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;
         }
     }
@@ -364,14 +368,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_ROLL]     = rcData[ROLL]     - rxConfig()->midrc;
-    input[INPUT_RC_PITCH]    = rcData[PITCH]    - rxConfig->midrc;
+    input[INPUT_RC_PITCH]    = rcData[PITCH]    - rxConfig()->midrc;
-    input[INPUT_RC_YAW]      = rcData[YAW]      - rxConfig->midrc;
+    input[INPUT_RC_YAW]      = rcData[YAW]      - rxConfig()->midrc;
-    input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
+    input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc;
-    input[INPUT_RC_AUX1]     = rcData[AUX1]     - rxConfig->midrc;
+    input[INPUT_RC_AUX1]     = rcData[AUX1]     - rxConfig()->midrc;
-    input[INPUT_RC_AUX2]     = rcData[AUX2]     - rxConfig->midrc;
+    input[INPUT_RC_AUX2]     = rcData[AUX2]     - rxConfig()->midrc;
-    input[INPUT_RC_AUX3]     = rcData[AUX3]     - rxConfig->midrc;
+    input[INPUT_RC_AUX3]     = rcData[AUX3]     - rxConfig()->midrc;
-    input[INPUT_RC_AUX4]     = rcData[AUX4]     - rxConfig->midrc;
+    input[INPUT_RC_AUX4]     = rcData[AUX4]     - rxConfig()->midrc;
 
     for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
         servo[i] = 0;
@@ -408,7 +412,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
 }
 
 
-void servoTable(void)
+static void servoTable(void)
 {
     // airplane / servo mixes
     switch (currentMixerMode) {
@@ -442,7 +446,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 {
@@ -463,7 +467,7 @@ bool isMixerUsingServos(void)
     return useServo;
 }
 
-void filterServos(void)
+static void filterServos(void)
 {
     static int16_t servoIdx;
     static bool servoFilterIsSet;
@@ -473,10 +477,10 @@ void filterServos(void)
     uint32_t startTime = micros();
 #endif
 
-    if (servoMixerConfig->servo_lowpass_enable) {
+    if (servoConfig()->servo_lowpass_freq) {
         for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
             if (!servoFilterIsSet) {
-                biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime);
+                biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
                 servoFilterIsSet = true;
             }
 

src/main/flight/servos.h

@@ -17,7 +17,9 @@
 
 #pragma once
 
-#define MAX_SUPPORTED_SERVOS 8
+#include "config/parameter_group.h"
+#include "drivers/io_types.h"
+#include "drivers/pwm_output.h"
 
 // These must be consecutive, see 'reversedSources'
 enum {
@@ -87,13 +89,10 @@ typedef struct servoMixer_s {
 #define MAX_SERVO_SPEED UINT8_MAX
 #define MAX_SERVO_BOXES 3
 
-// Custom mixer configuration
+PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);
-typedef struct mixerRules_s {
-    uint8_t servoRuleCount;
-    const servoMixer_t *rule;
-} 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,14 +100,17 @@ 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
+} servoParam_t;
-} __attribute__ ((__packed__)) servoParam_t;
 
-typedef struct servoMixerConfig_s{
+PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams);
+
+typedef struct servoConfig_s {
+    servoDevConfig_t dev;
+    uint16_t servo_lowpass_freq;            // lowpass servo filter frequency selection; 1/1000ths of loop freq
     uint8_t tri_unarmed_servo;              // send tail servo correction pulses even when unarmed
-    uint16_t servo_lowpass_freq;             // lowpass servo filter frequency selection; 1/1000ths of loop freq
+} servoConfig_t;
-    int8_t servo_lowpass_enable;            // enable/disable lowpass filter
+
-} servoMixerConfig_t;
+PG_DECLARE(servoConfig_t, servoConfig);
 
 typedef struct servoProfile_s {
     servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
@@ -120,16 +122,11 @@ typedef struct channelForwardingConfig_s {
 
 extern int16_t servo[MAX_SUPPORTED_SERVOS];
 
-void servoTable(void);
 bool isMixerUsingServos(void);
 void writeServos(void);
-void filterServos(void);
-
-void servoMixerInit(servoMixer_t *customServoMixers);
-struct gimbalConfig_s;
-void servoUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse);
 void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
 void loadCustomServoMixer(void);
-void servoConfigureOutput(void);
+void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse);
 int servoDirection(int servoIndex, int fromChannel);
-
+void servoConfigureOutput(void);
+void servosInit(void);

src/main/io/beeper.c

@@ -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_BLACKBOX_ERASE,        18, beep_2shortBeeps,     "BLACKBOX_ERASE") },
-    { BEEPER_ENTRY(BEEPER_ALL,                   18, NULL,                 "ALL") },
+    { BEEPER_ENTRY(BEEPER_ALL,                   19, NULL,                 "ALL") },
-    { BEEPER_ENTRY(BEEPER_PREFERENCE,            19, NULL,                 "PREFERRED") },
+    { BEEPER_ENTRY(BEEPER_PREFERENCE,            20, NULL,                 "PREFERRED") },
 };
 
 static const beeperTableEntry_t *currentBeeperEntry = NULL;

src/main/io/beeper.h

@@ -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

src/main/io/dashboard.c

@@ -84,7 +84,6 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex);
 static uint32_t nextDisplayUpdateAt = 0;
 static bool dashboardPresent = false;
 
-static rxConfig_t *rxConfig;
 static displayPort_t *displayPort;
 
 #define PAGE_TITLE_LINE_COUNT 1
@@ -701,7 +700,7 @@ void dashboardSetPage(pageId_e pageId)
     pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 }
 
-void dashboardInit(rxConfig_t *rxConfigToUse)
+void dashboardInit(void)
 {
     delay(200);
     resetDisplay();
@@ -714,8 +713,6 @@ void dashboardInit(rxConfig_t *rxConfigToUse)
     }
 #endif
 
-    rxConfig = rxConfigToUse;
-
     memset(&pageState, 0, sizeof(pageState));
     dashboardSetPage(PAGE_WELCOME);
 

src/main/io/dashboard.h

@@ -37,8 +37,7 @@ typedef enum {
 #endif
 } pageId_e;
 
-struct rxConfig_s;
+void dashboardInit(void);
-void dashboardInit(struct rxConfig_s *intialRxConfig);
 void dashboardUpdate(timeUs_t currentTimeUs);
 
 void dashboardShowFixedPage(pageId_e pageId);

src/main/io/displayport_max7456.c

@@ -24,13 +24,17 @@
 
 #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"
+#include "drivers/vcd.h"
+
+#include "io/displayport_max7456.h"
+#include "io/osd.h"
 
 displayPort_t max7456DisplayPort; // Referenced from osd.c
-displayPortProfile_t *max7456DisplayPortProfile;
 
 extern uint16_t refreshTimeout;
 
@@ -102,8 +106,8 @@ static void resync(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     max7456RefreshAll();
-    displayPort->rows = max7456GetRowsCount() + max7456DisplayPortProfile->rowAdjust;
+    displayPort->rows = max7456GetRowsCount() + displayPortProfileMax7456()->rowAdjust;
-    displayPort->cols = 30 + max7456DisplayPortProfile->colAdjust;
+    displayPort->cols = 30 + displayPortProfileMax7456()->colAdjust;
 }
 
 static int heartbeat(displayPort_t *displayPort)
@@ -132,9 +136,8 @@ static const displayPortVTable_t max7456VTable = {
     .txBytesFree = txBytesFree,
 };
 
-displayPort_t *max7456DisplayPortInit(const vcdProfile_t *vcdProfile, displayPortProfile_t *displayPortProfileToUse)
+displayPort_t *max7456DisplayPortInit(const vcdProfile_t *vcdProfile)
 {
-    max7456DisplayPortProfile = displayPortProfileToUse;
     displayInit(&max7456DisplayPort, &max7456VTable);
     max7456Init(vcdProfile);
     resync(&max7456DisplayPort);

src/main/io/displayport_max7456.h

@@ -17,5 +17,10 @@
 
 #pragma once
 
+#include "config/parameter_group.h"
+#include "drivers/display.h"
+
+PG_DECLARE(displayPortProfile_t, displayPortProfileMax7456);
+
 struct vcdProfile_s;
-displayPort_t *max7456DisplayPortInit(const struct vcdProfile_s *vcdProfile, displayPortProfile_t *displayPortProfileToUse);
+displayPort_t *max7456DisplayPortInit(const struct vcdProfile_s *vcdProfile);

src/main/io/displayport_msp.c

@@ -26,6 +26,8 @@
 
 #include "common/utils.h"
 
+#include "config/config_master.h"
+
 #include "drivers/display.h"
 #include "drivers/system.h"
 
@@ -34,9 +36,9 @@
 #include "msp/msp_protocol.h"
 #include "msp/msp_serial.h"
 
-static displayPort_t mspDisplayPort;
+#include "io/displayport_msp.h"
 
-static displayPortProfile_t *mspDisplayPortProfile;
+static displayPort_t mspDisplayPort;
 
 static int output(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, int len)
 {
@@ -118,8 +120,8 @@ static bool isTransferInProgress(const displayPort_t *displayPort)
 
 static void resync(displayPort_t *displayPort)
 {
-    displayPort->rows = 13 + mspDisplayPortProfile->rowAdjust; // XXX Will reflect NTSC/PAL in the future
+    displayPort->rows = 13 + displayPortProfileMsp()->rowAdjust; // XXX Will reflect NTSC/PAL in the future
-    displayPort->cols = 30 + mspDisplayPortProfile->colAdjust;
+    displayPort->cols = 30 + displayPortProfileMsp()->colAdjust;
 }
 
 static uint32_t txBytesFree(const displayPort_t *displayPort)
@@ -142,9 +144,8 @@ static const displayPortVTable_t mspDisplayPortVTable = {
     .txBytesFree = txBytesFree
 };
 
-displayPort_t *displayPortMspInit(displayPortProfile_t *displayPortProfileToUse)
+displayPort_t *displayPortMspInit(void)
 {
-    mspDisplayPortProfile = displayPortProfileToUse;
     displayInit(&mspDisplayPort, &mspDisplayPortVTable);
     resync(&mspDisplayPort);
     return &mspDisplayPort;

src/main/io/displayport_msp.h

@@ -17,5 +17,10 @@
 
 #pragma once
 
+#include "config/parameter_group.h"
+#include "drivers/display.h"
+
+PG_DECLARE(displayPortProfile_t, displayPortProfileMsp);
+
 struct displayPort_s;
-struct displayPort_s *displayPortMspInit(displayPortProfile_t *displayPortProfileToUse);
+struct displayPort_s *displayPortMspInit(void);

src/main/io/gimbal.h

@@ -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);

src/main/io/gps.c

@@ -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 "io/serial.h"
-#include "flight/gps_conversion.h"
-#include "flight/pid.h"
-#include "flight/navigation.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      '!'
@@ -87,15 +92,12 @@ uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS];    // Satellite ID
 uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
 uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS];     // Carrier to Noise Ratio (Signal Strength)
 
-static gpsConfig_t *gpsConfig;
-
 // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
 #define GPS_TIMEOUT (2500)
 // How many entries in gpsInitData array below
 #define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
 #define GPS_BAUDRATE_CHANGE_DELAY (200)
 
-static serialConfig_t *serialConfig;
 static serialPort_t *gpsPort;
 
 typedef struct gpsInitData_s {
@@ -202,19 +204,14 @@ static void gpsSetState(gpsState_e state)
     gpsData.messageState = GPS_MESSAGE_STATE_IDLE;
 }
 
-void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
+void gpsInit(void)
 {
-    serialConfig = initialSerialConfig;
-
-
     gpsData.baudrateIndex = 0;
     gpsData.errors = 0;
     gpsData.timeouts = 0;
 
     memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
 
-    gpsConfig = initialGpsConfig;
-
     // init gpsData structure. if we're not actually enabled, don't bother doing anything else
     gpsSetState(GPS_UNKNOWN);
 
@@ -237,7 +234,7 @@ void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
     portMode_t mode = MODE_RXTX;
     // only RX is needed for NMEA-style GPS
 #if !defined(COLIBRI_RACE) || !defined(LUX_RACE)
-    if (gpsConfig->provider == GPS_NMEA)
+    if (gpsConfig()->provider == GPS_NMEA)
         mode &= ~MODE_TX;
 #endif
 
@@ -345,7 +342,7 @@ void gpsInitUblox(void)
         case GPS_CONFIGURE:
 
             // Either use specific config file for GPS or let dynamically upload config
-            if( gpsConfig->autoConfig == GPS_AUTOCONFIG_OFF ) {
+            if( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) {
                 gpsSetState(GPS_RECEIVING_DATA);
                 break;
             }
@@ -367,7 +364,7 @@ void gpsInitUblox(void)
 
             if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
                 if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
-                    serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
+                    serialWrite(gpsPort, ubloxSbas[gpsConfig()->sbasMode].message[gpsData.state_position]);
                     gpsData.state_position++;
                 } else {
                     gpsData.messageState++;
@@ -384,7 +381,7 @@ void gpsInitUblox(void)
 
 void gpsInitHardware(void)
 {
-    switch (gpsConfig->provider) {
+    switch (gpsConfig()->provider) {
         case GPS_NMEA:
             gpsInitNmea();
             break;
@@ -424,7 +421,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
 
         case GPS_LOST_COMMUNICATION:
             gpsData.timeouts++;
-            if (gpsConfig->autoBaud) {
+            if (gpsConfig()->autoBaud) {
                 // try another rate
                 gpsData.baudrateIndex++;
                 gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
@@ -475,7 +472,7 @@ static void gpsNewData(uint16_t c)
 
 bool gpsNewFrame(uint8_t c)
 {
-    switch (gpsConfig->provider) {
+    switch (gpsConfig()->provider) {
         case GPS_NMEA:          // NMEA
             return gpsNewFrameNMEA(c);
         case GPS_UBLOX:         // UBX binary