Merge remote-tracking branch 'upstream/master' into blackbox-flash

docs/PID tuning.md

@@ -31,13 +31,13 @@ strength of the correction to be backed off in order to avoid overshooting the t
 
 ## PID controllers
 
-Cleanflight has three built-in PID controllers which each have different flight behavior. Each controller requires
+Cleanflight has 6 built-in PID controllers which each have different flight behavior. Each controller requires
 different PID settings for best performance, so if you tune your craft using one PID controller, those settings will
 likely not work well on any of the other controllers.
 
-You can change between PID controllers by running `set pid_controller = X` on the CLI tab of the Cleanflight
+You can change between PID controllers by running `set pid_controller=n` on the CLI tab of the Cleanflight
-Configurator, where X is the number of the controller you want to use. Please read these notes first before trying one
+Configurator, where `n` is the number of the controller you want to use. Please read these notes first before trying one
-out!
+out.
 
 ### PID controller 0, "MultiWii" (default)
 
@@ -71,7 +71,7 @@ need to be increased in order to perform like PID controller 0.
 
 The LEVEL "D" setting is not used by this controller.
 
-### PID controller 2, "Baseflight"
+### PID controller 2, "LuxFloat"
 
 PID Controller 2 is Lux's new floating point PID controller. Both controller 0 and 1 use integer arithmetic, which was
 faster in the days of the slower 8-bit MultiWii controllers, but is less precise.
@@ -82,10 +82,7 @@ don't have to be retuned when the looptime setting changes.
 There were initially some problems with horizon mode, and sluggishness in acro mode, that were recently fixed by
 nebbian in v1.6.0. The autotune feature does not work on this controller, so don't try to autotune it.
 
-Even though PC2 is called "pidBaseflight" in the code, it was never in Baseflight or MultiWii. A better name might have
+It is the first PID Controller designed for 32-bit processors and not derived from MultiWii.
-been pidFloatingPoint, or pidCleanflight. It is the first PID Controller designed for 32-bit processors and not derived
-from MultiWii. I believe it was named pidBaseflight because it was to be the first true 32-bit processor native PID
-controller, and thus the native Baseflight PC, but Timecop never accepted the code into Baseflight.
 
 The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which
 is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to
@@ -110,7 +107,7 @@ stick, and no self-leveling will be applied at 75% stick and onwards.
 
 PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later.
 
-The algorithm is handling roll and pitch differently to yaw. users with problems on yaw authority should try this one.
+The algorithm is handling roll and pitch differently to yaw. Users with problems on yaw authority should try this one.
 
 For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. This will provide best performance on very small multicopters with brushed motors.
 
@@ -118,6 +115,22 @@ For the ALIENWII32 targets the gyroscale is removed for even more yaw authority.
 
 PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm. 
 
-This PID controller was initialy implementd for testing purposes but is also performing quite well.
+This PID controller was initialy implemented for testing purposes but is also performing quite well.
 
 For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors.
+
+### PID controller 5, "Harakiri"
+
+PID Controller 5 is an port of the PID controller from the Harakiri firmware.
+
+The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don<6F>t need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri:
+
+        OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
+        MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
+
+The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly.
+
+Yaw authority is also quite good.
+
+
+

src/main/config/config.c

@@ -720,6 +720,12 @@ void validateAndFixConfig(void)
     }
 #endif
 
+#if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3)
+    if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) {
+        featureClear(FEATURE_DISPLAY);
+    }
+#endif
+
     useRxConfig(&masterConfig.rxConfig);
 
     serialConfig_t *serialConfig = &masterConfig.serialConfig;

src/main/drivers/pwm_rx.c

@@ -234,9 +234,7 @@ static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca
     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
 
     if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
-        if (pwmInputPort->state == 0) {
+        captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
-            captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
-        }
         pwmInputPort->missedEvents = 0;
     }
 }

src/main/drivers/serial_uart_stm32f10x.c

@@ -47,13 +47,20 @@ static uartPort_t uartPort2;
 static uartPort_t uartPort3;
 #endif
 
