From acbff49a1d964eec1f4a450373250895ebdb7fbc Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 21 Dec 2016 15:27:31 +0000
Subject: [PATCH 01/50] Tidied cms imu code

---
 src/main/cms/cms_menu_imu.c | 58 ++++++++++++++++++++-----------------
 1 file changed, 32 insertions(+), 26 deletions(-)

diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index df316179fc..f57f4f1c98 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -104,10 +104,11 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void
 static long cmsx_PidRead(void)
 {
 
+    const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
     for (uint8_t i = 0; i < 3; i++) {
-        tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i];
-        tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i];
-        tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i];
+        tempPid[i][0] = pidProfile->P8[i];
+        tempPid[i][1] = pidProfile->I8[i];
+        tempPid[i][2] = pidProfile->D8[i];
     }
 
     return 0;
@@ -125,10 +126,11 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
 {
     UNUSED(self);
 
+    pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
     for (uint8_t i = 0; i < 3; i++) {
-        masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0];
-        masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
-        masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
+        pidProfile->P8[i] = tempPid[i][0];
+        pidProfile->I8[i] = tempPid[i][1];
+        pidProfile->D8[i] = tempPid[i][2];
     }
     pidInitConfig(&currentProfile->pidProfile);
 
@@ -233,12 +235,13 @@ static long cmsx_profileOtherOnEnter(void)
 {
     profileIndexString[1] = '0' + tmpProfileIndex;
 
-    cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight;
-    cmsx_setpointRelaxRatio  = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio;
+    const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
+    cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight;
+    cmsx_setpointRelaxRatio  = pidProfile->setpointRelaxRatio;
 
-    cmsx_angleStrength =     masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL];
-    cmsx_horizonStrength =   masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL];
-    cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL];
+    cmsx_angleStrength =     pidProfile->P8[PIDLEVEL];
+    cmsx_horizonStrength =   pidProfile->I8[PIDLEVEL];
+    cmsx_horizonTransition = pidProfile->D8[PIDLEVEL];
 
     return 0;
 }
@@ -247,13 +250,14 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
 {
     UNUSED(self);
 
-    masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
-    masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
+    pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
+    pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight;
+    pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
     pidInitConfig(&currentProfile->pidProfile);
 
-    masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
-    masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;
-    masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition;
+    pidProfile->P8[PIDLEVEL] = cmsx_angleStrength;
+    pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength;
+    pidProfile->D8[PIDLEVEL] = cmsx_horizonTransition;
 
     return 0;
 }
@@ -311,11 +315,12 @@ static uint16_t cmsx_yaw_p_limit;
 
 static long cmsx_FilterPerProfileRead(void)
 {
-    cmsx_dterm_lpf_hz =       masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz;
-    cmsx_dterm_notch_hz =     masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz;
-    cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff;
-    cmsx_yaw_lpf_hz =         masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz;
-    cmsx_yaw_p_limit =        masterConfig.profile[profileIndex].pidProfile.yaw_p_limit;
+    const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
+    cmsx_dterm_lpf_hz =       pidProfile->dterm_lpf_hz;
+    cmsx_dterm_notch_hz =     pidProfile->dterm_notch_hz;
+    cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
+    cmsx_yaw_lpf_hz =         pidProfile->yaw_lpf_hz;
+    cmsx_yaw_p_limit =        pidProfile->yaw_p_limit;
 
     return 0;
 }
@@ -324,11 +329,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
 {
     UNUSED(self);
 
-    masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz =       cmsx_dterm_lpf_hz;
-    masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz =     cmsx_dterm_notch_hz;
-    masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
-    masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz =         cmsx_yaw_lpf_hz;
-    masterConfig.profile[profileIndex].pidProfile.yaw_p_limit =        cmsx_yaw_p_limit;
+    pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
+    pidProfile->dterm_lpf_hz =       cmsx_dterm_lpf_hz;
+    pidProfile->dterm_notch_hz =     cmsx_dterm_notch_hz;
+    pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
+    pidProfile->yaw_lpf_hz =         cmsx_yaw_lpf_hz;
+    pidProfile->yaw_p_limit =        cmsx_yaw_p_limit;
 
     return 0;
 }

From c932bd73131987192abb1259ca481d1e28a55c10 Mon Sep 17 00:00:00 2001
From: DieHertz <diehertz@gmail.com>
Date: Tue, 27 Dec 2016 13:25:37 +0300
Subject: [PATCH 02/50] Added PWM inversion to motor config

---
 src/main/drivers/pwm_output.c           | 14 ++++++++------
 src/main/drivers/pwm_output.h           |  2 +-
 src/main/drivers/pwm_output_stm32f3xx.c |  8 ++++----
 src/main/drivers/pwm_output_stm32f4xx.c |  8 ++++----
 src/main/drivers/pwm_output_stm32f7xx.c | 12 ++++++++----
 src/main/fc/cli.c                       |  1 +
 src/main/fc/fc_msp.c                    |  7 ++++++-
 src/main/io/motors.h                    |  3 ++-
 8 files changed, 34 insertions(+), 21 deletions(-)

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 2c76289888..f0ce496c67 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -82,7 +82,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);
@@ -90,7 +90,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);
@@ -256,7 +257,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;
         }
@@ -271,9 +273,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);
         }
         motors[motorIndex].enabled = true;
     }
@@ -335,7 +337,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;
     }
 }
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index a6ad7f38b3..c864889874 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -118,7 +118,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
 #ifdef USE_DSHOT
 uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
 void pwmWriteDigital(uint8_t index, uint16_t value);
-void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType);
+void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
 void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
 #endif
 
diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c
index 7a624c3632..6a4f3260d4 100644
--- a/src/main/drivers/pwm_output_stm32f3xx.c
+++ b/src/main/drivers/pwm_output_stm32f3xx.c
@@ -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;
 
diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c
index 55a48bcbaf..0f4b80f453 100644
--- a/src/main/drivers/pwm_output_stm32f4xx.c
+++ b/src/main/drivers/pwm_output_stm32f4xx.c
@@ -102,7 +102,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
     }
 }
 
-void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
+void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 {
     TIM_OCInitTypeDef TIM_OCInitStructure;
     DMA_InitTypeDef DMA_InitStructure;
@@ -136,14 +136,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
 
     TIM_OCStructInit(&TIM_OCInitStructure);
     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
-    if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+    if (output & TIMER_OUTPUT_N_CHANNEL) {
         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
-        TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
+        TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
     } else {
         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
-        TIM_OCInitStructure.TIM_OCPolarity =  (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
+        TIM_OCInitStructure.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
     }
     TIM_OCInitStructure.TIM_Pulse = 0;
 
diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c
index 30ccd983e2..58e4f37bfa 100644
--- a/src/main/drivers/pwm_output_stm32f7xx.c
+++ b/src/main/drivers/pwm_output_stm32f7xx.c
@@ -96,7 +96,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
     UNUSED(motorCount);
 }
 
-void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
+void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 {
     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
     motor->timerHardware = timerHardware;
@@ -174,9 +174,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
 
     /* PWM1 Mode configuration: Channel1 */
     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
-    TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
-    TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
-    TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+    if (output & TIMER_OUTPUT_N_CHANNEL) {
+        TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
+        TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+    } else {
+        TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
+        TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
+    }
     TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
     TIM_OCInitStructure.Pulse = 0;
 
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 218772e864..d8b5ad9809 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -524,6 +524,7 @@ const clivalue_t valueTable[] = {
     { "use_unsynced_pwm",           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } },
     { "motor_pwm_protocol",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } },
     { "motor_pwm_rate",             VAR_UINT16 | MASTER_VALUE,  &motorConfig()->motorPwmRate, .config.minmax = { 200, 32000 } },
+    { "motor_pwm_inversion",        VAR_UINT8  | MASTER_VALUE,  &motorConfig()->motorPwmInversion, .config.lookup = { TABLE_OFF_ON } },
 
     { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
     { "gyro_cal_on_first_arm",      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &armingConfig()->gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } },
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 88ea65d1fd..c80b6db8b7 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1143,6 +1143,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
         //!!TODO gyro_isr_update to be added pending decision
         //sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
+        sbufWriteU8(dst, motorConfig()->motorPwmInversion);
         break;
 
     case MSP_FILTER_CONFIG :
@@ -1479,7 +1480,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
 #endif
         motorConfig()->motorPwmRate = sbufReadU16(src);
-        if (dataSize > 7) {
+        if (sbufBytesRemaining(src) >= 2) {
             motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
         }
         if (sbufBytesRemaining(src)) {
@@ -1490,6 +1491,10 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             gyroConfig()->gyro_isr_update = sbufReadU8(src);
         }*/
         validateAndFixGyroConfig();
+
+        if (sbufBytesRemaining(src)) {        
+            motorConfig()->motorPwmInversion = sbufReadU8(src);
+        }
         break;
 
     case MSP_SET_FILTER_CONFIG:
diff --git a/src/main/io/motors.h b/src/main/io/motors.h
index 8486d507d2..82517b2e9d 100644
--- a/src/main/io/motors.h
+++ b/src/main/io/motors.h
@@ -24,8 +24,9 @@ typedef struct motorConfig_s {
     uint16_t minthrottle;                   // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
     uint16_t maxthrottle;                   // This is the maximum value for the ESCs at full power this value can be increased up to 2000
     uint16_t mincommand;                    // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
-    uint16_t motorPwmRate;                  // The update rate of motor outputs (50-498Hz)
+    uint16_t motorPwmRate;                  // The update rate of motor outputs
     uint8_t  motorPwmProtocol;              // Pwm Protocol
+    uint8_t  motorPwmInversion;              // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation
     uint8_t  useUnsyncedPwm;
     float    digitalIdleOffsetPercent;
     ioTag_t  ioTags[MAX_SUPPORTED_MOTORS];

From b8ee92c9ea07bd9bf70a87b66a14f27a6bf1fa7b Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Tue, 24 Jan 2017 22:38:36 +0000
Subject: [PATCH 03/50] Reviewed and revised compiler speed optimisations

---
 Makefile | 23 ++---------------------
 1 file changed, 2 insertions(+), 21 deletions(-)

diff --git a/Makefile b/Makefile
index 0381e7412e..3dc8169cf1 100644
--- a/Makefile
+++ b/Makefile
@@ -707,26 +707,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/mw.c \
             fc/rc_controls.c \
-            fc/rc_curves.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 \
@@ -747,25 +741,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 \
@@ -951,7 +932,7 @@ LTO_FLAGS           := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT)
 
 else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
 OPTIMISE_DEFAULT    := -O2
-OPTIMISE_SPEED      := -Ofast
+OPTIMISE_SPEED      := -O3
 OPTIMISE_SIZE       := -Os
 
 LTO_FLAGS           := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)

From deb2052d8e91d12f7e2fafaa83837087e2db9963 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 17:08:51 +0000
Subject: [PATCH 04/50] Updated version, MSP version and EEPROM_CONF_VERSION
 for 3.2

---
 src/main/build/version.h        | 2 +-
 src/main/config/config_eeprom.h | 2 +-
 src/main/msp/msp_protocol.h     | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/build/version.h b/src/main/build/version.h
index 59a418b265..4770ad98e9 100644
--- a/src/main/build/version.h
+++ b/src/main/build/version.h
@@ -17,7 +17,7 @@
 
 #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      0  // increment when a bug is fixed
 
 #define STR_HELPER(x) #x
diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h
index 8d73a31967..645689ffd0 100644
--- a/src/main/config/config_eeprom.h
+++ b/src/main/config/config_eeprom.h
@@ -17,7 +17,7 @@
 
 #pragma once
 
-#define EEPROM_CONF_VERSION 153
+#define EEPROM_CONF_VERSION 154
 
 void initEEPROM(void);
 void writeEEPROM();
diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h
index 63d2b70bdb..d2c368f28a 100644
--- a/src/main/msp/msp_protocol.h
+++ b/src/main/msp/msp_protocol.h
@@ -59,7 +59,7 @@
 #define MSP_PROTOCOL_VERSION                0
 
 #define API_VERSION_MAJOR                   1 // increment when major changes are made
-#define API_VERSION_MINOR                   31 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
+#define API_VERSION_MINOR                   32 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
 
 #define API_VERSION_LENGTH                  2
 

From 73308e82465d87ff899761045af17057fd7b30b9 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 18:16:05 +0000
Subject: [PATCH 05/50] Reorder accgyro_mpu.c functions for clarity and to
 avoid forward declarations

---
 src/main/drivers/accgyro_mpu.c | 227 ++++++++++++++++-----------------
 1 file changed, 109 insertions(+), 118 deletions(-)

diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index b452afc774..93405ba687 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -50,15 +50,6 @@
 
 mpuResetFuncPtr mpuReset;
 
-static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
-static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
-
-static void mpu6050FindRevision(gyroDev_t *gyro);
-
-#ifdef USE_SPI
-static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
-#endif
-
 #ifndef MPU_I2C_INSTANCE
 #define MPU_I2C_INSTANCE I2C_DEVICE
 #endif
@@ -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;
@@ -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,112 @@ 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.read = mpu6000ReadRegister;
+        gyro->mpuConfiguration.write = 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.read = mpu6500ReadRegister;
+        gyro->mpuConfiguration.write = 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.read = mpu9250ReadRegister;
+        gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
+        gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
+        gyro->mpuConfiguration.write = mpu9250WriteRegister;
+        gyro->mpuConfiguration.reset = 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.read = icm20689ReadRegister;
+        gyro->mpuConfiguration.write = icm20689WriteRegister;
+        return true;
+    }
+#endif
+
+    UNUSED(gyro);
+    return false;
+}
+#endif
+
+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;
+}
+
+void mpuGyroInit(gyroDev_t *gyro)
+{
+    mpuIntExtiInit(gyro);
+}

From d24e4e01a398881807f942936a47be4bb203cf6a Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 18:49:32 +0000
Subject: [PATCH 06/50] Changed back to using -Ofast optimisation

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index 3dc8169cf1..ca30b79e23 100644
--- a/Makefile
+++ b/Makefile
@@ -932,7 +932,7 @@ LTO_FLAGS           := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT)
 
 else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
 OPTIMISE_DEFAULT    := -O2
-OPTIMISE_SPEED      := -O3
+OPTIMISE_SPEED      := -Ofast
 OPTIMISE_SIZE       := -Os
 
 LTO_FLAGS           := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)

From 733e515610790efb877b3ba76b2a90082c0a65ba Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Thu, 26 Jan 2017 00:51:51 +1300
Subject: [PATCH 07/50] Fixed iBus telemetry handling (cherry-pick from
 @marcroe 's work in #2170.

---
 src/main/fc/config.c           | 3 +++
 src/main/target/common.h       | 4 ++--
 src/main/telemetry/ibus.c      | 4 ++--
 src/main/telemetry/telemetry.c | 6 ++++++
 4 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 22b28d1f63..a4cebaef36 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -407,6 +407,9 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
     telemetryConfig->frsky_vfas_cell_voltage = 0;
     telemetryConfig->hottAlarmSoundInterval = 5;
     telemetryConfig->pidValuesAsTelemetry = 0;
+#ifdef TELEMETRY_IBUS
+    telemetryConfig->report_cell_voltage = false;
+#endif
 }
 #endif
 
diff --git a/src/main/target/common.h b/src/main/target/common.h
index a6db3d0210..1a1a9ac57c 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -34,6 +34,7 @@
 #define STM_FAST_TARGET
 #define I2C3_OVERCLOCK true
 #define I2C4_OVERCLOCK true
+#define TELEMETRY_IBUS
 #endif
 
 /****************************
@@ -43,6 +44,7 @@
 #define STM_FAST_TARGET
 #define USE_DSHOT
 #define I2C3_OVERCLOCK true
+#define TELEMETRY_IBUS
 #endif
 
 #ifdef STM32F3
@@ -85,7 +87,6 @@
 #define TELEMETRY
 #define TELEMETRY_FRSKY
 #define TELEMETRY_HOTT
-#define TELEMETRY_IBUS
 #define TELEMETRY_LTM
 #define TELEMETRY_SMARTPORT
 #define USE_SERVOS
@@ -100,7 +101,6 @@
 #define TELEMETRY_SRXL
 #define TELEMETRY_JETIEXBUS
 #define TELEMETRY_MAVLINK
-#define TELEMETRY_IBUS
 #define USE_RX_MSP
 #define USE_SERIALRX_JETIEXBUS
 #define VTX_COMMON
diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c
index b18c29cb61..8e6341e90b 100644
--- a/src/main/telemetry/ibus.c
+++ b/src/main/telemetry/ibus.c
@@ -167,7 +167,7 @@ PG_RESET_TEMPLATE(ibusTelemetryConfig_t, ibusTelemetryConfig,
                  );
 */
 
-#define IBUS_TASK_PERIOD_US (500)
+#define IBUS_TASK_PERIOD_US (1000)
 
 #define IBUS_UART_MODE     (MODE_RXTX)
 #define IBUS_BAUDRATE      (115200)
@@ -423,4 +423,4 @@ void freeIbusTelemetryPort(void)
     ibusTelemetryEnabled = false;
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index fd475d50af..928acdc2b7 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -137,6 +137,9 @@ void telemetryCheckState(void)
 #ifdef TELEMETRY_SRXL
     checkSrxlTelemetryState();
 #endif
+#ifdef TELEMETRY_IBUS
+    checkIbusTelemetryState();
+#endif
 }
 
 void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
@@ -170,6 +173,9 @@ void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadb
 #ifdef TELEMETRY_SRXL
     handleSrxlTelemetry(currentTime);
 #endif
+#ifdef TELEMETRY_IBUS
+    handleIbusTelemetry();
+#endif
 }
 
 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)

From bdbd7b1a8aad70b2493e4094af8e8685a9c3318c Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <rco@iptel.org>
Date: Tue, 17 Jan 2017 01:04:38 +0100
Subject: [PATCH 08/50] some fixes for vtx_common

---
 src/main/drivers/vtx_common.c | 10 ++++--
 src/main/drivers/vtx_common.h |  7 +++--
 src/main/io/vtx_tramp.c       | 57 +++++++++++++++++------------------
 3 files changed, 40 insertions(+), 34 deletions(-)

diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c
index 7e617b03ac..e43505d26c 100644
--- a/src/main/drivers/vtx_common.c
+++ b/src/main/drivers/vtx_common.c
@@ -53,10 +53,10 @@ void vtxCommonProcess(uint32_t currentTimeUs)
 
 vtxDevType_e vtxCommonGetDeviceType(void)
 {
-    if (!vtxDevice)
+    if (!vtxDevice || !vtxDevice->vTable->getDeviceType)
         return VTXDEV_UNKNOWN;
 
-    return vtxDevice->devtype;
+    return vtxDevice->vTable->getDeviceType();
 }
 
 // band and chan are 1 origin
@@ -65,6 +65,9 @@ void vtxCommonSetBandChan(uint8_t band, uint8_t chan)
     if (!vtxDevice)
         return;
 
+    if ((band > vtxDevice->numBand)|| (chan > vtxDevice->numChan))
+        return;
+    
     if (vtxDevice->vTable->setBandChan)
         vtxDevice->vTable->setBandChan(band, chan);
 }
@@ -75,6 +78,9 @@ void vtxCommonSetPowerByIndex(uint8_t index)
     if (!vtxDevice)
         return;
 
+    if (index > vtxDevice->numPower)
+        return;
+    
     if (vtxDevice->vTable->setPowerByIndex)
         vtxDevice->vTable->setPowerByIndex(index);
 }
diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h
index c7c521c666..e0899cc85a 100644
--- a/src/main/drivers/vtx_common.h
+++ b/src/main/drivers/vtx_common.h
@@ -18,9 +18,12 @@
 /* Created by jflyper */
 
 typedef enum {
-    VTXDEV_UNKNOWN    = 0,
+    VTXDEV_UNSUPPORTED = 0, // reserved for MSP
+    // 1 reserved
+    // 2 reserved
     VTXDEV_SMARTAUDIO = 3,
     VTXDEV_TRAMP      = 4,
+    VTXDEV_UNKNOWN    = 0xFF,
 } vtxDevType_e;
 
 struct vtxVTable_s;
@@ -28,8 +31,6 @@ struct vtxVTable_s;
 typedef struct vtxDevice_s {
     const struct vtxVTable_s *vTable;
 
-    vtxDevType_e devtype;      // 3.1 only; eventually goes away
-
     uint8_t numBand;
     uint8_t numChan;
     uint8_t numPower;
diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c
index 749a0d40c3..34da6e97fa 100644
--- a/src/main/io/vtx_tramp.c
+++ b/src/main/io/vtx_tramp.c
@@ -46,14 +46,13 @@ static const uint16_t trampPowerTable[] = {
 };
 
 static const char * const trampPowerNames[] = {
-    "25 ", "100", "200", "400", "600"
+    "---", "25 ", "100", "200", "400", "600"
 };
 #endif
 
 #if defined(VTX_COMMON)
-static vtxVTable_t trampVTable;    // Forward
 static vtxDevice_t vtxTramp = {
-    .vTable = &trampVTable,
+    .vTable = NULL,
     .numBand = 5,
     .numChan = 8,
     .numPower = sizeof(trampPowerTable),
@@ -309,26 +308,6 @@ void trampQueryS(void)
     trampQuery('s');
 }
 
-bool trampInit()
-{
-    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
-
-    if (portConfig) {
-        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
-    }
-
-    if (!trampSerialPort) {
-        return false;
-    }
-
-#if defined(VTX_COMMON)
-    vtxTramp.vTable = &trampVTable;
-    vtxCommonRegisterDevice(&vtxTramp);
-#endif
-
-    return true;
-}
-
 void vtxTrampProcess(uint32_t currentTimeUs)
 {
     static uint32_t lastQueryTimeUs = 0;
@@ -472,9 +451,9 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL }
 
 static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 };
 
-static uint8_t trampCmsPower = 0;
+static uint8_t trampCmsPower = 1;
 
-static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampPowerNames, NULL };
+static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames, NULL };
 
 static void trampCmsUpdateFreqRef(void)
 {
@@ -539,7 +518,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self)
     UNUSED(self);
 
     trampSetBandChan(trampCmsBand, trampCmsChan);
-    trampSetRFPower(trampPowerTable[trampCmsPower]);
+    trampSetRFPower(trampPowerTable[trampCmsPower-1]);
 
     // If it fails, the user should retry later
     trampCommitChanges();
@@ -559,7 +538,7 @@ static void trampCmsInitSettings()
     if (trampCurConfigPower > 0) {
         for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
             if (trampCurConfigPower <= trampPowerTable[i]) {
-                trampCmsPower = i;
+                trampCmsPower = i + 1;
                 break;
             }
         }
@@ -640,7 +619,7 @@ void vtxTrampSetBandChan(uint8_t band, uint8_t chan)
 void vtxTrampSetPowerByIndex(uint8_t index)
 {
     if (index) {
-        trampSetRFPower(trampPowerTable[index]);
+        trampSetRFPower(trampPowerTable[index - 1]);
         trampCommitChanges();
     }
 }
@@ -668,7 +647,7 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex)
     if (trampCurConfigPower > 0) {
         for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
             if (trampCurConfigPower <= trampPowerTable[i]) {
-                *pIndex = i;
+                *pIndex = i + 1;
                 break;
             }
         }
@@ -700,4 +679,24 @@ static vtxVTable_t trampVTable = {
 
 #endif
 
+bool trampInit()
+{
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
+
+    if (portConfig) {
+        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
+    }
+
+    if (!trampSerialPort) {
+        return false;
+    }
+
+#if defined(VTX_COMMON)
+    vtxTramp.vTable = &trampVTable;
+    vtxCommonRegisterDevice(&vtxTramp);
+#endif
+    
+    return true;
+}
+
 #endif // VTX_TRAMP

From ebce3d984709ed5439b0a7d3595e0615042ea33a Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <rco@iptel.org>
Date: Tue, 17 Jan 2017 01:06:20 +0100
Subject: [PATCH 09/50] added MSP_VTX_CONFIG & MSP_SET_VTX_CONFIG

---
 src/main/fc/fc_msp.c | 75 +++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 67 insertions(+), 8 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index c4a4599e8a..6f4dd83037 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -47,6 +47,7 @@
 #include "drivers/vtx_soft_spi_rtc6705.h"
 #include "drivers/pwm_output.h"
 #include "drivers/serial_escserial.h"
+#include "drivers/vtx_common.h"
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
@@ -1187,6 +1188,35 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         }
         break;
 
+#if defined(VTX_COMMON)
+    case MSP_VTX_CONFIG:
+        {
+            uint8_t deviceType = vtxCommonGetDeviceType();
+            if (deviceType != VTXDEV_UNKNOWN) {
+
+                uint8_t band=0, channel=0;
+                vtxCommonGetBandChan(&band,&channel);
+                
+                uint8_t powerIdx=0; // debug
+                vtxCommonGetPowerIndex(&powerIdx);
+                
+                uint8_t pitmode=0;
+                vtxCommonGetPitmode(&pitmode);
+                
+                sbufWriteU8(dst, deviceType);
+                sbufWriteU8(dst, band);
+                sbufWriteU8(dst, channel);
+                sbufWriteU8(dst, powerIdx);
+                sbufWriteU8(dst, pitmode);
+                // future extensions here...
+            }
+            else {
+                sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
+            }
+        }
+#endif
+        break;
+
     default:
         return false;
     }
@@ -1634,15 +1664,44 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 #endif
 
-#ifdef USE_RTC6705
+#if defined(USE_RTC6705) || defined(VTX_COMMON)
     case MSP_SET_VTX_CONFIG:
