Msp

Makefile

@@ -501,9 +501,6 @@ $(CONFIGS_CLEAN):
 ## clean_all         : clean all targets
 clean_all: $(TARGETS_CLEAN) test_clean
 
-## configs           : Hydrate configuration
-configs: configs
-
 TARGETS_FLASH = $(addsuffix _flash,$(BASE_TARGETS))
 
 ## <TARGET>_flash    : build and flash a target

src/main/build/version.h

@@ -26,7 +26,7 @@
 #define FC_FIRMWARE_IDENTIFIER      "BTFL"
 #define FC_VERSION_MAJOR            4  // increment when a major release is made (big new feature, etc)
 #define FC_VERSION_MINOR            5  // 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 FC_VERSION_PATCH_LEVEL      2  // increment when a bug is fixed
 
 #define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL)
 

src/main/cli/cli.c

@@ -3926,7 +3926,7 @@ static void cliEscPassthrough(const char *cmdName, char *cmdline)
             } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) {
                 mode = PROTOCOL_KISS;
             } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) {
-                mode = PROTOCOL_KISSALL;
+                mode = PROTOCOL_CASTLE;
             } else {
                 cliShowParseError(cmdName);
 

src/main/common/streambuf.c

@@ -114,6 +114,7 @@ uint32_t sbufReadU32(sbuf_t *src)
 void sbufReadData(sbuf_t *src, void *data, int len)
 {
     memcpy(data, src->ptr, len);
+    src->ptr += len;
 }
 
 // reader - return bytes remaining in buffer

src/main/drivers/accgyro/accgyro_spi_lsm6dsv16x.c

@@ -860,23 +860,6 @@ uint8_t lsm6dsv16xSpiDetect(const extDevice_t *dev)
 
 void lsm6dsv16xAccInit(accDev_t *acc)
 {
-    const extDevice_t *dev = &acc->gyro->dev;
-
-    // Enable the accelerometer in high accuracy mode at 1kHz
-    spiWriteReg(dev, LSM6DSV_CTRL1,
-                LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_OP_MODE_XL_HIGH_ACCURACY,
-                                    LSM6DSV_CTRL1_OP_MODE_XL_MASK,
-                                    LSM6DSV_CTRL1_OP_MODE_XL_SHIFT) |
-                LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_ODR_XL_1000HZ,
-                                    LSM6DSV_CTRL1_ODR_XL_MASK,
-                                    LSM6DSV_CTRL1_ODR_XL_SHIFT));
-
-    // Enable 16G sensitivity
-    spiWriteReg(dev, LSM6DSV_CTRL8,
-                LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL8_FS_XL_16G,
-                                    LSM6DSV_CTRL8_FS_XL_MASK,
-                                    LSM6DSV_CTRL8_FS_XL_SHIFT));
-
     // ±16G mode
     acc->acc_1G = 512 * 4;
 }
@@ -961,22 +944,50 @@ void lsm6dsv16xGyroInit(gyroDev_t *gyro)
     // Perform a software reset
     spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_SW_RESET);
 
-    // Select high-accuracy ODR mode 1 before leaving power-off mode
+    // Wait for the device to be ready
+    while (spiReadRegMsk(dev, LSM6DSV_CTRL3) & LSM6DSV_CTRL3_SW_RESET) {}
+
+    // Autoincrement register address when doing block SPI reads and update continuously
+    spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_IF_INC | LSM6DSV_CTRL3_BDU);      /*BDU bit need to be set*/
+
+    // Select high-accuracy ODR mode 1
     spiWriteReg(dev, LSM6DSV_HAODR_CFG,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_HAODR_MODE1,
                                     LSM6DSV_HAODR_CFG_HAODR_SEL_MASK,
                                     LSM6DSV_HAODR_CFG_HAODR_SEL_SHIFT));
 