+// Using RX DMA disables the use of receive callbacks
+#define USE_USART1_RX_DMA
+
+#if defined(CC3D) // FIXME move board specific code to target.h files.
+#undef USE_USART1_RX_DMA
+#endif
+
 void uartStartTxDMA(uartPort_t *s);
 
 void usartIrqCallback(uartPort_t *s)
 {
     uint16_t SR = s->USARTx->SR;
 
-    if (SR & USART_FLAG_RXNE) {
+    if (SR & USART_FLAG_RXNE && !s->rxDMAChannel) {
         // If we registered a callback, pass crap there
         if (s->port.callback) {
             s->port.callback(s->USARTx->DR);
@@ -98,11 +105,13 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
     
     s->USARTx = USART1;
 
-    s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
-    s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
 
+#ifdef USE_USART1_RX_DMA
     s->rxDMAChannel = DMA1_Channel5;
+    s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
+#endif
     s->txDMAChannel = DMA1_Channel4;
+    s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
 
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
     RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
@@ -129,6 +138,15 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&NVIC_InitStructure);
 
+#ifndef USE_USART1_RX_DMA
+    // RX/TX Interrupt
+    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1);
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1);
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+#endif
+
     return s;
 }
 
@@ -146,20 +164,11 @@ void DMA1_Channel4_IRQHandler(void)
         s->txDMAEmpty = true;
 }
 
-// USART1 Tx IRQ Handler
+// USART1 Rx/Tx IRQ Handler
 void USART1_IRQHandler(void)
 {
     uartPort_t *s = &uartPort1;
-    uint16_t SR = s->USARTx->SR;
+    usartIrqCallback(s);
-
-    if (SR & USART_FLAG_TXE) {
-        if (s->port.txBufferTail != s->port.txBufferHead) {
-            s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
-            s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
-        } else {
-            USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
-        }
-    }
 }
 
 #endif

src/main/flight/altitudehold.c

@@ -53,7 +53,6 @@ uint8_t velocityControl = 0;
 int32_t errorVelocityI = 0;
 int32_t altHoldThrottleAdjustment = 0;
 int32_t AltHold;
-int32_t EstAlt;                // in cm
 int32_t vario = 0;                      // variometer in cm/s
 
 
@@ -78,7 +77,7 @@ void configureAltitudeHold(
 #if defined(BARO) || defined(SONAR)
 
 static int16_t initialThrottleHold;
-
+static int32_t EstAlt;                // in cm
 
 // 40hz update rate (20hz LPF on acc)
 #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
@@ -230,6 +229,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     float vel_acc;
     int32_t vel_tmp;
     float accZ_tmp;
+    int32_t sonarAlt = -1;
     static float accZ_old = 0.0f;
     static float vel = 0.0f;
     static float accAlt = 0.0f;
@@ -242,8 +242,6 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     int16_t tiltAngle;
 #endif
 
-
-
     dTime = currentTime - previousTime;
     if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
         return;
@@ -329,5 +327,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 
     accZ_old = accZ_tmp;
 }
+
+int32_t altitudeHoldGetEstimatedAltitude(void)
+{
+    return EstAlt;
+}
+
 #endif
 

src/main/flight/altitudehold.h

@@ -15,14 +15,16 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-//extern int32_t errorVelocityI;
+#include "flight/flight.h"
-//extern int32_t altHoldThrottleAdjustment;
-//extern uint8_t velocityControl;
-//extern int32_t setVelocity;
 
+#include "io/escservo.h"
+#include "io/rc_controls.h"
+
+#include "sensors/barometer.h"
 
 void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
 void applyAltHold(airplaneConfig_t *airplaneConfig);
 void updateAltHoldState(void);
 void updateSonarAltHoldState(void);
 
+int32_t altitudeHoldGetEstimatedAltitude(void);

src/main/flight/flight.c

@@ -97,7 +97,7 @@ bool shouldAutotune(void)
 }
 #endif
 
-static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
+static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
     float RateError, errorAngle, AngleRate, gyroRate;
@@ -506,6 +506,136 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
 #endif
 }
 