-        ;
-        uint16_t tmp = sbufReadU16(src);
-        if  (tmp < 40)
-            masterConfig.vtx_channel = tmp;
-        if (current_vtx_channel != masterConfig.vtx_channel) {
-            current_vtx_channel = masterConfig.vtx_channel;
-            rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
+        {
+            uint16_t tmp = sbufReadU16(src);
+#if defined(USE_RTC6705)
+            if  (tmp < 40)
+                masterConfig.vtx_channel = tmp;
+            if (current_vtx_channel != masterConfig.vtx_channel) {
+                current_vtx_channel = masterConfig.vtx_channel;
+                rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
+            }
+#else
+            if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) {
+
+                uint8_t band    = (tmp / 8) + 1;
+                uint8_t channel = (tmp % 8) + 1;
+
+                uint8_t current_band=0, current_channel=0;
+                vtxCommonGetBandChan(&current_band,&current_channel);
+                if ((current_band != band) || (current_channel != channel))
+                    vtxCommonSetBandChan(band,channel);
+
+                if (sbufBytesRemaining(src) < 2)
+                    break;
+            
+                uint8_t power = sbufReadU8(src);
+                uint8_t current_power = 0;
+                vtxCommonGetPowerIndex(&current_power);
+                if (current_power != power)
+                    vtxCommonSetPowerByIndex(power);
+            
+                uint8_t pitmode = sbufReadU8(src);
+                uint8_t current_pitmode = 0;
+                vtxCommonGetPitmode(&current_pitmode);
+                if (current_pitmode != pitmode)
+                    vtxCommonSetPitmode(pitmode);
+            }
+#endif
         }
         break;
 #endif

From 2070246314ecf3fbde487af6ccbe1346122ef05e Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Thu, 26 Jan 2017 08:56:22 +1300
Subject: [PATCH 10/50] Rebase of #1917: Update SDK to 6.2.1 2016q4 (thanks to
 @TheAngularity).

---
 .travis.yml             |  2 +-
 make/tools.mk           | 10 +++++-----
 src/main/build/atomic.h |  2 +-
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 5072324926..df472a95ec 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -82,7 +82,7 @@ install:
   - make arm_sdk_install
 
 before_script:
-  - tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version
+  - tools/gcc-arm-none-eabi-6_2-2016q4/bin/arm-none-eabi-gcc --version
   - clang --version
   - clang++ --version
 
diff --git a/make/tools.mk b/make/tools.mk
index 810716a57d..4ad98115d6 100644
--- a/make/tools.mk
+++ b/make/tools.mk
@@ -14,16 +14,16 @@
 ##############################
 
 # Set up ARM (STM32) SDK
-ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q3
+ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-6_2-2016q4
 # Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion)
-GCC_REQUIRED_VERSION ?= 5.4.1
+GCC_REQUIRED_VERSION ?= 6.2.1
 
 ## arm_sdk_install   : Install Arm SDK
 .PHONY: arm_sdk_install
 
-ARM_SDK_URL_BASE  := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update/+download/gcc-arm-none-eabi-5_4-2016q3-20160926
+ARM_SDK_URL_BASE  := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2016q4/gcc-arm-none-eabi-6_2-2016q4-20161216
 
-# source: https://launchpad.net/gcc-arm-embedded/5.0/
+# source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads
 ifdef LINUX
   ARM_SDK_URL  := $(ARM_SDK_URL_BASE)-linux.tar.bz2
 endif
@@ -33,7 +33,7 @@ ifdef MACOSX
 endif
 
 ifdef WINDOWS
-  ARM_SDK_URL  := $(ARM_SDK_URL_BASE)-win32.zip
+  ARM_SDK_URL  := $(ARM_SDK_URL_BASE)-win32-zip.zip
 endif
 
 ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index 4603091ed4..959c6e9d8d 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -85,7 +85,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
 // ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier
 // this macro can be used only ONCE PER LINE, but multiple uses per block are fine
 
-#if (__GNUC__ > 5)
+#if (__GNUC__ > 6)
 #warning "Please verify that ATOMIC_BARRIER works as intended"
 // increment version number is BARRIER works
 // TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead

From 44004b0b0b9438948effd42ad406cd8f9c7c34ce Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Tue, 20 Dec 2016 22:42:58 +0000
Subject: [PATCH 11/50] Parameter groups EEPROM migration

---
 Makefile                            |   1 +
 src/main/config/config_eeprom.c     | 475 +++++++++++++---------------
 src/main/config/config_eeprom.h     |   5 +-
 src/main/config/config_profile.h    |   1 -
 src/main/config/config_reset.h      |  40 +++
 src/main/config/config_streamer.c   | 177 +++++++++++
 src/main/config/config_streamer.h   |  45 +++
 src/main/fc/config.c                |  31 +-
 src/main/fc/config.h                |   5 +
 src/main/fc/fc_msp.c                |   1 -
 src/main/target/link/stm32_flash.ld |  13 +
 11 files changed, 539 insertions(+), 255 deletions(-)
 create mode 100644 src/main/config/config_reset.h
 create mode 100644 src/main/config/config_streamer.c
 create mode 100644 src/main/config/config_streamer.h

diff --git a/Makefile b/Makefile
index ca30b79e23..decd865b2e 100644
--- a/Makefile
+++ b/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 \
diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 5c32f18183..18cbfa8e9b 100644
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -18,294 +18,271 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <stddef.h>
 
 #include "platform.h"
 
-#include "drivers/system.h"
-
-#include "config/config_master.h"
-
 #include "build/build_config.h"
 
+#include "common/maths.h"
+
 #include "config/config_eeprom.h"
+#include "config/config_streamer.h"
+#include "config/config_master.h"
+#include "config/parameter_group.h"
 
-#if !defined(FLASH_SIZE)
-#error "Flash size not defined for target. (specify in KB)"
-#endif
+#include "drivers/system.h"
 
+#include "fc/config.h"
 
-#ifndef FLASH_PAGE_SIZE
-    #ifdef STM32F303xC
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
-    #endif
+extern uint8_t __config_start;   // configured via linker script when building binaries.
+extern uint8_t __config_end;
 
-    #ifdef STM32F10X_MD
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x400)
-    #endif
+typedef enum {
+    CR_CLASSICATION_SYSTEM   = 0,
+    CR_CLASSICATION_PROFILE1 = 1,
+    CR_CLASSICATION_PROFILE2 = 2,
+    CR_CLASSICATION_PROFILE3 = 3,
+    CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_PROFILE3,
+} configRecordFlags_e;
 
-    #ifdef STM32F10X_HD
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
-    #endif
+#define CR_CLASSIFICATION_MASK (0x3)
 
-    #if defined(STM32F745xx)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x40000)
-    #endif
+// Header for the saved copy.
+typedef struct {
+    uint8_t format;
+} PG_PACKED configHeader_t;
 
-    #if defined(STM32F746xx)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x40000)
-    #endif
+// Header for each stored PG.
+typedef struct {
+    // split up.
+    uint16_t size;
+    pgn_t pgn;
+    uint8_t version;
 
-    #if defined(STM32F722xx)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
-    #endif
+    // lower 2 bits used to indicate system or profile number, see CR_CLASSIFICATION_MASK
+    uint8_t flags;
 
-    #if defined(STM32F40_41xxx)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
-    #endif
+    uint8_t pg[];
+} PG_PACKED configRecord_t;
 
-    #if defined (STM32F411xE)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
-    #endif
-
-#endif
-
-#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT)
-    #ifdef STM32F10X_MD
-        #define FLASH_PAGE_COUNT 128
-    #endif
-
-    #ifdef STM32F10X_HD
-        #define FLASH_PAGE_COUNT 128
-    #endif
-#endif
-
-#if defined(FLASH_SIZE)
-#if defined(STM32F40_41xxx)
-#define FLASH_PAGE_COUNT 4
-#elif defined (STM32F411xE)
-#define FLASH_PAGE_COUNT 3
-#elif defined (STM32F722xx)
-#define FLASH_PAGE_COUNT 3
-#elif defined (STM32F745xx)
-#define FLASH_PAGE_COUNT 4
-#elif defined (STM32F746xx)
-#define FLASH_PAGE_COUNT 4
-#else
-#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
-#endif
-#endif
-
-#if !defined(FLASH_PAGE_SIZE)
-#error "Flash page size not defined for target."
-#endif
-
-#if !defined(FLASH_PAGE_COUNT)
-#error "Flash page count not defined for target."
-#endif
-
-#if FLASH_SIZE <= 128
-#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
-#else
-#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000
-#endif
-
-// use the last flash pages for storage
-#ifdef CUSTOM_FLASH_MEMORY_ADDRESS
-size_t custom_flash_memory_address = 0;
-#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address)
-#else
-// use the last flash pages for storage
-#ifndef CONFIG_START_FLASH_ADDRESS
-#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
-#endif
-#endif
+// Footer for the saved copy.
+typedef struct {
+    uint16_t terminator;
+} PG_PACKED configFooter_t;
+// checksum is appended just after footer. It is not included in footer to make checksum calculation consistent
 
+// Used to check the compiler packing at build time.
+typedef struct {
+    uint8_t byte;
+    uint32_t word;
+} PG_PACKED packingTest_t;
 
 void initEEPROM(void)
 {
+    // Verify that this architecture packs as expected.
+    BUILD_BUG_ON(offsetof(packingTest_t, byte) != 0);
+    BUILD_BUG_ON(offsetof(packingTest_t, word) != 1);
+    BUILD_BUG_ON(sizeof(packingTest_t) != 5);
+
+    BUILD_BUG_ON(sizeof(configHeader_t) != 1);
+    BUILD_BUG_ON(sizeof(configFooter_t) != 2);
+    BUILD_BUG_ON(sizeof(configRecord_t) != 6);
 }
 
-static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
+static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length)
 {
-    uint8_t checksum = 0;
-    const uint8_t *byteOffset;
+    const uint8_t *p = (const uint8_t *)data;
+    const uint8_t *pend = p + length;
 
-    for (byteOffset = data; byteOffset < (data + length); byteOffset++)
-        checksum ^= *byteOffset;
-    return checksum;
+    for (; p != pend; p++) {
+        chk ^= *p;
+    }
+    return chk;
+}
+
+// Scan the EEPROM config. Returns true if the config is valid.
+static bool scanEEPROM(void)
+{
+    uint8_t chk = 0;
+    const uint8_t *p = &__config_start;
+    const configHeader_t *header = (const configHeader_t *)p;
+
+    if (header->format != EEPROM_CONF_VERSION) {
+        return false;
+    }
+    chk = updateChecksum(chk, header, sizeof(*header));
+    p += sizeof(*header);
+    // include the transitional masterConfig record
+    chk = updateChecksum(chk, p, sizeof(masterConfig));
+    p += sizeof(masterConfig);
+
+    for (;;) {
+        const configRecord_t *record = (const configRecord_t *)p;
+
+        if (record->size == 0) {
+            // Found the end.  Stop scanning.
+            break;
+        }
+        if (p + record->size >= &__config_end
+            || record->size < sizeof(*record)) {
+            // Too big or too small.
+            return false;
+        }
+
+        chk = updateChecksum(chk, p, record->size);
+
+        p += record->size;
+    }
+
+    const configFooter_t *footer = (const configFooter_t *)p;
+    chk = updateChecksum(chk, footer, sizeof(*footer));
+    p += sizeof(*footer);
+    chk = ~chk;
+    return chk == *p;
+}
+
+// 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;
 }
 
 bool isEEPROMContentValid(void)
 {
-    const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS;
-    uint8_t checksum = 0;
-
-    // check version number
-    if (EEPROM_CONF_VERSION != temp->version)
-        return false;
-
-    // check size and magic numbers
-    if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
-        return false;
-
-    if (strncasecmp(temp->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)))
-        return false;
-
-    // verify integrity of temporary copy
-    checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
-    if (checksum != 0)
-        return false;
-
-    // looks good, let's roll!
-    return true;
+    return scanEEPROM();
 }
 
-#if defined(STM32F7)
-
-// FIXME: HAL for now this will only work for F4/F7 as flash layout is different
-void writeEEPROM(void)
+static bool writeSettingsToEEPROM(void)
 {
-    // Generate compile time error if the config does not fit in the reserved area of flash.
-    BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
+    config_streamer_t streamer;
+    config_streamer_init(&streamer);
 
-    HAL_StatusTypeDef status;
-    uint32_t wordOffset;
-    int8_t attemptsRemaining = 3;
+    config_streamer_start(&streamer, (uintptr_t)&__config_start, &__config_end - &__config_start);
+    uint8_t chk = 0;
 
-    suspendRxSignal();
+    configHeader_t header = {
+        .format = EEPROM_CONF_VERSION,
+    };
 
-    // prepare checksum/version constants
-    masterConfig.version = EEPROM_CONF_VERSION;
-    masterConfig.size = sizeof(master_t);
-    masterConfig.magic_be = 0xBE;
-    masterConfig.magic_ef = 0xEF;
-    masterConfig.chk = 0; // erase checksum before recalculating
-    masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
+    config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header));
+    chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header));
+    // write the transitional masterConfig record
+    config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig));
+    chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig));
+    PG_FOREACH(reg) {
+        const uint16_t regSize = pgSize(reg);
+        configRecord_t record = {
+            .size = sizeof(configRecord_t) + regSize,
+            .pgn = pgN(reg),
+            .version = pgVersion(reg),
+            .flags = 0
+        };
 
-    // write it
-    /* Unlock the Flash to enable the flash control register access *************/
-    HAL_FLASH_Unlock();
-    while (attemptsRemaining--)
-    {
-        /* Fill EraseInit structure*/
-        FLASH_EraseInitTypeDef EraseInitStruct = {0};
-        EraseInitStruct.TypeErase     = FLASH_TYPEERASE_SECTORS;
-        EraseInitStruct.VoltageRange  = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
-        EraseInitStruct.Sector        = (FLASH_SECTOR_TOTAL-1);
-        EraseInitStruct.NbSectors     = 1;
-        uint32_t SECTORError;
-        status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
-        if (status != HAL_OK)
-        {
-            continue;
-        }
-        else
-        {
-            for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4)
-            {
-                status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset));
-                if(status != HAL_OK)
-                {
-                    break;
-                }
+        if (pgIsSystem(reg)) {
+            // write the only instance
+            record.flags |= CR_CLASSICATION_SYSTEM;
+            config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
+            chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
+            config_streamer_write(&streamer, reg->address, regSize);
+            chk = updateChecksum(chk, reg->address, regSize);
+        } else {
+            // write one instance for each profile
+            for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) {
+                record.flags = 0;
+
+                record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
+                config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
+                chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
+                const uint8_t *address = reg->address + (regSize * profileIndex);
+                config_streamer_write(&streamer, address, regSize);
+                chk = updateChecksum(chk, address, regSize);
             }
         }
-        if (status == HAL_OK) {
-            break;
+    }
+
+    configFooter_t footer = {
+        .terminator = 0,
+    };
+
+    config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer));
+    chk = updateChecksum(chk, (uint8_t *)&footer, sizeof(footer));
+
+    // append checksum now
+    chk = ~chk;
+    config_streamer_write(&streamer, &chk, sizeof(chk));
+
+    config_streamer_flush(&streamer);
+
+    bool success = config_streamer_finish(&streamer) == 0;
+
+    return success;
+}
+
+void writeConfigToEEPROM(void)
+{
+    bool success = false;
+    // write it
+    for (int attempt = 0; attempt < 3 && !success; attempt++) {
+        if (writeSettingsToEEPROM()) {
+            success = true;
         }
     }
-    HAL_FLASH_Lock();
+
+    if (success && isEEPROMContentValid()) {
+        return;
+    }
 
     // Flash write failed - just die now
-    if (status != HAL_OK || !isEEPROMContentValid()) {
-        failureMode(FAILURE_FLASH_WRITE_FAILED);
-    }
-
-    resumeRxSignal();
-}
-#else
-void writeEEPROM(void)
-{
-    // Generate compile time error if the config does not fit in the reserved area of flash.
-    BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
-
-    FLASH_Status status = 0;
-    uint32_t wordOffset;
-    int8_t attemptsRemaining = 3;
-
-    suspendRxSignal();
-
-    // prepare checksum/version constants
-    masterConfig.version = EEPROM_CONF_VERSION;
-    masterConfig.size = sizeof(master_t);
-    masterConfig.magic_be = 0xBE;
-    masterConfig.magic_ef = 0xEF;
-    masterConfig.chk = 0; // erase checksum before recalculating
-    masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
-
-    // write it
-    FLASH_Unlock();
-    while (attemptsRemaining--) {
-#if defined(STM32F4)
-        FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
-#elif defined(STM32F303)
-        FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
-#elif defined(STM32F10X)
-        FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
-#endif
-        for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
-            if (wordOffset % FLASH_PAGE_SIZE == 0) {
-#if defined(STM32F40_41xxx)
-                status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000
-#elif defined (STM32F411xE)
-                status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000
-#else
-                status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
-#endif
-                if (status != FLASH_COMPLETE) {
-                    break;
-                }
-            }
-
-            status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset,
-                    *(uint32_t *) ((char *) &masterConfig + wordOffset));
-            if (status != FLASH_COMPLETE) {
-                break;
-            }
-        }
-        if (status == FLASH_COMPLETE) {
-            break;
-        }
-    }
-    FLASH_Lock();
-
-    // Flash write failed - just die now
-    if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
-        failureMode(FAILURE_FLASH_WRITE_FAILED);
-    }
-
-    resumeRxSignal();
-}
-#endif
-
-void readEEPROM(void)
-{
-    // Sanity check
-    if (!isEEPROMContentValid())
-        failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
-
-    suspendRxSignal();
-
-    // Read flash
-    memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
-
-    if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check
-        masterConfig.current_profile_index = 0;
-
-    setProfile(masterConfig.current_profile_index);
-
-    validateAndFixConfig();
-    activateConfig();
-
-    resumeRxSignal();
+    failureMode(FAILURE_FLASH_WRITE_FAILED);
 }
diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h
index 645689ffd0..a6aafc3ca7 100644
--- a/src/main/config/config_eeprom.h
+++ b/src/main/config/config_eeprom.h
@@ -19,7 +19,6 @@
 
 #define EEPROM_CONF_VERSION 154
 
-void initEEPROM(void);
-void writeEEPROM();
-void readEEPROM(void);
 bool isEEPROMContentValid(void);
+bool loadEEPROM(void);
+void writeConfigToEEPROM(void);
diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h
index a0ffa53905..d1f2c513fd 100644
--- a/src/main/config/config_profile.h
+++ b/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"
diff --git a/src/main/config/config_reset.h b/src/main/config/config_reset.h
new file mode 100644
index 0000000000..7ca0eda77a
--- /dev/null
+++ b/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 alloved to set structure field-by-field
+#define RESET_CONFIG_2(_type, _name, ...)                 \
+    *(_name) = (_type) {                                  \
+        __VA_ARGS__                                       \
+    };                                                    \
+    /**/
diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c
new file mode 100644
index 0000000000..cd48ea9f72
--- /dev/null
+++ b/src/main/config/config_streamer.c
@@ -0,0 +1,177 @@
+/*
+ * 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 "config_streamer.h"
+
+#include "platform.h"
+
+#include <string.h>
+
+#if !defined(FLASH_PAGE_SIZE)
+# if defined(STM32F10X_MD)
+#  define FLASH_PAGE_SIZE                 (0x400)
+# elif defined(STM32F10X_HD)
+#  define FLASH_PAGE_SIZE                 (0x800)
+# elif defined(STM32F303xC)
+#  define FLASH_PAGE_SIZE                 (0x800)
+# elif defined(STM32F40_41xxx)
+#  define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
+# elif defined (STM32F411xE)
+#  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)
+static int write_word(config_streamer_t *c, uint32_t value)
+{
+    if (c->err != 0) {
+        return c->err;
+    }
+
+    HAL_StatusTypeDef status;
+    if (c->address % FLASH_PAGE_SIZE == 0) {
+        FLASH_EraseInitTypeDef EraseInitStruct = {0};
+        EraseInitStruct.TypeErase     = FLASH_TYPEERASE_SECTORS;
+        EraseInitStruct.VoltageRange  = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
+        EraseInitStruct.Sector        = (FLASH_SECTOR_TOTAL-1);
+        EraseInitStruct.NbSectors     = 1;
+        uint32_t SECTORError;
+        status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
+        if (status != HAL_OK){
+            return -1;
+        }
+    }
+    status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value);
+    if (status != HAL_OK) {
+        return -2;
+    }
+    c->address += sizeof(value);
+    return 0;
+}
+#else
+static int write_word(config_streamer_t *c, uint32_t value)
+{
+    if (c->err != 0) {
+        return c->err;
+    }
+
+    FLASH_Status status;
+
+    if (c->address % 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(c->address);
+#endif
+        if (status != FLASH_COMPLETE) {
+            return -1;
+        }
+    }
+    status = FLASH_ProgramWord(c->address, value);
+    if (status != FLASH_COMPLETE) {
+        return -2;
+    }
+    c->address += sizeof(value);
+    return 0;
+}
+#endif
+
+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;
+}
diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h
new file mode 100644
index 0000000000..a62356fbea
--- /dev/null
+++ b/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);
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 22b28d1f63..59872dc309 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -91,6 +91,7 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 #include "config/feature.h"
+#include "config/parameter_group.h"
 
 #ifndef DEFAULT_RX_FEATURE
 #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
@@ -1122,12 +1123,40 @@ void validateAndFixGyroConfig(void)
     }
 }
 
+void readEEPROM(void)
+{
+    suspendRxSignal();
+
+    // Sanity check, read flash
+    if (!loadEEPROM()) {
+        failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
+    }
+
+    pgActivateProfile(getCurrentProfile());
+
+//    setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex);
+    setControlRateProfile(0);
+
+    validateAndFixConfig();
+    activateConfig();
+
+    resumeRxSignal();
+}
+
+void writeEEPROM(void)
+{
+    suspendRxSignal();
+
+    writeConfigToEEPROM();
+
+    resumeRxSignal();
+}
+
 void ensureEEPROMContainsValidData(void)
 {
     if (isEEPROMContentValid()) {
         return;
     }
-
     resetEEPROM();
 }
 
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 0fe4f9e616..4a77447dfa 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -69,7 +69,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);
@@ -80,6 +83,8 @@ 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);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index c4a4599e8a..c2b920deca 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -104,7 +104,6 @@
 #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;
diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld
index 2cc81c9fd5..eaf65e7b9a 100644
--- a/src/main/target/link/stm32_flash.ld
+++ b/src/main/target/link/stm32_flash.ld
@@ -81,6 +81,19 @@ SECTIONS
     KEEP (*(SORT(.fini_array.*)))
     PROVIDE_HIDDEN (__fini_array_end = .);
   } >FLASH
+  .pg_registry :
+  {
+    PROVIDE_HIDDEN (__pg_registry_start = .);
+    KEEP (*(.pg_registry))
+    KEEP (*(SORT(.pg_registry.*)))
+    PROVIDE_HIDDEN (__pg_registry_end = .);
+  } >FLASH
+  .pg_resetdata :
+  {
+    PROVIDE_HIDDEN (__pg_resetdata_start = .);
+    KEEP (*(.pg_resetdata))
+    PROVIDE_HIDDEN (__pg_resetdata_end = .);
+  } >FLASH
 
   /* used by the startup to initialize data */
   _sidata = .;

From 106542008f6713d8f5682e9714e6a69be691d459 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Thu, 29 Dec 2016 13:16:43 +0000
Subject: [PATCH 12/50] Fixed typo

---
 src/main/config/config_reset.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/config/config_reset.h b/src/main/config/config_reset.h
index 7ca0eda77a..5d13e12475 100644
--- a/src/main/config/config_reset.h
+++ b/src/main/config/config_reset.h
@@ -32,7 +32,7 @@
     memcpy((_name), &__UNIQL(_reset_template_), sizeof(*(_name)));      \
     /**/
 
-// overwrite _name with data passed as arguments. GCC is alloved to set structure field-by-field
+// 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__                                       \

From 6ee5cf1d5d914b851f9c1c83b5782b2afc077621 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Thu, 29 Dec 2016 17:15:16 +0000
Subject: [PATCH 13/50] Added dummy PG. Fixed setting of profile

---
 src/main/config/config_eeprom.c       |  9 +++++++++
 src/main/config/parameter_group_ids.h |  2 +-
 src/main/fc/config.c                  | 12 ++++++++----
 src/main/fc/config.h                  |  3 +--
 4 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 18cbfa8e9b..2850700c54 100644
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -35,6 +35,15 @@
 
 #include "fc/config.h"
 
+// declare a dummy PG, since scanEEPROM assumes there is at least one PG
+// !!TODO remove once first PG has been created out of masterConfg
+typedef struct dummpConfig_s {
+    uint8_t dummy;
+} dummyConfig_t;
+PG_DECLARE(dummyConfig_t, dummyConfig);
+#define PG_DUMMY_CONFIG 1
+PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0);
+
 extern uint8_t __config_start;   // configured via linker script when building binaries.
 extern uint8_t __config_end;
 
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index c42ef32f0e..fbef2e84d5 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -16,7 +16,7 @@
  */
 
 // 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
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 59872dc309..fccd15b8fa 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -547,7 +547,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;
@@ -1132,10 +1132,14 @@ void readEEPROM(void)
         failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
     }
 
-    pgActivateProfile(getCurrentProfile());
-
+//    pgActivateProfile(getCurrentProfile());
 //    setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex);
-    setControlRateProfile(0);
+
+    if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check
+        masterConfig.current_profile_index = 0;
+    }
+
+    setProfile(masterConfig.current_profile_index);
 
     validateAndFixConfig();
     activateConfig();
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 4a77447dfa..62841f5fd3 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -82,7 +82,6 @@ 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);
 
@@ -91,7 +90,7 @@ void changeControlRateProfile(uint8_t profileIndex);
 bool canSoftwareSerialBeUsed(void);
 
 uint16_t getCurrentMinthrottle(void);