-    // Enable the gyro in high accuracy mode at 8kHz
+    // Enable the accelerometer in high accuracy
+    spiWriteReg(dev, LSM6DSV_CTRL1,
+                LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_OP_MODE_XL_HIGH_ACCURACY,
+                                    LSM6DSV_CTRL1_OP_MODE_XL_MASK,
+                                    LSM6DSV_CTRL1_OP_MODE_XL_SHIFT));
+
+    // Enable the gyro in high accuracy
     spiWriteReg(dev, LSM6DSV_CTRL2,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_OP_MODE_G_HIGH_ACCURACY,
                                     LSM6DSV_CTRL2_OP_MODE_G_MASK,
-                                    LSM6DSV_CTRL2_OP_MODE_G_SHIFT) |
+                                    LSM6DSV_CTRL2_OP_MODE_G_SHIFT));
+
+    // Enable 16G sensitivity
+    spiWriteReg(dev, LSM6DSV_CTRL8,
+                LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL8_FS_XL_16G,
+                                    LSM6DSV_CTRL8_FS_XL_MASK,
+                                    LSM6DSV_CTRL8_FS_XL_SHIFT));
+
+    // Enable the accelerometer odr at 1kHz
+    spiWriteReg(dev, LSM6DSV_CTRL1,
+                LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_ODR_XL_1000HZ,
+                                    LSM6DSV_CTRL1_ODR_XL_MASK,
+                                    LSM6DSV_CTRL1_ODR_XL_SHIFT));
+
+    // Enable the gyro odr at 8kHz
+    spiWriteReg(dev, LSM6DSV_CTRL2,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_ODR_G_8000HZ,
                                     LSM6DSV_CTRL2_ODR_G_MASK,
                                     LSM6DSV_CTRL2_ODR_G_SHIFT));
 
     // Enable 2000 deg/s sensitivity and selected LPF1 filter setting
+    // Set the LPF1 filter bandwidth
     spiWriteReg(dev, LSM6DSV_CTRL6,
                 LSM6DSV_ENCODE_BITS(lsm6dsv16xLPF1BandwidthOptions[gyroConfig()->gyro_hardware_lpf],
                                     LSM6DSV_CTRL6_LPF1_G_BW_MASK,
@@ -985,8 +996,8 @@ void lsm6dsv16xGyroInit(gyroDev_t *gyro)
                                     LSM6DSV_CTRL6_FS_G_MASK,
                                     LSM6DSV_CTRL6_FS_G_SHIFT));
 
-    // Autoincrement register address when doing block SPI reads and update continuously
-    spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_IF_INC);
+    // Enable the gyro digital LPF1 filter
+    spiWriteReg(dev, LSM6DSV_CTRL7, LSM6DSV_CTRL7_LPF1_G_EN);
 
     // Generate pulse on interrupt line, not requiring a read to clear
     spiWriteReg(dev, LSM6DSV_CTRL4, LSM6DSV_CTRL4_DRDY_PULSED);