+#define RCconstPI   0.159154943092f // 0.5f / M_PI;
+#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
+#define OLD_YAW	0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
+
+static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
+rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
+{
+    UNUSED(rxConfig);
+
+    float delta, RCfactor, rcCommandAxis, MainDptCut;
+    float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0;
+    static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0};
+    float tmp0flt;
+    int32_t tmp0;
+    uint8_t axis;
+    float ACCDeltaTimeINS = 0;
+    float FLOATcycleTime = 0;
+
+//    MainDptCut = RCconstPI / (float)cfg.maincuthz;                           // Initialize Cut off frequencies for mainpid D
+    MainDptCut = RCconstPI / MAIN_CUT_HZ;                                      // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
+    FLOATcycleTime  = (float)constrain(cycleTime, 1, 100000);                  // 1us - 100ms
+    ACCDeltaTimeINS = FLOATcycleTime * 0.000001f;                              // ACCDeltaTimeINS is in seconds now
+    RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS);               // used for pt1 element
+
+    if (FLIGHT_MODE(HORIZON_MODE)) {
+        prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f;
+    }
+
+    for (axis = 0; axis < 2; axis++) {
+        rcCommandAxis = (float)rcCommand[axis];                                // Calculate common values for pid controllers
+        if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
+#ifdef GPS
+            error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+#else
+            error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+#endif
+
+#ifdef AUTOTUNE
+            if (shouldAutotune()) {
+                error = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(error)));
+            }
+#endif
+            PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
+            tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
+            PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt);
+            errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
+            ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
+        }
+
+        if (!FLIGHT_MODE(ANGLE_MODE)) {
+            if (ABS((int16_t)gyroData[axis]) > 2560) {
+                errorGyroI[axis] = 0.0f;
+            } else {
+                error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis];
+                errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
+            }
+
+            ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f;
+
+            if (FLIGHT_MODE(HORIZON_MODE)) {
+                PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
+                ITerm = ITermACC + prop * (ITermGYRO - ITermACC);
+            } else {
+                PTerm = rcCommandAxis;
+                ITerm = ITermGYRO;
+            }
+        } else {
+            PTerm = PTermACC;
+            ITerm = ITermACC;
+        }
+
+        PTerm -= gyroData[axis] * dynP8[axis] * 0.003f;
+        delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS;
+
+        lastGyro[axis] = gyroData[axis];
+        lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
+        DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
+
+        axisPID[axis] = lrintf(PTerm + ITerm - DTerm);                         // Round up result.
+
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = -DTerm;
+#endif
+    }
+
+    tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3;                          // Filter last 2 bit jitter
+    tmp0flt /= 3000.0f;
+
+    if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
+        PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
+        tmp0 = lrintf(gyroData[FD_YAW] * 0.25f);
+        PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80;
+        if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
+            errorGyroI[FD_YAW] = 0;
+        } else {
+            error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0;
+            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp
+            ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
+        }
+    } else {
+        tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
+        error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f);                       // Less Gyrojitter works actually better
+
+        if (ABS(tmp0) > 50) {
+            errorGyroI[FD_YAW] = 0;
+        } else {
+            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
+        }
+
+        ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
+        PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
+
+        if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
+            tmp0 = 300;
+            if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW];
+            PTerm = constrain(PTerm, -tmp0, tmp0);
+        }
+    }
+    axisPID[FD_YAW] = PTerm + ITerm;
+    axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]);                                 // Round up result.
+
+#ifdef BLACKBOX
+    axisPID_P[FD_YAW] = PTerm;
+    axisPID_I[FD_YAW] = ITerm;
+    axisPID_D[FD_YAW] = 0;
+#endif
+}
+
 static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
         rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
@@ -606,13 +736,16 @@ void setPIDController(int type)
             pid_controller = pidRewrite;
             break;
         case 2:
-            pid_controller = pidBaseflight;
+            pid_controller = pidLuxFloat;
             break;
         case 3:
             pid_controller = pidMultiWii23;
             break;
         case 4:
             pid_controller = pidMultiWiiHybrid;
+            break;
+        case 5:
+            pid_controller = pidHarakiri;
     }
 }
 