-struct master_s;
 
+struct master_s;
 void targetConfiguration(struct master_s *config);
 void targetValidateConfiguration(struct master_s *config);

From 6ef5f0579ac82d1b83491dc96332f37ab86b5835 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Mon, 2 Jan 2017 17:03:17 +0000
Subject: [PATCH 14/50] Updates as per iNav

---
 src/main/config/config_streamer.c | 138 +++++++++++++++++++++++-------
 src/main/drivers/system.h         |   3 +
 src/main/fc/cli.c                 |  99 ++++++++++++++++-----
 src/main/fc/config.c              |   7 +-
 src/main/fc/config.h              |   1 +
 5 files changed, 195 insertions(+), 53 deletions(-)

diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c
index cd48ea9f72..e63363f22d 100644
--- a/src/main/config/config_streamer.c
+++ b/src/main/config/config_streamer.c
@@ -15,11 +15,16 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "config_streamer.h"
+#include <string.h>
 
 #include "platform.h"
 
-#include <string.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)
 # if defined(STM32F10X_MD)
@@ -32,6 +37,8 @@
 #  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
 # elif defined(STM32F745xx)
 #  define FLASH_PAGE_SIZE                 ((uint32_t)0x40000)
 # elif defined(STM32F746xx)
@@ -79,61 +86,134 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
 }
 
 #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;
     }
-
-    HAL_StatusTypeDef status;
+#if defined(STM32F7)
     if (c->address % FLASH_PAGE_SIZE == 0) {
-        FLASH_EraseInitTypeDef EraseInitStruct = {0};
-        EraseInitStruct.TypeErase     = FLASH_TYPEERASE_SECTORS;
-        EraseInitStruct.VoltageRange  = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
-        EraseInitStruct.Sector        = (FLASH_SECTOR_TOTAL-1);
-        EraseInitStruct.NbSectors     = 1;
+        FLASH_EraseInitTypeDef EraseInitStruct = {
+            .TypeErase     = FLASH_TYPEERASE_SECTORS,
+            .VoltageRange  = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V
+            .NbSectors     = 1
+        };
+        EraseInitStruct.Sector = getFLASHSectorForEEPROM();
         uint32_t SECTORError;
-        status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
+        const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
         if (status != HAL_OK){
             return -1;
         }
     }
-    status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value);
+    const HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value);
     if (status != HAL_OK) {
         return -2;
     }
-    c->address += sizeof(value);
-    return 0;
-}
 #else
-static int write_word(config_streamer_t *c, uint32_t value)
-{
-    if (c->err != 0) {
-        return c->err;
-    }
-
-    FLASH_Status status;
-
     if (c->address % 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
+#if defined(STM32F4)
+        const FLASH_Status status = FLASH_EraseSector(getFLASHSectorForEEPROM(), VoltageRange_3); //0x08080000 to 0x080A0000
 #else
-        status = FLASH_ErasePage(c->address);
+        const FLASH_Status status = FLASH_ErasePage(c->address);
 #endif
         if (status != FLASH_COMPLETE) {
             return -1;
         }
     }
-    status = FLASH_ProgramWord(c->address, value);
+    const FLASH_Status status = FLASH_ProgramWord(c->address, value);
     if (status != FLASH_COMPLETE) {
         return -2;
     }
+#endif
     c->address += sizeof(value);
     return 0;
 }
-#endif
 
 int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size)
 {
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index 87af44df54..5ac340c31e 100644
--- a/src/main/drivers/system.h
+++ b/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);
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index d233d3852d..d44b854e79 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -23,6 +23,7 @@
 #include <math.h>
 #include <ctype.h>
 
+//#define USE_PARAMETER_GROUPS
 #include "platform.h"
 
 // FIXME remove this for targets that don't need a CLI.  Perhaps use a no-op macro when USE_CLI is not enabled
@@ -48,6 +49,9 @@ uint8_t cliMode = 0;
 #include "config/config_master.h"
 #include "config/feature.h"
 
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "drivers/accgyro.h"
 #include "drivers/buf_writer.h"
 #include "drivers/bus_i2c.h"
@@ -460,6 +464,7 @@ typedef enum {
     MASTER_VALUE = (0 << VALUE_SECTION_OFFSET),
     PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET),
     PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET),
+    CONTROL_RATE_VALUE = (3 << VALUE_SECTION_OFFSET),
     // value mode
     MODE_DIRECT = (0 << VALUE_MODE_OFFSET),
     MODE_LOOKUP = (1 << VALUE_MODE_OFFSET)
@@ -483,6 +488,22 @@ typedef union {
     cliMinMaxConfig_t minmax;
 } cliValueConfig_t;
 
+#ifdef USE_PARAMETER_GROUPS
+typedef struct {
+    const char *name;
+    const uint8_t type; // see cliValueFlag_e
+    const cliValueConfig_t config;
+
+    pgn_t pgn;
+    uint16_t offset;
+} __attribute__((packed)) clivalue_t;
+
+static const clivalue_t valueTable[] = {
+        { "dummy",               VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0,  255 }, 0, 0 }
+};
+
+#else
+
 typedef struct {
     const char *name;
     const uint8_t type; // see cliValueFlag_e
@@ -490,7 +511,7 @@ typedef struct {
     const cliValueConfig_t config;
 } clivalue_t;
 
-const clivalue_t valueTable[] = {
+static const clivalue_t valueTable[] = {
 #ifndef SKIP_TASK_STATISTICS
     { "task_statistics",            VAR_INT8   | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.task_statistics, .config.lookup = { TABLE_OFF_ON } },
 #endif
@@ -808,8 +829,7 @@ const clivalue_t valueTable[] = {
     { "displayport_max7456_row_adjust", VAR_INT8    | MASTER_VALUE, &displayPortProfileMax7456()->rowAdjust, .config.minmax = { -3, 0 } },
 #endif
 };
-
-#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
+#endif
 
 static void cliPrint(const char *str)
 {
@@ -928,6 +948,24 @@ static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_
     }
 }
 
+#ifdef USE_PARAMETER_GROUPS
+static void* getValuePointer(const clivalue_t *var)
+{
+    const pgRegistry_t* rec = pgFind(var->pgn);
+
+    switch (var->type & VALUE_SECTION_MASK) {
+    case MASTER_VALUE:
+        return rec->address + var->offset;
+    case PROFILE_RATE_VALUE:
+        return rec->address + var->offset + sizeof(profile_t) * getCurrentProfile();
+    case CONTROL_RATE_VALUE:
+        return rec->address + var->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile();
+    case PROFILE_VALUE:
+        return *rec->ptr + var->offset;
+    }
+    return NULL;
+}
+#else
 void *getValuePointer(const clivalue_t *value)
 {
     void *ptr = value->ptr;
@@ -942,6 +980,7 @@ void *getValuePointer(const clivalue_t *value)
 
     return ptr;
 }
+#endif
 
 static void *getDefaultPointer(void *valuePointer, const master_t *defaultConfig)
 {
@@ -1003,7 +1042,7 @@ static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, const maste
 static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *defaultConfig)
 {
     const clivalue_t *value;
-    for (uint32_t i = 0; i < VALUE_COUNT; i++) {
+    for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
         value = &valueTable[i];
 
         if ((value->type & VALUE_SECTION_MASK) != valueSection) {
@@ -1050,13 +1089,7 @@ typedef union {
 
 static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
 {
-    void *ptr = var->ptr;
-    if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) {
-        ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
-    }
-    if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) {
-        ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile());
-    }
+    void *ptr = getValuePointer(var);
 
     switch (var->type & VALUE_TYPE_MASK) {
         case VAR_UINT8:
@@ -3072,7 +3105,7 @@ static void cliGet(char *cmdline)
     const clivalue_t *val;
     int matchedCommands = 0;
 
-    for (uint32_t i = 0; i < VALUE_COUNT; i++) {
+    for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
         if (strstr(valueTable[i].name, cmdline)) {
             val = &valueTable[i];
             cliPrintf("%s = ", valueTable[i].name);
@@ -3103,7 +3136,7 @@ static void cliSet(char *cmdline)
 
     if (len == 0 || (len == 1 && cmdline[0] == '*')) {
         cliPrint("Current settings: \r\n");
-        for (uint32_t i = 0; i < VALUE_COUNT; i++) {
+        for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
             val = &valueTable[i];
             cliPrintf("%s = ", valueTable[i].name);
             cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
@@ -3124,7 +3157,7 @@ static void cliSet(char *cmdline)
             eqptr++;
         }
 
-        for (uint32_t i = 0; i < VALUE_COUNT; i++) {
+        for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
             val = &valueTable[i];
             // ensure exact match when setting to prevent setting variables with shorter names
             if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) {
@@ -3540,6 +3573,18 @@ static void cliResource(char *cmdline)
 }
 #endif /* USE_RESOURCE_MGMT */
 
+#ifdef USE_PARAMETER_GROUPS
+static void backupConfigs(void)
+{
+     // make copies of configs to do differencing
+
+}
+
+static void restoreConfigs(void)
+{
+}
+#endif
+
 static void printConfig(char *cmdline, bool doDiff)
 {
     uint8_t dumpMask = DUMP_MASTER;
@@ -3556,13 +3601,21 @@ static void printConfig(char *cmdline, bool doDiff)
         options = cmdline;
     }
 
-    static master_t defaultConfig;
     if (doDiff) {
         dumpMask = dumpMask | DO_DIFF;
     }
 
+    static master_t defaultConfig;
     createDefaultConfig(&defaultConfig);
 
+#ifdef USE_PARAMETER_GROUPS
+    backupConfigs();
+    // reset all configs to defaults to do differencing
+    resetConfigs();
+#if defined(TARGET_CONFIG)
+    targetConfiguration(&defaultConfig);
+#endif
+#endif
     if (checkCommand(options, "showdefaults")) {
         dumpMask = dumpMask | SHOW_DEFAULTS;   // add default values as comments for changed values
     }
@@ -3685,6 +3738,10 @@ static void printConfig(char *cmdline, bool doDiff)
     if (dumpMask & DUMP_RATES) {
         cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig);
     }
+#ifdef USE_PARAMETER_GROUPS
+    // restore configs from copies
+    restoreConfigs();
+#endif
 }
 
 static void cliDump(char *cmdline)
@@ -3811,13 +3868,11 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx),
 #endif
 };
-#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
-
 static void cliHelp(char *cmdline)
 {
     UNUSED(cmdline);
 
-    for (uint32_t i = 0; i < CMD_COUNT; i++) {
+    for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) {
         cliPrint(cmdTable[i].name);
 #ifndef SKIP_CLI_COMMAND_HELP
         if (cmdTable[i].description) {
@@ -3846,7 +3901,7 @@ void cliProcess(void)
             // do tab completion
             const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
             uint32_t i = bufferIndex;
-            for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) {
+            for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) {
                 if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0))
                     continue;
                 if (!pstart)
@@ -3907,12 +3962,12 @@ void cliProcess(void)
 
                 const clicmd_t *cmd;
                 char *options;
-                for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) {
+                for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) {
                     if ((options = checkCommand(cliBuffer, cmd->name))) {
                         break;
-            }
+                    }
                 }
-                if(cmd < cmdTable + CMD_COUNT)
+                if(cmd < cmdTable + ARRAYLEN(cmdTable))
                     cmd->func(options);
                 else
                     cliPrint("Unknown command, try 'help'");
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index fccd15b8fa..1bbd9894bc 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -869,11 +869,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();
@@ -1166,7 +1169,7 @@ void ensureEEPROMContainsValidData(void)
 
 void resetEEPROM(void)
 {
-    resetConf();
+    resetConfigs();
     writeEEPROM();
 }
 
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 62841f5fd3..6466507e61 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -91,6 +91,7 @@ bool canSoftwareSerialBeUsed(void);
 
 uint16_t getCurrentMinthrottle(void);
 
+void resetConfigs(void);
 struct master_s;
 void targetConfiguration(struct master_s *config);
 void targetValidateConfiguration(struct master_s *config);

From 0bca9e0c7acd5b10ff9d7424ab7fbdeef369bb50 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 10:26:25 +0000
Subject: [PATCH 15/50] Preparation for doing PG differencing in CLI

---
 src/main/config/parameter_group.h |  30 ++--
 src/main/fc/cli.c                 | 220 +++++++++++++++++++++---------
 2 files changed, 175 insertions(+), 75 deletions(-)

diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h
index 734933ca9c..2533b50158 100644
--- a/src/main/config/parameter_group.h
+++ b/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))
 
@@ -100,22 +103,25 @@ extern const uint8_t __pg_resetdata_end[];
 // Declare system config
 #define PG_DECLARE(_type, _name)                                        \
     extern _type _name ## _System;                                      \
-    static inline _type* _name(void) { return &_name ## _System; }      \
+    static inline const _type* _name(void) { return &_name ## _System; }\
+    static inline _type* _name ## Mutable(void) { return &_name ## _System; }\
     struct _dummy                                                       \
     /**/
 
 // Declare system config array
-#define PG_DECLARE_ARR(_type, _size, _name)                             \
+#define PG_DECLARE_ARRAY(_type, _size, _name)                             \
     extern _type _name ## _SystemArray[_size];                          \
-    static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
-    static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \
+    static inline const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
+    static inline _type* _name ## Mutable(int _index) { return &_name ## _SystemArray[_index]; } \
+    static inline _type (* _name ## _array(void))[_size] { return &_name ## _SystemArray; } \
     struct _dummy                                                       \
     /**/
 
 // Declare profile config
 #define PG_DECLARE_PROFILE(_type, _name)                                \
     extern _type *_name ## _ProfileCurrent;                             \
-    static inline _type* _name(void) { return _name ## _ProfileCurrent; } \
+    static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \
+    static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \
     struct _dummy                                                       \
     /**/
 
@@ -148,7 +154,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 +166,20 @@ extern const uint8_t __pg_resetdata_end[];
     }                                                                   \
     /**/
 
-#define PG_REGISTER_ARR(_type, _size, _name, _pgn, _version)            \
-    PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
+#define PG_REGISTER_ARRAY(_type, _size, _name, _pgn, _version)            \
+    PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
     /**/
 
-#define PG_REGISTER_ARR_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \
+#define PG_REGISTER_ARRAY_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \
     extern void pgResetFn_ ## _name(_type *);    \
-    PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
+    PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
     /**/
 
 #if 0
 // ARRAY reset mechanism is not implemented yet, only few places in code would benefit from it - See pgResetInstance
-#define PG_REGISTER_ARR_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \
+#define PG_REGISTER_ARRAY_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \
     extern const _type pgResetTemplate_ ## _name;                       \
-    PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
+    PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
     /**/
 #endif
 
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index d44b854e79..52f7545367 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -117,16 +117,6 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128];
 static char cliBuffer[48];
 static uint32_t bufferIndex = 0;
 
-typedef enum {
-    DUMP_MASTER = (1 << 0),
-    DUMP_PROFILE = (1 << 1),
-    DUMP_RATES = (1 << 2),
-    DUMP_ALL = (1 << 3),
-    DO_DIFF = (1 << 4),
-    SHOW_DEFAULTS = (1 << 5),
-    HIDE_UNUSED = (1 << 6)
-} dumpFlags_e;
-
 static const char* const emptyName = "-";
 
 #ifndef USE_QUAD_MIXER_ONLY
@@ -452,22 +442,21 @@ static const lookupTableEntry_t lookupTables[] = {
 #define VALUE_MODE_OFFSET 6
 
 typedef enum {
-    // value type
+    // value type, bits 0-3
     VAR_UINT8 = (0 << VALUE_TYPE_OFFSET),
     VAR_INT8 = (1 << VALUE_TYPE_OFFSET),
     VAR_UINT16 = (2 << VALUE_TYPE_OFFSET),
     VAR_INT16 = (3 << VALUE_TYPE_OFFSET),
     //VAR_UINT32 = (4 << VALUE_TYPE_OFFSET),
-    VAR_FLOAT = (5 << VALUE_TYPE_OFFSET),
+    VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), // 0x05
 
-    // value section
+    // value section, bits 4-5
     MASTER_VALUE = (0 << VALUE_SECTION_OFFSET),
     PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET),
-    PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET),
-    CONTROL_RATE_VALUE = (3 << VALUE_SECTION_OFFSET),
+    PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // 0x20
     // value mode
-    MODE_DIRECT = (0 << VALUE_MODE_OFFSET),
-    MODE_LOOKUP = (1 << VALUE_MODE_OFFSET)
+    MODE_DIRECT = (0 << VALUE_MODE_OFFSET), // 0x40
+    MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) // 0x80
 } cliValueFlag_e;
 
 #define VALUE_TYPE_MASK (0x0F)
@@ -855,6 +844,16 @@ static void cliPutp(void *p, char ch)
     bufWriterAppend(p, ch);
 }
 
+typedef enum {
+    DUMP_MASTER = (1 << 0),
+    DUMP_PROFILE = (1 << 1),
+    DUMP_RATES = (1 << 2),
+    DUMP_ALL = (1 << 3),
+    DO_DIFF = (1 << 4),
+    SHOW_DEFAULTS = (1 << 5),
+    HIDE_UNUSED = (1 << 6)
+} dumpFlags_e;
+
 static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...)
 {
     if (!((dumpMask & DO_DIFF) && equalsDefault)) {
@@ -949,23 +948,121 @@ static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_
 }
 
 #ifdef USE_PARAMETER_GROUPS
-static void* getValuePointer(const clivalue_t *var)
-{
-    const pgRegistry_t* rec = pgFind(var->pgn);
 
-    switch (var->type & VALUE_SECTION_MASK) {
+static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptrDefault)
+{
+    bool result = false;
+    switch (type & VALUE_TYPE_MASK) {
+    case VAR_UINT8:
+        result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault;
+        break;
+
+    case VAR_INT8:
+        result = *(int8_t *)ptr == *(int8_t *)ptrDefault;
+        break;
+
+    case VAR_UINT16:
+        result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault;
+        break;
+
+    case VAR_INT16:
+        result = *(int16_t *)ptr == *(int16_t *)ptrDefault;
+        break;
+
+/* not currently used
+    case VAR_UINT32:
+        result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault;
+        break;*/
+
+    case VAR_FLOAT:
+        result = *(float *)ptr == *(float *)ptrDefault;
+        break;
+    }
+    return result;
+}
+
+typedef struct cliCurrentAndDefaultConfig_s {
+    const void *currentConfig; // the copy
+    const void *defaultConfig; // the PG value as set by default
+} cliCurrentAndDefaultConfig_t;
+
+static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn)
+{
+    static cliCurrentAndDefaultConfig_t ret;
+
+    switch (pgn) {
+    default:
+        ret.currentConfig = NULL;
+        ret.defaultConfig = NULL;
+        break;
+    }
+    return &ret;
+}
+
+static uint16_t getValueOffset(const clivalue_t *value)
+{
+    switch (value->type & VALUE_SECTION_MASK) {
     case MASTER_VALUE:
-        return rec->address + var->offset;
-    case PROFILE_RATE_VALUE:
-        return rec->address + var->offset + sizeof(profile_t) * getCurrentProfile();
-    case CONTROL_RATE_VALUE:
-        return rec->address + var->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile();
     case PROFILE_VALUE:
-        return *rec->ptr + var->offset;
+        return value->offset;
+    case PROFILE_RATE_VALUE:
+        return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile();
+    }
+    return 0;
+}
+
+static void *getValuePointer(const clivalue_t *value)
+{
+    const pgRegistry_t* rec = pgFind(value->pgn);
+
+    switch (value->type & VALUE_SECTION_MASK) {
+    case MASTER_VALUE:
+    case PROFILE_VALUE:
+        return rec->address + value->offset;
+    case PROFILE_RATE_VALUE:
+        return rec->address + value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile();
     }
     return NULL;
 }
+
+static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask)
+{
+    const char *format = "set %s = ";
+    const cliCurrentAndDefaultConfig_t *config = getCurrentAndDefaultConfigs(value->pgn);
+    if (config->currentConfig == NULL || config->defaultConfig == NULL) {
+        // has not been set up properly
+        cliPrintf("VALUE %s ERROR\r\n", value->name);
+        return;
+    }
+    const int valueOffset = getValueOffset(value);
+    switch (dumpMask & (DO_DIFF | SHOW_DEFAULTS)) {
+    case DO_DIFF:
+        if (valuePtrEqualsDefault(value->type, (uint8_t*)config->currentConfig + valueOffset, (uint8_t*)config->defaultConfig + valueOffset)) {
+            break;
+        }
+        // drop through, since not equal to default
+    case 0:
+    case SHOW_DEFAULTS:
+        cliPrintf(format, value->name);
+        printValuePointer(value, (uint8_t*)config->currentConfig + valueOffset, 0);
+        cliPrint("\r\n");
+        break;
+    }
+}
+
+static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask)
+{
+    for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
+        const clivalue_t *value = &valueTable[i];
+        bufWriterFlush(cliWriter);
+        if ((value->type & VALUE_SECTION_MASK) == valueSection) {
+            dumpPgValue(value, dumpMask);
+        }
+    }
+}
+
 #else
+
 void *getValuePointer(const clivalue_t *value)
 {
     void *ptr = value->ptr;
@@ -1138,9 +1235,9 @@ static void cliShowArgumentRangeError(char *name, int min, int max)
     cliPrintf("%s must be between %d and %d\r\n", name, min, max);
 }
 
-static char *nextArg(char *currentArg)
+static const char *nextArg(const char *currentArg)
 {
-    char *ptr = strchr(currentArg, ' ');
+    const char *ptr = strchr(currentArg, ' ');
     while (ptr && *ptr == ' ') {
         ptr++;
     }
@@ -1148,14 +1245,12 @@ static char *nextArg(char *currentArg)
     return ptr;
 }
 