@@ -1037,7 +1048,7 @@ bool lsm6dsv16xGyroReadSPI(gyroDev_t *gyro)
     case GYRO_EXTI_INT:
     case GYRO_EXTI_NO_INT:
     {
-        gyro->dev.txBuf[0] = gyro->gyroDataReg | 0x80;
+        gyro->dev.txBuf[0] = LSM6DSV_OUTX_L_G | 0x80;
 
         busSegment_t segments[] = {
                 {.u.buffers = {NULL, NULL}, 7, true, NULL},

src/main/drivers/at32/bus_i2c_atbsp.c

@@ -245,7 +245,8 @@ bool i2cBusy(I2CDevice device, bool *error)
         *error = pHandle->error_code;
     }
 
-    if (pHandle->error_code == I2C_OK) {
+    // I2C_ERR_ACKFAIL indicates that the last access wasn't acknowledged, but doesn't mean the bus is busy
+    if ((pHandle->error_code == I2C_OK) || (pHandle->error_code == I2C_ERR_ACKFAIL)) {
         if (i2c_flag_get(pHandle->i2cx, I2C_BUSYF_FLAG) == SET) {
             return true;
         }

src/main/drivers/at32/bus_i2c_atbsp_init.c

@@ -49,10 +49,12 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
         .sclPins = {
             I2CPINDEF(PB6, GPIO_MUX_4),
             I2CPINDEF(PB8, GPIO_MUX_4),
+            I2CPINDEF(PC6, GPIO_MUX_4),
         },
         .sdaPins = {
             I2CPINDEF(PB7, GPIO_MUX_4),
             I2CPINDEF(PB9, GPIO_MUX_4),
+            I2CPINDEF(PC7, GPIO_MUX_4),
         },
         .rcc = RCC_APB1(I2C1),
         .ev_irq = I2C1_EVT_IRQn,

src/main/drivers/at32/serial_uart_at32f43x.c

@@ -391,7 +391,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
             GPIO_MODE_MUX,
             GPIO_DRIVE_STRENGTH_STRONGER,
             ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_OUTPUT_PUSH_PULL : GPIO_OUTPUT_OPEN_DRAIN,
-            (options & SERIAL_INVERTED) ? GPIO_PULL_DOWN : GPIO_PULL_UP
+            ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_PULL_DOWN : GPIO_PULL_UP
         );
         IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
         IOConfigGPIOAF(txIO, ioCfg, uartdev->tx.af);

src/main/drivers/max7456.c

@@ -512,7 +512,7 @@ void max7456Write(uint8_t x, uint8_t y, const char *text)
 
 bool max7456LayerSupported(displayPortLayer_e layer)
 {
-    if (layer == DISPLAYPORT_LAYER_FOREGROUND || layer == DISPLAYPORT_LAYER_BACKGROUND) {
+    if (layer == DISPLAYPORT_LAYER_FOREGROUND) {
         return true;
     } else {
         return false;

src/main/drivers/motor.c

@@ -63,7 +63,7 @@ void motorWriteAll(float *values)
 #ifdef USE_PWM_OUTPUT
     if (motorDevice->enabled) {
 #ifdef USE_DSHOT_BITBANG
-        if (useDshotTelemetry && isDshotBitbangActive(&motorConfig()->dev)) {
+        if (isDshotBitbangActive(&motorConfig()->dev)) {
             // Initialise the output buffers
             if (motorDevice->vTable.updateInit) {
                 motorDevice->vTable.updateInit();

src/main/drivers/serial_escserial.c

@@ -661,10 +661,13 @@ static serialPort_t *openEscSerial(const motorDevConfig_t *motorConfig, escSeria
 {
     escSerial_t *escSerial = &(escSerialPorts[portIndex]);
 
-    if (escSerialConfig()->ioTag == IO_TAG_NONE) {
-        return NULL;
-    }
+
     if (mode != PROTOCOL_KISSALL) {
+
+    	if (escSerialConfig()->ioTag == IO_TAG_NONE) {
+    	    return NULL;
+    	}
+
         const ioTag_t tag = motorConfig->ioTags[output];
         const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, 0);
 
@@ -684,22 +687,22 @@ static serialPort_t *openEscSerial(const motorDevConfig_t *motorConfig, escSeria
         // Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below
         // this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first.
         timerConfigure(escSerial->rxTimerHardware, 0xffff, 1);
-    }
 
-    escSerial->mode = mode;
-    escSerial->txTimerHardware = timerAllocate(escSerialConfig()->ioTag, OWNER_MOTOR, 0);
-    if (escSerial->txTimerHardware == NULL) {
-        return NULL;
-    }
+        escSerial->txTimerHardware = timerAllocate(escSerialConfig()->ioTag, OWNER_MOTOR, 0);
+        if (escSerial->txTimerHardware == NULL) {
+            return NULL;
+        }
 
 #ifdef USE_HAL_DRIVER
-    escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim);
+        escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim);
 #endif
 
-    // Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below
-    // this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first.
-    timerConfigure(escSerial->txTimerHardware, 0xffff, 1);
+    	// Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below
+    	// this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first.
+    	timerConfigure(escSerial->txTimerHardware, 0xffff, 1);
+    }
 
+    escSerial->mode = mode;
     escSerial->port.vTable = escSerialVTable;
     escSerial->port.baudRate = baud;
     escSerial->port.mode = MODE_RXTX;
@@ -752,11 +755,15 @@ static serialPort_t *openEscSerial(const motorDevConfig_t *motorConfig, escSeria
                 if (tag != IO_TAG_NONE) {
                     const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, 0);
                     if (timerHardware) {
+                        // Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below
+                        // this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first.
+                        timerConfigure(timerHardware, 0xffff, 1);
                         escSerialOutputPortConfig(timerHardware);
                         escOutputs[escSerial->outputCount].io = pwmMotors[i].io;
                         if (timerHardware->output & TIMER_OUTPUT_INVERTED) {
                             escOutputs[escSerial->outputCount].inverted = 1;
                         }
+                        escSerial->txTimerHardware = timerHardware;
                         escSerial->outputCount++;
                     }
                 }
@@ -1011,7 +1018,7 @@ bool escEnablePassthrough(serialPort_t *escPassthroughPort, const motorDevConfig
                     closeEscSerial(ESCSERIAL1, mode);
                     return true;
                 }
-                if (mode==PROTOCOL_BLHELI) {
+                if (mode==PROTOCOL_BLHELI || mode==PROTOCOL_KISS || mode==PROTOCOL_KISSALL) {
                     serialWrite(escPassthroughPort, ch); // blheli loopback
                 }
                 serialWrite(escPort, ch);

src/main/drivers/stm32/serial_uart_stm32f4xx.c

@@ -337,7 +337,9 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
 
     if (options & SERIAL_BIDIR) {
         IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
-        IOConfigGPIOAF(txIO, ((options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? IOCFG_AF_PP : IOCFG_AF_OD, hardware->af);
+        IOConfigGPIOAF(txIO, (options & SERIAL_BIDIR_PP_PD) ? IOCFG_AF_PP_PD 
+                             : (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP
+                             : IOCFG_AF_OD, hardware->af);
     } else {
         if ((mode & MODE_TX) && txIO) {
             IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));

src/main/drivers/stm32/serial_uart_stm32g4xx.c

@@ -300,7 +300,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
         ioConfig_t ioCfg = IO_CONFIG(
             ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_MODE_AF_PP : GPIO_MODE_AF_OD,
             GPIO_SPEED_FREQ_HIGH,
-            (options & SERIAL_INVERTED) ? GPIO_PULLDOWN : GPIO_PULLUP
+            ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_PULLDOWN : GPIO_PULLUP
         );
 
         IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));

src/main/drivers/stm32/usbd_msc_desc.c

@@ -33,9 +33,15 @@
 #include "usb_regs.h"
 #include "platform.h"
 
+#if defined (STM32F4)
+#define         DEVICE_ID1          (0x1FFF7A10)
+#define         DEVICE_ID2          (0x1FFF7A14)
+#define         DEVICE_ID3          (0x1FFF7A18)
+#else
 #define         DEVICE_ID1          (0x1FFFF7E8)
 #define         DEVICE_ID2          (0x1FFFF7EA)
 #define         DEVICE_ID3          (0x1FFFF7EC)
+#endif
 #define  USB_SIZ_STRING_SERIAL       0x1A
 
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY

src/main/fc/core.c

@@ -371,7 +371,7 @@ void updateArmingStatus(void)
 #endif
 
 #ifdef USE_DSHOT_BITBANG
-        if (useDshotTelemetry && isDshotBitbangActive(&motorConfig()->dev) && dshotBitbangGetStatus() != DSHOT_BITBANG_STATUS_OK) {
+        if (isDshotBitbangActive(&motorConfig()->dev) && dshotBitbangGetStatus() != DSHOT_BITBANG_STATUS_OK) {
             setArmingDisabled(ARMING_DISABLED_DSHOT_BITBANG);
         } else {
             unsetArmingDisabled(ARMING_DISABLED_DSHOT_BITBANG);

src/main/flight/pid.c

@@ -376,15 +376,17 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
     // We now use Acro Rates, transformed into the range +/- 1, to provide setpoints
     const float angleLimit = pidProfile->angle_limit;
     float angleFeedforward = 0.0f;
+    // if user changes rates profile, update the max setpoint for angle mode
+    const float maxSetpointRateInv = 1.0f / getMaxRcRate(axis);
 
 #ifdef USE_FEEDFORWARD
-    angleFeedforward = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * pidRuntime.maxRcRateInv[axis];
+    angleFeedforward = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * maxSetpointRateInv;
     //  angle feedforward must be heavily filtered, at the PID loop rate, with limited user control over time constant
     // it MUST be very delayed to avoid early overshoot and being too aggressive
     angleFeedforward = pt3FilterApply(&pidRuntime.angleFeedforwardPt3[axis], angleFeedforward);
 #endif
 
-    float angleTarget = angleLimit * currentPidSetpoint * pidRuntime.maxRcRateInv[axis];
+    float angleTarget = angleLimit * currentPidSetpoint * maxSetpointRateInv;
     // use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate
 
 #ifdef USE_GPS_RESCUE

src/main/flight/pid.h

@@ -421,7 +421,6 @@ typedef struct pidRuntime_s {
     float angleEarthRef;
     float angleTarget[2];
     bool axisInAngleMode[3];
-    float maxRcRateInv[2];
 #endif
 } pidRuntime_t;
 

src/main/flight/pid_init.c

@@ -251,7 +251,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     for (int axis = 0; axis < 2; axis++) {  // ROLL and PITCH only
         pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
         pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2);
-        pidRuntime.maxRcRateInv[axis] = 1.0f / getMaxRcRate(axis);
     }
     pidRuntime.angleYawSetpoint = 0.0f;
 #endif

src/main/io/gps.c

@@ -1325,6 +1325,16 @@ static void updateGpsIndicator(timeUs_t currentTimeUs)
     }
 }
 
+static void calculateNavInterval (void)
+{
+    // calculate the interval between nav packets, handling iTow wraparound at the end of the week
+    const uint32_t weekDurationMs = 7 * 24 * 3600 * 1000;
+    const uint32_t navDeltaTimeMs = (weekDurationMs + gpsSol.time - gpsData.lastNavSolTs) % weekDurationMs;
+    gpsData.lastNavSolTs = gpsSol.time;
+    // constrain the interval between 50ms / 20hz or 2.5s, when we would get a connection failure anyway
+    gpsSol.navIntervalMs = constrain(navDeltaTimeMs, 50, 2500);
+}
+
 void gpsUpdate(timeUs_t currentTimeUs)
 {
     static timeDelta_t gpsStateDurationFractionUs[GPS_STATE_COUNT];
@@ -1368,7 +1378,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
             sensorsSet(SENSOR_GPS);
 
             GPS_update ^= GPS_DIRECT_TICK;
-
+            calculateNavInterval();
             onGpsNewData();
 
             GPS_update &= ~GPS_MSP_UPDATE;
@@ -2097,16 +2107,6 @@ typedef enum {
 static ubxFrameParseState_e ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1;
 static uint16_t ubxFrameParsePayloadCounter;
 
-static void calculateNavInterval (void)
-{
-    // calculate the interval between nav packets, handling iTow wraparound at the end of the week
-    const uint32_t weekDurationMs = 7 * 24 * 3600 * 1000;
-    const uint32_t navDeltaTimeMs = (weekDurationMs + gpsSol.time - gpsData.lastNavSolTs) % weekDurationMs;
-    gpsData.lastNavSolTs = gpsSol.time;
-    // constrain the interval between 50ms / 20hz or 2.5s, when we would get a connection failure anyway
-    gpsSol.navIntervalMs = constrain(navDeltaTimeMs, 50, 2500);
-}
-
 // SCEDEBUG To help debug which message is slow to process
 // static uint8_t lastUbxRcvMsgClass;
 // static uint8_t lastUbxRcvMsgID;