src/main/flight/flight.h

@@ -111,7 +111,6 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 extern int16_t heading, magHold;
 
 extern int32_t AltHold;
-extern int32_t EstAlt;
 extern int32_t vario;
 
 void setPIDController(int type);

src/main/io/rc_controls.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "rx/rx.h"
+
 typedef enum {
     BOXARM = 0,
     BOXANGLE,

src/main/io/serial.c

@@ -44,7 +44,6 @@
 
 void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate);
 
-void mspInit(serialConfig_t *serialConfig);
 void cliInit(serialConfig_t *serialConfig);
 
 // this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask.
@@ -92,13 +91,13 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
     {SERIAL_PORT_USB_VCP,       9600,   115200,   SPF_NONE },
 #endif
 #ifdef USE_USART1
-    {SERIAL_PORT_USART1,        9600,   115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART1,        9600,   250000,   SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #endif
 #ifdef USE_USART2
-    {SERIAL_PORT_USART2,        9600,   115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART2,        9600,   250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #endif
 #ifdef USE_USART3
-    {SERIAL_PORT_USART3,        9600,   115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART3,        9600,   250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #endif
 };
 
@@ -118,8 +117,8 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
 #ifdef USE_VCP
     {SERIAL_PORT_USB_VCP,       9600,   115200,   SPF_NONE },
 #endif
-    {SERIAL_PORT_USART1,        9600, 115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART1,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
-    {SERIAL_PORT_USART3,        9600, 115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART3,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
     {SERIAL_PORT_SOFTSERIAL1,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
 };
 #else
@@ -134,7 +133,7 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
 };
 
 const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
-    {SERIAL_PORT_USART1,        9600, 250000,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART1,        9600, 250000,   SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
     {SERIAL_PORT_USART2,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #if (SERIAL_PORT_COUNT > 2)
     {SERIAL_PORT_SOFTSERIAL1,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE},

src/main/io/serial_cli.c

@@ -384,7 +384,7 @@ const clivalue_t valueTable[] = {
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, 0, MAG_NONE },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
 
-    { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 4 },
+    { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 },
 
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },

src/main/io/serial_msp.c

@@ -45,6 +45,8 @@
 #include "flight/mixer.h"
 #include "flight/failsafe.h"
 #include "flight/navigation.h"
+#include "flight/altitudehold.h"
+
 #include "rx/rx.h"
 #include "rx/msp.h"
 #include "io/escservo.h"
@@ -55,9 +57,11 @@
 #include "io/ledstrip.h"
 #include "io/flashfs.h"
 #include "telemetry/telemetry.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/compass.h"
@@ -204,6 +208,10 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
 #define MSP_VOLTAGE_METER_CONFIG        56
 #define MSP_SET_VOLTAGE_METER_CONFIG    57
 
+#define MSP_SONAR_ALTITUDE              58 //out message get sonar altitude [cm]
+
+#define MSP_PID_CONTROLLER              59
+#define MSP_SET_PID_CONTROLLER          60
 //
 // Baseflight MSP commands (if enabled they exist in Cleanflight)
 //
@@ -373,7 +381,7 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
 static mspPort_t *currentPort;
 
-void serialize32(uint32_t a)
+static void serialize32(uint32_t a)
 {
     static uint8_t t;
     t = a;
@@ -390,7 +398,7 @@ void serialize32(uint32_t a)
     currentPort->checksum ^= t;
 }
 
-void serialize16(int16_t a)
+static void serialize16(int16_t a)
 {
     static uint8_t t;
     t = a;
@@ -401,32 +409,32 @@ void serialize16(int16_t a)
     currentPort->checksum ^= t;
 }
 
-void serialize8(uint8_t a)
+static void serialize8(uint8_t a)
 {
     serialWrite(mspSerialPort, a);
     currentPort->checksum ^= a;
 }
 
-uint8_t read8(void)
+static uint8_t read8(void)
 {
     return currentPort->inBuf[currentPort->indRX++] & 0xff;
 }
 
-uint16_t read16(void)
+static uint16_t read16(void)
 {
     uint16_t t = read8();
     t += (uint16_t)read8() << 8;
     return t;
 }
 
-uint32_t read32(void)
+static uint32_t read32(void)
 {
     uint32_t t = read16();
     t += (uint32_t)read16() << 16;
     return t;
 }
 
-void headSerialResponse(uint8_t err, uint8_t responseBodySize)
+static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
 {
     serialize8('$');
     serialize8('M');
@@ -436,36 +444,36 @@ void headSerialResponse(uint8_t err, uint8_t responseBodySize)
     serialize8(currentPort->cmdMSP);
 }
 
-void headSerialReply(uint8_t responseBodySize)
+static void headSerialReply(uint8_t responseBodySize)
 {
     headSerialResponse(0, responseBodySize);
 }
 
-void headSerialError(uint8_t responseBodySize)
+static void headSerialError(uint8_t responseBodySize)
 {
     headSerialResponse(1, responseBodySize);
 }
 
-void tailSerialReply(void)
+static void tailSerialReply(void)
 {
     serialize8(currentPort->checksum);
 }
 
-void s_struct(uint8_t *cb, uint8_t siz)
+static void s_struct(uint8_t *cb, uint8_t siz)
 {
     headSerialReply(siz);
     while (siz--)
         serialize8(*cb++);
 }
 
-void serializeNames(const char *s)
+static void serializeNames(const char *s)
 {
     const char *c;
     for (c = s; *c; c++)
         serialize8(*c);
 }
 
-const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
+static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
 {
     uint8_t boxIndex;
     const box_t *candidate;
@@ -478,7 +486,7 @@ const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
     return NULL;
 }
 
-const box_t *findBoxByPermenantId(uint8_t permenantId)
+static const box_t *findBoxByPermenantId(uint8_t permenantId)
 {
     uint8_t boxIndex;
     const box_t *candidate;
@@ -491,7 +499,7 @@ const box_t *findBoxByPermenantId(uint8_t permenantId)
     return NULL;
 }
 
-void serializeBoxNamesReply(void)
+static void serializeBoxNamesReply(void)
 {
     int i, activeBoxId, j, flag = 1, count = 0, len;
     const box_t *box;
@@ -523,7 +531,7 @@ reset:
     }
 }
 
-void serializeDataflashSummaryReply(void)
+static void serializeDataflashSummaryReply(void)
 {
 #ifdef FLASHFS
     const flashGeometry_t *geometry = flashfsGetGeometry();
@@ -538,7 +546,7 @@ void serializeDataflashSummaryReply(void)
 }
 
 #ifdef FLASHFS
-void serializeDataflashReadReply(uint32_t address, uint8_t size)
+static void serializeDataflashReadReply(uint32_t address, uint8_t size)
 {
     enum { DATAFLASH_READ_REPLY_CHUNK_SIZE = 128 };
 
@@ -695,7 +703,6 @@ static bool processOutCommand(uint8_t cmdMSP)
 {
     uint32_t i, tmp, junk;
 
-
 #ifdef GPS
     uint8_t wp_no;
     int32_t lat = 0, lon = 0;
@@ -864,18 +871,30 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
     case MSP_ALTITUDE:
         headSerialReply(6);
-        serialize32(EstAlt);
+#if defined(BARO) || defined(SONAR)
+        serialize32(altitudeHoldGetEstimatedAltitude());
+#else
+        serialize32(0);
+#endif
         serialize16(vario);
         break;
+    case MSP_SONAR_ALTITUDE:
+        headSerialReply(4);
+#if defined(SONAR)
+        serialize32(sonarGetLatestAltitude());
+#else
+        serialize32(0);
+#endif
+        break;
     case MSP_ANALOG:
         headSerialReply(7);
         serialize8((uint8_t)constrain(vbat, 0, 255));
         serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery
         serialize16(rssi);
         if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
-            serialize16((uint16_t)constrain((ABS(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
+            serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
         } else
-            serialize16((uint16_t)constrain(ABS(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
+            serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
         break;
     case MSP_RC_TUNING:
         headSerialReply(7);
@@ -918,6 +937,10 @@ static bool processOutCommand(uint8_t cmdMSP)
         headSerialReply(sizeof(pidnames) - 1);
         serializeNames(pidnames);
         break;
+    case MSP_PID_CONTROLLER:
+        headSerialReply(1);
+        serialize8(currentProfile->pidController);
+        break;
     case MSP_MODE_RANGES:
         headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
         for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
@@ -1240,6 +1263,10 @@ static bool processInCommand(void)
         currentProfile->accelerometerTrims.values.pitch = read16();
         currentProfile->accelerometerTrims.values.roll  = read16();
         break;
+    case MSP_SET_PID_CONTROLLER:
+        currentProfile->pidController = read8();
+        setPIDController(currentProfile->pidController);
+        break;
     case MSP_SET_PID:
         if (currentProfile->pidController == 2) {
             for (i = 0; i < 3; i++) {

src/main/io/serial_msp.h

@@ -17,9 +17,14 @@
 
 #pragma once
 
+#include "io/serial.h"
+#include "drivers/serial.h"
+
 // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
 #define MAX_MSP_PORT_COUNT 2
 
+void mspInit(serialConfig_t *serialConfig);
+
 void mspProcess(void);
 void sendMspTelemetry(void);
 void mspSetTelemetryPort(serialPort_t *mspTelemetryPort);

src/main/main.c

@@ -333,7 +333,7 @@ void init(void)
 
 #ifdef SONAR
     if (feature(FEATURE_SONAR)) {
-        Sonar_init();
+        sonarInit();
     }
 #endif
 

src/main/mw.c

@@ -469,7 +469,7 @@ void executePeriodicTasks(void)
 #ifdef SONAR
     case UPDATE_SONAR_TASK:
         if (sensors(SENSOR_SONAR)) {
-            Sonar_update();
+            sonarUpdate();
         }
         break;
 #endif

src/main/sensors/sonar.c

@@ -33,11 +33,13 @@
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
 
-int32_t sonarAlt = -1;                  // in cm , -1 indicate sonar is not in range - inclination adjusted by imu
+// in cm , -1 indicate sonar is not in range - inclination adjusted by imu
 
 #ifdef SONAR
 
-void Sonar_init(void)
+static int32_t calculatedAltitude;
+
+void sonarInit(void)
 {
 #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
     static const sonarHardware_t const sonarPWM56 = {
@@ -74,25 +76,33 @@ void Sonar_init(void)
 #endif
 
     sensorsSet(SENSOR_SONAR);
-    sonarAlt = 0;
+    calculatedAltitude = -1;
 }
 
-void Sonar_update(void)
+void sonarUpdate(void)
 {
     hcsr04_start_reading();
 }
 
+int32_t sonarRead(void)
+{
+    return hcsr04_get_distance();
+}
+
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
 {
     // calculate sonar altitude only if the sonar is facing downwards(<25deg)
     if (tiltAngle > 250)
-        return -1;
+        calculatedAltitude = -1;
+    else
+        calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
 
-    return sonarAlt * (900.0f - tiltAngle) / 900.0f;
+    return calculatedAltitude;
 }
 
-int32_t sonarRead(void) {
+int32_t sonarGetLatestAltitude(void)
-    return hcsr04_get_distance();
+{
+    return calculatedAltitude;
 }
 
 #endif

src/main/sensors/sonar.h

@@ -17,10 +17,9 @@
 
 #pragma once
 
-extern int32_t sonarAlt;
+void sonarInit(void);
-
+void sonarUpdate(void);
-void Sonar_init(void);
-void Sonar_update(void);
 
 int32_t sonarRead(void);
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
+int32_t sonarGetLatestAltitude(void);

src/main/version.h

@@ -16,7 +16,7 @@
  */
 
 #define FC_VERSION_MAJOR            1  // increment when a major release is made (big new feature, etc)
-#define FC_VERSION_MINOR            6  // increment when a minor release is made (small new feature, change etc)
+#define FC_VERSION_MINOR            7  // 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 MW_VERSION              231