-static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount)
+static const char *processChannelRangeArgs(const char *ptr, channelRange_t *range, uint8_t *validArgumentCount)
 {
-    int val;
-
     for (uint32_t argIndex = 0; argIndex < 2; argIndex++) {
         ptr = nextArg(ptr);
         if (ptr) {
-            val = atoi(ptr);
+            int val = atoi(ptr);
             val = CHANNEL_VALUE_TO_STEP(val);
             if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
                 if (argIndex == 0) {
@@ -1174,10 +1269,10 @@ static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *
 // Check if a string's length is zero
 static bool isEmpty(const char *string)
 {
-    return *string == '\0';
+    return (string == NULL || *string == '\0') ? true : false;
 }
 
-static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig)
+static void printRxFailsafe(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig)
 {
     // print out rxConfig failsafe settings
     for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) {
@@ -1216,7 +1311,7 @@ static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxCo
     }
 }
 
-static void cliRxFail(char *cmdline)
+static void cliRxFailsafe(char *cmdline)
 {
     uint8_t channel;
     char buf[3];
@@ -1224,10 +1319,10 @@ static void cliRxFail(char *cmdline)
     if (isEmpty(cmdline)) {
         // print out rxConfig failsafe settings
         for (channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) {
-            cliRxFail(itoa(channel, buf, 10));
+            cliRxFailsafe(itoa(channel, buf, 10));
         }
     } else {
-        char *ptr = cmdline;
+        const char *ptr = cmdline;
         channel = atoi(ptr++);
         if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) {
 
@@ -1334,7 +1429,7 @@ static void printAux(uint8_t dumpMask, const modeActivationProfile_t *modeActiva
 static void cliAux(char *cmdline)
 {
     int i, val = 0;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printAux(DUMP_MASTER, modeActivationProfile(), NULL);
@@ -1408,13 +1503,9 @@ static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, co
 
 static void cliSerial(char *cmdline)
 {
-    int i, val;
-    char *ptr;
-
     if (isEmpty(cmdline)) {
         printSerial(DUMP_MASTER, serialConfig(), NULL);
-
-    return;
+        return;
     }
     serialPortConfig_t portConfig;
     memset(&portConfig, 0 , sizeof(portConfig));
@@ -1423,9 +1514,9 @@ static void cliSerial(char *cmdline)
 
     uint8_t validArgumentCount = 0;
 
-    ptr = cmdline;
+    const char *ptr = cmdline;
 
-    val = atoi(ptr++);
+    int val = atoi(ptr++);
     currentConfig = serialFindPortConfiguration(val);
     if (currentConfig) {
         portConfig.identifier = val;
@@ -1439,7 +1530,7 @@ static void cliSerial(char *cmdline)
         validArgumentCount++;
     }
 
-    for (i = 0; i < 4; i ++) {
+    for (int i = 0; i < 4; i ++) {
         ptr = nextArg(ptr);
         if (!ptr) {
             break;
@@ -1488,7 +1579,6 @@ static void cliSerial(char *cmdline)
     }
 
     memcpy(currentConfig, &portConfig, sizeof(portConfig));
-
 }
 
 #ifndef SKIP_SERIAL_PASSTHROUGH
@@ -1607,7 +1697,7 @@ static void printAdjustmentRange(uint8_t dumpMask, const adjustmentProfile_t *ad
 static void cliAdjustmentRange(char *cmdline)
 {
     int i, val = 0;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printAdjustmentRange(DUMP_MASTER, adjustmentProfile(), NULL);
@@ -1711,7 +1801,7 @@ static void cliMotorMix(char *cmdline)
 #else
     int check = 0;
     uint8_t len;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL);
@@ -1799,7 +1889,7 @@ static void printRxRange(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxC
 static void cliRxRange(char *cmdline)
 {
     int i, validArgumentCount = 0;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printRxRange(DUMP_MASTER, rxConfig(), NULL);
@@ -1861,7 +1951,7 @@ static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledC
 static void cliLed(char *cmdline)
 {
     int i;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printLed(DUMP_MASTER, ledStripConfig()->ledConfigs, NULL);
@@ -1899,7 +1989,7 @@ static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColo
 static void cliColor(char *cmdline)
 {
     int i;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printColor(DUMP_MASTER, ledStripConfig()->colors, NULL);
@@ -2177,10 +2267,8 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig)
 
 static void cliServoMix(char *cmdline)
 {
-    uint8_t len;
-    char *ptr;
     int args[8], check = 0;
-    len = strlen(cmdline);
+    int len = strlen(cmdline);
 
     if (len == 0) {
         printServoMix(DUMP_MASTER, NULL);
@@ -2191,7 +2279,7 @@ static void cliServoMix(char *cmdline)
             servoProfile()->servoConf[i].reversedSources = 0;
         }
     } else if (strncasecmp(cmdline, "load", 4) == 0) {
-        ptr = nextArg(cmdline);
+        const char *ptr = nextArg(cmdline);
         if (ptr) {
             len = strlen(ptr);
             for (uint32_t i = 0; ; i++) {
@@ -2209,7 +2297,7 @@ static void cliServoMix(char *cmdline)
         }
     } else if (strncasecmp(cmdline, "reverse", 7) == 0) {
         enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT};
-        ptr = strchr(cmdline, ' ');
+        char *ptr = strchr(cmdline, ' ');
 
         len = strlen(ptr);
         if (len == 0) {
@@ -2251,7 +2339,7 @@ static void cliServoMix(char *cmdline)
         cliServoMix("reverse");
     } else {
         enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT};
-        ptr = strtok(cmdline, " ");
+        char *ptr = strtok(cmdline, " ");
         while (ptr != NULL && check < ARGS_COUNT) {
             args[check++] = atoi(ptr);
             ptr = strtok(NULL, " ");
@@ -2425,7 +2513,7 @@ static void cliFlashRead(char *cmdline)
 
     uint8_t buffer[32];
 
-    char *nextArg = strchr(cmdline, ' ');
+    const char *nextArg = strchr(cmdline, ' ');
 
     if (!nextArg) {
         cliShowParseError();
@@ -2494,7 +2582,7 @@ static void printVtx(uint8_t dumpMask, const master_t *defaultConfig)
 static void cliVtx(char *cmdline)
 {
     int i, val = 0;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printVtx(DUMP_MASTER, NULL);
@@ -3065,7 +3153,13 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_
     cliPrintHashLine("profile");
     cliProfile("");
     cliPrint("\r\n");
+#ifdef USE_PARAMETER_GROUPS
+    (void)(defaultConfig);
+    dumpAllValues(PROFILE_VALUE, dumpMask);
+    dumpAllValues(PROFILE_RATE_VALUE, dumpMask);
+#else
     dumpValues(PROFILE_VALUE, dumpMask, defaultConfig);
+#endif
     cliRateProfile("");
 }
 
@@ -3699,7 +3793,7 @@ static void printConfig(char *cmdline, bool doDiff)
 #endif
 
         cliPrintHashLine("rxfail");
-        printRxFail(dumpMask, rxConfig(), &defaultConfig.rxConfig);
+        printRxFailsafe(dumpMask, rxConfig(), &defaultConfig.rxConfig);
 
         cliPrintHashLine("master");
         dumpValues(MASTER_VALUE, dumpMask, &defaultConfig);
@@ -3839,7 +3933,7 @@ const clicmd_t cmdTable[] = {
 #if defined(USE_RESOURCE_MGMT)
     CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource),
 #endif
-    CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail),
+    CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFailsafe),
     CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
     CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
 #ifdef USE_SDCARD

From fc296533cd01a4765d42f9a84c3f79327a07899b Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 11:34:20 +0000
Subject: [PATCH 16/50] Fixed up unit tests and ROM sizes

---
 src/main/target/NAZE/target.h              |  3 +--
 src/test/unit/parameter_groups_unittest.cc | 10 ++++------
 2 files changed, 5 insertions(+), 8 deletions(-)

diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index b9cc396a3a..315faf2fe5 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -101,8 +101,7 @@
 #define ACC_MPU6500_ALIGN       CW0_DEG
 
 #define BARO
-#define USE_BARO_MS5611
-#define USE_BARO_BMP085
+#define USE_BARO_MS5611 // needed for Flip32 board
 #define USE_BARO_BMP280
 
 /*
diff --git a/src/test/unit/parameter_groups_unittest.cc b/src/test/unit/parameter_groups_unittest.cc
index df45a1829c..a48e98e055 100644
--- a/src/test/unit/parameter_groups_unittest.cc
+++ b/src/test/unit/parameter_groups_unittest.cc
@@ -22,10 +22,8 @@
 #include <limits.h>
 
 extern "C" {
-    #include "build/debug.h"
-
     #include <platform.h>
-
+    #include "build/debug.h"
     #include "config/parameter_group.h"
     #include "config/parameter_group_ids.h"
 
@@ -49,7 +47,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
 
 TEST(ParameterGroupsfTest, Test_pgResetAll)
 {
-    memset(motorConfig(), 0, sizeof(motorConfig_t));
+    memset(motorConfigMutable(), 0, sizeof(motorConfig_t));
     pgResetAll(0);
     EXPECT_EQ(1150, motorConfig()->minthrottle);
     EXPECT_EQ(1850, motorConfig()->maxthrottle);
@@ -59,7 +57,7 @@ TEST(ParameterGroupsfTest, Test_pgResetAll)
 
 TEST(ParameterGroupsfTest, Test_pgFind)
 {
-    memset(motorConfig(), 0, sizeof(motorConfig_t));
+    memset(motorConfigMutable(), 0, sizeof(motorConfig_t));
     const pgRegistry_t *pgRegistry = pgFind(PG_MOTOR_CONFIG);
     pgResetCurrent(pgRegistry);
     EXPECT_EQ(1150, motorConfig()->minthrottle);
@@ -69,7 +67,7 @@ TEST(ParameterGroupsfTest, Test_pgFind)
 
     motorConfig_t motorConfig2;
     memset(&motorConfig2, 0, sizeof(motorConfig_t));
-    motorConfig()->motorPwmRate = 500;
+    motorConfigMutable()->motorPwmRate = 500;
     pgStore(pgRegistry, &motorConfig2, sizeof(motorConfig_t), 0);
     EXPECT_EQ(1150, motorConfig2.minthrottle);
     EXPECT_EQ(1850, motorConfig2.maxthrottle);

From e4c860d4ddd8497a7ae6f99bc2dee18ff4b5acf0 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 12:04:41 +0000
Subject: [PATCH 17/50] Added STM32F722xx FLASH page size

---
 src/main/config/config_streamer.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c
index e63363f22d..c4b6f6af03 100644
--- a/src/main/config/config_streamer.c
+++ b/src/main/config/config_streamer.c
@@ -27,18 +27,24 @@ extern uint8_t __config_start;   // configured via linker script when building b
 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)

From 107fc11939c9c873ae890d583ad2153cd4b0eead Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Dec 2016 19:31:02 +0100
Subject: [PATCH 18/50] Fix atomic.h

`=m` output operand means that value is write only, gcc may discard previous value because it assumes that it will be overwritten. This bug was not triggered in gcc v4 because `asm volatile` triggered full memory barrier.

With old version, this code will increase `markme` only by 2, not 3:
```
static int markme = 0;
markme++;
ATOMIC_BLOCK_NB(0xff) {
  ATOMIC_BARRIER(markme);  // markme is marked as overwritten, previous increment can be discarded
  markme++;
}
markme++;
```
---
 src/main/build/atomic.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index 959c6e9d8d..f2436d5534 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -104,7 +104,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
         __asm__ volatile ("\t# barier(" #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# barier (" #data ") start\n" : "+m" (*__UNIQL(__barrier)))
 
 
 // define these wrappers for atomic operations, use gcc buildins

From afc71be60ce8eae404b736f1d1b36eac4eb05f64 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Fri, 30 Dec 2016 12:57:34 +0100
Subject: [PATCH 19/50] Improve ATOMIC_BARRIER documentation

---
 src/main/build/atomic.h | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index f2436d5534..3708e911ae 100644
--- a/src/main/build/atomic.h
+++ b/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.
-// Be carefull when using this, you must use some method to prevent optimizer form breaking things
-// - lto is used for baseflight compillation, so function call is not memory barrier
-// - use ATOMIC_BARRIER or propper volatile to protect used variables
-// - gcc 4.8.4 does write all values in registes to memory before 'asm volatile', so this optimization does not help much
-//    but that can change in future versions
+// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier.
+// Be careful when using this, you must use some method to prevent optimizer form breaking things
+// - lto is used for Cleanflight compilation, so function call is not memory barrier
+// - use ATOMIC_BARRIER or volatile to protect used variables
+// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
+// - gcc 5 and later works as intended, generating quite optimal code
 #define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
                                     __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
 
 // ATOMIC_BARRIER
 // Create memory barrier
-// - at the beginning (all data must be reread from memory)
-// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
-// ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier
+// - at the beginning of containing block (value of parameter must be reread from memory)
+// - at exit of block (all exit paths) (parameter value if written into memory, but may be cached in register for subsequent use)
+// On gcc 5 and higher, this protects only memory passed as parameter (any type should work)
 // this macro can be used only ONCE PER LINE, but multiple uses per block are fine
 
 #if (__GNUC__ > 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

From 7f8f7deab97bb28c211e35663ce76c2876bf116d Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Thu, 26 Jan 2017 10:05:17 +1300
Subject: [PATCH 20/50] Fixed typo in generated comment.

---
 src/main/build/atomic.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index 3708e911ae..0508f77e4f 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -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

From f839e658e756ab3496aa8d821a59f50bf5ba451a Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 21:06:01 +0000
Subject: [PATCH 21/50] Alignment with iNav

---
 src/main/config/config_eeprom.c | 19 +++++----
 src/main/config/config_eeprom.h |  4 ++
 src/main/fc/config.c            | 69 +++++++++++++++++----------------
 src/main/fc/config.h            |  3 ++
 4 files changed, 54 insertions(+), 41 deletions(-)

diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 2850700c54..295cd77b97 100644
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -47,6 +47,8 @@ PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0);
 extern uint8_t __config_start;   // configured via linker script when building binaries.
 extern uint8_t __config_end;
 
+static uint16_t eepromConfigSize;
+
 typedef enum {
     CR_CLASSICATION_SYSTEM   = 0,
     CR_CLASSICATION_PROFILE1 = 1,
@@ -111,7 +113,7 @@ static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length)
 }
 
 // Scan the EEPROM config. Returns true if the config is valid.
-static bool scanEEPROM(void)
+bool isEEPROMContentValid(void)
 {
     uint8_t chk = 0;
     const uint8_t *p = &__config_start;
@@ -148,7 +150,15 @@ static bool scanEEPROM(void)
     chk = updateChecksum(chk, footer, sizeof(*footer));
     p += sizeof(*footer);
     chk = ~chk;
-    return chk == *p;
+    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
@@ -207,11 +217,6 @@ bool loadEEPROM(void)
     return true;
 }
 
-bool isEEPROMContentValid(void)
-{
-    return scanEEPROM();
-}
-
 static bool writeSettingsToEEPROM(void)
 {
     config_streamer_t streamer;
diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h
index a6aafc3ca7..62c8c6181a 100644
--- a/src/main/config/config_eeprom.h
+++ b/src/main/config/config_eeprom.h
@@ -17,8 +17,12 @@
 
 #pragma once
 
+#include <stdint.h>
+#include <stdbool.h>
+
 #define EEPROM_CONF_VERSION 154
 
 bool isEEPROMContentValid(void);
 bool loadEEPROM(void);
 void writeConfigToEEPROM(void);
+uint16_t getEEPROMConfigSize(void);
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 1bbd9894bc..63f5dc6a65 100755
--- a/src/main/fc/config.c
+++ b/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,43 +63,37 @@
 #include "fc/rc_curves.h"
 #include "fc/runtime_config.h"
 
-#include "sensors/sensors.h"
-#include "sensors/gyro.h"
-#include "sensors/compass.h"
-#include "sensors/acceleration.h"
-#include "sensors/barometer.h"
-#include "sensors/battery.h"
-#include "sensors/boardalignment.h"
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
+#include "flight/servos.h"
 
 #include "io/beeper.h"
-#include "io/serial.h"
 #include "io/gimbal.h"
-#include "io/motors.h"
-#include "io/servos.h"
-#include "io/ledstrip.h"
 #include "io/gps.h"
+#include "io/ledstrip.h"
+#include "io/motors.h"
 #include "io/osd.h"
+#include "io/serial.h"
+#include "io/servos.h"
 #include "io/vtx.h"
 
 #include "rx/rx.h"
 #include "rx/rx_spi.h"
 
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/battery.h"
+#include "sensors/boardalignment.h"
+#include "sensors/compass.h"
+#include "sensors/gyro.h"
+#include "sensors/sensors.h"
+
 #include "telemetry/telemetry.h"
 
-#include "flight/mixer.h"
-#include "flight/servos.h"
-#include "flight/pid.h"
-#include "flight/imu.h"
-#include "flight/failsafe.h"
-#include "flight/altitudehold.h"
-#include "flight/navigation.h"
-
-#include "config/config_eeprom.h"
-#include "config/config_profile.h"
-#include "config/config_master.h"
-#include "config/feature.h"
-#include "config/parameter_group.h"
-
 #ifndef DEFAULT_RX_FEATURE
 #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
 #endif
@@ -1159,6 +1160,12 @@ void writeEEPROM(void)
     resumeRxSignal();
 }
 
+void resetEEPROM(void)
+{
+    resetConfigs();
+    writeEEPROM();
+}
+
 void ensureEEPROMContainsValidData(void)
 {
     if (isEEPROMContentValid()) {
@@ -1167,12 +1174,6 @@ void ensureEEPROMContainsValidData(void)
     resetEEPROM();
 }
 
-void resetEEPROM(void)
-{
-    resetConfigs();
-    writeEEPROM();
-}
-
 void saveConfigAndNotify(void)
 {
     writeEEPROM();
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 6466507e61..7a85b85384 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -17,8 +17,11 @@
 
 #pragma once
 
+#include <stdint.h>
 #include <stdbool.h>
 
+#include "config/parameter_group.h"
+
 #if FLASH_SIZE <= 128
 #define MAX_PROFILE_COUNT 2
 #else

From 6a4e6e58859aab85b9b2f9cd583c65bea563a12d Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Thu, 26 Jan 2017 11:33:29 +0900
Subject: [PATCH 22/50] Fix TARGET_IO_PORTx defs
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

So bogus ports don’t show up on “resource list”.
---
 src/main/target/OMNIBUSF4/target.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 6d4289da5a..e97ff6468b 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -173,10 +173,10 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
-#define TARGET_IO_PORTA 0xffff
-#define TARGET_IO_PORTB 0xffff
-#define TARGET_IO_PORTC 0xffff
-#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
+#define TARGET_IO_PORTB (0xffff & ~(BIT(2)))
+#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13)))
+#define TARGET_IO_PORTD BIT(2)
 
 #define USABLE_TIMER_CHANNEL_COUNT 12
 #define USED_TIMERS  ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))

From 65e0d9990bf06ebd3232ed0a3c705e650327fb12 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Fri, 27 Jan 2017 17:12:01 +0000
Subject: [PATCH 23/50] Moved gps_conversion to common/ directory

---
 Makefile                                     |  2 +-
 src/main/{flight => common}/gps_conversion.c |  0
 src/main/{flight => common}/gps_conversion.h |  0
 src/main/flight/navigation.c                 | 16 ++++++++--------
 src/main/io/gps.c                            | 20 ++++++++++----------
 5 files changed, 19 insertions(+), 19 deletions(-)
 rename src/main/{flight => common}/gps_conversion.c (100%)
 rename src/main/{flight => common}/gps_conversion.h (100%)

diff --git a/Makefile b/Makefile
index decd865b2e..4350bba43a 100644
--- a/Makefile
+++ b/Makefile
@@ -649,6 +649,7 @@ HIGHEND_SRC = \
             cms/cms_menu_osd.c \
             cms/cms_menu_vtx.c \
             common/colorconversion.c \
+            common/gps_conversion.c \
             drivers/display_ug2864hsweg01.c \
             drivers/light_ws2811strip.c \
             drivers/serial_escserial.c \
@@ -656,7 +657,6 @@ HIGHEND_SRC = \
             drivers/sonar_hcsr04.c \
             drivers/vtx_common.c \
             flight/navigation.c \
-            flight/gps_conversion.c \
             io/dashboard.c \
             io/displayport_max7456.c \
             io/displayport_msp.c \
diff --git a/src/main/flight/gps_conversion.c b/src/main/common/gps_conversion.c
similarity index 100%
rename from src/main/flight/gps_conversion.c
rename to src/main/common/gps_conversion.c
diff --git a/src/main/flight/gps_conversion.h b/src/main/common/gps_conversion.h
similarity index 100%
rename from src/main/flight/gps_conversion.h
rename to src/main/common/gps_conversion.h
diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c
index dffd456893..fc1c4a144d 100644
--- a/src/main/flight/navigation.c
+++ b/src/main/flight/navigation.c
@@ -27,6 +27,7 @@
 #include "build/debug.h"
 
 #include "common/axis.h"
+#include "common/gps_conversion.h"
 #include "common/maths.h"
 #include "common/time.h"
 
@@ -36,21 +37,20 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "sensors/sensors.h"
-#include "sensors/boardalignment.h"
-#include "sensors/acceleration.h"
+#include "flight/imu.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
 
 #include "io/beeper.h"
 #include "io/serial.h"
 #include "io/gps.h"
 
-#include "flight/pid.h"
-#include "flight/navigation.h"
-#include "flight/gps_conversion.h"
-#include "flight/imu.h"
-
 #include "rx/rx.h"
 
+#include "sensors/acceleration.h"
+#include "sensors/boardalignment.h"
+#include "sensors/sensors.h"
+
 
 extern int16_t magHold;
 
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index c4925d8f4e..9cae15f265 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -29,27 +29,27 @@
 #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 "drivers/system.h"
+#include "config/feature.h"
+
 #include "drivers/light_led.h"
+#include "drivers/system.h"
 
-#include "sensors/sensors.h"
-
-#include "io/serial.h"
 #include "io/dashboard.h"
 #include "io/gps.h"
-
-#include "flight/gps_conversion.h"
-#include "flight/pid.h"
-#include "flight/navigation.h"
+#include "io/serial.h"
 
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
-#include "config/feature.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
+
+#include "sensors/sensors.h"
 
 #define LOG_ERROR        '?'
 #define LOG_IGNORED      '!'

From bfbacf8b444b34ab1d0831b316162f50d790a299 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sat, 28 Jan 2017 07:37:15 +0000
Subject: [PATCH 24/50] Fixup test code

---
 src/test/Makefile                        | 16 ++++++++--------
 src/test/unit/telemetry_crsf_unittest.cc | 14 +++++++-------
 src/test/unit/telemetry_hott_unittest.cc | 20 ++++++++++----------
 3 files changed, 25 insertions(+), 25 deletions(-)

diff --git a/src/test/Makefile b/src/test/Makefile
index 9f1e83940d..26b95987ce 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -256,24 +256,24 @@ $(OBJECT_DIR)/altitude_hold_unittest : \
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
 
-$(OBJECT_DIR)/flight/gps_conversion.o : \
-	$(USER_DIR)/flight/gps_conversion.c \
-	$(USER_DIR)/flight/gps_conversion.h \
+$(OBJECT_DIR)/common/gps_conversion.o : \
+	$(USER_DIR)/common/gps_conversion.c \
+	$(USER_DIR)/common/gps_conversion.h \
 	$(GTEST_HEADERS)
 
 	@mkdir -p $(dir $@)
-	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/gps_conversion.c -o $@
 
 $(OBJECT_DIR)/gps_conversion_unittest.o : \
 	$(TEST_DIR)/gps_conversion_unittest.cc \
-	$(USER_DIR)/flight/gps_conversion.h \
+	$(USER_DIR)/common/gps_conversion.h \
 	$(GTEST_HEADERS)
 
 	@mkdir -p $(dir $@)
 	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@
 
 $(OBJECT_DIR)/gps_conversion_unittest : \
-	$(OBJECT_DIR)/flight/gps_conversion.o \
+	$(OBJECT_DIR)/common/gps_conversion.o \
 	$(OBJECT_DIR)/gps_conversion_unittest.o \
 	$(OBJECT_DIR)/gtest_main.a
 
@@ -359,7 +359,7 @@ $(OBJECT_DIR)/telemetry_hott_unittest.o : \
 $(OBJECT_DIR)/telemetry_hott_unittest : \
 	$(OBJECT_DIR)/telemetry/hott.o \
 	$(OBJECT_DIR)/telemetry_hott_unittest.o \
-	$(OBJECT_DIR)/flight/gps_conversion.o \
+	$(OBJECT_DIR)/common/gps_conversion.o \
 	$(OBJECT_DIR)/gtest_main.a
 
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
@@ -537,7 +537,7 @@ $(OBJECT_DIR)/telemetry_crsf_unittest : \
 	$(OBJECT_DIR)/telemetry_crsf_unittest.o \
 	$(OBJECT_DIR)/common/maths.o \
 	$(OBJECT_DIR)/common/streambuf.o \
-	$(OBJECT_DIR)/flight/gps_conversion.o \
+	$(OBJECT_DIR)/common/gps_conversion.o \
 	$(OBJECT_DIR)/fc/runtime_config.o \
 	$(OBJECT_DIR)/gtest_main.a
 
diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc
index bb1a526d7e..cfe99b7943 100644
--- a/src/test/unit/telemetry_crsf_unittest.cc
+++ b/src/test/unit/telemetry_crsf_unittest.cc
@@ -28,30 +28,30 @@ extern "C" {
 
     #include "common/axis.h"
     #include "common/filter.h"
+    #include "common/gps_conversion.h"
     #include "common/maths.h"
 
     #include "config/parameter_group.h"
     #include "config/parameter_group_ids.h"
 
-    #include "drivers/system.h"
     #include "drivers/serial.h"
+    #include "drivers/system.h"
 
     #include "fc/runtime_config.h"
 
+    #include "flight/pid.h"
+    #include "flight/imu.h"
+
     #include "io/gps.h"
     #include "io/serial.h"
 
     #include "rx/crsf.h"
 
-    #include "sensors/sensors.h"
     #include "sensors/battery.h"
+    #include "sensors/sensors.h"
 
-    #include "telemetry/telemetry.h"
     #include "telemetry/crsf.h"
-
-    #include "flight/pid.h"
-    #include "flight/imu.h"
-    #include "flight/gps_conversion.h"
+    #include "telemetry/telemetry.h"
 
     bool airMode;
     uint16_t vbat;
diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc
index ff984b097b..bc0d60ad01 100644
--- a/src/test/unit/telemetry_hott_unittest.cc
+++ b/src/test/unit/telemetry_hott_unittest.cc
@@ -27,24 +27,24 @@ extern "C" {
     #include "build/debug.h"
 
     #include "common/axis.h"
+    #include "common/gps_conversion.h"
 
-    #include "drivers/system.h"
     #include "drivers/serial.h"
+    #include "drivers/system.h"
 
-    #include "sensors/sensors.h"
-    #include "sensors/battery.h"
-    #include "sensors/barometer.h"
+    #include "fc/runtime_config.h"
+
+    #include "flight/pid.h"
 
-    #include "io/serial.h"
     #include "io/gps.h"
+    #include "io/serial.h"
+
+    #include "sensors/barometer.h"
+    #include "sensors/battery.h"
+    #include "sensors/sensors.h"
 
     #include "telemetry/telemetry.h"
     #include "telemetry/hott.h"
-
-    #include "flight/pid.h"
-    #include "flight/gps_conversion.h"
-
-    #include "fc/runtime_config.h"
 }
 
 #include "unittest_macros.h"

From f3b38f4f09b2c8bb6bb8dc3713e6ba39eb83d9db Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sat, 28 Jan 2017 07:30:32 +0000
Subject: [PATCH 25/50] Put #includes into alphabetical order

---
 src/main/fc/cli.c    |  2 +-
 src/main/fc/fc_msp.c | 60 ++++++++++++++++++++++----------------------
 2 files changed, 31 insertions(+), 31 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index ef80bd96d1..8e063dbed3 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -45,8 +45,8 @@ uint8_t cliMode = 0;
 #include "common/typeconversion.h"
 
 #include "config/config_eeprom.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"
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index e28f09b081..2923c7aac6 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -34,20 +34,25 @@
 #include "common/maths.h"
 #include "common/streambuf.h"
 
-#include "drivers/system.h"
+#include "config/config_eeprom.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+#include "config/feature.h"
+
 #include "drivers/accgyro.h"
-#include "drivers/compass.h"
-#include "drivers/serial.h"
 #include "drivers/bus_i2c.h"
-#include "drivers/io.h"
+#include "drivers/compass.h"
 #include "drivers/flash.h"
-#include "drivers/sdcard.h"
-#include "drivers/vcd.h"
+#include "drivers/io.h"
 #include "drivers/max7456.h"
-#include "drivers/vtx_soft_spi_rtc6705.h"
 #include "drivers/pwm_output.h"
+#include "drivers/sdcard.h"
+#include "drivers/serial.h"
 #include "drivers/serial_escserial.h"
+#include "drivers/system.h"
+#include "drivers/vcd.h"
 #include "drivers/vtx_common.h"
+#include "drivers/vtx_soft_spi_rtc6705.h"
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
@@ -55,17 +60,25 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
+#include "flight/servos.h"
+
+#include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
-#include "io/motors.h"
-#include "io/servos.h"
+#include "io/flashfs.h"
 #include "io/gps.h"
 #include "io/gimbal.h"
-#include "io/serial.h"
 #include "io/ledstrip.h"
-#include "io/flashfs.h"
-#include "io/transponder_ir.h"
-#include "io/asyncfatfs/asyncfatfs.h"
+#include "io/motors.h"
+#include "io/serial.h"
 #include "io/serial_4way.h"
+#include "io/servos.h"
+#include "io/transponder_ir.h"
 
 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
@@ -76,30 +89,17 @@
 
 #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

From 8f3ef2676fd8b5244aabe994c490a0b8bc17d942 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 21 Dec 2016 15:27:31 +0000
Subject: [PATCH 26/50] Tidied cms imu code

---
 src/main/cms/cms_menu_imu.c | 58 ++++++++++++++++++++-----------------
 1 file changed, 32 insertions(+), 26 deletions(-)

diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 26cce45584..5899c69f87 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -104,10 +104,11 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void
 static long cmsx_PidRead(void)
 {
 
+    const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
     for (uint8_t i = 0; i < 3; i++) {
-        tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i];
-        tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i];
-        tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i];
+        tempPid[i][0] = pidProfile->P8[i];
+        tempPid[i][1] = pidProfile->I8[i];
+        tempPid[i][2] = pidProfile->D8[i];
     }
 
     return 0;
@@ -125,10 +126,11 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
 {
     UNUSED(self);
 
+    pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
     for (uint8_t i = 0; i < 3; i++) {
-        masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0];
-        masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
-        masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
+        pidProfile->P8[i] = tempPid[i][0];
+        pidProfile->I8[i] = tempPid[i][1];
+        pidProfile->D8[i] = tempPid[i][2];
     }
     pidInitConfig(&currentProfile->pidProfile);
 
@@ -233,12 +235,13 @@ static long cmsx_profileOtherOnEnter(void)
 {
     profileIndexString[1] = '0' + tmpProfileIndex;
 
-    cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight;
-    cmsx_setpointRelaxRatio  = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio;
+    const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
+    cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight;
+    cmsx_setpointRelaxRatio  = pidProfile->setpointRelaxRatio;
 
-    cmsx_angleStrength =     masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL];
-    cmsx_horizonStrength =   masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL];
-    cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL];
+    cmsx_angleStrength =     pidProfile->P8[PIDLEVEL];
+    cmsx_horizonStrength =   pidProfile->I8[PIDLEVEL];
+    cmsx_horizonTransition = pidProfile->D8[PIDLEVEL];
 
     return 0;
 }
@@ -247,13 +250,14 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
 {
     UNUSED(self);
 
-    masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
-    masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
+    pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
+    pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight;
+    pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
     pidInitConfig(&currentProfile->pidProfile);
 
-    masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
-    masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;
-    masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition;
+    pidProfile->P8[PIDLEVEL] = cmsx_angleStrength;
+    pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength;
+    pidProfile->D8[PIDLEVEL] = cmsx_horizonTransition;
 
     return 0;
 }
@@ -311,11 +315,12 @@ static uint16_t cmsx_yaw_p_limit;
 
 static long cmsx_FilterPerProfileRead(void)
 {
-    cmsx_dterm_lpf_hz =       masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz;
-    cmsx_dterm_notch_hz =     masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz;
-    cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff;
-    cmsx_yaw_lpf_hz =         masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz;
-    cmsx_yaw_p_limit =        masterConfig.profile[profileIndex].pidProfile.yaw_p_limit;
+    const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
+    cmsx_dterm_lpf_hz =       pidProfile->dterm_lpf_hz;
+    cmsx_dterm_notch_hz =     pidProfile->dterm_notch_hz;
+    cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
+    cmsx_yaw_lpf_hz =         pidProfile->yaw_lpf_hz;
+    cmsx_yaw_p_limit =        pidProfile->yaw_p_limit;
 
     return 0;
 }
@@ -324,11 +329,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
 {
     UNUSED(self);
 
-    masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz =       cmsx_dterm_lpf_hz;
-    masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz =     cmsx_dterm_notch_hz;
-    masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
-    masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz =         cmsx_yaw_lpf_hz;
-    masterConfig.profile[profileIndex].pidProfile.yaw_p_limit =        cmsx_yaw_p_limit;
+    pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
+    pidProfile->dterm_lpf_hz =       cmsx_dterm_lpf_hz;
+    pidProfile->dterm_notch_hz =     cmsx_dterm_notch_hz;
+    pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
+    pidProfile->yaw_lpf_hz =         cmsx_yaw_lpf_hz;
+    pidProfile->yaw_p_limit =        cmsx_yaw_p_limit;
 
     return 0;
 }

From 90e7bef9e4a286394dcc58f3e2941054ae5a65bd Mon Sep 17 00:00:00 2001
From: DieHertz <diehertz@gmail.com>
Date: Tue, 27 Dec 2016 13:25:37 +0300
Subject: [PATCH 27/50] Added PWM inversion to motor config

---
 src/main/drivers/pwm_output.c           | 14 ++++++++------
 src/main/drivers/pwm_output.h           |  2 +-
 src/main/drivers/pwm_output_stm32f3xx.c |  8 ++++----
 src/main/drivers/pwm_output_stm32f4xx.c |  8 ++++----
 src/main/drivers/pwm_output_stm32f7xx.c | 12 ++++++++----
 src/main/fc/cli.c                       |  1 +
 src/main/fc/fc_msp.c                    |  7 ++++++-
 src/main/io/motors.h                    |  3 ++-
 8 files changed, 34 insertions(+), 21 deletions(-)

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 8f99c16adc..eb71818010 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -83,7 +83,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
 #endif
 }
 
-static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
+static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion)
 {
 #if defined(USE_HAL_DRIVER)
     TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
@@ -91,7 +91,8 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
 #endif
 
     configTimeBase(timerHardware->tim, period, mhz);
-    pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
+    pwmOCConfig(timerHardware->tim, timerHardware->channel, value,
+        inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
 
 #if defined(USE_HAL_DRIVER)
     HAL_TIM_PWM_Start(Handle, timerHardware->channel);
@@ -259,7 +260,8 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
 
 #ifdef USE_DSHOT
         if (isDigital) {
-            pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
+            pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol,
+                motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
             motors[motorIndex].enabled = true;
             continue;
         }
@@ -274,9 +276,9 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
 
         if (useUnsyncedPwm) {
             const uint32_t hz = timerMhzCounter * 1000000;
-            pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse);
+            pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion);
         } else {
-            pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0);
+            pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion);
         }
 
         bool timerAlreadyUsed = false;
@@ -348,7 +350,7 @@ void servoInit(const servoConfig_t *servoConfig)
             break;
         }
 
-        pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
+        pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
         servos[servoIndex].enabled = true;
     }
 }
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 7102e7e351..97c5f5e662 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -119,7 +119,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
 #ifdef USE_DSHOT
 uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
 void pwmWriteDigital(uint8_t index, uint16_t value);
-void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType);
+void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
 void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
 #endif
 
diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c
index 5ae299dcf0..9e29cedaa5 100644
--- a/src/main/drivers/pwm_output_stm32f3xx.c
+++ b/src/main/drivers/pwm_output_stm32f3xx.c
@@ -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;
 
diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c
index 03156953a0..95d53b3d5d 100644
--- a/src/main/drivers/pwm_output_stm32f4xx.c
+++ b/src/main/drivers/pwm_output_stm32f4xx.c
@@ -102,7 +102,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
     }
 }
 
-void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
+void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 {
     TIM_OCInitTypeDef TIM_OCInitStructure;
     DMA_InitTypeDef DMA_InitStructure;
@@ -136,14 +136,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
 
     TIM_OCStructInit(&TIM_OCInitStructure);
     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
-    if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+    if (output & TIMER_OUTPUT_N_CHANNEL) {
         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
-        TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
+        TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
     } else {
         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
-        TIM_OCInitStructure.TIM_OCPolarity =  (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
+        TIM_OCInitStructure.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
     }
     TIM_OCInitStructure.TIM_Pulse = 0;
 
diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c
index b898493250..848cb88ea6 100644
--- a/src/main/drivers/pwm_output_stm32f7xx.c
+++ b/src/main/drivers/pwm_output_stm32f7xx.c
@@ -96,7 +96,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
     UNUSED(motorCount);
 }
 
-void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
+void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 {
     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
     motor->timerHardware = timerHardware;
@@ -174,9 +174,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
 
     /* PWM1 Mode configuration: Channel1 */
     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
-    TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
-    TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
-    TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+    if (output & TIMER_OUTPUT_N_CHANNEL) {
+        TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
+        TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+    } else {
+        TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
+        TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
+    }
     TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
     TIM_OCInitStructure.Pulse = 0;
 
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index bb242e96ce..87ca6b2e1f 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -524,6 +524,7 @@ const clivalue_t valueTable[] = {
     { "use_unsynced_pwm",           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } },
     { "motor_pwm_protocol",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } },
     { "motor_pwm_rate",             VAR_UINT16 | MASTER_VALUE,  &motorConfig()->motorPwmRate, .config.minmax = { 200, 32000 } },
+    { "motor_pwm_inversion",        VAR_UINT8  | MASTER_VALUE,  &motorConfig()->motorPwmInversion, .config.lookup = { TABLE_OFF_ON } },
 
     { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
     { "gyro_cal_on_first_arm",      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &armingConfig()->gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } },
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 404a895468..8e74b28931 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1144,6 +1144,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
         //!!TODO gyro_isr_update to be added pending decision
         //sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
+        sbufWriteU8(dst, motorConfig()->motorPwmInversion);
         break;
 
     case MSP_FILTER_CONFIG :
@@ -1480,7 +1481,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
 #endif
         motorConfig()->motorPwmRate = sbufReadU16(src);
-        if (dataSize > 7) {
+        if (sbufBytesRemaining(src) >= 2) {
             motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
         }
         if (sbufBytesRemaining(src)) {
@@ -1491,6 +1492,10 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             gyroConfig()->gyro_isr_update = sbufReadU8(src);
         }*/
         validateAndFixGyroConfig();
+
+        if (sbufBytesRemaining(src)) {        
+            motorConfig()->motorPwmInversion = sbufReadU8(src);
+        }
         break;
 
     case MSP_SET_FILTER_CONFIG:
diff --git a/src/main/io/motors.h b/src/main/io/motors.h
index 8486d507d2..82517b2e9d 100644
--- a/src/main/io/motors.h
+++ b/src/main/io/motors.h
@@ -24,8 +24,9 @@ typedef struct motorConfig_s {
     uint16_t minthrottle;                   // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
     uint16_t maxthrottle;                   // This is the maximum value for the ESCs at full power this value can be increased up to 2000
     uint16_t mincommand;                    // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
-    uint16_t motorPwmRate;                  // The update rate of motor outputs (50-498Hz)
+    uint16_t motorPwmRate;                  // The update rate of motor outputs
     uint8_t  motorPwmProtocol;              // Pwm Protocol
+    uint8_t  motorPwmInversion;              // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation
     uint8_t  useUnsyncedPwm;
     float    digitalIdleOffsetPercent;
     ioTag_t  ioTags[MAX_SUPPORTED_MOTORS];

From 135acdddaadb0211259128490740d1757b04c686 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Tue, 24 Jan 2017 22:38:36 +0000
Subject: [PATCH 28/50] Reviewed and revised compiler speed optimisations

---
 Makefile | 23 ++---------------------
 1 file changed, 2 insertions(+), 21 deletions(-)

diff --git a/Makefile b/Makefile
index 56c02912f9..012a03b231 100644
--- a/Makefile
+++ b/Makefile
@@ -708,26 +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/rc_curves.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 \
@@ -748,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 \
@@ -952,7 +933,7 @@ LTO_FLAGS           := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT)
 
 else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
 OPTIMISE_DEFAULT    := -O2
-OPTIMISE_SPEED      := -Ofast
+OPTIMISE_SPEED      := -O3
 OPTIMISE_SIZE       := -Os
 
 LTO_FLAGS           := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)

From 0aa83ad4f80159380c378a82df44509de0ee3839 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 17:08:51 +0000
Subject: [PATCH 29/50] Updated version, MSP version and EEPROM_CONF_VERSION
 for 3.2

---
 src/main/build/version.h    | 4 ++--
 src/main/msp/msp_protocol.h | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/build/version.h b/src/main/build/version.h
index 3df77b8cf7..4770ad98e9 100644
--- a/src/main/build/version.h
+++ b/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_PATCH_LEVEL      1  // increment when a bug is fixed
+#define FC_VERSION_MINOR            2  // increment when a minor release is made (small new feature, change etc)
+#define FC_VERSION_PATCH_LEVEL      0  // increment when a bug is fixed
 
 #define STR_HELPER(x) #x
 #define STR(x) STR_HELPER(x)
diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h
index 63d2b70bdb..d2c368f28a 100644
--- a/src/main/msp/msp_protocol.h
+++ b/src/main/msp/msp_protocol.h
@@ -59,7 +59,7 @@
 #define MSP_PROTOCOL_VERSION                0
 
 #define API_VERSION_MAJOR                   1 // increment when major changes are made
-#define API_VERSION_MINOR                   31 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
+#define API_VERSION_MINOR                   32 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
 
 #define API_VERSION_LENGTH                  2
 

From b8b790ba1fef6e89efcc4d1905a627179ce63586 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 18:16:05 +0000
Subject: [PATCH 30/50] Reorder accgyro_mpu.c functions for clarity and to
 avoid forward declarations

---
 src/main/drivers/accgyro_mpu.c | 227 ++++++++++++++++-----------------
 1 file changed, 109 insertions(+), 118 deletions(-)

diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index b452afc774..93405ba687 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -50,15 +50,6 @@
 
 mpuResetFuncPtr mpuReset;
 
-static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
-static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
-
-static void mpu6050FindRevision(gyroDev_t *gyro);
-
-#ifdef USE_SPI
-static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
-#endif
-
 #ifndef MPU_I2C_INSTANCE
 #define MPU_I2C_INSTANCE I2C_DEVICE
 #endif
@@ -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;
@@ -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,112 @@ 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.read = mpu6000ReadRegister;
+        gyro->mpuConfiguration.write = 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.read = mpu6500ReadRegister;
+        gyro->mpuConfiguration.write = 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.read = mpu9250ReadRegister;
+        gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
+        gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
+        gyro->mpuConfiguration.write = mpu9250WriteRegister;
+        gyro->mpuConfiguration.reset = 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.read = icm20689ReadRegister;
+        gyro->mpuConfiguration.write = icm20689WriteRegister;
+        return true;
+    }
+#endif
+
+    UNUSED(gyro);
+    return false;
+}
+#endif
+
+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;
+}
+
+void mpuGyroInit(gyroDev_t *gyro)
+{
+    mpuIntExtiInit(gyro);
+}

From 6dc16ce42c8013fe03632055b77124da54609796 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 18:49:32 +0000
Subject: [PATCH 31/50] Changed back to using -Ofast optimisation

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index 012a03b231..5e9cbb6303 100644
--- a/Makefile
+++ b/Makefile
@@ -933,7 +933,7 @@ LTO_FLAGS           := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT)
 
 else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
 OPTIMISE_DEFAULT    := -O2
-OPTIMISE_SPEED      := -O3
+OPTIMISE_SPEED      := -Ofast
 OPTIMISE_SIZE       := -Os
 
 LTO_FLAGS           := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)

From 82405fd50cae4e1835f15db0c2f1ce7704dbcb6b Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <rco@iptel.org>
Date: Tue, 17 Jan 2017 01:04:38 +0100
Subject: [PATCH 32/50] some fixes for vtx_common

---
 src/main/drivers/vtx_common.c | 10 ++++--
 src/main/drivers/vtx_common.h |  7 +++--
 src/main/io/vtx_tramp.c       | 57 +++++++++++++++++------------------
 3 files changed, 40 insertions(+), 34 deletions(-)

diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c
index 7e617b03ac..e43505d26c 100644
--- a/src/main/drivers/vtx_common.c
+++ b/src/main/drivers/vtx_common.c
@@ -53,10 +53,10 @@ void vtxCommonProcess(uint32_t currentTimeUs)
 
 vtxDevType_e vtxCommonGetDeviceType(void)
 {
-    if (!vtxDevice)
+    if (!vtxDevice || !vtxDevice->vTable->getDeviceType)
         return VTXDEV_UNKNOWN;
 
-    return vtxDevice->devtype;
+    return vtxDevice->vTable->getDeviceType();
 }
 
 // band and chan are 1 origin
@@ -65,6 +65,9 @@ void vtxCommonSetBandChan(uint8_t band, uint8_t chan)
     if (!vtxDevice)
         return;
 
+    if ((band > vtxDevice->numBand)|| (chan > vtxDevice->numChan))
+        return;
+    
     if (vtxDevice->vTable->setBandChan)
         vtxDevice->vTable->setBandChan(band, chan);
 }
@@ -75,6 +78,9 @@ void vtxCommonSetPowerByIndex(uint8_t index)
     if (!vtxDevice)
         return;
 
+    if (index > vtxDevice->numPower)
+        return;
+    
     if (vtxDevice->vTable->setPowerByIndex)
         vtxDevice->vTable->setPowerByIndex(index);
 }
diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h
index c7c521c666..e0899cc85a 100644
--- a/src/main/drivers/vtx_common.h
+++ b/src/main/drivers/vtx_common.h
@@ -18,9 +18,12 @@
 /* Created by jflyper */
 
 typedef enum {
-    VTXDEV_UNKNOWN    = 0,
+    VTXDEV_UNSUPPORTED = 0, // reserved for MSP
+    // 1 reserved
+    // 2 reserved
     VTXDEV_SMARTAUDIO = 3,
     VTXDEV_TRAMP      = 4,
+    VTXDEV_UNKNOWN    = 0xFF,
 } vtxDevType_e;
 
 struct vtxVTable_s;
@@ -28,8 +31,6 @@ struct vtxVTable_s;
 typedef struct vtxDevice_s {
     const struct vtxVTable_s *vTable;
 
-    vtxDevType_e devtype;      // 3.1 only; eventually goes away
-
     uint8_t numBand;
     uint8_t numChan;
     uint8_t numPower;
diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c
index 749a0d40c3..34da6e97fa 100644
--- a/src/main/io/vtx_tramp.c
+++ b/src/main/io/vtx_tramp.c
@@ -46,14 +46,13 @@ static const uint16_t trampPowerTable[] = {
 };
 
 static const char * const trampPowerNames[] = {
-    "25 ", "100", "200", "400", "600"
+    "---", "25 ", "100", "200", "400", "600"
 };
 #endif
 
 #if defined(VTX_COMMON)
-static vtxVTable_t trampVTable;    // Forward
 static vtxDevice_t vtxTramp = {
-    .vTable = &trampVTable,
+    .vTable = NULL,
     .numBand = 5,
     .numChan = 8,
     .numPower = sizeof(trampPowerTable),
@@ -309,26 +308,6 @@ void trampQueryS(void)
     trampQuery('s');
 }
 
-bool trampInit()
-{
-    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
-
-    if (portConfig) {
-        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
-    }
-
-    if (!trampSerialPort) {
-        return false;
-    }
-
-#if defined(VTX_COMMON)
-    vtxTramp.vTable = &trampVTable;
-    vtxCommonRegisterDevice(&vtxTramp);
-#endif
-
-    return true;
-}
-
 void vtxTrampProcess(uint32_t currentTimeUs)
 {
     static uint32_t lastQueryTimeUs = 0;
@@ -472,9 +451,9 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL }
 
 static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 };
 
-static uint8_t trampCmsPower = 0;
+static uint8_t trampCmsPower = 1;
 
-static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampPowerNames, NULL };
+static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames, NULL };
 
 static void trampCmsUpdateFreqRef(void)
 {
@@ -539,7 +518,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self)
     UNUSED(self);
 
     trampSetBandChan(trampCmsBand, trampCmsChan);
-    trampSetRFPower(trampPowerTable[trampCmsPower]);
+    trampSetRFPower(trampPowerTable[trampCmsPower-1]);
 
     // If it fails, the user should retry later
     trampCommitChanges();
@@ -559,7 +538,7 @@ static void trampCmsInitSettings()
     if (trampCurConfigPower > 0) {
         for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
             if (trampCurConfigPower <= trampPowerTable[i]) {
-                trampCmsPower = i;
+                trampCmsPower = i + 1;
                 break;
             }
         }
@@ -640,7 +619,7 @@ void vtxTrampSetBandChan(uint8_t band, uint8_t chan)
 void vtxTrampSetPowerByIndex(uint8_t index)
 {
     if (index) {
-        trampSetRFPower(trampPowerTable[index]);
+        trampSetRFPower(trampPowerTable[index - 1]);
         trampCommitChanges();
     }
 }
@@ -668,7 +647,7 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex)
     if (trampCurConfigPower > 0) {
         for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
             if (trampCurConfigPower <= trampPowerTable[i]) {
-                *pIndex = i;
+                *pIndex = i + 1;
                 break;
             }
         }
@@ -700,4 +679,24 @@ static vtxVTable_t trampVTable = {
 
 #endif
 
+bool trampInit()
+{
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
+
+    if (portConfig) {
+        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
+    }
+
+    if (!trampSerialPort) {
+        return false;
+    }
+
+#if defined(VTX_COMMON)
+    vtxTramp.vTable = &trampVTable;
+    vtxCommonRegisterDevice(&vtxTramp);
+#endif
+    
+    return true;
+}
+
 #endif // VTX_TRAMP

From 50ddce722787c13505b5e784d3225165b452faf3 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <rco@iptel.org>
Date: Tue, 17 Jan 2017 01:06:20 +0100
Subject: [PATCH 33/50] added MSP_VTX_CONFIG & MSP_SET_VTX_CONFIG

---
 src/main/fc/fc_msp.c | 75 +++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 67 insertions(+), 8 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 8e74b28931..6b3b86a87e 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -47,6 +47,7 @@
 #include "drivers/vtx_soft_spi_rtc6705.h"
 #include "drivers/pwm_output.h"
 #include "drivers/serial_escserial.h"
+#include "drivers/vtx_common.h"
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
@@ -1188,6 +1189,35 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         }
         break;
 
+#if defined(VTX_COMMON)
+    case MSP_VTX_CONFIG:
+        {
+            uint8_t deviceType = vtxCommonGetDeviceType();
+            if (deviceType != VTXDEV_UNKNOWN) {
+
+                uint8_t band=0, channel=0;
+                vtxCommonGetBandChan(&band,&channel);
+                
+                uint8_t powerIdx=0; // debug
+                vtxCommonGetPowerIndex(&powerIdx);
+                
+                uint8_t pitmode=0;
+                vtxCommonGetPitmode(&pitmode);
+                
+                sbufWriteU8(dst, deviceType);
+                sbufWriteU8(dst, band);
+                sbufWriteU8(dst, channel);
+                sbufWriteU8(dst, powerIdx);
+                sbufWriteU8(dst, pitmode);
+                // future extensions here...
+            }
+            else {
+                sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
+            }
+        }
+#endif
+        break;
+
     default:
         return false;
     }
@@ -1639,15 +1669,44 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 #endif
 
-#ifdef USE_RTC6705
+#if defined(USE_RTC6705) || defined(VTX_COMMON)
     case MSP_SET_VTX_CONFIG:
-        ;
-        uint16_t tmp = sbufReadU16(src);
-        if  (tmp < 40)
-            masterConfig.vtx_channel = tmp;
-        if (current_vtx_channel != masterConfig.vtx_channel) {
-            current_vtx_channel = masterConfig.vtx_channel;
-            rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
+        {
+            uint16_t tmp = sbufReadU16(src);
+#if defined(USE_RTC6705)
+            if  (tmp < 40)
+                masterConfig.vtx_channel = tmp;
+            if (current_vtx_channel != masterConfig.vtx_channel) {
+                current_vtx_channel = masterConfig.vtx_channel;
+                rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
+            }
+#else
+            if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) {
+
+                uint8_t band    = (tmp / 8) + 1;
+                uint8_t channel = (tmp % 8) + 1;
+
+                uint8_t current_band=0, current_channel=0;
+                vtxCommonGetBandChan(&current_band,&current_channel);
+                if ((current_band != band) || (current_channel != channel))
+                    vtxCommonSetBandChan(band,channel);
+
+                if (sbufBytesRemaining(src) < 2)
+                    break;
+            
+                uint8_t power = sbufReadU8(src);
+                uint8_t current_power = 0;
+                vtxCommonGetPowerIndex(&current_power);
+                if (current_power != power)
+                    vtxCommonSetPowerByIndex(power);
+            
+                uint8_t pitmode = sbufReadU8(src);
+                uint8_t current_pitmode = 0;
+                vtxCommonGetPitmode(&current_pitmode);
+                if (current_pitmode != pitmode)
+                    vtxCommonSetPitmode(pitmode);
+            }
+#endif
         }
         break;
 #endif

From 0307bf6f87d33437bcba9d1d9d5cef356fcb702b Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Thu, 26 Jan 2017 08:56:22 +1300
Subject: [PATCH 34/50] Rebase of #1917: Update SDK to 6.2.1 2016q4 (thanks to
 @TheAngularity).

---
 .travis.yml             |  2 +-
 make/tools.mk           | 10 +++++-----
 src/main/build/atomic.h |  2 +-
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 5072324926..df472a95ec 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -82,7 +82,7 @@ install:
   - make arm_sdk_install
 
 before_script:
-  - tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version
+  - tools/gcc-arm-none-eabi-6_2-2016q4/bin/arm-none-eabi-gcc --version
   - clang --version
   - clang++ --version
 
diff --git a/make/tools.mk b/make/tools.mk
index 810716a57d..4ad98115d6 100644
--- a/make/tools.mk
+++ b/make/tools.mk
@@ -14,16 +14,16 @@
 ##############################
 
 # Set up ARM (STM32) SDK
-ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q3
+ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-6_2-2016q4
 # Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion)
-GCC_REQUIRED_VERSION ?= 5.4.1
+GCC_REQUIRED_VERSION ?= 6.2.1
 
 ## arm_sdk_install   : Install Arm SDK
 .PHONY: arm_sdk_install
 
-ARM_SDK_URL_BASE  := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update/+download/gcc-arm-none-eabi-5_4-2016q3-20160926
+ARM_SDK_URL_BASE  := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2016q4/gcc-arm-none-eabi-6_2-2016q4-20161216
 
-# source: https://launchpad.net/gcc-arm-embedded/5.0/
+# source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads
 ifdef LINUX
   ARM_SDK_URL  := $(ARM_SDK_URL_BASE)-linux.tar.bz2
 endif
@@ -33,7 +33,7 @@ ifdef MACOSX
 endif
 
 ifdef WINDOWS
-  ARM_SDK_URL  := $(ARM_SDK_URL_BASE)-win32.zip
+  ARM_SDK_URL  := $(ARM_SDK_URL_BASE)-win32-zip.zip
 endif
 
 ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index 4603091ed4..959c6e9d8d 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -85,7 +85,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
 // ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier
 // this macro can be used only ONCE PER LINE, but multiple uses per block are fine
 
-#if (__GNUC__ > 5)
+#if (__GNUC__ > 6)
 #warning "Please verify that ATOMIC_BARRIER works as intended"
 // increment version number is BARRIER works
 // TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead

From 44f4e479eb9de56c92f0e58da65d848d787cdc27 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Tue, 20 Dec 2016 22:42:58 +0000
Subject: [PATCH 35/50] Parameter groups EEPROM migration

---
 Makefile                            |   1 +
 src/main/config/config_eeprom.c     | 475 +++++++++++++---------------
 src/main/config/config_eeprom.h     |   5 +-
 src/main/config/config_profile.h    |   1 -
 src/main/config/config_reset.h      |  40 +++
 src/main/config/config_streamer.c   | 177 +++++++++++
 src/main/config/config_streamer.h   |  45 +++
 src/main/fc/config.c                |  31 +-
 src/main/fc/config.h                |   5 +
 src/main/fc/fc_msp.c                |   1 -
 src/main/target/link/stm32_flash.ld |  13 +
 11 files changed, 539 insertions(+), 255 deletions(-)
 create mode 100644 src/main/config/config_reset.h
 create mode 100644 src/main/config/config_streamer.c
 create mode 100644 src/main/config/config_streamer.h

diff --git a/Makefile b/Makefile
index 5e9cbb6303..e5bc1e4ad3 100644
--- a/Makefile
+++ b/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 \
diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 5c32f18183..18cbfa8e9b 100644
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -18,294 +18,271 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <stddef.h>
 
 #include "platform.h"
 
-#include "drivers/system.h"
-
-#include "config/config_master.h"
-
 #include "build/build_config.h"
 
+#include "common/maths.h"
+
 #include "config/config_eeprom.h"
+#include "config/config_streamer.h"
+#include "config/config_master.h"
+#include "config/parameter_group.h"
 
-#if !defined(FLASH_SIZE)
-#error "Flash size not defined for target. (specify in KB)"
-#endif
+#include "drivers/system.h"
 
+#include "fc/config.h"
 
-#ifndef FLASH_PAGE_SIZE
-    #ifdef STM32F303xC
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
-    #endif
+extern uint8_t __config_start;   // configured via linker script when building binaries.
+extern uint8_t __config_end;
 
-    #ifdef STM32F10X_MD
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x400)
-    #endif
+typedef enum {
+    CR_CLASSICATION_SYSTEM   = 0,
+    CR_CLASSICATION_PROFILE1 = 1,
+    CR_CLASSICATION_PROFILE2 = 2,
+    CR_CLASSICATION_PROFILE3 = 3,
+    CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_PROFILE3,
+} configRecordFlags_e;
 
-    #ifdef STM32F10X_HD
-        #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
-    #endif
+#define CR_CLASSIFICATION_MASK (0x3)
 
-    #if defined(STM32F745xx)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x40000)
-    #endif
+// Header for the saved copy.
+typedef struct {
+    uint8_t format;
+} PG_PACKED configHeader_t;
 
-    #if defined(STM32F746xx)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x40000)
-    #endif
+// Header for each stored PG.
+typedef struct {
+    // split up.
+    uint16_t size;
+    pgn_t pgn;
+    uint8_t version;
 
-    #if defined(STM32F722xx)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
-    #endif
+    // lower 2 bits used to indicate system or profile number, see CR_CLASSIFICATION_MASK
+    uint8_t flags;
 
-    #if defined(STM32F40_41xxx)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
-    #endif
+    uint8_t pg[];
+} PG_PACKED configRecord_t;
 
-    #if defined (STM32F411xE)
-        #define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
-    #endif
-
-#endif
-
-#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT)
-    #ifdef STM32F10X_MD
-        #define FLASH_PAGE_COUNT 128
-    #endif
-
-    #ifdef STM32F10X_HD
-        #define FLASH_PAGE_COUNT 128
-    #endif
-#endif
-
-#if defined(FLASH_SIZE)
-#if defined(STM32F40_41xxx)
-#define FLASH_PAGE_COUNT 4
-#elif defined (STM32F411xE)
-#define FLASH_PAGE_COUNT 3
-#elif defined (STM32F722xx)
-#define FLASH_PAGE_COUNT 3
-#elif defined (STM32F745xx)
-#define FLASH_PAGE_COUNT 4
-#elif defined (STM32F746xx)
-#define FLASH_PAGE_COUNT 4
-#else
-#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
-#endif
-#endif
-
-#if !defined(FLASH_PAGE_SIZE)
-#error "Flash page size not defined for target."
-#endif
-
-#if !defined(FLASH_PAGE_COUNT)
-#error "Flash page count not defined for target."
-#endif
-
-#if FLASH_SIZE <= 128
-#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
-#else
-#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000
-#endif
-
-// use the last flash pages for storage
-#ifdef CUSTOM_FLASH_MEMORY_ADDRESS
-size_t custom_flash_memory_address = 0;
-#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address)
-#else
-// use the last flash pages for storage
-#ifndef CONFIG_START_FLASH_ADDRESS
-#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
-#endif
-#endif
+// Footer for the saved copy.
+typedef struct {
+    uint16_t terminator;
+} PG_PACKED configFooter_t;
+// checksum is appended just after footer. It is not included in footer to make checksum calculation consistent
 
+// Used to check the compiler packing at build time.
+typedef struct {
+    uint8_t byte;
+    uint32_t word;
+} PG_PACKED packingTest_t;
 
 void initEEPROM(void)
 {
+    // Verify that this architecture packs as expected.
+    BUILD_BUG_ON(offsetof(packingTest_t, byte) != 0);
+    BUILD_BUG_ON(offsetof(packingTest_t, word) != 1);
+    BUILD_BUG_ON(sizeof(packingTest_t) != 5);
+
+    BUILD_BUG_ON(sizeof(configHeader_t) != 1);
+    BUILD_BUG_ON(sizeof(configFooter_t) != 2);
+    BUILD_BUG_ON(sizeof(configRecord_t) != 6);
 }
 
-static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
+static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length)
 {
-    uint8_t checksum = 0;
-    const uint8_t *byteOffset;
+    const uint8_t *p = (const uint8_t *)data;
+    const uint8_t *pend = p + length;
 
-    for (byteOffset = data; byteOffset < (data + length); byteOffset++)
-        checksum ^= *byteOffset;
-    return checksum;
+    for (; p != pend; p++) {
+        chk ^= *p;
+    }
+    return chk;
+}
+
+// Scan the EEPROM config. Returns true if the config is valid.
+static bool scanEEPROM(void)
+{
+    uint8_t chk = 0;
+    const uint8_t *p = &__config_start;
+    const configHeader_t *header = (const configHeader_t *)p;
+
+    if (header->format != EEPROM_CONF_VERSION) {
+        return false;
+    }
+    chk = updateChecksum(chk, header, sizeof(*header));
+    p += sizeof(*header);
+    // include the transitional masterConfig record
+    chk = updateChecksum(chk, p, sizeof(masterConfig));
+    p += sizeof(masterConfig);
+
+    for (;;) {
+        const configRecord_t *record = (const configRecord_t *)p;
+
+        if (record->size == 0) {
+            // Found the end.  Stop scanning.
+            break;
+        }
+        if (p + record->size >= &__config_end
+            || record->size < sizeof(*record)) {
+            // Too big or too small.
+            return false;
+        }
+
+        chk = updateChecksum(chk, p, record->size);
+
+        p += record->size;
+    }
+
+    const configFooter_t *footer = (const configFooter_t *)p;
+    chk = updateChecksum(chk, footer, sizeof(*footer));
+    p += sizeof(*footer);
+    chk = ~chk;
+    return chk == *p;
+}
+
+// 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;
 }
 
 bool isEEPROMContentValid(void)
 {
-    const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS;
-    uint8_t checksum = 0;
-
-    // check version number
-    if (EEPROM_CONF_VERSION != temp->version)
-        return false;
-
-    // check size and magic numbers
-    if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
-        return false;
-
-    if (strncasecmp(temp->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)))
-        return false;
-
-    // verify integrity of temporary copy
-    checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
-    if (checksum != 0)
-        return false;
-
-    // looks good, let's roll!
-    return true;
+    return scanEEPROM();
 }
 
-#if defined(STM32F7)
-
-// FIXME: HAL for now this will only work for F4/F7 as flash layout is different
-void writeEEPROM(void)
+static bool writeSettingsToEEPROM(void)
 {
-    // Generate compile time error if the config does not fit in the reserved area of flash.
-    BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
+    config_streamer_t streamer;
+    config_streamer_init(&streamer);
 
-    HAL_StatusTypeDef status;
-    uint32_t wordOffset;
-    int8_t attemptsRemaining = 3;
+    config_streamer_start(&streamer, (uintptr_t)&__config_start, &__config_end - &__config_start);
+    uint8_t chk = 0;
 
-    suspendRxSignal();
+    configHeader_t header = {
+        .format = EEPROM_CONF_VERSION,
+    };
 
-    // prepare checksum/version constants
-    masterConfig.version = EEPROM_CONF_VERSION;
-    masterConfig.size = sizeof(master_t);
-    masterConfig.magic_be = 0xBE;
-    masterConfig.magic_ef = 0xEF;
-    masterConfig.chk = 0; // erase checksum before recalculating
-    masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
+    config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header));
+    chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header));
+    // write the transitional masterConfig record
+    config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig));
+    chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig));
+    PG_FOREACH(reg) {
+        const uint16_t regSize = pgSize(reg);
+        configRecord_t record = {
+            .size = sizeof(configRecord_t) + regSize,
+            .pgn = pgN(reg),
+            .version = pgVersion(reg),
+            .flags = 0
+        };
 
-    // write it
-    /* Unlock the Flash to enable the flash control register access *************/
-    HAL_FLASH_Unlock();
-    while (attemptsRemaining--)
-    {
-        /* Fill EraseInit structure*/
-        FLASH_EraseInitTypeDef EraseInitStruct = {0};
-        EraseInitStruct.TypeErase     = FLASH_TYPEERASE_SECTORS;
-        EraseInitStruct.VoltageRange  = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
-        EraseInitStruct.Sector        = (FLASH_SECTOR_TOTAL-1);
-        EraseInitStruct.NbSectors     = 1;
-        uint32_t SECTORError;
-        status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
-        if (status != HAL_OK)
-        {
-            continue;
-        }
-        else
-        {
-            for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4)
-            {
-                status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset));
-                if(status != HAL_OK)
-                {
-                    break;
-                }
+        if (pgIsSystem(reg)) {
+            // write the only instance
+            record.flags |= CR_CLASSICATION_SYSTEM;
+            config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
+            chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
+            config_streamer_write(&streamer, reg->address, regSize);
+            chk = updateChecksum(chk, reg->address, regSize);
+        } else {
+            // write one instance for each profile
+            for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) {
+                record.flags = 0;
+
+                record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
+                config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
+                chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
+                const uint8_t *address = reg->address + (regSize * profileIndex);
+                config_streamer_write(&streamer, address, regSize);
+                chk = updateChecksum(chk, address, regSize);
             }
         }
-        if (status == HAL_OK) {
-            break;
+    }
+
+    configFooter_t footer = {
+        .terminator = 0,
+    };
+
+    config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer));
+    chk = updateChecksum(chk, (uint8_t *)&footer, sizeof(footer));
+
+    // append checksum now
+    chk = ~chk;
+    config_streamer_write(&streamer, &chk, sizeof(chk));
+
+    config_streamer_flush(&streamer);
+
+    bool success = config_streamer_finish(&streamer) == 0;
+
+    return success;
+}
+
+void writeConfigToEEPROM(void)
+{
+    bool success = false;
+    // write it
+    for (int attempt = 0; attempt < 3 && !success; attempt++) {
+        if (writeSettingsToEEPROM()) {
+            success = true;
         }
     }
-    HAL_FLASH_Lock();
+
+    if (success && isEEPROMContentValid()) {
+        return;
+    }
 
     // Flash write failed - just die now
-    if (status != HAL_OK || !isEEPROMContentValid()) {
-        failureMode(FAILURE_FLASH_WRITE_FAILED);
-    }
-
-    resumeRxSignal();
-}
-#else
-void writeEEPROM(void)
-{
-    // Generate compile time error if the config does not fit in the reserved area of flash.
-    BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
-
-    FLASH_Status status = 0;
-    uint32_t wordOffset;
-    int8_t attemptsRemaining = 3;
-
-    suspendRxSignal();
-
-    // prepare checksum/version constants
-    masterConfig.version = EEPROM_CONF_VERSION;
-    masterConfig.size = sizeof(master_t);
-    masterConfig.magic_be = 0xBE;
-    masterConfig.magic_ef = 0xEF;
-    masterConfig.chk = 0; // erase checksum before recalculating
-    masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
-
-    // write it
-    FLASH_Unlock();
-    while (attemptsRemaining--) {
-#if defined(STM32F4)
-        FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
-#elif defined(STM32F303)
-        FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
-#elif defined(STM32F10X)
-        FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
-#endif
-        for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
-            if (wordOffset % FLASH_PAGE_SIZE == 0) {
-#if defined(STM32F40_41xxx)
-                status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000
-#elif defined (STM32F411xE)
-                status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000
-#else
-                status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
-#endif
-                if (status != FLASH_COMPLETE) {
-                    break;
-                }
-            }
-
-            status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset,
-                    *(uint32_t *) ((char *) &masterConfig + wordOffset));
-            if (status != FLASH_COMPLETE) {
-                break;
-            }
-        }
-        if (status == FLASH_COMPLETE) {
-            break;
-        }
-    }
-    FLASH_Lock();
-
-    // Flash write failed - just die now
-    if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
-        failureMode(FAILURE_FLASH_WRITE_FAILED);
-    }
-
-    resumeRxSignal();
-}
-#endif
-
-void readEEPROM(void)
-{
-    // Sanity check
-    if (!isEEPROMContentValid())
-        failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
-
-    suspendRxSignal();
-
-    // Read flash
-    memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
-
-    if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check
-        masterConfig.current_profile_index = 0;
-
-    setProfile(masterConfig.current_profile_index);
-
-    validateAndFixConfig();
-    activateConfig();
-
-    resumeRxSignal();
+    failureMode(FAILURE_FLASH_WRITE_FAILED);
 }
diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h
index 645689ffd0..a6aafc3ca7 100644
--- a/src/main/config/config_eeprom.h
+++ b/src/main/config/config_eeprom.h
@@ -19,7 +19,6 @@
 
 #define EEPROM_CONF_VERSION 154
 
-void initEEPROM(void);
-void writeEEPROM();
-void readEEPROM(void);
 bool isEEPROMContentValid(void);
+bool loadEEPROM(void);
+void writeConfigToEEPROM(void);
diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h
index a0ffa53905..d1f2c513fd 100644
--- a/src/main/config/config_profile.h
+++ b/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"
diff --git a/src/main/config/config_reset.h b/src/main/config/config_reset.h
new file mode 100644
index 0000000000..7ca0eda77a
--- /dev/null
+++ b/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 alloved to set structure field-by-field
+#define RESET_CONFIG_2(_type, _name, ...)                 \
+    *(_name) = (_type) {                                  \
+        __VA_ARGS__                                       \
+    };                                                    \
+    /**/
diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c
new file mode 100644
index 0000000000..cd48ea9f72
--- /dev/null
+++ b/src/main/config/config_streamer.c
@@ -0,0 +1,177 @@
+/*
+ * 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 "config_streamer.h"
+
+#include "platform.h"
+
+#include <string.h>
+
+#if !defined(FLASH_PAGE_SIZE)
+# if defined(STM32F10X_MD)
+#  define FLASH_PAGE_SIZE                 (0x400)
+# elif defined(STM32F10X_HD)
+#  define FLASH_PAGE_SIZE                 (0x800)
+# elif defined(STM32F303xC)
+#  define FLASH_PAGE_SIZE                 (0x800)
+# elif defined(STM32F40_41xxx)
+#  define FLASH_PAGE_SIZE                 ((uint32_t)0x20000)
+# elif defined (STM32F411xE)
+#  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)
+static int write_word(config_streamer_t *c, uint32_t value)
+{
+    if (c->err != 0) {
+        return c->err;
+    }
+
+    HAL_StatusTypeDef status;
+    if (c->address % FLASH_PAGE_SIZE == 0) {
+        FLASH_EraseInitTypeDef EraseInitStruct = {0};
+        EraseInitStruct.TypeErase     = FLASH_TYPEERASE_SECTORS;
+        EraseInitStruct.VoltageRange  = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
+        EraseInitStruct.Sector        = (FLASH_SECTOR_TOTAL-1);
+        EraseInitStruct.NbSectors     = 1;
+        uint32_t SECTORError;
+        status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
+        if (status != HAL_OK){
+            return -1;
+        }
+    }
+    status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value);
+    if (status != HAL_OK) {
+        return -2;
+    }
+    c->address += sizeof(value);
+    return 0;
+}
+#else
+static int write_word(config_streamer_t *c, uint32_t value)
+{
+    if (c->err != 0) {
+        return c->err;
+    }
+
+    FLASH_Status status;
+
+    if (c->address % 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(c->address);
+#endif
+        if (status != FLASH_COMPLETE) {
+            return -1;
+        }
+    }
+    status = FLASH_ProgramWord(c->address, value);
+    if (status != FLASH_COMPLETE) {
+        return -2;
+    }
+    c->address += sizeof(value);
+    return 0;
+}
+#endif
+
+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;
+}
diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h
new file mode 100644
index 0000000000..a62356fbea
--- /dev/null
+++ b/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);
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index e4d2b69be3..8ceecd6225 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -91,6 +91,7 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 #include "config/feature.h"
+#include "config/parameter_group.h"
 
 #ifndef DEFAULT_RX_FEATURE
 #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
@@ -1129,12 +1130,40 @@ void validateAndFixGyroConfig(void)
     }
 }
 
+void readEEPROM(void)
+{
+    suspendRxSignal();
+
+    // Sanity check, read flash
+    if (!loadEEPROM()) {
+        failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
+    }
+
+    pgActivateProfile(getCurrentProfile());
+
+//    setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex);
+    setControlRateProfile(0);
+
+    validateAndFixConfig();
+    activateConfig();
+
+    resumeRxSignal();
+}
+
+void writeEEPROM(void)
+{
+    suspendRxSignal();
+
+    writeConfigToEEPROM();
+
+    resumeRxSignal();
+}
+
 void ensureEEPROMContainsValidData(void)
 {
     if (isEEPROMContentValid()) {
         return;
     }
-
     resetEEPROM();
 }
 
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 0fe4f9e616..4a77447dfa 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -69,7 +69,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);
@@ -80,6 +83,8 @@ 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);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 6b3b86a87e..6de708dc6d 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -105,7 +105,6 @@
 #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;
diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld
index 2cc81c9fd5..eaf65e7b9a 100644
--- a/src/main/target/link/stm32_flash.ld
+++ b/src/main/target/link/stm32_flash.ld
@@ -81,6 +81,19 @@ SECTIONS
     KEEP (*(SORT(.fini_array.*)))
     PROVIDE_HIDDEN (__fini_array_end = .);
   } >FLASH
+  .pg_registry :
+  {
+    PROVIDE_HIDDEN (__pg_registry_start = .);
+    KEEP (*(.pg_registry))
+    KEEP (*(SORT(.pg_registry.*)))
+    PROVIDE_HIDDEN (__pg_registry_end = .);
+  } >FLASH
+  .pg_resetdata :
+  {
+    PROVIDE_HIDDEN (__pg_resetdata_start = .);
+    KEEP (*(.pg_resetdata))
+    PROVIDE_HIDDEN (__pg_resetdata_end = .);
+  } >FLASH
 
   /* used by the startup to initialize data */
   _sidata = .;

From dd87908a564e03697359206f243e89118dfe7de0 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Thu, 29 Dec 2016 13:16:43 +0000
Subject: [PATCH 36/50] Fixed typo

---
 src/main/config/config_reset.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/config/config_reset.h b/src/main/config/config_reset.h
index 7ca0eda77a..5d13e12475 100644
--- a/src/main/config/config_reset.h
+++ b/src/main/config/config_reset.h
@@ -32,7 +32,7 @@
     memcpy((_name), &__UNIQL(_reset_template_), sizeof(*(_name)));      \
     /**/
 
-// overwrite _name with data passed as arguments. GCC is alloved to set structure field-by-field
+// 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__                                       \

From 87570f2dc9bf0e6755c727c933cc4f8b1c7d5372 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Thu, 29 Dec 2016 17:15:16 +0000
Subject: [PATCH 37/50] Added dummy PG. Fixed setting of profile

---
 src/main/config/config_eeprom.c       |  9 +++++++++
 src/main/config/parameter_group_ids.h |  2 +-
 src/main/fc/config.c                  | 12 ++++++++----
 src/main/fc/config.h                  |  3 +--
 4 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 18cbfa8e9b..2850700c54 100644
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -35,6 +35,15 @@
 
 #include "fc/config.h"
 
+// declare a dummy PG, since scanEEPROM assumes there is at least one PG
+// !!TODO remove once first PG has been created out of masterConfg
+typedef struct dummpConfig_s {
+    uint8_t dummy;
+} dummyConfig_t;
+PG_DECLARE(dummyConfig_t, dummyConfig);
+#define PG_DUMMY_CONFIG 1
+PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0);
+
 extern uint8_t __config_start;   // configured via linker script when building binaries.
 extern uint8_t __config_end;
 
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index c42ef32f0e..fbef2e84d5 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -16,7 +16,7 @@
  */
 
 // 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
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 8ceecd6225..e65ef76b22 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -550,7 +550,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;
@@ -1139,10 +1139,14 @@ void readEEPROM(void)
         failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
     }
 
-    pgActivateProfile(getCurrentProfile());
-
+//    pgActivateProfile(getCurrentProfile());
 //    setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex);
-    setControlRateProfile(0);
+
+    if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check
+        masterConfig.current_profile_index = 0;
+    }
+
+    setProfile(masterConfig.current_profile_index);
 
     validateAndFixConfig();
     activateConfig();
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 4a77447dfa..62841f5fd3 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -82,7 +82,6 @@ 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);
 
@@ -91,7 +90,7 @@ void changeControlRateProfile(uint8_t profileIndex);
 bool canSoftwareSerialBeUsed(void);
 
 uint16_t getCurrentMinthrottle(void);
-struct master_s;
 
+struct master_s;
 void targetConfiguration(struct master_s *config);
 void targetValidateConfiguration(struct master_s *config);

From 9e529a28cad88787e1e997ce0c5a94c512c9800f Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Mon, 2 Jan 2017 17:03:17 +0000
Subject: [PATCH 38/50] Updates as per iNav

---
 src/main/config/config_streamer.c | 138 +++++++++++++++++++++++-------
 src/main/drivers/system.h         |   3 +
 src/main/fc/cli.c                 |  99 ++++++++++++++++-----
 src/main/fc/config.c              |   7 +-
 src/main/fc/config.h              |   1 +
 5 files changed, 195 insertions(+), 53 deletions(-)

diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c
index cd48ea9f72..e63363f22d 100644
--- a/src/main/config/config_streamer.c
+++ b/src/main/config/config_streamer.c
@@ -15,11 +15,16 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "config_streamer.h"
+#include <string.h>
 
 #include "platform.h"
 
-#include <string.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)
 # if defined(STM32F10X_MD)
@@ -32,6 +37,8 @@
 #  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
 # elif defined(STM32F745xx)
 #  define FLASH_PAGE_SIZE                 ((uint32_t)0x40000)
 # elif defined(STM32F746xx)
@@ -79,61 +86,134 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
 }
 
 #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;
     }
-
-    HAL_StatusTypeDef status;
+#if defined(STM32F7)
     if (c->address % FLASH_PAGE_SIZE == 0) {
-        FLASH_EraseInitTypeDef EraseInitStruct = {0};
-        EraseInitStruct.TypeErase     = FLASH_TYPEERASE_SECTORS;
-        EraseInitStruct.VoltageRange  = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
-        EraseInitStruct.Sector        = (FLASH_SECTOR_TOTAL-1);
-        EraseInitStruct.NbSectors     = 1;
+        FLASH_EraseInitTypeDef EraseInitStruct = {
+            .TypeErase     = FLASH_TYPEERASE_SECTORS,
+            .VoltageRange  = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V
+            .NbSectors     = 1
+        };
+        EraseInitStruct.Sector = getFLASHSectorForEEPROM();
         uint32_t SECTORError;
-        status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
+        const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
         if (status != HAL_OK){
             return -1;
         }
     }
-    status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value);
+    const HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value);
     if (status != HAL_OK) {
         return -2;
     }
-    c->address += sizeof(value);
-    return 0;
-}
 #else
-static int write_word(config_streamer_t *c, uint32_t value)
-{
-    if (c->err != 0) {
-        return c->err;
-    }
-
-    FLASH_Status status;
-
     if (c->address % 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
+#if defined(STM32F4)
+        const FLASH_Status status = FLASH_EraseSector(getFLASHSectorForEEPROM(), VoltageRange_3); //0x08080000 to 0x080A0000
 #else
-        status = FLASH_ErasePage(c->address);
+        const FLASH_Status status = FLASH_ErasePage(c->address);
 #endif
         if (status != FLASH_COMPLETE) {
             return -1;
         }
     }
-    status = FLASH_ProgramWord(c->address, value);
+    const FLASH_Status status = FLASH_ProgramWord(c->address, value);
     if (status != FLASH_COMPLETE) {
         return -2;
     }
+#endif
     c->address += sizeof(value);
     return 0;
 }
-#endif
 
 int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size)
 {
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index 87af44df54..5ac340c31e 100644
--- a/src/main/drivers/system.h
+++ b/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);
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 87ca6b2e1f..abb8dc147e 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -23,6 +23,7 @@
 #include <math.h>
 #include <ctype.h>
 
+//#define USE_PARAMETER_GROUPS
 #include "platform.h"
 
 // FIXME remove this for targets that don't need a CLI.  Perhaps use a no-op macro when USE_CLI is not enabled
@@ -48,6 +49,9 @@ uint8_t cliMode = 0;
 #include "config/config_master.h"
 #include "config/feature.h"
 
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "drivers/accgyro.h"
 #include "drivers/buf_writer.h"
 #include "drivers/bus_i2c.h"
@@ -460,6 +464,7 @@ typedef enum {
     MASTER_VALUE = (0 << VALUE_SECTION_OFFSET),
     PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET),
     PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET),
+    CONTROL_RATE_VALUE = (3 << VALUE_SECTION_OFFSET),
     // value mode
     MODE_DIRECT = (0 << VALUE_MODE_OFFSET),
     MODE_LOOKUP = (1 << VALUE_MODE_OFFSET)
@@ -483,6 +488,22 @@ typedef union {
     cliMinMaxConfig_t minmax;
 } cliValueConfig_t;
 
+#ifdef USE_PARAMETER_GROUPS
+typedef struct {
+    const char *name;
+    const uint8_t type; // see cliValueFlag_e
+    const cliValueConfig_t config;
+
+    pgn_t pgn;
+    uint16_t offset;
+} __attribute__((packed)) clivalue_t;
+
+static const clivalue_t valueTable[] = {
+        { "dummy",               VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0,  255 }, 0, 0 }
+};
+
+#else
+
 typedef struct {
     const char *name;
     const uint8_t type; // see cliValueFlag_e
@@ -490,7 +511,7 @@ typedef struct {
     const cliValueConfig_t config;
 } clivalue_t;
 
-const clivalue_t valueTable[] = {
+static const clivalue_t valueTable[] = {
 #ifndef SKIP_TASK_STATISTICS
     { "task_statistics",            VAR_INT8   | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.task_statistics, .config.lookup = { TABLE_OFF_ON } },
 #endif
@@ -811,8 +832,7 @@ const clivalue_t valueTable[] = {
     { "displayport_max7456_row_adjust", VAR_INT8    | MASTER_VALUE, &displayPortProfileMax7456()->rowAdjust, .config.minmax = { -3, 0 } },
 #endif
 };
-
-#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
+#endif
 
 static void cliPrint(const char *str)
 {
@@ -931,6 +951,24 @@ static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_
     }
 }
 
+#ifdef USE_PARAMETER_GROUPS
+static void* getValuePointer(const clivalue_t *var)
+{
+    const pgRegistry_t* rec = pgFind(var->pgn);
+
+    switch (var->type & VALUE_SECTION_MASK) {
+    case MASTER_VALUE:
+        return rec->address + var->offset;
+    case PROFILE_RATE_VALUE:
+        return rec->address + var->offset + sizeof(profile_t) * getCurrentProfile();
+    case CONTROL_RATE_VALUE:
+        return rec->address + var->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile();
+    case PROFILE_VALUE:
+        return *rec->ptr + var->offset;
+    }
+    return NULL;
+}
+#else
 void *getValuePointer(const clivalue_t *value)
 {
     void *ptr = value->ptr;
@@ -945,6 +983,7 @@ void *getValuePointer(const clivalue_t *value)
 
     return ptr;
 }
+#endif
 
 static void *getDefaultPointer(void *valuePointer, const master_t *defaultConfig)
 {
@@ -1006,7 +1045,7 @@ static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, const maste
 static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *defaultConfig)
 {
     const clivalue_t *value;
-    for (uint32_t i = 0; i < VALUE_COUNT; i++) {
+    for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
         value = &valueTable[i];
 
         if ((value->type & VALUE_SECTION_MASK) != valueSection) {
@@ -1053,13 +1092,7 @@ typedef union {
 
 static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
 {
-    void *ptr = var->ptr;
-    if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) {
-        ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
-    }
-    if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) {
-        ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile());
-    }
+    void *ptr = getValuePointer(var);
 
     switch (var->type & VALUE_TYPE_MASK) {
         case VAR_UINT8:
@@ -3074,7 +3107,7 @@ static void cliGet(char *cmdline)
     const clivalue_t *val;
     int matchedCommands = 0;
 
-    for (uint32_t i = 0; i < VALUE_COUNT; i++) {
+    for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
         if (strstr(valueTable[i].name, cmdline)) {
             val = &valueTable[i];
             cliPrintf("%s = ", valueTable[i].name);
@@ -3105,7 +3138,7 @@ static void cliSet(char *cmdline)
 
     if (len == 0 || (len == 1 && cmdline[0] == '*')) {
         cliPrint("Current settings: \r\n");
-        for (uint32_t i = 0; i < VALUE_COUNT; i++) {
+        for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
             val = &valueTable[i];
             cliPrintf("%s = ", valueTable[i].name);
             cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
@@ -3126,7 +3159,7 @@ static void cliSet(char *cmdline)
             eqptr++;
         }
 
-        for (uint32_t i = 0; i < VALUE_COUNT; i++) {
+        for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
             val = &valueTable[i];
             // ensure exact match when setting to prevent setting variables with shorter names
             if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) {
@@ -3529,6 +3562,18 @@ static void cliResource(char *cmdline)
 }
 #endif /* USE_RESOURCE_MGMT */
 
+#ifdef USE_PARAMETER_GROUPS
+static void backupConfigs(void)
+{
+     // make copies of configs to do differencing
+
+}
+
+static void restoreConfigs(void)
+{
+}
+#endif
+
 static void printConfig(char *cmdline, bool doDiff)
 {
     uint8_t dumpMask = DUMP_MASTER;
@@ -3545,13 +3590,21 @@ static void printConfig(char *cmdline, bool doDiff)
         options = cmdline;
     }
 
-    static master_t defaultConfig;
     if (doDiff) {
         dumpMask = dumpMask | DO_DIFF;
     }
 
+    static master_t defaultConfig;
     createDefaultConfig(&defaultConfig);
 
+#ifdef USE_PARAMETER_GROUPS
+    backupConfigs();
+    // reset all configs to defaults to do differencing
+    resetConfigs();
+#if defined(TARGET_CONFIG)
+    targetConfiguration(&defaultConfig);
+#endif
+#endif
     if (checkCommand(options, "showdefaults")) {
         dumpMask = dumpMask | SHOW_DEFAULTS;   // add default values as comments for changed values
     }
@@ -3674,6 +3727,10 @@ static void printConfig(char *cmdline, bool doDiff)
     if (dumpMask & DUMP_RATES) {
         cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig);
     }
+#ifdef USE_PARAMETER_GROUPS
+    // restore configs from copies
+    restoreConfigs();
+#endif
 }
 
 static void cliDump(char *cmdline)
@@ -3800,13 +3857,11 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx),
 #endif
 };
-#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
-
 static void cliHelp(char *cmdline)
 {
     UNUSED(cmdline);
 
-    for (uint32_t i = 0; i < CMD_COUNT; i++) {
+    for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) {
         cliPrint(cmdTable[i].name);
 #ifndef MINIMAL_CLI
         if (cmdTable[i].description) {
@@ -3835,7 +3890,7 @@ void cliProcess(void)
             // do tab completion
             const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
             uint32_t i = bufferIndex;
-            for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) {
+            for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) {
                 if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0))
                     continue;
                 if (!pstart)
@@ -3896,12 +3951,12 @@ void cliProcess(void)
 
                 const clicmd_t *cmd;
                 char *options;
-                for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) {
+                for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) {
                     if ((options = checkCommand(cliBuffer, cmd->name))) {
                         break;
-            }
+                    }
                 }
-                if(cmd < cmdTable + CMD_COUNT)
+                if(cmd < cmdTable + ARRAYLEN(cmdTable))
                     cmd->func(options);
                 else
                     cliPrint("Unknown command, try 'help'");
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index e65ef76b22..e66c6e1395 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -872,11 +872,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();
@@ -1173,7 +1176,7 @@ void ensureEEPROMContainsValidData(void)
 
 void resetEEPROM(void)
 {
-    resetConf();
+    resetConfigs();
     writeEEPROM();
 }
 
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 62841f5fd3..6466507e61 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -91,6 +91,7 @@ bool canSoftwareSerialBeUsed(void);
 
 uint16_t getCurrentMinthrottle(void);
 
+void resetConfigs(void);
 struct master_s;
 void targetConfiguration(struct master_s *config);
 void targetValidateConfiguration(struct master_s *config);

From 83343e1d86f72da485c598d982d1b0ac939097bc Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 10:26:25 +0000
Subject: [PATCH 39/50] Preparation for doing PG differencing in CLI

---
 src/main/config/parameter_group.h |  30 ++--
 src/main/fc/cli.c                 | 220 +++++++++++++++++++++---------
 2 files changed, 175 insertions(+), 75 deletions(-)

diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h
index 734933ca9c..2533b50158 100644
--- a/src/main/config/parameter_group.h
+++ b/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))
 
@@ -100,22 +103,25 @@ extern const uint8_t __pg_resetdata_end[];
 // Declare system config
 #define PG_DECLARE(_type, _name)                                        \
     extern _type _name ## _System;                                      \
-    static inline _type* _name(void) { return &_name ## _System; }      \
+    static inline const _type* _name(void) { return &_name ## _System; }\
+    static inline _type* _name ## Mutable(void) { return &_name ## _System; }\
     struct _dummy                                                       \
     /**/
 
 // Declare system config array
-#define PG_DECLARE_ARR(_type, _size, _name)                             \
+#define PG_DECLARE_ARRAY(_type, _size, _name)                             \
     extern _type _name ## _SystemArray[_size];                          \
-    static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
-    static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \
+    static inline const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
+    static inline _type* _name ## Mutable(int _index) { return &_name ## _SystemArray[_index]; } \
+    static inline _type (* _name ## _array(void))[_size] { return &_name ## _SystemArray; } \
     struct _dummy                                                       \
     /**/
 
 // Declare profile config
 #define PG_DECLARE_PROFILE(_type, _name)                                \
     extern _type *_name ## _ProfileCurrent;                             \
-    static inline _type* _name(void) { return _name ## _ProfileCurrent; } \
+    static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \
+    static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \
     struct _dummy                                                       \
     /**/
 
@@ -148,7 +154,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 +166,20 @@ extern const uint8_t __pg_resetdata_end[];
     }                                                                   \
     /**/
 
-#define PG_REGISTER_ARR(_type, _size, _name, _pgn, _version)            \
-    PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
+#define PG_REGISTER_ARRAY(_type, _size, _name, _pgn, _version)            \
+    PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
     /**/
 
-#define PG_REGISTER_ARR_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \
+#define PG_REGISTER_ARRAY_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \
     extern void pgResetFn_ ## _name(_type *);    \
-    PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
+    PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
     /**/
 
 #if 0
 // ARRAY reset mechanism is not implemented yet, only few places in code would benefit from it - See pgResetInstance
-#define PG_REGISTER_ARR_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \
+#define PG_REGISTER_ARRAY_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \
     extern const _type pgResetTemplate_ ## _name;                       \
-    PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
+    PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
     /**/
 #endif
 
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index abb8dc147e..4782d812e8 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -117,16 +117,6 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128];
 static char cliBuffer[48];
 static uint32_t bufferIndex = 0;
 
-typedef enum {
-    DUMP_MASTER = (1 << 0),
-    DUMP_PROFILE = (1 << 1),
-    DUMP_RATES = (1 << 2),
-    DUMP_ALL = (1 << 3),
-    DO_DIFF = (1 << 4),
-    SHOW_DEFAULTS = (1 << 5),
-    HIDE_UNUSED = (1 << 6)
-} dumpFlags_e;
-
 static const char* const emptyName = "-";
 
 #ifndef USE_QUAD_MIXER_ONLY
@@ -452,22 +442,21 @@ static const lookupTableEntry_t lookupTables[] = {
 #define VALUE_MODE_OFFSET 6
 
 typedef enum {
-    // value type
+    // value type, bits 0-3
     VAR_UINT8 = (0 << VALUE_TYPE_OFFSET),
     VAR_INT8 = (1 << VALUE_TYPE_OFFSET),
     VAR_UINT16 = (2 << VALUE_TYPE_OFFSET),
     VAR_INT16 = (3 << VALUE_TYPE_OFFSET),
     //VAR_UINT32 = (4 << VALUE_TYPE_OFFSET),
-    VAR_FLOAT = (5 << VALUE_TYPE_OFFSET),
+    VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), // 0x05
 
-    // value section
+    // value section, bits 4-5
     MASTER_VALUE = (0 << VALUE_SECTION_OFFSET),
     PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET),
-    PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET),
-    CONTROL_RATE_VALUE = (3 << VALUE_SECTION_OFFSET),
+    PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // 0x20
     // value mode
-    MODE_DIRECT = (0 << VALUE_MODE_OFFSET),
-    MODE_LOOKUP = (1 << VALUE_MODE_OFFSET)
+    MODE_DIRECT = (0 << VALUE_MODE_OFFSET), // 0x40
+    MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) // 0x80
 } cliValueFlag_e;
 
 #define VALUE_TYPE_MASK (0x0F)
@@ -858,6 +847,16 @@ static void cliPutp(void *p, char ch)
     bufWriterAppend(p, ch);
 }
 
+typedef enum {
+    DUMP_MASTER = (1 << 0),
+    DUMP_PROFILE = (1 << 1),
+    DUMP_RATES = (1 << 2),
+    DUMP_ALL = (1 << 3),
+    DO_DIFF = (1 << 4),
+    SHOW_DEFAULTS = (1 << 5),
+    HIDE_UNUSED = (1 << 6)
+} dumpFlags_e;
+
 static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...)
 {
     if (!((dumpMask & DO_DIFF) && equalsDefault)) {
@@ -952,23 +951,121 @@ static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_
 }
 
 #ifdef USE_PARAMETER_GROUPS
-static void* getValuePointer(const clivalue_t *var)
-{
-    const pgRegistry_t* rec = pgFind(var->pgn);
 
-    switch (var->type & VALUE_SECTION_MASK) {
+static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptrDefault)
+{
+    bool result = false;
+    switch (type & VALUE_TYPE_MASK) {
+    case VAR_UINT8:
+        result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault;
+        break;
+
+    case VAR_INT8:
+        result = *(int8_t *)ptr == *(int8_t *)ptrDefault;
+        break;
+
+    case VAR_UINT16:
+        result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault;
+        break;
+
+    case VAR_INT16:
+        result = *(int16_t *)ptr == *(int16_t *)ptrDefault;
+        break;
+
+/* not currently used
+    case VAR_UINT32:
+        result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault;
+        break;*/
+
+    case VAR_FLOAT:
+        result = *(float *)ptr == *(float *)ptrDefault;
+        break;
+    }
+    return result;
+}
+
+typedef struct cliCurrentAndDefaultConfig_s {
+    const void *currentConfig; // the copy
+    const void *defaultConfig; // the PG value as set by default
+} cliCurrentAndDefaultConfig_t;
+
+static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn)
+{
+    static cliCurrentAndDefaultConfig_t ret;
+
+    switch (pgn) {
+    default:
+        ret.currentConfig = NULL;
+        ret.defaultConfig = NULL;
+        break;
+    }
+    return &ret;
+}
+
+static uint16_t getValueOffset(const clivalue_t *value)
+{
+    switch (value->type & VALUE_SECTION_MASK) {
     case MASTER_VALUE:
-        return rec->address + var->offset;
-    case PROFILE_RATE_VALUE:
-        return rec->address + var->offset + sizeof(profile_t) * getCurrentProfile();
-    case CONTROL_RATE_VALUE:
-        return rec->address + var->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile();
     case PROFILE_VALUE:
-        return *rec->ptr + var->offset;
+        return value->offset;
+    case PROFILE_RATE_VALUE:
+        return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile();
+    }
+    return 0;
+}
+
+static void *getValuePointer(const clivalue_t *value)
+{
+    const pgRegistry_t* rec = pgFind(value->pgn);
+
+    switch (value->type & VALUE_SECTION_MASK) {
+    case MASTER_VALUE:
+    case PROFILE_VALUE:
+        return rec->address + value->offset;
+    case PROFILE_RATE_VALUE:
+        return rec->address + value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile();
     }
     return NULL;
 }
+
+static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask)
+{
+    const char *format = "set %s = ";
+    const cliCurrentAndDefaultConfig_t *config = getCurrentAndDefaultConfigs(value->pgn);
+    if (config->currentConfig == NULL || config->defaultConfig == NULL) {
+        // has not been set up properly
+        cliPrintf("VALUE %s ERROR\r\n", value->name);
+        return;
+    }
+    const int valueOffset = getValueOffset(value);
+    switch (dumpMask & (DO_DIFF | SHOW_DEFAULTS)) {
+    case DO_DIFF:
+        if (valuePtrEqualsDefault(value->type, (uint8_t*)config->currentConfig + valueOffset, (uint8_t*)config->defaultConfig + valueOffset)) {
+            break;
+        }
+        // drop through, since not equal to default
+    case 0:
+    case SHOW_DEFAULTS:
+        cliPrintf(format, value->name);
+        printValuePointer(value, (uint8_t*)config->currentConfig + valueOffset, 0);
+        cliPrint("\r\n");
+        break;
+    }
+}
+
+static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask)
+{
+    for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
+        const clivalue_t *value = &valueTable[i];
+        bufWriterFlush(cliWriter);
+        if ((value->type & VALUE_SECTION_MASK) == valueSection) {
+            dumpPgValue(value, dumpMask);
+        }
+    }
+}
+
 #else
+
 void *getValuePointer(const clivalue_t *value)
 {
     void *ptr = value->ptr;
@@ -1141,9 +1238,9 @@ static void cliShowArgumentRangeError(char *name, int min, int max)
     cliPrintf("%s not between %d and %d\r\n", name, min, max);
 }
 
-static char *nextArg(char *currentArg)
+static const char *nextArg(const char *currentArg)
 {
-    char *ptr = strchr(currentArg, ' ');
+    const char *ptr = strchr(currentArg, ' ');
     while (ptr && *ptr == ' ') {
         ptr++;
     }
@@ -1151,14 +1248,12 @@ static char *nextArg(char *currentArg)
     return ptr;
 }
 
-static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount)
+static const char *processChannelRangeArgs(const char *ptr, channelRange_t *range, uint8_t *validArgumentCount)
 {
-    int val;
-
     for (uint32_t argIndex = 0; argIndex < 2; argIndex++) {
         ptr = nextArg(ptr);
         if (ptr) {
-            val = atoi(ptr);
+            int val = atoi(ptr);
             val = CHANNEL_VALUE_TO_STEP(val);
             if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
                 if (argIndex == 0) {
@@ -1177,10 +1272,10 @@ static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *
 // Check if a string's length is zero
 static bool isEmpty(const char *string)
 {
-    return *string == '\0';
+    return (string == NULL || *string == '\0') ? true : false;
 }
 
-static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig)
+static void printRxFailsafe(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig)
 {
     // print out rxConfig failsafe settings
     for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) {
@@ -1219,7 +1314,7 @@ static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxCo
     }
 }
 
-static void cliRxFail(char *cmdline)
+static void cliRxFailsafe(char *cmdline)
 {
     uint8_t channel;
     char buf[3];
@@ -1227,10 +1322,10 @@ static void cliRxFail(char *cmdline)
     if (isEmpty(cmdline)) {
         // print out rxConfig failsafe settings
         for (channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) {
-            cliRxFail(itoa(channel, buf, 10));
+            cliRxFailsafe(itoa(channel, buf, 10));
         }
     } else {
-        char *ptr = cmdline;
+        const char *ptr = cmdline;
         channel = atoi(ptr++);
         if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) {
 
@@ -1337,7 +1432,7 @@ static void printAux(uint8_t dumpMask, const modeActivationProfile_t *modeActiva
 static void cliAux(char *cmdline)
 {
     int i, val = 0;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printAux(DUMP_MASTER, modeActivationProfile(), NULL);
@@ -1411,13 +1506,9 @@ static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, co
 
 static void cliSerial(char *cmdline)
 {
-    int i, val;
-    char *ptr;
-
     if (isEmpty(cmdline)) {
         printSerial(DUMP_MASTER, serialConfig(), NULL);
-
-    return;
+        return;
     }
     serialPortConfig_t portConfig;
     memset(&portConfig, 0 , sizeof(portConfig));
@@ -1426,9 +1517,9 @@ static void cliSerial(char *cmdline)
 
     uint8_t validArgumentCount = 0;
 
-    ptr = cmdline;
+    const char *ptr = cmdline;
 
-    val = atoi(ptr++);
+    int val = atoi(ptr++);
     currentConfig = serialFindPortConfiguration(val);
     if (currentConfig) {
         portConfig.identifier = val;
@@ -1442,7 +1533,7 @@ static void cliSerial(char *cmdline)
         validArgumentCount++;
     }
 
-    for (i = 0; i < 4; i ++) {
+    for (int i = 0; i < 4; i ++) {
         ptr = nextArg(ptr);
         if (!ptr) {
             break;
@@ -1491,7 +1582,6 @@ static void cliSerial(char *cmdline)
     }
 
     memcpy(currentConfig, &portConfig, sizeof(portConfig));
-
 }
 
 #ifndef SKIP_SERIAL_PASSTHROUGH
@@ -1609,7 +1699,7 @@ static void printAdjustmentRange(uint8_t dumpMask, const adjustmentProfile_t *ad
 static void cliAdjustmentRange(char *cmdline)
 {
     int i, val = 0;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printAdjustmentRange(DUMP_MASTER, adjustmentProfile(), NULL);
@@ -1713,7 +1803,7 @@ static void cliMotorMix(char *cmdline)
 #else
     int check = 0;
     uint8_t len;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL);
@@ -1801,7 +1891,7 @@ static void printRxRange(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxC
 static void cliRxRange(char *cmdline)
 {
     int i, validArgumentCount = 0;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printRxRange(DUMP_MASTER, rxConfig(), NULL);
@@ -1863,7 +1953,7 @@ static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledC
 static void cliLed(char *cmdline)
 {
     int i;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printLed(DUMP_MASTER, ledStripConfig()->ledConfigs, NULL);
@@ -1901,7 +1991,7 @@ static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColo
 static void cliColor(char *cmdline)
 {
     int i;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printColor(DUMP_MASTER, ledStripConfig()->colors, NULL);
@@ -2179,10 +2269,8 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig)
 
 static void cliServoMix(char *cmdline)
 {
-    uint8_t len;
-    char *ptr;
     int args[8], check = 0;
-    len = strlen(cmdline);
+    int len = strlen(cmdline);
 
     if (len == 0) {
         printServoMix(DUMP_MASTER, NULL);
@@ -2193,7 +2281,7 @@ static void cliServoMix(char *cmdline)
             servoProfile()->servoConf[i].reversedSources = 0;
         }
     } else if (strncasecmp(cmdline, "load", 4) == 0) {
-        ptr = nextArg(cmdline);
+        const char *ptr = nextArg(cmdline);
         if (ptr) {
             len = strlen(ptr);
             for (uint32_t i = 0; ; i++) {
@@ -2211,7 +2299,7 @@ static void cliServoMix(char *cmdline)
         }
     } else if (strncasecmp(cmdline, "reverse", 7) == 0) {
         enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT};
-        ptr = strchr(cmdline, ' ');
+        char *ptr = strchr(cmdline, ' ');
 
         len = strlen(ptr);
         if (len == 0) {
@@ -2253,7 +2341,7 @@ static void cliServoMix(char *cmdline)
         cliServoMix("reverse");
     } else {
         enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT};
-        ptr = strtok(cmdline, " ");
+        char *ptr = strtok(cmdline, " ");
         while (ptr != NULL && check < ARGS_COUNT) {
             args[check++] = atoi(ptr);
             ptr = strtok(NULL, " ");
@@ -2427,7 +2515,7 @@ static void cliFlashRead(char *cmdline)
 
     uint8_t buffer[32];
 
-    char *nextArg = strchr(cmdline, ' ');
+    const char *nextArg = strchr(cmdline, ' ');
 
     if (!nextArg) {
         cliShowParseError();
@@ -2496,7 +2584,7 @@ static void printVtx(uint8_t dumpMask, const master_t *defaultConfig)
 static void cliVtx(char *cmdline)
 {
     int i, val = 0;
-    char *ptr;
+    const char *ptr;
 
     if (isEmpty(cmdline)) {
         printVtx(DUMP_MASTER, NULL);
@@ -3067,7 +3155,13 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_
     cliPrintHashLine("profile");
     cliProfile("");
     cliPrint("\r\n");
+#ifdef USE_PARAMETER_GROUPS
+    (void)(defaultConfig);
+    dumpAllValues(PROFILE_VALUE, dumpMask);
+    dumpAllValues(PROFILE_RATE_VALUE, dumpMask);
+#else
     dumpValues(PROFILE_VALUE, dumpMask, defaultConfig);
+#endif
     cliRateProfile("");
 }
 
@@ -3688,7 +3782,7 @@ static void printConfig(char *cmdline, bool doDiff)
 #endif
 
         cliPrintHashLine("rxfail");
-        printRxFail(dumpMask, rxConfig(), &defaultConfig.rxConfig);
+        printRxFailsafe(dumpMask, rxConfig(), &defaultConfig.rxConfig);
 
         cliPrintHashLine("master");
         dumpValues(MASTER_VALUE, dumpMask, &defaultConfig);
@@ -3828,7 +3922,7 @@ const clicmd_t cmdTable[] = {
 #if defined(USE_RESOURCE_MGMT)
     CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource),
 #endif
-    CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail),
+    CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFailsafe),
     CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
     CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
 #ifdef USE_SDCARD

From 4dee6b7dddf27e2e39f97b8d00e7d9a206db5228 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 11:34:20 +0000
Subject: [PATCH 40/50] Fixed up unit tests and ROM sizes

---
 src/main/target/NAZE/target.h              |  3 +--
 src/test/unit/parameter_groups_unittest.cc | 10 ++++------
 2 files changed, 5 insertions(+), 8 deletions(-)

diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index b9cc396a3a..315faf2fe5 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -101,8 +101,7 @@
 #define ACC_MPU6500_ALIGN       CW0_DEG
 
 #define BARO
-#define USE_BARO_MS5611
-#define USE_BARO_BMP085
+#define USE_BARO_MS5611 // needed for Flip32 board
 #define USE_BARO_BMP280
 
 /*
diff --git a/src/test/unit/parameter_groups_unittest.cc b/src/test/unit/parameter_groups_unittest.cc
index df45a1829c..a48e98e055 100644
--- a/src/test/unit/parameter_groups_unittest.cc
+++ b/src/test/unit/parameter_groups_unittest.cc
@@ -22,10 +22,8 @@
 #include <limits.h>
 
 extern "C" {
-    #include "build/debug.h"
-
     #include <platform.h>
-
+    #include "build/debug.h"
     #include "config/parameter_group.h"
     #include "config/parameter_group_ids.h"
 
@@ -49,7 +47,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
 
 TEST(ParameterGroupsfTest, Test_pgResetAll)
 {
-    memset(motorConfig(), 0, sizeof(motorConfig_t));
+    memset(motorConfigMutable(), 0, sizeof(motorConfig_t));
     pgResetAll(0);
     EXPECT_EQ(1150, motorConfig()->minthrottle);
     EXPECT_EQ(1850, motorConfig()->maxthrottle);
@@ -59,7 +57,7 @@ TEST(ParameterGroupsfTest, Test_pgResetAll)
 
 TEST(ParameterGroupsfTest, Test_pgFind)
 {
-    memset(motorConfig(), 0, sizeof(motorConfig_t));
+    memset(motorConfigMutable(), 0, sizeof(motorConfig_t));
     const pgRegistry_t *pgRegistry = pgFind(PG_MOTOR_CONFIG);
     pgResetCurrent(pgRegistry);
     EXPECT_EQ(1150, motorConfig()->minthrottle);
@@ -69,7 +67,7 @@ TEST(ParameterGroupsfTest, Test_pgFind)
 
     motorConfig_t motorConfig2;
     memset(&motorConfig2, 0, sizeof(motorConfig_t));
-    motorConfig()->motorPwmRate = 500;
+    motorConfigMutable()->motorPwmRate = 500;
     pgStore(pgRegistry, &motorConfig2, sizeof(motorConfig_t), 0);
     EXPECT_EQ(1150, motorConfig2.minthrottle);
     EXPECT_EQ(1850, motorConfig2.maxthrottle);

From 46fa01a4b82c1fa3adaab35ebf0716a234132c75 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 12:04:41 +0000
Subject: [PATCH 41/50] Added STM32F722xx FLASH page size

---
 src/main/config/config_streamer.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c
index e63363f22d..c4b6f6af03 100644
--- a/src/main/config/config_streamer.c
+++ b/src/main/config/config_streamer.c
@@ -27,18 +27,24 @@ extern uint8_t __config_start;   // configured via linker script when building b
 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)

From 108d9d6b61b05d96163dfd4cdea2d4d86f413b8e Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Dec 2016 19:31:02 +0100
Subject: [PATCH 42/50] Fix atomic.h

`=m` output operand means that value is write only, gcc may discard previous value because it assumes that it will be overwritten. This bug was not triggered in gcc v4 because `asm volatile` triggered full memory barrier.

With old version, this code will increase `markme` only by 2, not 3:
```
static int markme = 0;
markme++;
ATOMIC_BLOCK_NB(0xff) {
  ATOMIC_BARRIER(markme);  // markme is marked as overwritten, previous increment can be discarded
  markme++;
}
markme++;
```
---
 src/main/build/atomic.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index 959c6e9d8d..f2436d5534 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -104,7 +104,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
         __asm__ volatile ("\t# barier(" #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# barier (" #data ") start\n" : "+m" (*__UNIQL(__barrier)))
 
 
 // define these wrappers for atomic operations, use gcc buildins

From 0fb5128d4aec97955c03cd3cc2df1e76f53b6f57 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Fri, 30 Dec 2016 12:57:34 +0100
Subject: [PATCH 43/50] Improve ATOMIC_BARRIER documentation

---
 src/main/build/atomic.h | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index f2436d5534..3708e911ae 100644
--- a/src/main/build/atomic.h
+++ b/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.
-// Be carefull when using this, you must use some method to prevent optimizer form breaking things
-// - lto is used for baseflight compillation, so function call is not memory barrier
-// - use ATOMIC_BARRIER or propper volatile to protect used variables
-// - gcc 4.8.4 does write all values in registes to memory before 'asm volatile', so this optimization does not help much
-//    but that can change in future versions
+// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier.
+// Be careful when using this, you must use some method to prevent optimizer form breaking things
+// - lto is used for Cleanflight compilation, so function call is not memory barrier
+// - use ATOMIC_BARRIER or volatile to protect used variables
+// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
+// - gcc 5 and later works as intended, generating quite optimal code
 #define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
                                     __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
 
 // ATOMIC_BARRIER
 // Create memory barrier
-// - at the beginning (all data must be reread from memory)
-// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
-// ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier
+// - at the beginning of containing block (value of parameter must be reread from memory)
+// - at exit of block (all exit paths) (parameter value if written into memory, but may be cached in register for subsequent use)
+// On gcc 5 and higher, this protects only memory passed as parameter (any type should work)
 // this macro can be used only ONCE PER LINE, but multiple uses per block are fine
 
 #if (__GNUC__ > 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

From 7cdd3818577ad272c2b03d010b6febb5703c84c5 Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Thu, 26 Jan 2017 10:05:17 +1300
Subject: [PATCH 44/50] Fixed typo in generated comment.

---
 src/main/build/atomic.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index 3708e911ae..0508f77e4f 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -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

From cdbbad2012fed66f7392c4a009ae5ffb42cc696b Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 25 Jan 2017 21:06:01 +0000
Subject: [PATCH 45/50] Alignment with iNav

---
 src/main/config/config_eeprom.c | 19 +++++----
 src/main/config/config_eeprom.h |  4 ++
 src/main/fc/config.c            | 69 +++++++++++++++++----------------
 src/main/fc/config.h            |  3 ++
 4 files changed, 54 insertions(+), 41 deletions(-)

diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 2850700c54..295cd77b97 100644
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -47,6 +47,8 @@ PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0);
 extern uint8_t __config_start;   // configured via linker script when building binaries.
 extern uint8_t __config_end;
 
+static uint16_t eepromConfigSize;
+
 typedef enum {
     CR_CLASSICATION_SYSTEM   = 0,
     CR_CLASSICATION_PROFILE1 = 1,
@@ -111,7 +113,7 @@ static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length)
 }
 
 // Scan the EEPROM config. Returns true if the config is valid.
-static bool scanEEPROM(void)
+bool isEEPROMContentValid(void)
 {
     uint8_t chk = 0;
     const uint8_t *p = &__config_start;
@@ -148,7 +150,15 @@ static bool scanEEPROM(void)
     chk = updateChecksum(chk, footer, sizeof(*footer));
     p += sizeof(*footer);
     chk = ~chk;
-    return chk == *p;
+    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
@@ -207,11 +217,6 @@ bool loadEEPROM(void)
     return true;
 }
 
-bool isEEPROMContentValid(void)
-{
-    return scanEEPROM();
-}
-
 static bool writeSettingsToEEPROM(void)
 {
     config_streamer_t streamer;
diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h
index a6aafc3ca7..62c8c6181a 100644
--- a/src/main/config/config_eeprom.h
+++ b/src/main/config/config_eeprom.h
@@ -17,8 +17,12 @@
 
 #pragma once
 
+#include <stdint.h>
+#include <stdbool.h>
+
 #define EEPROM_CONF_VERSION 154
 
 bool isEEPROMContentValid(void);
 bool loadEEPROM(void);
 void writeConfigToEEPROM(void);
+uint16_t getEEPROMConfigSize(void);
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index e66c6e1395..8ee0519c81 100755
--- a/src/main/fc/config.c
+++ b/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,43 +63,37 @@
 #include "fc/rc_curves.h"
 #include "fc/runtime_config.h"
 
-#include "sensors/sensors.h"
-#include "sensors/gyro.h"
-#include "sensors/compass.h"
-#include "sensors/acceleration.h"
-#include "sensors/barometer.h"
-#include "sensors/battery.h"
-#include "sensors/boardalignment.h"
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
+#include "flight/servos.h"
 
 #include "io/beeper.h"
-#include "io/serial.h"
 #include "io/gimbal.h"
-#include "io/motors.h"
-#include "io/servos.h"
-#include "io/ledstrip.h"
 #include "io/gps.h"
+#include "io/ledstrip.h"
+#include "io/motors.h"
 #include "io/osd.h"
+#include "io/serial.h"
+#include "io/servos.h"
 #include "io/vtx.h"
 
 #include "rx/rx.h"
 #include "rx/rx_spi.h"
 
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/battery.h"
+#include "sensors/boardalignment.h"
+#include "sensors/compass.h"
+#include "sensors/gyro.h"
+#include "sensors/sensors.h"
+
 #include "telemetry/telemetry.h"
 
-#include "flight/mixer.h"
-#include "flight/servos.h"
-#include "flight/pid.h"
-#include "flight/imu.h"
-#include "flight/failsafe.h"
-#include "flight/altitudehold.h"
-#include "flight/navigation.h"
-
-#include "config/config_eeprom.h"
-#include "config/config_profile.h"
-#include "config/config_master.h"
-#include "config/feature.h"
-#include "config/parameter_group.h"
-
 #ifndef DEFAULT_RX_FEATURE
 #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
 #endif
@@ -1166,6 +1167,12 @@ void writeEEPROM(void)
     resumeRxSignal();
 }
 
+void resetEEPROM(void)
+{
+    resetConfigs();
+    writeEEPROM();
+}
+
 void ensureEEPROMContainsValidData(void)
 {
     if (isEEPROMContentValid()) {
@@ -1174,12 +1181,6 @@ void ensureEEPROMContainsValidData(void)
     resetEEPROM();
 }
 
-void resetEEPROM(void)
-{
-    resetConfigs();
-    writeEEPROM();
-}
-
 void saveConfigAndNotify(void)
 {
     writeEEPROM();
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 6466507e61..7a85b85384 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -17,8 +17,11 @@
 
 #pragma once
 
+#include <stdint.h>
 #include <stdbool.h>
 
+#include "config/parameter_group.h"
+
 #if FLASH_SIZE <= 128
 #define MAX_PROFILE_COUNT 2
 #else

From 7ce3753eac35003c5309132378dc5bf4f7e2c7ef Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Thu, 26 Jan 2017 11:33:29 +0900
Subject: [PATCH 46/50] Fix TARGET_IO_PORTx defs
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

So bogus ports don’t show up on “resource list”.
---
 src/main/target/OMNIBUSF4/target.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 6d4289da5a..e97ff6468b 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -173,10 +173,10 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
-#define TARGET_IO_PORTA 0xffff
-#define TARGET_IO_PORTB 0xffff
-#define TARGET_IO_PORTC 0xffff
-#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
+#define TARGET_IO_PORTB (0xffff & ~(BIT(2)))
+#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13)))
+#define TARGET_IO_PORTD BIT(2)
 
 #define USABLE_TIMER_CHANNEL_COUNT 12
 #define USED_TIMERS  ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))

From 14a126c52e05800603bafa05652bb3971c32234f Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Fri, 27 Jan 2017 17:12:01 +0000
Subject: [PATCH 47/50] Moved gps_conversion to common/ directory

---
 Makefile                                     |  2 +-
 src/main/{flight => common}/gps_conversion.c |  0
 src/main/{flight => common}/gps_conversion.h |  0
 src/main/flight/navigation.c                 | 16 ++++++++--------
 src/main/io/gps.c                            | 20 ++++++++++----------
 5 files changed, 19 insertions(+), 19 deletions(-)
 rename src/main/{flight => common}/gps_conversion.c (100%)
 rename src/main/{flight => common}/gps_conversion.h (100%)

diff --git a/Makefile b/Makefile
index e5bc1e4ad3..f64a88aea7 100644
--- a/Makefile
+++ b/Makefile
@@ -650,6 +650,7 @@ HIGHEND_SRC = \
             cms/cms_menu_osd.c \
             cms/cms_menu_vtx.c \
             common/colorconversion.c \
+            common/gps_conversion.c \
             drivers/display_ug2864hsweg01.c \
             drivers/light_ws2811strip.c \
             drivers/serial_escserial.c \
@@ -657,7 +658,6 @@ HIGHEND_SRC = \
             drivers/sonar_hcsr04.c \
             drivers/vtx_common.c \
             flight/navigation.c \
-            flight/gps_conversion.c \
             io/dashboard.c \
             io/displayport_max7456.c \
             io/displayport_msp.c \
diff --git a/src/main/flight/gps_conversion.c b/src/main/common/gps_conversion.c
similarity index 100%
rename from src/main/flight/gps_conversion.c
rename to src/main/common/gps_conversion.c
diff --git a/src/main/flight/gps_conversion.h b/src/main/common/gps_conversion.h
similarity index 100%
rename from src/main/flight/gps_conversion.h
rename to src/main/common/gps_conversion.h
diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c
index dffd456893..fc1c4a144d 100644
--- a/src/main/flight/navigation.c
+++ b/src/main/flight/navigation.c
@@ -27,6 +27,7 @@
 #include "build/debug.h"
 
 #include "common/axis.h"
+#include "common/gps_conversion.h"
 #include "common/maths.h"
 #include "common/time.h"
 
@@ -36,21 +37,20 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "sensors/sensors.h"
-#include "sensors/boardalignment.h"
-#include "sensors/acceleration.h"
+#include "flight/imu.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
 
 #include "io/beeper.h"
 #include "io/serial.h"
 #include "io/gps.h"
 
-#include "flight/pid.h"
-#include "flight/navigation.h"
-#include "flight/gps_conversion.h"
-#include "flight/imu.h"
-
 #include "rx/rx.h"
 
+#include "sensors/acceleration.h"
+#include "sensors/boardalignment.h"
+#include "sensors/sensors.h"
+
 
 extern int16_t magHold;
 
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index c4925d8f4e..9cae15f265 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -29,27 +29,27 @@
 #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 "drivers/system.h"
+#include "config/feature.h"
+
 #include "drivers/light_led.h"
+#include "drivers/system.h"
 
-#include "sensors/sensors.h"
-
-#include "io/serial.h"
 #include "io/dashboard.h"
 #include "io/gps.h"
-
-#include "flight/gps_conversion.h"
-#include "flight/pid.h"
-#include "flight/navigation.h"
+#include "io/serial.h"
 
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
-#include "config/feature.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
+
+#include "sensors/sensors.h"
 
 #define LOG_ERROR        '?'
 #define LOG_IGNORED      '!'

From 431e0e23dd0be7b70b6950a70116815518c8bcb6 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sat, 28 Jan 2017 07:37:15 +0000
Subject: [PATCH 48/50] Fixup test code

---
 src/test/Makefile                        | 16 ++++++++--------
 src/test/unit/telemetry_crsf_unittest.cc | 14 +++++++-------
 src/test/unit/telemetry_hott_unittest.cc | 20 ++++++++++----------
 3 files changed, 25 insertions(+), 25 deletions(-)

diff --git a/src/test/Makefile b/src/test/Makefile
index 9f1e83940d..26b95987ce 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -256,24 +256,24 @@ $(OBJECT_DIR)/altitude_hold_unittest : \
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
 
-$(OBJECT_DIR)/flight/gps_conversion.o : \
-	$(USER_DIR)/flight/gps_conversion.c \
-	$(USER_DIR)/flight/gps_conversion.h \
+$(OBJECT_DIR)/common/gps_conversion.o : \
+	$(USER_DIR)/common/gps_conversion.c \
+	$(USER_DIR)/common/gps_conversion.h \
 	$(GTEST_HEADERS)
 
 	@mkdir -p $(dir $@)
-	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/gps_conversion.c -o $@
 
 $(OBJECT_DIR)/gps_conversion_unittest.o : \
 	$(TEST_DIR)/gps_conversion_unittest.cc \
-	$(USER_DIR)/flight/gps_conversion.h \
+	$(USER_DIR)/common/gps_conversion.h \
 	$(GTEST_HEADERS)
 
 	@mkdir -p $(dir $@)
 	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@
 
 $(OBJECT_DIR)/gps_conversion_unittest : \
-	$(OBJECT_DIR)/flight/gps_conversion.o \
+	$(OBJECT_DIR)/common/gps_conversion.o \
 	$(OBJECT_DIR)/gps_conversion_unittest.o \
 	$(OBJECT_DIR)/gtest_main.a
 
@@ -359,7 +359,7 @@ $(OBJECT_DIR)/telemetry_hott_unittest.o : \
 $(OBJECT_DIR)/telemetry_hott_unittest : \
 	$(OBJECT_DIR)/telemetry/hott.o \
 	$(OBJECT_DIR)/telemetry_hott_unittest.o \
-	$(OBJECT_DIR)/flight/gps_conversion.o \
+	$(OBJECT_DIR)/common/gps_conversion.o \
 	$(OBJECT_DIR)/gtest_main.a
 
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
@@ -537,7 +537,7 @@ $(OBJECT_DIR)/telemetry_crsf_unittest : \
 	$(OBJECT_DIR)/telemetry_crsf_unittest.o \
 	$(OBJECT_DIR)/common/maths.o \
 	$(OBJECT_DIR)/common/streambuf.o \
-	$(OBJECT_DIR)/flight/gps_conversion.o \
+	$(OBJECT_DIR)/common/gps_conversion.o \
 	$(OBJECT_DIR)/fc/runtime_config.o \
 	$(OBJECT_DIR)/gtest_main.a
 
diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc
index bb1a526d7e..cfe99b7943 100644
--- a/src/test/unit/telemetry_crsf_unittest.cc
+++ b/src/test/unit/telemetry_crsf_unittest.cc
@@ -28,30 +28,30 @@ extern "C" {
 
     #include "common/axis.h"
     #include "common/filter.h"
+    #include "common/gps_conversion.h"
     #include "common/maths.h"
 
     #include "config/parameter_group.h"
     #include "config/parameter_group_ids.h"
 
-    #include "drivers/system.h"
     #include "drivers/serial.h"
+    #include "drivers/system.h"
 
     #include "fc/runtime_config.h"
 
+    #include "flight/pid.h"
+    #include "flight/imu.h"
+
     #include "io/gps.h"
     #include "io/serial.h"
 
     #include "rx/crsf.h"
 
-    #include "sensors/sensors.h"
     #include "sensors/battery.h"
+    #include "sensors/sensors.h"
 
-    #include "telemetry/telemetry.h"
     #include "telemetry/crsf.h"
-
-    #include "flight/pid.h"
-    #include "flight/imu.h"
-    #include "flight/gps_conversion.h"
+    #include "telemetry/telemetry.h"
 
     bool airMode;
     uint16_t vbat;
diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc
index ff984b097b..bc0d60ad01 100644
--- a/src/test/unit/telemetry_hott_unittest.cc
+++ b/src/test/unit/telemetry_hott_unittest.cc
@@ -27,24 +27,24 @@ extern "C" {
     #include "build/debug.h"
 
     #include "common/axis.h"
+    #include "common/gps_conversion.h"
 
-    #include "drivers/system.h"
     #include "drivers/serial.h"
+    #include "drivers/system.h"
 
-    #include "sensors/sensors.h"
-    #include "sensors/battery.h"
-    #include "sensors/barometer.h"
+    #include "fc/runtime_config.h"
+
+    #include "flight/pid.h"
 
-    #include "io/serial.h"
     #include "io/gps.h"
+    #include "io/serial.h"
+
+    #include "sensors/barometer.h"
+    #include "sensors/battery.h"
+    #include "sensors/sensors.h"
 
     #include "telemetry/telemetry.h"
     #include "telemetry/hott.h"
-
-    #include "flight/pid.h"
-    #include "flight/gps_conversion.h"
-
-    #include "fc/runtime_config.h"
 }
 
 #include "unittest_macros.h"

From 4868b60b5b129cacbaf547254214fb00386393a6 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sat, 28 Jan 2017 07:30:32 +0000
Subject: [PATCH 49/50] Put #includes into alphabetical order

---
 src/main/fc/cli.c    |  2 +-
 src/main/fc/fc_msp.c | 60 ++++++++++++++++++++++----------------------
 2 files changed, 31 insertions(+), 31 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 4782d812e8..ca70962113 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -45,8 +45,8 @@ uint8_t cliMode = 0;
 #include "common/typeconversion.h"
 
 #include "config/config_eeprom.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"
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 6de708dc6d..45587418d7 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -34,20 +34,25 @@
 #include "common/maths.h"
 #include "common/streambuf.h"
 
-#include "drivers/system.h"
+#include "config/config_eeprom.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+#include "config/feature.h"
+
 #include "drivers/accgyro.h"
-#include "drivers/compass.h"
-#include "drivers/serial.h"
 #include "drivers/bus_i2c.h"
-#include "drivers/io.h"
+#include "drivers/compass.h"
 #include "drivers/flash.h"
-#include "drivers/sdcard.h"
-#include "drivers/vcd.h"
+#include "drivers/io.h"
 #include "drivers/max7456.h"
-#include "drivers/vtx_soft_spi_rtc6705.h"
 #include "drivers/pwm_output.h"
+#include "drivers/sdcard.h"
+#include "drivers/serial.h"
 #include "drivers/serial_escserial.h"
+#include "drivers/system.h"
+#include "drivers/vcd.h"
 #include "drivers/vtx_common.h"
+#include "drivers/vtx_soft_spi_rtc6705.h"
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
@@ -55,17 +60,25 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/navigation.h"
+#include "flight/pid.h"
+#include "flight/servos.h"
+
+#include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
-#include "io/motors.h"
-#include "io/servos.h"
+#include "io/flashfs.h"
 #include "io/gps.h"
 #include "io/gimbal.h"
-#include "io/serial.h"
 #include "io/ledstrip.h"
-#include "io/flashfs.h"
-#include "io/transponder_ir.h"
-#include "io/asyncfatfs/asyncfatfs.h"
+#include "io/motors.h"
+#include "io/serial.h"
 #include "io/serial_4way.h"
+#include "io/servos.h"
+#include "io/transponder_ir.h"
 
 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
@@ -76,30 +89,17 @@
 
 #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

From 4765ff410948fe7ef327e8a67d359fb12d3a3175 Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Tue, 31 Jan 2017 14:00:25 +1300
Subject: [PATCH 50/50] Clarified resource remapping output and fixed output in
 case of double mapping.

---
 src/main/fc/cli.c | 26 +++++++++++++++++++-------
 1 file changed, 19 insertions(+), 7 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index ca70962113..d6507da396 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -3501,9 +3501,18 @@ static void printResource(uint8_t dumpMask, const master_t *defaultConfig)
     }
 }
 
+static void printResourceOwner(uint8_t owner, uint8_t index)
+{
+    cliPrintf("%s", ownerNames[resourceTable[owner].owner]);
+
+    if (resourceTable[owner].maxIndex > 0) {
+        cliPrintf(" %d", RESOURCE_INDEX(index));
+    }
+}
+
 static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag)
 {
-    const char * format = "\r\n* NOTE * %c%d moved from %s";
+    const char * format = "\r\nNOTE: %c%02d already assigned to ";
     for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) {
         for (int i = 0; i < MAX_RESOURCE_INDEX(resourceTable[r].maxIndex); i++) {
             if (*(resourceTable[r].ptr + i) == tag) {
@@ -3516,14 +3525,17 @@ static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag)
                     cleared = true;
                 }
 
-                cliPrintf(format, DEFIO_TAG_GPIOID(tag) + 'A', DEFIO_TAG_PIN(tag), ownerNames[resourceTable[r].owner]);
-                if (resourceTable[r].maxIndex > 0) {
-                    cliPrintf(" %d", RESOURCE_INDEX(i));
-                }
+                cliPrintf(format, DEFIO_TAG_GPIOID(tag) + 'A', DEFIO_TAG_PIN(tag));
+
+                printResourceOwner(r, i);
+
                 if (cleared) {
-                    cliPrintf(". Cleared.");
+                    cliPrintf(". ");
+                    printResourceOwner(r, i);
+                    cliPrintf(" disabled");
                 }
-                cliPrint("\r\n");
+
+                cliPrint(".\r\n");
             }
         }
     }