From cd84e10fa5e75494d8a29330ed488fd32c9810f4 Mon Sep 17 00:00:00 2001 From: Jay Blackman Date: Thu, 30 Jan 2025 00:49:11 +1100 Subject: [PATCH 01/58] FIX: Serial ESC communication when using digital protocols (#14214) * FIX: Serial ESC communication when using digital protocols * Fix error following rebase, and amending based on comments from @ledvinap --- src/main/drivers/motor.c | 9 +++++++++ src/main/drivers/motor.h | 2 ++ src/main/drivers/motor_types.h | 2 ++ src/main/drivers/pwm_output.c | 19 +++++++++++++++++ src/main/drivers/pwm_output_impl.h | 28 ++++++++++++++++++++++++++ src/main/io/serial_4way.c | 24 +++++++++++----------- src/platform/APM32/dshot_bitbang.c | 9 +++++++++ src/platform/APM32/pwm_output_apm32.c | 12 ++--------- src/platform/AT32/pwm_output_at32bsp.c | 14 ++----------- src/platform/SIMULATOR/sitl.c | 21 ++++--------------- src/platform/STM32/pwm_output_hw.c | 13 ++---------- src/platform/common/stm32/dshot_dpwm.c | 9 +++++++++ 12 files changed, 100 insertions(+), 62 deletions(-) create mode 100644 src/main/drivers/pwm_output_impl.h diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index 124554928e..22f2415a11 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -362,6 +362,14 @@ timeMs_t motorGetMotorEnableTimeMs(void) } #endif +IO_t motorGetIo(unsigned index) +{ + if (index >= motorDevice.count) { + return IO_NONE; + } + return motorDevice.vTable->getMotorIO ? motorDevice.vTable->getMotorIO(index) : IO_NONE; +} + /* functions below for empty methods and no active motors */ void motorPostInitNull(void) { @@ -433,6 +441,7 @@ static const motorVTable_t motorNullVTable = { .shutdown = motorShutdownNull, .requestTelemetry = NULL, .isMotorIdle = NULL, + .getMotorIO = NULL, }; void motorNullDevInit(motorDevice_t *device) diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index d43f430446..c4a39877f6 100644 --- a/src/main/drivers/motor.h +++ b/src/main/drivers/motor.h @@ -54,5 +54,7 @@ float motorEstimateMaxRpm(void); bool motorIsEnabled(void); bool motorIsMotorEnabled(unsigned index); bool motorIsMotorIdle(unsigned index); +IO_t motorGetIo(unsigned index); + timeMs_t motorGetMotorEnableTimeMs(void); void motorShutdown(void); // Replaces stopPwmAllMotors diff --git a/src/main/drivers/motor_types.h b/src/main/drivers/motor_types.h index c70fcee0b0..f3a88eb7c1 100644 --- a/src/main/drivers/motor_types.h +++ b/src/main/drivers/motor_types.h @@ -22,6 +22,7 @@ #pragma once #include "common/time.h" +#include "drivers/io_types.h" #define ALL_MOTORS 255 #define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1 @@ -64,6 +65,7 @@ typedef struct motorVTable_s { void (*updateComplete)(void); void (*shutdown)(void); bool (*isMotorIdle)(unsigned index); + IO_t (*getMotorIO)(unsigned index); // Digital commands void (*requestTelemetry)(unsigned index); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 813b97e856..e8ae90887b 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -48,4 +48,23 @@ void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, fl } } +IO_t pwmGetMotorIO(unsigned index) +{ + if (index >= pwmMotorCount) { + return IO_NONE; + } + return motors[index].io; +} + +bool pwmIsMotorEnabled(unsigned index) +{ + return motors[index].enabled; +} + +bool pwmEnableMotors(void) +{ + /* check motors can be enabled */ + return pwmMotorCount > 0; +} + #endif diff --git a/src/main/drivers/pwm_output_impl.h b/src/main/drivers/pwm_output_impl.h new file mode 100644 index 0000000000..403bf4782e --- /dev/null +++ b/src/main/drivers/pwm_output_impl.h @@ -0,0 +1,28 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software 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. + * + * Betaflight 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 this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/io_types.h" + +IO_t pwmGetMotorIO(unsigned index); +bool pwmIsMotorEnabled(unsigned index); +bool pwmEnableMotors(void); diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 552fc5d58c..644e37cbb0 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -36,7 +36,7 @@ #include "drivers/time.h" #include "drivers/timer.h" #include "drivers/light_led.h" -#include "drivers/pwm_output.h" +#include "drivers/motor.h" #include "flight/mixer.h" #include "io/beeper.h" @@ -143,23 +143,23 @@ inline void setEscOutput(uint8_t selEsc) uint8_t esc4wayInit(void) { - // StopPwmAllMotors(); - // XXX Review effect of motor refactor - //pwmDisableMotors(); - escCount = 0; + motorShutdown(); + uint8_t escIndex = 0; + memset(&escHardware, 0, sizeof(escHardware)); - pwmOutputPort_t *pwmMotors = pwmGetMotors(); for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - if (pwmMotors[i].enabled) { - if (pwmMotors[i].io != IO_NONE) { - escHardware[escCount].io = pwmMotors[i].io; - setEscInput(escCount); - setEscHi(escCount); - escCount++; + if (motorIsMotorEnabled(i)) { + const IO_t io = motorGetIo(i); + if (io != IO_NONE) { + escHardware[escIndex].io = io; + setEscInput(escIndex); + setEscHi(escIndex); + escIndex++; } } } motorDisable(); + escCount = escIndex; return escCount; } diff --git a/src/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c index 5776305d62..33b114fbc0 100644 --- a/src/platform/APM32/dshot_bitbang.c +++ b/src/platform/APM32/dshot_bitbang.c @@ -669,6 +669,14 @@ static void bbPostInit(void) } } +static IO_t bbGetMotorIO(unsigned index) +{ + if (index >= dshotMotorCount) { + return IO_NONE; + } + return bbMotors[index].io; +} + static motorVTable_t bbVTable = { .postInit = bbPostInit, .enable = bbEnableMotors, @@ -685,6 +693,7 @@ static motorVTable_t bbVTable = { .shutdown = bbShutdown, .isMotorIdle = bbDshotIsMotorIdle, .requestTelemetry = bbDshotRequestTelemetry, + .getMotorIO = bbGetMotorIO, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) diff --git a/src/platform/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c index d3de5ba53a..706591e0a0 100644 --- a/src/platform/APM32/pwm_output_apm32.c +++ b/src/platform/APM32/pwm_output_apm32.c @@ -31,6 +31,7 @@ #include "drivers/io.h" #include "drivers/motor.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -108,16 +109,6 @@ static void pwmDisableMotors(void) } static motorVTable_t motorPwmVTable; -static bool pwmEnableMotors(void) -{ - /* check motors can be enabled */ - return pwmMotorCount > 0; -} - -static bool pwmIsMotorEnabled(unsigned index) -{ - return motors[index].enabled; -} static void pwmCompleteMotorUpdate(void) { @@ -158,6 +149,7 @@ static motorVTable_t motorPwmVTable = { .updateComplete = pwmCompleteMotorUpdate, .requestTelemetry = NULL, .isMotorIdle = NULL, + .getMotorIO = pwmGetMotorIO, }; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) diff --git a/src/platform/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c index 8ebc593340..e04b39fc82 100644 --- a/src/platform/AT32/pwm_output_at32bsp.c +++ b/src/platform/AT32/pwm_output_at32bsp.c @@ -31,6 +31,7 @@ #include "drivers/io.h" #include "drivers/motor_impl.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -98,18 +99,6 @@ static void pwmDisableMotors(void) pwmShutdownPulsesForAllMotors(); } -static motorVTable_t motorPwmVTable; -static bool pwmEnableMotors(void) -{ - /* check motors can be enabled */ - return (pwmMotorDevice->vTable); -} - -static bool pwmIsMotorEnabled(unsigned index) -{ - return motors[index].enabled; -} - static bool useContinuousUpdate = true; static void pwmCompleteMotorUpdate(void) @@ -151,6 +140,7 @@ static motorVTable_t motorPwmVTable = { .updateComplete = pwmCompleteMotorUpdate, .requestTelemetry = NULL, .isMotorIdle = NULL, + .getMotorIO = pwmGetMotorIO, }; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index bd52fe2564..aa58bc45a8 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -41,6 +41,7 @@ #include "drivers/system.h" #include "drivers/time.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" #include "drivers/light_led.h" #include "drivers/timer.h" @@ -543,8 +544,6 @@ void servoDevInit(const servoDevConfig_t *servoConfig) } } -static motorDevice_t pwmMotorDevice; // Forward - pwmOutputPort_t *pwmGetMotors(void) { return motors; @@ -562,14 +561,7 @@ static uint16_t pwmConvertToExternal(float motorValue) static void pwmDisableMotors(void) { - pwmMotorDevice.enabled = false; -} - -static bool pwmEnableMotors(void) -{ - pwmMotorDevice.enabled = true; - - return true; + // NOOP } static void pwmWriteMotor(uint8_t index, float value) @@ -594,12 +586,7 @@ static void pwmWriteMotorInt(uint8_t index, uint16_t value) static void pwmShutdownPulsesForAllMotors(void) { - pwmMotorDevice.enabled = false; -} - -static bool pwmIsMotorEnabled(unsigned index) -{ - return motors[index].enabled; + // NOOP } static void pwmCompleteMotorUpdate(void) @@ -647,7 +634,7 @@ static const motorVTable_t vTable = { .shutdown = pwmShutdownPulsesForAllMotors, .requestTelemetry = NULL, .isMotorIdle = NULL, - + .getMotorIO = NULL, }; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t _idlePulse) diff --git a/src/platform/STM32/pwm_output_hw.c b/src/platform/STM32/pwm_output_hw.c index 99a2a3ce69..aed9bda188 100644 --- a/src/platform/STM32/pwm_output_hw.c +++ b/src/platform/STM32/pwm_output_hw.c @@ -29,6 +29,7 @@ #include "drivers/io.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -128,17 +129,6 @@ static void pwmDisableMotors(void) pwmShutdownPulsesForAllMotors(); } -static bool pwmEnableMotors(void) -{ - /* check motors can be enabled */ - return pwmMotorCount > 0; -} - -static bool pwmIsMotorEnabled(unsigned index) -{ - return motors[index].enabled; -} - static void pwmCompleteMotorUpdate(void) { if (useContinuousUpdate) { @@ -178,6 +168,7 @@ static const motorVTable_t motorPwmVTable = { .updateComplete = pwmCompleteMotorUpdate, .requestTelemetry = NULL, .isMotorIdle = NULL, + .getMotorIO = pwmGetMotorIO, }; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) diff --git a/src/platform/common/stm32/dshot_dpwm.c b/src/platform/common/stm32/dshot_dpwm.c index cee7aa6003..29c099396e 100644 --- a/src/platform/common/stm32/dshot_dpwm.c +++ b/src/platform/common/stm32/dshot_dpwm.c @@ -126,6 +126,14 @@ static bool dshotPwmIsMotorEnabled(unsigned index) return motors[index].enabled; } +static IO_t pwmDshotGetMotorIO(unsigned index) +{ + if (index >= dshotMotorCount) { + return IO_NONE; + } + return motors[index].io; +} + static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value) { pwmWriteDshotInt(index, value); @@ -150,6 +158,7 @@ static const motorVTable_t dshotPwmVTable = { .shutdown = dshotPwmShutdown, .requestTelemetry = pwmDshotRequestTelemetry, .isMotorIdle = pwmDshotIsMotorIdle, + .getMotorIO = pwmDshotGetMotorIO, }; bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) From bfea69a04f80ff9766decd6c7fbbf4e15fbc1832 Mon Sep 17 00:00:00 2001 From: blckmn Date: Thu, 30 Jan 2025 20:33:37 +1100 Subject: [PATCH 02/58] STM32H5: adding MCU_TYPE --- src/platform/common/stm32/system.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platform/common/stm32/system.c b/src/platform/common/stm32/system.c index aa663e2950..a2424f20a8 100644 --- a/src/platform/common/stm32/system.c +++ b/src/platform/common/stm32/system.c @@ -346,6 +346,8 @@ const mcuTypeInfo_t *getMcuTypeInfo(void) { .id = MCU_TYPE_F746, .name = "STM32F746" }, #elif defined(STM32F765xx) { .id = MCU_TYPE_F765, .name = "STM32F765" }, +#elif defined(STM32H563xx) + { .id = MCU_TYPE_H563, .name = "STM32H563" }, #elif defined(STM32H750xx) { .id = MCU_TYPE_H750, .name = "STM32H750" }, #elif defined(STM32H730xx) From 3dba5e65e4ab237e328b021b69df80d3109d2315 Mon Sep 17 00:00:00 2001 From: Jay Blackman Date: Fri, 31 Jan 2025 06:54:23 +1100 Subject: [PATCH 03/58] FIX: AT32 not reading ESC (#14220) --- src/main/drivers/pwm_output.c | 6 ++-- src/main/drivers/pwm_output.h | 2 +- src/platform/APM32/dshot_bitbang.c | 8 ----- src/platform/APM32/pwm_output_apm32.c | 34 +++++++++---------- src/platform/AT32/dshot_bitbang.c | 3 +- src/platform/AT32/io_at32.c | 12 +------ src/platform/AT32/pwm_output_at32bsp.c | 34 +++++++++---------- src/platform/SIMULATOR/sitl.c | 4 +-- src/platform/STM32/dshot_bitbang.c | 1 + src/platform/STM32/pwm_output_hw.c | 34 +++++++++---------- .../common/stm32/dshot_bitbang_impl.h | 1 + .../common/stm32/dshot_bitbang_shared.c | 8 +++++ src/platform/common/stm32/dshot_dpwm.c | 10 +++--- 13 files changed, 75 insertions(+), 82 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index e8ae90887b..901e9e1773 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -28,7 +28,7 @@ #if defined(USE_PWM_OUTPUT) && defined(USE_MOTOR) -FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +FAST_DATA_ZERO_INIT pwmOutputPort_t pwmMotors[MAX_SUPPORTED_MOTORS]; FAST_DATA_ZERO_INIT uint8_t pwmMotorCount; void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) @@ -53,12 +53,12 @@ IO_t pwmGetMotorIO(unsigned index) if (index >= pwmMotorCount) { return IO_NONE; } - return motors[index].io; + return pwmMotors[index].io; } bool pwmIsMotorEnabled(unsigned index) { - return motors[index].enabled; + return pwmMotors[index].enabled; } bool pwmEnableMotors(void) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 644e2415cc..38198a1837 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -50,7 +50,7 @@ typedef struct { IO_t io; } pwmOutputPort_t; -extern FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +extern FAST_DATA_ZERO_INIT pwmOutputPort_t pwmMotors[MAX_SUPPORTED_MOTORS]; extern FAST_DATA_ZERO_INIT uint8_t pwmMotorCount; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorDevConfig, uint16_t idlePulse); diff --git a/src/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c index 33b114fbc0..cf971adf97 100644 --- a/src/platform/APM32/dshot_bitbang.c +++ b/src/platform/APM32/dshot_bitbang.c @@ -669,14 +669,6 @@ static void bbPostInit(void) } } -static IO_t bbGetMotorIO(unsigned index) -{ - if (index >= dshotMotorCount) { - return IO_NONE; - } - return bbMotors[index].io; -} - static motorVTable_t bbVTable = { .postInit = bbPostInit, .enable = bbEnableMotors, diff --git a/src/platform/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c index 706591e0a0..cdec437029 100644 --- a/src/platform/APM32/pwm_output_apm32.c +++ b/src/platform/APM32/pwm_output_apm32.c @@ -90,15 +90,15 @@ static bool useContinuousUpdate = true; static void pwmWriteStandard(uint8_t index, float value) { /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ - *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset); + *pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset); } static void pwmShutdownPulsesForAllMotors(void) { for (int index = 0; index < pwmMotorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows - if (motors[index].channel.ccr) { - *motors[index].channel.ccr = 0; + if (pwmMotors[index].channel.ccr) { + *pwmMotors[index].channel.ccr = 0; } } } @@ -117,12 +117,12 @@ static void pwmCompleteMotorUpdate(void) } for (int index = 0; index < pwmMotorCount; index++) { - if (motors[index].forceOverflow) { - timerForceOverflow(motors[index].channel.tim); + if (pwmMotors[index].forceOverflow) { + timerForceOverflow(pwmMotors[index].channel.tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. - *motors[index].channel.ccr = 0; + *pwmMotors[index].channel.ccr = 0; } } @@ -154,7 +154,7 @@ static motorVTable_t motorPwmVTable = { bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { - memset(motors, 0, sizeof(motors)); + memset(pwmMotors, 0, sizeof(pwmMotors)); if (!device || !motorConfig) { return false; @@ -206,10 +206,10 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, return false; } - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); - IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); + IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); /* standard PWM outputs */ // margin of safety is 4 periods when unsynced @@ -226,20 +226,20 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; - motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); + pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); + pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { - if (motors[i].channel.tim == motors[motorIndex].channel.tim) { + if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) { timerAlreadyUsed = true; break; } } - motors[motorIndex].forceOverflow = !timerAlreadyUsed; - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed; + pwmMotors[motorIndex].enabled = true; } return true; @@ -247,7 +247,7 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return pwmMotors; } #ifdef USE_SERVOS diff --git a/src/platform/AT32/dshot_bitbang.c b/src/platform/AT32/dshot_bitbang.c index d3ac4f2e88..fff7eea8b8 100644 --- a/src/platform/AT32/dshot_bitbang.c +++ b/src/platform/AT32/dshot_bitbang.c @@ -583,7 +583,7 @@ static void bbUpdateComplete(void) } #ifdef USE_DSHOT_CACHE_MGMT - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { // Only clean each buffer once. If all motors are on a common port they'll share a buffer. bool clean = false; for (int i = 0; i < motorIndex; i++) { @@ -677,6 +677,7 @@ static motorVTable_t bbVTable = { .shutdown = bbShutdown, .isMotorIdle = bbDshotIsMotorIdle, .requestTelemetry = bbDshotRequestTelemetry, + .getMotorIO = bbGetMotorIO, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) diff --git a/src/platform/AT32/io_at32.c b/src/platform/AT32/io_at32.c index 231f9e5e47..5905f4055f 100644 --- a/src/platform/AT32/io_at32.c +++ b/src/platform/AT32/io_at32.c @@ -122,16 +122,6 @@ void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) return; } - const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; - RCC_ClockCmd(rcc, ENABLE); - - gpio_init_type init = { - .gpio_pins = IO_Pin(io), - .gpio_mode = (cfg >> 0) & 0x03, - .gpio_drive_strength = (cfg >> 2) & 0x03, - .gpio_out_type = (cfg >> 4) & 0x01, - .gpio_pull = (cfg >> 5) & 0x03, - }; - gpio_init(IO_GPIO(io), &init); + IOConfigGPIO(io, cfg); gpio_pin_mux_config(IO_GPIO(io), IO_GPIO_PinSource(io), af); } diff --git a/src/platform/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c index e04b39fc82..82c1332cb6 100644 --- a/src/platform/AT32/pwm_output_at32bsp.c +++ b/src/platform/AT32/pwm_output_at32bsp.c @@ -81,15 +81,15 @@ static FAST_DATA_ZERO_INIT motorDevice_t *pwmMotorDevice; static void pwmWriteStandard(uint8_t index, float value) { /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ - *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset); + *pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset); } static void pwmShutdownPulsesForAllMotors(void) { for (int index = 0; index < pwmMotorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows - if (motors[index].channel.ccr) { - *motors[index].channel.ccr = 0; + if (pwmMotors[index].channel.ccr) { + *pwmMotors[index].channel.ccr = 0; } } } @@ -108,12 +108,12 @@ static void pwmCompleteMotorUpdate(void) } for (int index = 0; index < pwmMotorCount; index++) { - if (motors[index].forceOverflow) { - timerForceOverflow(motors[index].channel.tim); + if (pwmMotors[index].forceOverflow) { + timerForceOverflow(pwmMotors[index].channel.tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. - *motors[index].channel.ccr = 0; + *pwmMotors[index].channel.ccr = 0; } } @@ -145,7 +145,7 @@ static motorVTable_t motorPwmVTable = { bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { - memset(motors, 0, sizeof(motors)); + memset(pwmMotors, 0, sizeof(pwmMotors)); if (!device) { return false; @@ -198,10 +198,10 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, return false; } - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); - IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); + IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); /* standard PWM outputs */ // margin of safety is 4 periods when unsynced @@ -218,27 +218,27 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; - motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); + pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); + pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { - if (motors[i].channel.tim == motors[motorIndex].channel.tim) { + if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) { timerAlreadyUsed = true; break; } } - motors[motorIndex].forceOverflow = !timerAlreadyUsed; - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed; + pwmMotors[motorIndex].enabled = true; } return true; } pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return pwmMotors; } #ifdef USE_SERVOS diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index aa58bc45a8..862d2835fd 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -546,7 +546,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig) pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return pwmMotors; } static float pwmConvertFromExternal(uint16_t externalValue) @@ -652,7 +652,7 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, idlePulse = _idlePulse; for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].enabled = true; } return true; diff --git a/src/platform/STM32/dshot_bitbang.c b/src/platform/STM32/dshot_bitbang.c index 3cf7db42ce..38f895b1ba 100644 --- a/src/platform/STM32/dshot_bitbang.c +++ b/src/platform/STM32/dshot_bitbang.c @@ -716,6 +716,7 @@ static const motorVTable_t bbVTable = { .shutdown = bbShutdown, .isMotorIdle = bbDshotIsMotorIdle, .requestTelemetry = bbDshotRequestTelemetry, + .getMotorIO = bbGetMotorIO, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) diff --git a/src/platform/STM32/pwm_output_hw.c b/src/platform/STM32/pwm_output_hw.c index aed9bda188..ae8c6a21af 100644 --- a/src/platform/STM32/pwm_output_hw.c +++ b/src/platform/STM32/pwm_output_hw.c @@ -111,15 +111,15 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, static void pwmWriteStandard(uint8_t index, float value) { /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ - *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset); + *pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset); } static void pwmShutdownPulsesForAllMotors(void) { for (int index = 0; pwmMotorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows - if (motors[index].channel.ccr) { - *motors[index].channel.ccr = 0; + if (pwmMotors[index].channel.ccr) { + *pwmMotors[index].channel.ccr = 0; } } } @@ -136,12 +136,12 @@ static void pwmCompleteMotorUpdate(void) } for (int index = 0; pwmMotorCount; index++) { - if (motors[index].forceOverflow) { - timerForceOverflow(motors[index].channel.tim); + if (pwmMotors[index].forceOverflow) { + timerForceOverflow(pwmMotors[index].channel.tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. - *motors[index].channel.ccr = 0; + *pwmMotors[index].channel.ccr = 0; } } @@ -173,7 +173,7 @@ static const motorVTable_t motorPwmVTable = { bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { - memset(motors, 0, sizeof(motors)); + memset(pwmMotors, 0, sizeof(pwmMotors)); pwmMotorCount = device->count; device->vTable = &motorPwmVTable; @@ -221,10 +221,10 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, return false; } - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); - IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); + IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); /* standard PWM outputs */ // margin of safety is 4 periods when unsynced @@ -241,20 +241,20 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; - motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); + pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); + pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { - if (motors[i].channel.tim == motors[motorIndex].channel.tim) { + if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) { timerAlreadyUsed = true; break; } } - motors[motorIndex].forceOverflow = !timerAlreadyUsed; - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed; + pwmMotors[motorIndex].enabled = true; } return true; @@ -262,7 +262,7 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return pwmMotors; } #ifdef USE_SERVOS diff --git a/src/platform/common/stm32/dshot_bitbang_impl.h b/src/platform/common/stm32/dshot_bitbang_impl.h index 4e062e86c2..76b6b06196 100644 --- a/src/platform/common/stm32/dshot_bitbang_impl.h +++ b/src/platform/common/stm32/dshot_bitbang_impl.h @@ -285,3 +285,4 @@ int bbDMA_Count(bbPort_t *bbPort); void bbDshotRequestTelemetry(unsigned motorIndex); bool bbDshotIsMotorIdle(unsigned motorIndex); +IO_t bbGetMotorIO(unsigned index); diff --git a/src/platform/common/stm32/dshot_bitbang_shared.c b/src/platform/common/stm32/dshot_bitbang_shared.c index c5152d59f3..3339df045b 100644 --- a/src/platform/common/stm32/dshot_bitbang_shared.c +++ b/src/platform/common/stm32/dshot_bitbang_shared.c @@ -54,6 +54,14 @@ bool bbDshotIsMotorIdle(unsigned motorIndex) return bbmotor->protocolControl.value == 0; } +IO_t bbGetMotorIO(unsigned index) +{ + if (index >= dshotMotorCount) { + return IO_NONE; + } + return bbMotors[index].io; +} + #ifdef USE_DSHOT_BITBANG bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig) { diff --git a/src/platform/common/stm32/dshot_dpwm.c b/src/platform/common/stm32/dshot_dpwm.c index 29c099396e..2e2186a1ac 100644 --- a/src/platform/common/stm32/dshot_dpwm.c +++ b/src/platform/common/stm32/dshot_dpwm.c @@ -123,7 +123,7 @@ static bool dshotPwmEnableMotors(void) static bool dshotPwmIsMotorEnabled(unsigned index) { - return motors[index].enabled; + return pwmMotors[index].enabled; } static IO_t pwmDshotGetMotorIO(unsigned index) @@ -131,7 +131,7 @@ static IO_t pwmDshotGetMotorIO(unsigned index) if (index >= dshotMotorCount) { return IO_NONE; } - return motors[index].io; + return pwmMotors[index].io; } static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value) @@ -190,15 +190,15 @@ bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); if (timerHardware != NULL) { - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); if (pwmDshotMotorHardwareConfig(timerHardware, motorIndex, reorderedMotorIndex, motorConfig->motorProtocol, motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) { - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].enabled = true; continue; } From 7cc8ee9a5470cb0b72900df88ff2be6a05f0cd0a Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Sat, 1 Feb 2025 06:25:03 +0000 Subject: [PATCH 04/58] Auto updated submodule references [01-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 5e33fe422e..33605beffb 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 5e33fe422e2c1fb095db84508a9e3ebb465a80a4 +Subproject commit 33605beffbd9b39aa3f6aa957a2d65ecd9edf365 From 79ecc1ac74574b23123b84597aeaa93c3a09dc41 Mon Sep 17 00:00:00 2001 From: Jeff Haynes Date: Sat, 1 Feb 2025 15:59:24 -0500 Subject: [PATCH 05/58] Fix MSP memory issue (#14221) * fixes #14219 * Update src/main/msp/msp.c Co-authored-by: Petr Ledvina --------- Co-authored-by: Petr Ledvina --- src/main/msp/msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index df8e717cb9..38d5d126ba 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -4147,7 +4147,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, } const unsigned textLength = MIN(textSpace, sbufReadU8(src)); - memset(textVar, 0, strlen(textVar)); + textVar[textLength] = '\0'; sbufReadData(src, textVar, textLength); #ifdef USE_OSD From e7f0486ebe259bfc937b4d211c302abde4df8688 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Sat, 1 Feb 2025 22:45:21 +0100 Subject: [PATCH 06/58] fix compilation without ACC (#14218) * fix compilation without ACC - compilatiopn may fail with cryptic message when no ACC is selected (unused variable `dev`). Mark dev as unused - MPU6500 is handled differently than all other accs (it is only driver that adds case labels) - also make dev UNUSED for gyro * Update src/main/sensors/acceleration_init.c --------- Co-authored-by: Mark Haslinghuis --- src/main/sensors/acceleration_init.c | 5 +++-- src/main/sensors/gyro_init.c | 1 + 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c index 1488b8b28a..6624253658 100644 --- a/src/main/sensors/acceleration_init.c +++ b/src/main/sensors/acceleration_init.c @@ -167,11 +167,11 @@ retry: FALLTHROUGH; #endif +#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) case ACC_MPU6500: case ACC_ICM20601: case ACC_ICM20602: case ACC_ICM20608G: -#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #ifdef USE_ACC_SPI_MPU6500 if (mpu6500SpiAccDetect(dev)) { #else @@ -195,8 +195,8 @@ retry: } break; } -#endif FALLTHROUGH; +#endif #ifdef USE_ACC_SPI_ICM20649 case ACC_ICM20649: @@ -287,6 +287,7 @@ retry: default: case ACC_NONE: // disable ACC + UNUSED(dev); accHardware = ACC_NONE; break; } diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 4e09d73c18..151d9f8aeb 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -500,6 +500,7 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) #endif default: + UNUSED(dev); gyroHardware = GYRO_NONE; } From bde342c8cbd18613e164054c84d5d76da183eb48 Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Mon, 3 Feb 2025 12:07:44 +0100 Subject: [PATCH 07/58] Fix PG ID and corruption in MSP_SET_SENSOR_CONFIG (#14230) * Fix PG ID * Fix corruption in MSP_SET_SENSOR_CONFIG * No need to specify ammount of bytes remaining * Update after review from @ledvinap * Remove unused PG_BETAFLIGHT_END --- src/main/msp/msp.c | 13 +++++++++---- src/main/pg/pg_ids.h | 1 - src/main/sensors/opticalflow.c | 4 ++-- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 38d5d126ba..3738779796 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -3350,18 +3350,23 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU8(src); #endif + if (sbufBytesRemaining(src) >= 1) { #ifdef USE_RANGEFINDER - rangefinderConfigMutable()->rangefinder_hardware = sbufReadU8(src); + rangefinderConfigMutable()->rangefinder_hardware = sbufReadU8(src); #else - sbufReadU8(src); + sbufReadU8(src); #endif + } + if (sbufBytesRemaining(src) >= 1) { #ifdef USE_OPTICALFLOW - opticalflowConfigMutable()->opticalflow_hardware = sbufReadU8(src); + opticalflowConfigMutable()->opticalflow_hardware = sbufReadU8(src); #else - sbufReadU8(src); + sbufReadU8(src); #endif + } break; + #ifdef USE_ACC case MSP_ACC_CALIBRATION: if (!ARMING_FLAG(ARMED)) diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 97e3d125d9..ee9fee1a0b 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -159,7 +159,6 @@ //#define PG_SOFTSERIAL_PIN_CONFIG 558 // removed, merged into SERIAL_PIN_CONFIG #define PG_GIMBAL_TRACK_CONFIG 559 #define PG_OPTICALFLOW_CONFIG 560 -#define PG_BETAFLIGHT_END 560 // OSD configuration (subject to change) diff --git a/src/main/sensors/opticalflow.c b/src/main/sensors/opticalflow.c index 27dc57e970..796e5383ad 100644 --- a/src/main/sensors/opticalflow.c +++ b/src/main/sensors/opticalflow.c @@ -68,8 +68,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(opticalflowConfig_t, opticalflowConfig, PG_OPTIC PG_RESET_TEMPLATE(opticalflowConfig_t, opticalflowConfig, .opticalflow_hardware = OPTICALFLOW_NONE, .rotation = 0, - .flow_lpf = 0, - .flip_x = 0 + .flip_x = 0, + .flow_lpf = 0 ); static opticalflow_t opticalflow; From f2e9dac32200076e92b4d37f7d357478f0b63afe Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Tue, 4 Feb 2025 06:25:05 +0000 Subject: [PATCH 08/58] Auto updated submodule references [04-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 33605beffb..760a18fe97 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 33605beffbd9b39aa3f6aa957a2d65ecd9edf365 +Subproject commit 760a18fe97a25c4b987b4b8a5c7fb4826f0e5736 From 896bf03b4ec7f240c1b16d7862a6528d915e7478 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 4 Feb 2025 10:00:11 -0900 Subject: [PATCH 09/58] lis2mdl: fix read (#14227) * lis2mdl: revert reading status register, use measure/collect paradigm * Update src/main/drivers/compass/compass_lis2mdl.c Co-authored-by: Mark Haslinghuis * fix typo * fixed borked logic --------- Co-authored-by: Mark Haslinghuis --- src/main/drivers/compass/compass_lis2mdl.c | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/compass/compass_lis2mdl.c b/src/main/drivers/compass/compass_lis2mdl.c index f270b3ca29..f2c1d75730 100644 --- a/src/main/drivers/compass/compass_lis2mdl.c +++ b/src/main/drivers/compass/compass_lis2mdl.c @@ -77,20 +77,15 @@ static bool lis2mdlInit(magDev_t *mag) static bool lis2mdlRead(magDev_t *mag, int16_t *magData) { - uint8_t status = 0; - uint8_t buf[6]; + static uint8_t buf[6]; + static bool pendingRead = true; extDevice_t *dev = &mag->dev; - if (!busReadRegisterBuffer(dev, LIS2MDL_ADDR_STATUS_REG, &status, sizeof(status))) { - return false; - } - - if (!(status & LIS2MDL_STATUS_REG_READY)) { - return false; - } - - if (!busReadRegisterBuffer(dev, LIS2MDL_ADDR_OUTX_L_REG, (uint8_t *)&buf, sizeof(buf))) { + if (pendingRead) { + if (busReadRegisterBufferStart(dev, LIS2MDL_ADDR_OUTX_L_REG, (uint8_t *)buf, sizeof(buf))) { + pendingRead = false; + } return false; } @@ -107,6 +102,8 @@ static bool lis2mdlRead(magDev_t *mag, int16_t *magData) magData[Y] = y; magData[Z] = z; + pendingRead = true; + return true; } From caa8935e61f44d696d30676760ac98dae290c644 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Wed, 5 Feb 2025 06:25:04 +0000 Subject: [PATCH 10/58] Auto updated submodule references [05-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 760a18fe97..1ea2d4e896 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 760a18fe97a25c4b987b4b8a5c7fb4826f0e5736 +Subproject commit 1ea2d4e896f3c4854c26108e938e76f31bcd0441 From 54cee626999e59bf5bc6ba446844838ec8cacaa5 Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Wed, 5 Feb 2025 21:12:15 +0100 Subject: [PATCH 11/58] Fix opticalflow detection (msp) (#14241) * Fix opticalflow detection (msp) * Add opticalflow to detected sensors in MSP_STATUS_EX --- src/main/msp/msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 3738779796..e537abbfd1 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1092,7 +1092,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t #else sbufWriteU16(dst, 0); #endif - sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5); + sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5 | sensors(SENSOR_OPTICALFLOW) << 6); sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits sbufWriteU8(dst, getCurrentPidProfileIndex()); sbufWriteU16(dst, constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE)); @@ -2131,7 +2131,7 @@ case MSP_NAME: #else sbufWriteU8(dst, SENSOR_NOT_AVAILABLE); #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPTICALFLOW sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_OPTICALFLOW]); #else sbufWriteU8(dst, SENSOR_NOT_AVAILABLE); From 39f82e79df119cc191b27199077b0908c80f32db Mon Sep 17 00:00:00 2001 From: ke deng Date: Thu, 6 Feb 2025 05:16:36 +0800 Subject: [PATCH 12/58] Change sed search regex to be compatible with BSD sed (#14233) Change sed search regex to be compatible with BSD sed, # symbol should be escaped. --- mk/config.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mk/config.mk b/mk/config.mk index edd101b6d2..5f85d4987d 100644 --- a/mk/config.mk +++ b/mk/config.mk @@ -34,17 +34,17 @@ CONFIG_REVISION := $(shell git -C $(CONFIG_DIR) log -1 --format="%h") CONFIG_REVISION_DEFINE := -D'__CONFIG_REVISION__="$(CONFIG_REVISION)"' endif -HSE_VALUE_MHZ := $(shell sed -E -n "/^\s*#\s*define\s+SYSTEM_HSE_MHZ\s+([0-9]+).*/s//\1/p" $(CONFIG_HEADER_FILE)) +HSE_VALUE_MHZ := $(shell sed -E -n "/^[[:space:]]*\#[[:space:]]*define[[:space:]]+SYSTEM_HSE_MHZ[[:space:]]+([0-9]+).*/s//\1/p" $(CONFIG_HEADER_FILE)) ifneq ($(HSE_VALUE_MHZ),) HSE_VALUE := $(shell echo $$(( $(HSE_VALUE_MHZ) * 1000000 )) ) endif -TARGET := $(shell sed -E -n "/^\s*#\s*define\s+FC_TARGET_MCU\s+(\w+).*/s//\1/p" $(CONFIG_HEADER_FILE)) +TARGET := $(shell sed -E -n "/^[[:space:]]*\#[[:space:]]*define[[:space:]]+FC_TARGET_MCU[[:space:]]+([[:alnum:]_]+).*/s//\1/p" $(CONFIG_HEADER_FILE)) ifeq ($(TARGET),) $(error No TARGET identified. Is the $(CONFIG_HEADER_FILE) valid for $(CONFIG)?) endif -EXST_ADJUST_VMA := $(shell sed -E -n "/^\s*#\s*define\s+FC_VMA_ADDRESS\s+((0[xX])?[[:xdigit:]]+).*/s//\1/p" $(CONFIG_HEADER_FILE)) +EXST_ADJUST_VMA := $(shell sed -E -n "/^[[:space:]]*\#[[:space:]]*define[[:space:]]+FC_VMA_ADDRESS[[:space:]]+((0[xX])?[[:xdigit:]]+).*/s//\1/p" $(CONFIG_HEADER_FILE)) ifneq ($(EXST_ADJUST_VMA),) EXST = yes endif From c2381fb98f1f41160e20601fa1dc35b7bf261e9b Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Thu, 6 Feb 2025 22:11:05 +0100 Subject: [PATCH 13/58] Make number of rate profiles conditional using define (#14239) * Revert reducing number of rate profiles * Add CONTROL_RATE_PROFILE_COUNT to STATUS_EX (msp) * Update src/main/target/common_pre.h Co-authored-by: Petr Ledvina * Update src/test/unit/platform.h Co-authored-by: Petr Ledvina --------- Co-authored-by: Petr Ledvina --- src/main/msp/msp.c | 1 + src/main/target/common_pre.h | 4 +++- src/test/unit/platform.h | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index e537abbfd1..0d39f90533 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1128,6 +1128,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t #else sbufWriteU16(dst, 0); #endif + sbufWriteU8(dst, CONTROL_RATE_PROFILE_COUNT); break; } diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 1220594f3d..b616303190 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -301,7 +301,9 @@ #define USE_HUFFMAN #define PID_PROFILE_COUNT 4 -#define CONTROL_RATE_PROFILE_COUNT 4 +#ifndef CONTROL_RATE_PROFILE_COUNT +#define CONTROL_RATE_PROFILE_COUNT 4 // or maybe 6 +#endif #define USE_CLI_BATCH #define USE_RESOURCE_MGMT diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index d7d5eb505e..dd03dce802 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -43,7 +43,7 @@ #define PID_PROFILE_COUNT 4 -#define CONTROL_RATE_PROFILE_COUNT 4 +#define CONTROL_RATE_PROFILE_COUNT 4 #define USE_MAG #define USE_BARO #define USE_GPS From a45f74047b61b24738bc397953853b5303fdb202 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Fri, 7 Feb 2025 06:25:04 +0000 Subject: [PATCH 14/58] Auto updated submodule references [07-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 1ea2d4e896..ebfa401cb2 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 1ea2d4e896f3c4854c26108e938e76f31bcd0441 +Subproject commit ebfa401cb2a458f80018e45bcdbbcacd19304736 From 6d7425bfa3d823ffd6c94d9fdeeebc9af88a8a59 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Sat, 8 Feb 2025 06:25:03 +0000 Subject: [PATCH 15/58] Auto updated submodule references [08-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index ebfa401cb2..fd1c46d757 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit ebfa401cb2a458f80018e45bcdbbcacd19304736 +Subproject commit fd1c46d757eeaaa6389ac4764ca9de6a8e5c0810 From 9ef4857eebe1cbe777091f4e198b68fbde519eea Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Sun, 9 Feb 2025 06:25:03 +0000 Subject: [PATCH 16/58] Auto updated submodule references [09-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index fd1c46d757..64337752ec 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit fd1c46d757eeaaa6389ac4764ca9de6a8e5c0810 +Subproject commit 64337752ec6af16a8663b4df48d8a9eb8fb1fbdb From 86767ba112fa05722a0eeee1ca53a6034a890d1e Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sun, 9 Feb 2025 23:34:12 +0100 Subject: [PATCH 17/58] Consistent autopilot variable names, use alt_hold_climb_rate (#14136) * consistent autopilot parameter names - add colons after each blackbox header line - change alt_hold_throttle_response to alt_hold_climb_rate - don't use cfg abbreviation for apConfig() because it is harder to search * enforce semicolon with do... while (0) * don't abbreviate autopilotConfig to apConfig * remove semicolon after break to enforce semicolon at input line ending * explanatory message * fix copy and paste errors * restore local cfg alias in place of autopilotConfig() * struct name formatting as requested by blckmn Co-Authored-By: Jay Blackman * underscore the CLI names, fix prev commit Co-Authored-By: Jay Blackman * Update src/main/fc/parameter_names.h * Update src/main/fc/parameter_names.h --------- Co-authored-by: Jay Blackman Co-authored-by: Mark Haslinghuis --- src/main/blackbox/blackbox.c | 99 ++++++++++--------- src/main/cli/settings.c | 36 +++---- src/main/cms/cms_menu_gps_rescue_multirotor.c | 58 +++++------ src/main/fc/parameter_names.h | 30 +++--- src/main/flight/alt_hold_multirotor.c | 14 +-- src/main/flight/autopilot_multirotor.c | 29 +++--- src/main/flight/gps_rescue_multirotor.c | 2 +- src/main/flight/pid.c | 2 +- src/main/flight/pos_hold_multirotor.c | 6 +- src/main/msp/msp.c | 28 +++--- src/main/pg/alt_hold_multirotor.c | 4 +- src/main/pg/alt_hold_multirotor.h | 4 +- src/main/pg/autopilot_multirotor.c | 32 +++--- src/main/pg/autopilot_multirotor.h | 34 +++---- src/main/pg/autopilot_wing.c | 4 +- src/main/pg/autopilot_wing.h | 6 +- src/main/pg/pos_hold_multirotor.c | 4 +- src/main/pg/pos_hold_multirotor.h | 4 +- src/test/unit/althold_unittest.cc | 2 +- src/test/unit/arming_prevention_unittest.cc | 2 +- src/test/unit/flight_imu_unittest.cc | 2 +- 21 files changed, 202 insertions(+), 200 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ba8db32289..42d274979f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1441,12 +1441,15 @@ STATIC_UNIT_TESTED char *blackboxGetStartDateTime(char *buf) } #ifndef BLACKBOX_PRINT_HEADER_LINE -#define BLACKBOX_PRINT_HEADER_LINE(name, format, ...) case __COUNTER__: \ - blackboxPrintfHeaderLine(name, format, __VA_ARGS__); \ - break; -#define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \ - {__VA_ARGS__}; \ - break; +#define BLACKBOX_PRINT_HEADER_LINE(name, format, ...) \ + case __COUNTER__: { \ + blackboxPrintfHeaderLine(name, format, __VA_ARGS__); \ + } break // absence of semicolon on this line is what enforces the presence of a semicolon at the end of each input line + +#define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) \ + case __COUNTER__: { \ + __VA_ARGS__; \ + } break #endif /** @@ -1707,20 +1710,20 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf); #ifndef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", apConfig()->landing_altitude_m); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", apConfig()->hover_throttle); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", apConfig()->throttle_min); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d", apConfig()->throttle_max); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", apConfig()->altitude_P); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", apConfig()->altitude_I); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", apConfig()->altitude_D); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", apConfig()->altitude_F); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_P, "%d", apConfig()->position_P); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d", apConfig()->position_I); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_D, "%d", apConfig()->position_D); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_A, "%d", apConfig()->position_A); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_CUTOFF, "%d", apConfig()->position_cutoff); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d", apConfig()->max_angle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_LANDING_ALTITUDE_M, "%d", autopilotConfig()->landingAltitudeM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_HOVER_THROTTLE, "%d", autopilotConfig()->hoverThrottle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_THROTTLE_MIN, "%d", autopilotConfig()->throttleMin); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_THROTTLE_MAX, "%d", autopilotConfig()->throttleMax); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_ALTITUDE_P, "%d", autopilotConfig()->altitudeP); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_ALTITUDE_I, "%d", autopilotConfig()->altitudeI); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_ALTITUDE_D, "%d", autopilotConfig()->altitudeD); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_ALTITUDE_F, "%d", autopilotConfig()->altitudeF); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_P, "%d", autopilotConfig()->positionP); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_I, "%d", autopilotConfig()->positionI); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_D, "%d", autopilotConfig()->positionD); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_A, "%d", autopilotConfig()->positionA); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_CUTOFF, "%d", autopilotConfig()->positionCutoff); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d", autopilotConfig()->maxAngle); #endif // !USE_WING #ifdef USE_MAG @@ -1793,38 +1796,38 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THRUST_LINEARIZATION, "%d", currentPidProfile->thrustLinearization); #ifdef USE_GPS - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed); #ifdef USE_GPS_RESCUE #ifndef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minStartDistM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->initialClimbM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minStartDistM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->initialClimbM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->returnAltitudeM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->groundSpeedCmS) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_IMU_YAW_GAIN, "%d", gpsRescueConfig()->imuYawGain) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->returnAltitudeM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->groundSpeedCmS); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_IMU_YAW_GAIN, "%d", gpsRescueConfig()->imuYawGain); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP); #ifdef USE_MAG - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag); #endif // USE_MAG #endif // !USE_WING #endif // USE_GPS_RESCUE @@ -1832,15 +1835,15 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_ALTITUDE_HOLD #ifndef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, "%d", altHoldConfig()->alt_hold_adjust_rate); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", altHoldConfig()->alt_hold_deadband); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_CLIMB_RATE, "%d", altHoldConfig()->climbRate); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", altHoldConfig()->deadband); #endif // !USE_WING #endif // USE_ALTITUDE_HOLD #ifdef USE_POSITION_HOLD #ifndef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", posHoldConfig()->pos_hold_deadband); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->posHoldWithoutMag); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", posHoldConfig()->deadband); #endif // !USE_WING #endif diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 82df9227dd..cc5ef8571d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1155,15 +1155,15 @@ const clivalue_t valueTable[] = { #ifdef USE_ALTITUDE_HOLD #ifndef USE_WING - { PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) }, - { PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 70 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_deadband) }, + { PARAM_NAME_ALT_HOLD_CLIMB_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, climbRate) }, + { PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 70 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, deadband) }, #endif // !USE_WING #endif // USE_ALTITUDE_HOLD #ifdef USE_POSITION_HOLD #ifndef USE_WING - { PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_without_mag) }, - { PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_deadband) }, + { PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, posHoldWithoutMag) }, + { PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, deadband) }, #endif // !USE_WING #endif // USE_POSITION_HOLD @@ -1918,20 +1918,20 @@ const clivalue_t valueTable[] = { // PG_AUTOPILOT #ifndef USE_WING - { PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, landing_altitude_m) }, - { PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(apConfig_t, hover_throttle) }, - { PARAM_NAME_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(apConfig_t, throttle_min) }, - { PARAM_NAME_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_AUTOPILOT, offsetof(apConfig_t, throttle_max) }, - { PARAM_NAME_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, altitude_P) }, - { PARAM_NAME_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, altitude_I) }, - { PARAM_NAME_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, altitude_D) }, - { PARAM_NAME_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, altitude_F) }, - { PARAM_NAME_POSITION_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_P) }, - { PARAM_NAME_POSITION_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_I) }, - { PARAM_NAME_POSITION_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_D) }, - { PARAM_NAME_POSITION_A, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_A) }, - { PARAM_NAME_POSITION_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_AUTOPILOT, offsetof(apConfig_t, position_cutoff) }, - { PARAM_NAME_AP_MAX_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 70 }, PG_AUTOPILOT, offsetof(apConfig_t, max_angle) }, + { PARAM_NAME_AP_LANDING_ALTITUDE_M, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, landingAltitudeM) }, + { PARAM_NAME_AP_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, hoverThrottle) }, + { PARAM_NAME_AP_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttleMin) }, + { PARAM_NAME_AP_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttleMax) }, + { PARAM_NAME_AP_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitudeP) }, + { PARAM_NAME_AP_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitudeI) }, + { PARAM_NAME_AP_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitudeD) }, + { PARAM_NAME_AP_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitudeF) }, + { PARAM_NAME_AP_POSITION_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionP) }, + { PARAM_NAME_AP_POSITION_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionI) }, + { PARAM_NAME_AP_POSITION_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionD) }, + { PARAM_NAME_AP_POSITION_A, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionA) }, + { PARAM_NAME_AP_POSITION_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionCutoff) }, + { PARAM_NAME_AP_MAX_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 70 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, maxAngle) }, #endif // !USE_WING // PG_MODE_ACTIVATION_CONFIG diff --git a/src/main/cms/cms_menu_gps_rescue_multirotor.c b/src/main/cms/cms_menu_gps_rescue_multirotor.c index a6520d0734..d4826c3d4d 100644 --- a/src/main/cms/cms_menu_gps_rescue_multirotor.c +++ b/src/main/cms/cms_menu_gps_rescue_multirotor.c @@ -55,16 +55,16 @@ static uint8_t gpsRescueConfig_angle; //degrees static uint16_t gpsRescueConfig_descentDistanceM; //meters static uint16_t gpsRescueConfig_descendRate; -static uint8_t apConfig_landingAltitudeM; +static uint8_t autopilotConfig_landingAltitudeM; -static uint16_t apConfig_throttleMin; -static uint16_t apConfig_throttleMax; -static uint16_t apConfig_hoverThrottle; +static uint16_t autopilotConfig_throttleMin; +static uint16_t autopilotConfig_throttleMax; +static uint16_t autopilotConfig_hoverThrottle; static uint8_t gpsRescueConfig_minSats; static uint8_t gpsRescueConfig_allowArmingWithoutFix; -static uint8_t apConfig_altitude_P, apConfig_altitude_I, apConfig_altitude_D, apConfig_altitude_F; +static uint8_t autopilotConfig_altitude_P, autopilotConfig_altitude_I, autopilotConfig_altitude_D, autopilotConfig_altitude_F; static uint8_t gpsRescueConfig_yawP; static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD; @@ -75,10 +75,10 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp) { UNUSED(pDisp); - apConfig_altitude_P = apConfig()->altitude_P; - apConfig_altitude_I = apConfig()->altitude_I; - apConfig_altitude_D = apConfig()->altitude_D; - apConfig_altitude_F = apConfig()->altitude_F; + autopilotConfig_altitude_P = autopilotConfig()->altitudeP; + autopilotConfig_altitude_I = autopilotConfig()->altitudeI; + autopilotConfig_altitude_D = autopilotConfig()->altitudeD; + autopilotConfig_altitude_F = autopilotConfig()->altitudeF; gpsRescueConfig_yawP = gpsRescueConfig()->yawP; @@ -97,10 +97,10 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En UNUSED(pDisp); UNUSED(self); - apConfigMutable()->altitude_P = apConfig_altitude_P; - apConfigMutable()->altitude_I = apConfig_altitude_I; - apConfigMutable()->altitude_D = apConfig_altitude_D; - apConfigMutable()->altitude_F = apConfig_altitude_F; + autopilotConfigMutable()->altitudeP = autopilotConfig_altitude_P; + autopilotConfigMutable()->altitudeI = autopilotConfig_altitude_I; + autopilotConfigMutable()->altitudeD = autopilotConfig_altitude_D; + autopilotConfigMutable()->altitudeF = autopilotConfig_altitude_F; gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP; @@ -118,10 +118,10 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] = { {"--- GPS RESCUE PID---", OME_Label, NULL, NULL}, - { "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_P, 0, 200, 1 } }, - { "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_I, 0, 200, 1 } }, - { "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_D, 0, 200, 1 } }, - { "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_F, 0, 200, 1 } }, + { "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_P, 0, 200, 1 } }, + { "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_I, 0, 200, 1 } }, + { "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_D, 0, 200, 1 } }, + { "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_F, 0, 200, 1 } }, { "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 200, 1 } }, @@ -162,11 +162,11 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp) gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM; gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate; - apConfig_landingAltitudeM = apConfig()->landing_altitude_m; + autopilotConfig_landingAltitudeM = autopilotConfig()->landingAltitudeM; - apConfig_throttleMin = apConfig()->throttle_min; - apConfig_throttleMax = apConfig()->throttle_max; - apConfig_hoverThrottle = apConfig()->hover_throttle; + autopilotConfig_throttleMin = autopilotConfig()->throttleMin; + autopilotConfig_throttleMax = autopilotConfig()->throttleMax; + autopilotConfig_hoverThrottle = autopilotConfig()->hoverThrottle; gpsRescueConfig_minSats = gpsRescueConfig()->minSats; gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix; @@ -190,11 +190,11 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM; gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate; - apConfigMutable()->landing_altitude_m = apConfig_landingAltitudeM; + autopilotConfigMutable()->landingAltitudeM = autopilotConfig_landingAltitudeM; - apConfigMutable()->throttle_min = apConfig_throttleMin; - apConfigMutable()->throttle_max = apConfig_throttleMax; - apConfigMutable()->hover_throttle = apConfig_hoverThrottle; + autopilotConfigMutable()->throttleMin = autopilotConfig_throttleMin; + autopilotConfigMutable()->throttleMax = autopilotConfig_throttleMax; + autopilotConfigMutable()->hoverThrottle = autopilotConfig_hoverThrottle; gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats; gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix; @@ -217,11 +217,11 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] = { "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 5, 500, 1 } }, { "DESCENT RATE CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } }, - { "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &apConfig_landingAltitudeM, 1, 15, 1 } }, + { "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &autopilotConfig_landingAltitudeM, 1, 15, 1 } }, - { "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_throttleMin, 1050, 1400, 1 } }, - { "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_throttleMax, 1400, 2000, 1 } }, - { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_hoverThrottle, 1100, 1700, 1 } }, + { "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMin, 1050, 1400, 1 } }, + { "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMax, 1400, 2000, 1 } }, + { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_hoverThrottle, 1100, 1700, 1 } }, { "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } }, { "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index e677b6fe26..070a516d6e 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -161,20 +161,20 @@ #define PARAM_NAME_ALTITUDE_LPF "altitude_lpf" #define PARAM_NAME_ALTITUDE_D_LPF "altitude_d_lpf" -#define PARAM_NAME_HOVER_THROTTLE "hover_throttle" -#define PARAM_NAME_LANDING_ALTITUDE "landing_altitude_m" -#define PARAM_NAME_THROTTLE_MIN "autopilot_throttle_min" -#define PARAM_NAME_THROTTLE_MAX "autopilot_throttle_max" -#define PARAM_NAME_ALTITUDE_P "autopilot_altitude_P" -#define PARAM_NAME_ALTITUDE_I "autopilot_altitude_I" -#define PARAM_NAME_ALTITUDE_D "autopilot_altitude_D" -#define PARAM_NAME_ALTITUDE_F "autopilot_altitude_F" -#define PARAM_NAME_POSITION_P "autopilot_position_P" -#define PARAM_NAME_POSITION_I "autopilot_position_I" -#define PARAM_NAME_POSITION_D "autopilot_position_D" -#define PARAM_NAME_POSITION_A "autopilot_position_A" -#define PARAM_NAME_POSITION_CUTOFF "autopilot_position_cutoff" -#define PARAM_NAME_AP_MAX_ANGLE "autopilot_max_angle" +#define PARAM_NAME_AP_LANDING_ALTITUDE_M "ap_landing_altitude_m" +#define PARAM_NAME_AP_HOVER_THROTTLE "ap_hover_throttle" +#define PARAM_NAME_AP_THROTTLE_MIN "ap_throttle_min" +#define PARAM_NAME_AP_THROTTLE_MAX "ap_throttle_max" +#define PARAM_NAME_AP_ALTITUDE_P "ap_altitude_p" +#define PARAM_NAME_AP_ALTITUDE_I "ap_altitude_i" +#define PARAM_NAME_AP_ALTITUDE_D "ap_altitude_d" +#define PARAM_NAME_AP_ALTITUDE_F "ap_altitude_f" +#define PARAM_NAME_AP_POSITION_P "ap_position_p" +#define PARAM_NAME_AP_POSITION_I "ap_position_i" +#define PARAM_NAME_AP_POSITION_D "ap_position_d" +#define PARAM_NAME_AP_POSITION_A "ap_position_a" +#define PARAM_NAME_AP_POSITION_CUTOFF "ap_position_cutoff" +#define PARAM_NAME_AP_MAX_ANGLE "ap_max_angle" #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward" #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms" @@ -259,7 +259,7 @@ #ifdef USE_ALTITUDE_HOLD #define PARAM_NAME_ALT_HOLD_DEADBAND "alt_hold_deadband" -#define PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE "alt_hold_throttle_response" +#define PARAM_NAME_ALT_HOLD_CLIMB_RATE "alt_hold_climb_rate" #endif #ifdef USE_POSITION_HOLD diff --git a/src/main/flight/alt_hold_multirotor.c b/src/main/flight/alt_hold_multirotor.c index 68db59a12c..ff04c70f9a 100644 --- a/src/main/flight/alt_hold_multirotor.c +++ b/src/main/flight/alt_hold_multirotor.c @@ -62,9 +62,9 @@ static void altHoldReset(void) void altHoldInit(void) { altHold.isActive = false; - altHold.deadband = altHoldConfig()->alt_hold_deadband / 100.0f; - altHold.allowStickAdjustment = altHoldConfig()->alt_hold_deadband; - altHold.maxVelocity = altHoldConfig()->alt_hold_adjust_rate * 10.0f; // 50 in CLI means 500cm/s + altHold.deadband = altHoldConfig()->deadband / 100.0f; + altHold.allowStickAdjustment = altHoldConfig()->deadband; + altHold.maxVelocity = altHoldConfig()->climbRate * 10.0f; // 50 in CLI means 500cm/s altHoldReset(); } @@ -81,7 +81,7 @@ static void altHoldProcessTransitions(void) { // ** the transition out of alt hold (exiting altHold) may be rough. Some notes... ** // The original PR had a gradual transition from hold throttle to pilot control throttle - // using !(altHoldRequested && altHold.isAltHoldActive) to run an exit function + // using !(altHoldRequested && altHold.isActive) to run an exit function // a cross-fade factor was sent to mixer.c based on time since the flight mode request was terminated // it was removed primarily to simplify this PR @@ -101,8 +101,8 @@ static void altHoldUpdateTargetAltitude(void) if (altHold.allowStickAdjustment && calculateThrottleStatus() != THROTTLE_LOW) { const float rcThrottle = rcCommand[THROTTLE]; - const float lowThreshold = apConfig()->hover_throttle - altHold.deadband * (apConfig()->hover_throttle - PWM_RANGE_MIN); - const float highThreshold = apConfig()->hover_throttle + altHold.deadband * (PWM_RANGE_MAX - apConfig()->hover_throttle); + const float lowThreshold = autopilotConfig()->hoverThrottle - altHold.deadband * (autopilotConfig()->hoverThrottle - PWM_RANGE_MIN); + const float highThreshold = autopilotConfig()->hoverThrottle + altHold.deadband * (PWM_RANGE_MAX - autopilotConfig()->hoverThrottle); if (rcThrottle < lowThreshold) { stickFactor = scaleRangef(rcThrottle, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f); @@ -136,7 +136,7 @@ static void altHoldUpdateTargetAltitude(void) static void altHoldUpdate(void) { // check if the user has changed the target altitude using sticks - if (altHoldConfig()->alt_hold_adjust_rate) { + if (altHoldConfig()->climbRate) { altHoldUpdateTargetAltitude(); } altitudeControl(altHold.targetAltitudeCm, taskIntervalSeconds, altHold.targetVelocity); diff --git a/src/main/flight/autopilot_multirotor.c b/src/main/flight/autopilot_multirotor.c index 00eff1555b..cd42049226 100644 --- a/src/main/flight/autopilot_multirotor.c +++ b/src/main/flight/autopilot_multirotor.c @@ -138,23 +138,22 @@ void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned t void autopilotInit(void) { - const apConfig_t *cfg = apConfig(); - + const autopilotConfig_t *cfg = autopilotConfig(); ap.sticksActive = false; - ap.maxAngle = cfg->max_angle; - altitudePidCoeffs.Kp = cfg->altitude_P * ALTITUDE_P_SCALE; - altitudePidCoeffs.Ki = cfg->altitude_I * ALTITUDE_I_SCALE; - altitudePidCoeffs.Kd = cfg->altitude_D * ALTITUDE_D_SCALE; - altitudePidCoeffs.Kf = cfg->altitude_F * ALTITUDE_F_SCALE; - positionPidCoeffs.Kp = cfg->position_P * POSITION_P_SCALE; - positionPidCoeffs.Ki = cfg->position_I * POSITION_I_SCALE; - positionPidCoeffs.Kd = cfg->position_D * POSITION_D_SCALE; - positionPidCoeffs.Kf = cfg->position_A * POSITION_A_SCALE; // Kf used for acceleration + ap.maxAngle = cfg->maxAngle; + altitudePidCoeffs.Kp = cfg->altitudeP * ALTITUDE_P_SCALE; + altitudePidCoeffs.Ki = cfg->altitudeI * ALTITUDE_I_SCALE; + altitudePidCoeffs.Kd = cfg->altitudeD * ALTITUDE_D_SCALE; + altitudePidCoeffs.Kf = cfg->altitudeF * ALTITUDE_F_SCALE; + positionPidCoeffs.Kp = cfg->positionP * POSITION_P_SCALE; + positionPidCoeffs.Ki = cfg->positionI * POSITION_I_SCALE; + positionPidCoeffs.Kd = cfg->positionD * POSITION_D_SCALE; + positionPidCoeffs.Kf = cfg->positionA * POSITION_A_SCALE; // Kf used for acceleration // initialise filters with approximate filter gains; location isn't used at this point. ap.upsampleLpfGain = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate at init resetUpsampleFilters(); // Initialise PT1 filters for velocity and acceleration in earth frame axes - ap.vaLpfCutoff = cfg->position_cutoff * 0.01f; + ap.vaLpfCutoff = cfg->positionCutoff * 0.01f; const float vaGain = pt1FilterGain(ap.vaLpfCutoff, 0.1f); // assume 10Hz GPS connection at start; value is overwritten before first filter use for (unsigned i = 0; i < ARRAYLEN(ap.efAxis); i++) { resetEFAxisFilters(&ap.efAxis[i], vaGain); @@ -193,7 +192,7 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAl const float altitudeF = targetAltitudeStep * altitudePidCoeffs.Kf; - const float hoverOffset = apConfig()->hover_throttle - PWM_RANGE_MIN; + const float hoverOffset = autopilotConfig()->hoverThrottle - PWM_RANGE_MIN; float throttleOffset = altitudeP + altitudeI - altitudeD + altitudeF + hoverOffset; const float tiltMultiplier = 1.0f / fmaxf(getCosTiltAngle(), 0.5f); @@ -203,7 +202,7 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAl throttleOffset *= tiltMultiplier; float newThrottle = PWM_RANGE_MIN + throttleOffset; - newThrottle = constrainf(newThrottle, apConfig()->throttle_min, apConfig()->throttle_max); + newThrottle = constrainf(newThrottle, autopilotConfig()->throttleMin, autopilotConfig()->throttleMax); DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 0, lrintf(newThrottle)); // normal range 1000-2000 but is before constraint newThrottle = scaleRangef(newThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f); @@ -386,7 +385,7 @@ bool positionControl(void) bool isBelowLandingAltitude(void) { - return getAltitudeCm() < 100.0f * apConfig()->landing_altitude_m; + return getAltitudeCm() < 100.0f * autopilotConfig()->landingAltitudeM; } float getAutopilotThrottle(void) diff --git a/src/main/flight/gps_rescue_multirotor.c b/src/main/flight/gps_rescue_multirotor.c index cfb3b83feb..2321629ad0 100644 --- a/src/main/flight/gps_rescue_multirotor.c +++ b/src/main/flight/gps_rescue_multirotor.c @@ -630,7 +630,7 @@ static void descend(bool newGpsData) { if (newGpsData) { // consider landing area to be a circle half landing height around home, to avoid overshooting home point - const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * apConfig()->landing_altitude_m); + const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * autopilotConfig()->landingAltitudeM); const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); // increase the velocity lowpass filter cutoff for more aggressive responses when descending, especially close to home diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index dbc9b08da0..38dfdee169 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -586,7 +586,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_ angleLimit = 85.0f; // allow autopilot to use whatever angle it needs to stop } // limit pilot requested angle to half the autopilot angle to avoid excess speed and chaotic stops - angleLimit = fminf(0.5f * apConfig()->max_angle, angleLimit); + angleLimit = fminf(0.5f * autopilotConfig()->maxAngle, angleLimit); } #endif diff --git a/src/main/flight/pos_hold_multirotor.c b/src/main/flight/pos_hold_multirotor.c index de4147a2df..ba977d8589 100644 --- a/src/main/flight/pos_hold_multirotor.c +++ b/src/main/flight/pos_hold_multirotor.c @@ -51,8 +51,8 @@ static posHoldState_t posHold; void posHoldInit(void) { - posHold.deadband = posHoldConfig()->pos_hold_deadband * 0.01f; - posHold.useStickAdjustment = posHoldConfig()->pos_hold_deadband; + posHold.deadband = posHoldConfig()->deadband * 0.01f; + posHold.useStickAdjustment = posHoldConfig()->deadband; } static void posHoldCheckSticks(void) @@ -73,7 +73,7 @@ static bool sensorsOk(void) #ifdef USE_MAG !compassIsHealthy() && #endif - (!posHoldConfig()->pos_hold_without_mag || !canUseGPSHeading)) { + (!posHoldConfig()->posHoldWithoutMag || !canUseGPSHeading)) { return false; } return true; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 0d39f90533..025df2fc7b 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1530,9 +1530,9 @@ case MSP_NAME: sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM); sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM); sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS); - sbufWriteU16(dst, apConfig()->throttle_min); - sbufWriteU16(dst, apConfig()->throttle_max); - sbufWriteU16(dst, apConfig()->hover_throttle); + sbufWriteU16(dst, autopilotConfig()->throttleMin); + sbufWriteU16(dst, autopilotConfig()->throttleMax); + sbufWriteU16(dst, autopilotConfig()->hoverThrottle); sbufWriteU8(dst, gpsRescueConfig()->sanityChecks); sbufWriteU8(dst, gpsRescueConfig()->minSats); @@ -1548,9 +1548,9 @@ case MSP_NAME: break; case MSP_GPS_RESCUE_PIDS: - sbufWriteU16(dst, apConfig()->altitude_P); - sbufWriteU16(dst, apConfig()->altitude_I); - sbufWriteU16(dst, apConfig()->altitude_D); + sbufWriteU16(dst, autopilotConfig()->altitudeP); + sbufWriteU16(dst, autopilotConfig()->altitudeI); + sbufWriteU16(dst, autopilotConfig()->altitudeD); // altitude_F not implemented yet sbufWriteU16(dst, gpsRescueConfig()->velP); sbufWriteU16(dst, gpsRescueConfig()->velI); @@ -1797,7 +1797,7 @@ case MSP_NAME: sbufWriteU8(dst, rcControlsConfig()->deadband); sbufWriteU8(dst, rcControlsConfig()->yaw_deadband); #if defined(USE_POSITION_HOLD) && !defined(USE_WING) - sbufWriteU8(dst, posHoldConfig()->pos_hold_deadband); + sbufWriteU8(dst, posHoldConfig()->deadband); #else sbufWriteU8(dst, 0); #endif @@ -2901,9 +2901,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src); gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src); gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src); - apConfigMutable()->throttle_min = sbufReadU16(src); - apConfigMutable()->throttle_max = sbufReadU16(src); - apConfigMutable()->hover_throttle = sbufReadU16(src); + autopilotConfigMutable()->throttleMin = sbufReadU16(src); + autopilotConfigMutable()->throttleMax = sbufReadU16(src); + autopilotConfigMutable()->hoverThrottle = sbufReadU16(src); gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src); gpsRescueConfigMutable()->minSats = sbufReadU8(src); if (sbufBytesRemaining(src) >= 6) { @@ -2924,9 +2924,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, break; case MSP_SET_GPS_RESCUE_PIDS: - apConfigMutable()->altitude_P = sbufReadU16(src); - apConfigMutable()->altitude_I = sbufReadU16(src); - apConfigMutable()->altitude_D = sbufReadU16(src); + autopilotConfigMutable()->altitudeP = sbufReadU16(src); + autopilotConfigMutable()->altitudeI = sbufReadU16(src); + autopilotConfigMutable()->altitudeD = sbufReadU16(src); // altitude_F not included in msp yet gpsRescueConfigMutable()->velP = sbufReadU16(src); gpsRescueConfigMutable()->velI = sbufReadU16(src); @@ -2990,7 +2990,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, rcControlsConfigMutable()->deadband = sbufReadU8(src); rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); #if defined(USE_POSITION_HOLD) && !defined(USE_WING) - posHoldConfigMutable()->pos_hold_deadband = sbufReadU8(src); + posHoldConfigMutable()->deadband = sbufReadU8(src); #else sbufReadU8(src); #endif diff --git a/src/main/pg/alt_hold_multirotor.c b/src/main/pg/alt_hold_multirotor.c index c229fcf66a..5c29527219 100644 --- a/src/main/pg/alt_hold_multirotor.c +++ b/src/main/pg/alt_hold_multirotor.c @@ -35,8 +35,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 4); PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, - .alt_hold_adjust_rate = 50, // max vertical velocity change at full/zero throttle. 50 means 5 m/s - .alt_hold_deadband = 20, // throttle deadband in percent of stick travel + .climbRate = 50, // max vertical velocity change at full/zero throttle. 50 means 5 m/s + .deadband = 20, // throttle deadband in percent of stick travel ); #endif diff --git a/src/main/pg/alt_hold_multirotor.h b/src/main/pg/alt_hold_multirotor.h index 88f0933b0c..2310328fe0 100644 --- a/src/main/pg/alt_hold_multirotor.h +++ b/src/main/pg/alt_hold_multirotor.h @@ -28,8 +28,8 @@ #include "pg/pg.h" typedef struct altHoldConfig_s { - uint8_t alt_hold_adjust_rate; - uint8_t alt_hold_deadband; + uint8_t climbRate; + uint8_t deadband; } altHoldConfig_t; PG_DECLARE(altHoldConfig_t, altHoldConfig); diff --git a/src/main/pg/autopilot_multirotor.c b/src/main/pg/autopilot_multirotor.c index 898cfdc205..7854c33c4b 100644 --- a/src/main/pg/autopilot_multirotor.c +++ b/src/main/pg/autopilot_multirotor.c @@ -30,23 +30,23 @@ #include "autopilot.h" -PG_REGISTER_WITH_RESET_TEMPLATE(apConfig_t, apConfig, PG_AUTOPILOT, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 2); -PG_RESET_TEMPLATE(apConfig_t, apConfig, - .landing_altitude_m = 4, - .hover_throttle = 1275, - .throttle_min = 1100, - .throttle_max = 1700, - .altitude_P = 15, - .altitude_I = 15, - .altitude_D = 15, - .altitude_F = 15, - .position_P = 30, - .position_I = 30, - .position_D = 30, - .position_A = 30, - .position_cutoff = 80, - .max_angle = 50, +PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, + .landingAltitudeM = 4, + .hoverThrottle = 1275, + .throttleMin = 1100, + .throttleMax = 1700, + .altitudeP = 15, + .altitudeI = 15, + .altitudeD = 15, + .altitudeF = 15, + .positionP = 30, + .positionI = 30, + .positionD = 30, + .positionA = 30, + .positionCutoff = 80, + .maxAngle = 50, ); #endif // !USE_WING diff --git a/src/main/pg/autopilot_multirotor.h b/src/main/pg/autopilot_multirotor.h index 6e1f6cfecd..978c9a0c92 100644 --- a/src/main/pg/autopilot_multirotor.h +++ b/src/main/pg/autopilot_multirotor.h @@ -27,23 +27,23 @@ #include "pg/pg.h" -typedef struct apConfig_s { - uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres - uint16_t hover_throttle; // value used at the start of a rescue or position hold - uint16_t throttle_min; - uint16_t throttle_max; - uint8_t altitude_P; - uint8_t altitude_I; - uint8_t altitude_D; - uint8_t altitude_F; - uint8_t position_P; - uint8_t position_I; - uint8_t position_D; - uint8_t position_A; - uint8_t position_cutoff; - uint8_t max_angle; -} apConfig_t; +typedef struct autopilotConfig_s { + uint8_t landingAltitudeM; // altitude below which landing behaviours can change, metres + uint16_t hoverThrottle; // value used at the start of a rescue or position hold + uint16_t throttleMin; + uint16_t throttleMax; + uint8_t altitudeP; + uint8_t altitudeI; + uint8_t altitudeD; + uint8_t altitudeF; + uint8_t positionP; + uint8_t positionI; + uint8_t positionD; + uint8_t positionA; + uint8_t positionCutoff; + uint8_t maxAngle; +} autopilotConfig_t; -PG_DECLARE(apConfig_t, apConfig); +PG_DECLARE(autopilotConfig_t, autopilotConfig); #endif // !USE_WING diff --git a/src/main/pg/autopilot_wing.c b/src/main/pg/autopilot_wing.c index 06dc476c1c..73a351f45f 100644 --- a/src/main/pg/autopilot_wing.c +++ b/src/main/pg/autopilot_wing.c @@ -30,9 +30,9 @@ #include "autopilot.h" -PG_REGISTER_WITH_RESET_TEMPLATE(apConfig_t, apConfig, PG_AUTOPILOT, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 2); -PG_RESET_TEMPLATE(apConfig_t, apConfig, +PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, ); #endif // USE_WING diff --git a/src/main/pg/autopilot_wing.h b/src/main/pg/autopilot_wing.h index ee01e0a6f9..567db81cc3 100644 --- a/src/main/pg/autopilot_wing.h +++ b/src/main/pg/autopilot_wing.h @@ -27,10 +27,10 @@ #include "pg/pg.h" -typedef struct apConfig_s { +typedef struct autopilotConfig_s { uint8_t dummy; -} apConfig_t; +} autopilotConfig_t; -PG_DECLARE(apConfig_t, apConfig); +PG_DECLARE(autopilotConfig_t, autopilotConfig); #endif // USE_WING diff --git a/src/main/pg/pos_hold_multirotor.c b/src/main/pg/pos_hold_multirotor.c index 2a7a332241..68e907165f 100644 --- a/src/main/pg/pos_hold_multirotor.c +++ b/src/main/pg/pos_hold_multirotor.c @@ -35,8 +35,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 0); PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, - .pos_hold_without_mag = false, // position hold within this percentage stick deflection - .pos_hold_deadband = 5, // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks + .posHoldWithoutMag = false, // position hold within this percentage stick deflection + .deadband = 5, // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks ); #endif diff --git a/src/main/pg/pos_hold_multirotor.h b/src/main/pg/pos_hold_multirotor.h index 6187d9ba62..64f8f26716 100644 --- a/src/main/pg/pos_hold_multirotor.h +++ b/src/main/pg/pos_hold_multirotor.h @@ -28,8 +28,8 @@ #include "pg/pg.h" typedef struct posHoldConfig_s { - bool pos_hold_without_mag; - uint8_t pos_hold_deadband; + bool posHoldWithoutMag; + uint8_t deadband; } posHoldConfig_t; PG_DECLARE(posHoldConfig_t, posHoldConfig); diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc index 4dbba143bb..98e1d93abc 100644 --- a/src/test/unit/althold_unittest.cc +++ b/src/test/unit/althold_unittest.cc @@ -50,7 +50,7 @@ extern "C" { PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); PG_REGISTER(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 0); - PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0); + PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0); PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0); PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 82b0df4ac1..8b92be036f 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -79,7 +79,7 @@ extern "C" { PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0); - PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0); + PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0); float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint16_t averageSystemLoadPercent = 0; diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index b8c1dda675..af839a3266 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -75,7 +75,7 @@ extern "C" { PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); - PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0); + PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = 0 From 6df8d0edda8bf5d32abea601b6ba43e66792c67d Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Mon, 10 Feb 2025 15:38:25 +0100 Subject: [PATCH 18/58] Fix codestyle (#14245) Fix codestyle and prevent loop when commandLen is zero --- src/main/io/gps.c | 1299 +++++++++++++++++++++++---------------------- 1 file changed, 650 insertions(+), 649 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 129592a005..1343d5bb3a 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -425,10 +425,10 @@ void gpsInit(void) initBaudRateCycleCount = 0; gpsData.userBaudRateIndex = DEFAULT_BAUD_RATE_INDEX; for (unsigned i = 0; i < ARRAYLEN(gpsInitData); i++) { - if (gpsInitData[i].baudrateIndex == gpsPortConfig->gps_baudrateIndex) { - gpsData.userBaudRateIndex = i; - break; - } + if (gpsInitData[i].baudrateIndex == gpsPortConfig->gps_baudrateIndex) { + gpsData.userBaudRateIndex = i; + break; + } } // the user's intended baud rate will be used as the initial baud rate when connecting gpsData.tempBaudRateIndex = gpsData.userBaudRateIndex; @@ -462,44 +462,45 @@ void gpsInit(void) #ifdef USE_GPS_UBLOX const uint8_t ubloxUTCStandardConfig_int[5] = { - UBLOX_UTC_STANDARD_AUTO, - UBLOX_UTC_STANDARD_USNO, - UBLOX_UTC_STANDARD_EU, - UBLOX_UTC_STANDARD_SU, - UBLOX_UTC_STANDARD_NTSC + UBLOX_UTC_STANDARD_AUTO, + UBLOX_UTC_STANDARD_USNO, + UBLOX_UTC_STANDARD_EU, + UBLOX_UTC_STANDARD_SU, + UBLOX_UTC_STANDARD_NTSC }; struct ubloxVersion_s ubloxVersionMap[] = { - [UBX_VERSION_UNDEF] = {~0, "UNKNOWN"}, - [UBX_VERSION_M5] = {0x00040005, "M5"}, - [UBX_VERSION_M6] = {0x00040007, "M6"}, - [UBX_VERSION_M7] = {0x00070000, "M7"}, - [UBX_VERSION_M8] = {0x00080000, "M8"}, - [UBX_VERSION_M9] = {0x00190000, "M9"}, - [UBX_VERSION_M10] = {0x000A0000, "M10"}, + [UBX_VERSION_UNDEF] = { ~0, "UNKNOWN" }, + [UBX_VERSION_M5] = { 0x00040005, "M5" }, + [UBX_VERSION_M6] = { 0x00040007, "M6" }, + [UBX_VERSION_M7] = { 0x00070000, "M7" }, + [UBX_VERSION_M8] = { 0x00080000, "M8" }, + [UBX_VERSION_M9] = { 0x00190000, "M9" }, + [UBX_VERSION_M10] = { 0x000A0000, "M10" }, }; -static uint8_t ubloxAddValSet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, const uint8_t * payload, const uint8_t offset) { +static uint8_t ubloxAddValSet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, const uint8_t * payload, const uint8_t offset) +{ size_t len; switch((key >> (8 * 3)) & 0xff) { - case 0x10: - case 0x20: - len = 1; - break; - case 0x30: - len = 2; - break; - case 0x40: - len = 4; - break; - case 0x50: - len = 8; - break; - default: - return 0; + case 0x10: + case 0x20: + len = 1; + break; + case 0x30: + len = 2; + break; + case 0x40: + len = 4; + break; + case 0x50: + len = 8; + break; + default: + return 0; } - if (offset + 4 + len > MAX_VALSET_SIZE) - { + + if (offset + 4 + len > MAX_VALSET_SIZE) { return 0; } @@ -538,7 +539,8 @@ static size_t ubloxValGet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, ubl } #endif // not used -static uint8_t ubloxValSet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, uint8_t * payload, ubloxValLayer_e layer) { +static uint8_t ubloxValSet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, uint8_t * payload, ubloxValLayer_e layer) +{ memset(&tx_buffer->payload.cfg_valset, 0, sizeof(ubxCfgValSet_t)); // tx_buffer->payload.cfg_valset.version = 0; @@ -615,7 +617,8 @@ static void ubloxSendPollMessage(uint8_t msg_id) ubloxSendMessage(&msg, false); } -static void ubloxSendNAV5Message(uint8_t model) { +static void ubloxSendNAV5Message(uint8_t model) +{ DEBUG_SET(DEBUG_GPS_CONNECTION, 0, model); ubxMessage_t tx_buffer; if (gpsData.ubloxM9orAbove) { @@ -875,21 +878,21 @@ static void ubloxSetSbas(void) uint64_t mask = SBAS_SEARCH_ALL; switch (gpsConfig()->sbasMode) { - case SBAS_EGNOS: - mask = SBAS_SEARCH_PRN(123) | SBAS_SEARCH_PRN(126) | SBAS_SEARCH_PRN(136); - break; - case SBAS_WAAS: - mask = SBAS_SEARCH_PRN(131) | SBAS_SEARCH_PRN(133) | SBAS_SEARCH_PRN(135) | SBAS_SEARCH_PRN(138); - break; - case SBAS_MSAS: - mask = SBAS_SEARCH_PRN(129) | SBAS_SEARCH_PRN(137); - break; - case SBAS_GAGAN: - mask = SBAS_SEARCH_PRN(127) | SBAS_SEARCH_PRN(128) | SBAS_SEARCH_PRN(132); - break; - case SBAS_AUTO: - default: - break; + case SBAS_EGNOS: + mask = SBAS_SEARCH_PRN(123) | SBAS_SEARCH_PRN(126) | SBAS_SEARCH_PRN(136); + break; + case SBAS_WAAS: + mask = SBAS_SEARCH_PRN(131) | SBAS_SEARCH_PRN(133) | SBAS_SEARCH_PRN(135) | SBAS_SEARCH_PRN(138); + break; + case SBAS_MSAS: + mask = SBAS_SEARCH_PRN(129) | SBAS_SEARCH_PRN(137); + break; + case SBAS_GAGAN: + mask = SBAS_SEARCH_PRN(127) | SBAS_SEARCH_PRN(128) | SBAS_SEARCH_PRN(132); + break; + case SBAS_AUTO: + default: + break; } payload[0] = (uint8_t)(mask >> (8 * 0)); @@ -920,24 +923,24 @@ static void ubloxSetSbas(void) tx_buffer.payload.cfg_sbas.maxSBAS = 3; tx_buffer.payload.cfg_sbas.scanmode2 = 0; switch (gpsConfig()->sbasMode) { - case SBAS_AUTO: - tx_buffer.payload.cfg_sbas.scanmode1 = 0; - break; - case SBAS_EGNOS: - tx_buffer.payload.cfg_sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136 - break; - case SBAS_WAAS: - tx_buffer.payload.cfg_sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138 - break; - case SBAS_MSAS: - tx_buffer.payload.cfg_sbas.scanmode1 = 0x00020200; //PRN129, PRN137 - break; - case SBAS_GAGAN: - tx_buffer.payload.cfg_sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132 - break; - default: - tx_buffer.payload.cfg_sbas.scanmode1 = 0; - break; + case SBAS_AUTO: + tx_buffer.payload.cfg_sbas.scanmode1 = 0; + break; + case SBAS_EGNOS: + tx_buffer.payload.cfg_sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136 + break; + case SBAS_WAAS: + tx_buffer.payload.cfg_sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138 + break; + case SBAS_MSAS: + tx_buffer.payload.cfg_sbas.scanmode1 = 0x00020200; //PRN129, PRN137 + break; + case SBAS_GAGAN: + tx_buffer.payload.cfg_sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132 + break; + default: + tx_buffer.payload.cfg_sbas.scanmode1 = 0; + break; } ubloxSendConfigMessage(&tx_buffer, MSG_CFG_SBAS, sizeof(ubxCfgSbas_t), false); } @@ -980,51 +983,51 @@ static void gpsConfigureNmea(void) switch (gpsData.state) { - case GPS_STATE_DETECT_BAUD: - // no attempt to read the baud rate of the GPS module, or change it - gpsSetState(GPS_STATE_CHANGE_BAUD); - break; + case GPS_STATE_DETECT_BAUD: + // no attempt to read the baud rate of the GPS module, or change it + gpsSetState(GPS_STATE_CHANGE_BAUD); + break; - case GPS_STATE_CHANGE_BAUD: + case GPS_STATE_CHANGE_BAUD: #if !defined(GPS_NMEA_TX_ONLY) - if (gpsData.state_position < 1) { - // set the FC's baud rate to the user's configured baud rate - serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]); - gpsData.state_position++; - } else if (gpsData.state_position < 2) { - // send NMEA custom commands to select which messages being sent, data rate etc - // use PUBX, MTK, SiRF or GTK format commands, depending on module type - static int commandOffset = 0; - const char *commands = gpsConfig()->nmeaCustomCommands; - const char *cmd = commands + commandOffset; - // skip leading whitespaces and get first command length - int commandLen; - while (*cmd && (commandLen = strcspn(cmd, " \0")) == 0) { - cmd++; // skip separators - } - if (*cmd) { - // Send the current command to the GPS - serialWriteBuf(gpsPort, (uint8_t *)cmd, commandLen); - serialWriteBuf(gpsPort, (uint8_t *)"\r\n", 2); - // Move to the next command - cmd += commandLen; - } - // skip trailing whitespaces - while (*cmd && strcspn(cmd, " \0") == 0) cmd++; - if (*cmd) { - // more commands to send - commandOffset = cmd - commands; - } else { - gpsData.state_position++; - commandOffset = 0; - } - gpsData.state_position++; - gpsSetState(GPS_STATE_RECEIVING_DATA); + if (gpsData.state_position < 1) { + // set the FC's baud rate to the user's configured baud rate + serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]); + gpsData.state_position++; + } else if (gpsData.state_position < 2) { + // send NMEA custom commands to select which messages being sent, data rate etc + // use PUBX, MTK, SiRF or GTK format commands, depending on module type + static int commandOffset = 0; + const char *commands = gpsConfig()->nmeaCustomCommands; + const char *cmd = commands + commandOffset; + // skip leading whitespaces and get first command length + int commandLen; + while (*cmd && (commandLen = strcspn(cmd, " \0")) == 0) { + cmd++; // skip separators } -#else // !GPS_NMEA_TX_ONLY + if (*cmd) { + // Send the current command to the GPS + serialWriteBuf(gpsPort, (uint8_t *)cmd, commandLen); + serialWriteBuf(gpsPort, (uint8_t *)"\r\n", 2); + // Move to the next command + cmd += commandLen; + } + // skip trailing whitespaces + while (*cmd && strcspn(cmd, " \0") == 0) cmd++; + if (*cmd) { + // more commands to send + commandOffset = cmd - commands; + } else { + gpsData.state_position++; + commandOffset = 0; + } + gpsData.state_position++; gpsSetState(GPS_STATE_RECEIVING_DATA); + } +#else // !GPS_NMEA_TX_ONLY + gpsSetState(GPS_STATE_RECEIVING_DATA); #endif // !GPS_NMEA_TX_ONLY - break; + break; } } #endif // USE_GPS_NMEA @@ -1040,136 +1043,136 @@ static void gpsConfigureUblox(void) } switch (gpsData.state) { - case GPS_STATE_DETECT_BAUD: + case GPS_STATE_DETECT_BAUD: - DEBUG_SET(DEBUG_GPS_CONNECTION, 3, baudRates[gpsInitData[gpsData.tempBaudRateIndex].baudrateIndex] / 100); + DEBUG_SET(DEBUG_GPS_CONNECTION, 3, baudRates[gpsInitData[gpsData.tempBaudRateIndex].baudrateIndex] / 100); - // check to see if there has been a response to the version command - // initially the FC will be at the user-configured baud rate. - if (gpsData.platformVersion > UBX_VERSION_UNDEF) { - // set the GPS module's serial port to the user's intended baud rate - serialPrint(gpsPort, gpsInitData[gpsData.userBaudRateIndex].ubx); - // use this baud rate for re-connections - gpsData.tempBaudRateIndex = gpsData.userBaudRateIndex; - // we're done here, let's move the the next state - gpsSetState(GPS_STATE_CHANGE_BAUD); + // check to see if there has been a response to the version command + // initially the FC will be at the user-configured baud rate. + if (gpsData.platformVersion > UBX_VERSION_UNDEF) { + // set the GPS module's serial port to the user's intended baud rate + serialPrint(gpsPort, gpsInitData[gpsData.userBaudRateIndex].ubx); + // use this baud rate for re-connections + gpsData.tempBaudRateIndex = gpsData.userBaudRateIndex; + // we're done here, let's move the the next state + gpsSetState(GPS_STATE_CHANGE_BAUD); + return; + } + + // Send MON-VER messages at GPS_CONFIG_BAUD_CHANGE_INTERVAL for GPS_BAUDRATE_TEST_COUNT times + static bool messageSent = false; + static uint8_t messageCounter = 0; + DEBUG_SET(DEBUG_GPS_CONNECTION, 2, initBaudRateCycleCount * 100 + messageCounter); + + if (messageCounter < GPS_BAUDRATE_TEST_COUNT) { + if (!messageSent) { + gpsData.platformVersion = UBX_VERSION_UNDEF; + ubloxSendClassMessage(CLASS_MON, MSG_MON_VER, 0); + gpsData.ackState = UBLOX_ACK_IDLE; // ignore ACK for this message + messageSent = true; + } + if (cmp32(gpsData.now, gpsData.state_ts) > GPS_CONFIG_BAUD_CHANGE_INTERVAL) { + gpsData.state_ts = gpsData.now; + messageSent = false; + messageCounter++; + } + return; + } + messageCounter = 0; + gpsData.state_ts = gpsData.now; + + // failed to connect at that rate after five attempts + // try other GPS baudrates, starting at 9600 and moving up + if (gpsData.tempBaudRateIndex == 0) { + gpsData.tempBaudRateIndex = ARRAYLEN(gpsInitData) - 1; // slowest baud rate (9600) + } else { + gpsData.tempBaudRateIndex--; + } + // set the FC baud rate to the new temp baud rate + serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.tempBaudRateIndex].baudrateIndex]); + initBaudRateCycleCount++; + + break; + + case GPS_STATE_CHANGE_BAUD: + // give time for the GPS module's serial port to settle + // very important for M8 to give the module a lot of time before sending commands + // M10 only need about 200ms but M8 and below sometimes need as long as 1000ms + if (cmp32(gpsData.now, gpsData.state_ts) < (3 * GPS_CONFIG_BAUD_CHANGE_INTERVAL)) { + return; + } + // set the FC's serial port to the configured rate + serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]); + DEBUG_SET(DEBUG_GPS_CONNECTION, 3, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex] / 100); + // then start sending configuration settings + gpsSetState(GPS_STATE_CONFIGURE); + break; + + case GPS_STATE_CONFIGURE: + // Either use specific config file for GPS or let dynamically upload config + if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF) { + gpsSetState(GPS_STATE_RECEIVING_DATA); + break; + } + + // Add delay to stabilize the connection + if (cmp32(gpsData.now, gpsData.state_ts) < 1000) { + return; + } + + if (gpsData.ackState == UBLOX_ACK_IDLE) { + + // short delay before between commands, including the first command + static uint32_t last_state_position_time = 0; + if (last_state_position_time == 0) { + last_state_position_time = gpsData.now; + } + if (cmp32(gpsData.now, last_state_position_time) < GPS_CONFIG_CHANGE_INTERVAL) { return; } + last_state_position_time = gpsData.now; - // Send MON-VER messages at GPS_CONFIG_BAUD_CHANGE_INTERVAL for GPS_BAUDRATE_TEST_COUNT times - static bool messageSent = false; - static uint8_t messageCounter = 0; - DEBUG_SET(DEBUG_GPS_CONNECTION, 2, initBaudRateCycleCount * 100 + messageCounter); - - if (messageCounter < GPS_BAUDRATE_TEST_COUNT) { - if (!messageSent) { - gpsData.platformVersion = UBX_VERSION_UNDEF; + switch (gpsData.state_position) { + // if a UBX command is sent, ack is supposed to give position++ once the reply happens + case UBLOX_DETECT_UNIT: // 400 in debug + if (gpsData.platformVersion == UBX_VERSION_UNDEF) { ubloxSendClassMessage(CLASS_MON, MSG_MON_VER, 0); - gpsData.ackState = UBLOX_ACK_IDLE; // ignore ACK for this message - messageSent = true; + } else { + gpsData.state_position++; } - if (cmp32(gpsData.now, gpsData.state_ts) > GPS_CONFIG_BAUD_CHANGE_INTERVAL) { - gpsData.state_ts = gpsData.now; - messageSent = false; - messageCounter++; - } - return; - } - messageCounter = 0; - gpsData.state_ts = gpsData.now; - - // failed to connect at that rate after five attempts - // try other GPS baudrates, starting at 9600 and moving up - if (gpsData.tempBaudRateIndex == 0) { - gpsData.tempBaudRateIndex = ARRAYLEN(gpsInitData) - 1; // slowest baud rate (9600) - } else { - gpsData.tempBaudRateIndex--; - } - // set the FC baud rate to the new temp baud rate - serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.tempBaudRateIndex].baudrateIndex]); - initBaudRateCycleCount++; - - break; - - case GPS_STATE_CHANGE_BAUD: - // give time for the GPS module's serial port to settle - // very important for M8 to give the module a lot of time before sending commands - // M10 only need about 200ms but M8 and below sometimes need as long as 1000ms - if (cmp32(gpsData.now, gpsData.state_ts) < (3 * GPS_CONFIG_BAUD_CHANGE_INTERVAL)) { - return; - } - // set the FC's serial port to the configured rate - serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]); - DEBUG_SET(DEBUG_GPS_CONNECTION, 3, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex] / 100); - // then start sending configuration settings - gpsSetState(GPS_STATE_CONFIGURE); - break; - - case GPS_STATE_CONFIGURE: - // Either use specific config file for GPS or let dynamically upload config - if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF) { - gpsSetState(GPS_STATE_RECEIVING_DATA); break; - } - - // Add delay to stabilize the connection - if (cmp32(gpsData.now, gpsData.state_ts) < 1000) { - return; - } - - if (gpsData.ackState == UBLOX_ACK_IDLE) { - - // short delay before between commands, including the first command - static uint32_t last_state_position_time = 0; - if (last_state_position_time == 0) { - last_state_position_time = gpsData.now; + case UBLOX_SLOW_NAV_RATE: // 401 in debug + ubloxSetNavRate(1, 1, 1); // throttle nav data rate to one per second, until configured + break; + case UBLOX_MSG_DISABLE_NMEA: + if (gpsData.ubloxM9orAbove) { + ubloxDisableNMEAValSet(); + gpsData.state_position = UBLOX_MSG_RMC; // skip UBX NMEA entries - goes to RMC plus one, or ACQUIRE_MODEL + } else { + gpsData.state_position++; } - if (cmp32(gpsData.now, last_state_position_time) < GPS_CONFIG_CHANGE_INTERVAL) { - return; - } - last_state_position_time = gpsData.now; - - switch (gpsData.state_position) { - // if a UBX command is sent, ack is supposed to give position++ once the reply happens - case UBLOX_DETECT_UNIT: // 400 in debug - if (gpsData.platformVersion == UBX_VERSION_UNDEF) { - ubloxSendClassMessage(CLASS_MON, MSG_MON_VER, 0); - } else { - gpsData.state_position++; - } - break; - case UBLOX_SLOW_NAV_RATE: // 401 in debug - ubloxSetNavRate(1, 1, 1); // throttle nav data rate to one per second, until configured - break; - case UBLOX_MSG_DISABLE_NMEA: - if (gpsData.ubloxM9orAbove) { - ubloxDisableNMEAValSet(); - gpsData.state_position = UBLOX_MSG_RMC; // skip UBX NMEA entries - goes to RMC plus one, or ACQUIRE_MODEL - } else { - gpsData.state_position++; - } - break; - case UBLOX_MSG_VGS: //Disable NMEA Messages - ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_VTG, 0); // VGS: Course over ground and Ground speed - break; - case UBLOX_MSG_GSV: - ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GSV, 0); // GSV: GNSS Satellites in View - break; - case UBLOX_MSG_GLL: - ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GLL, 0); // GLL: Latitude and longitude, with time of position fix and status - break; - case UBLOX_MSG_GGA: - ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GGA, 0); // GGA: Global positioning system fix data - break; - case UBLOX_MSG_GSA: - ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GSA, 0); // GSA: GNSS DOP and Active Satellites - break; - case UBLOX_MSG_RMC: - ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_RMC, 0); // RMC: Recommended Minimum data - break; - case UBLOX_ACQUIRE_MODEL: - ubloxSendNAV5Message(gpsConfig()->gps_ublox_acquire_model); - break; + break; + case UBLOX_MSG_VGS: //Disable NMEA Messages + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_VTG, 0); // VGS: Course over ground and Ground speed + break; + case UBLOX_MSG_GSV: + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GSV, 0); // GSV: GNSS Satellites in View + break; + case UBLOX_MSG_GLL: + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GLL, 0); // GLL: Latitude and longitude, with time of position fix and status + break; + case UBLOX_MSG_GGA: + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GGA, 0); // GGA: Global positioning system fix data + break; + case UBLOX_MSG_GSA: + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GSA, 0); // GSA: GNSS DOP and Active Satellites + break; + case UBLOX_MSG_RMC: + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_RMC, 0); // RMC: Recommended Minimum data + break; + case UBLOX_ACQUIRE_MODEL: + ubloxSendNAV5Message(gpsConfig()->gps_ublox_acquire_model); + break; // *** temporarily disabled // case UBLOX_CFG_ANA: // i f (gpsData.ubloxM7orAbove) { // NavX5 support existed in M5 - in theory we could remove that check @@ -1178,126 +1181,126 @@ static void gpsConfigureUblox(void) // gpsData.state_position++; // } // break; - case UBLOX_SET_SBAS: - ubloxSetSbas(); - break; - case UBLOX_SET_PMS: - if (gpsData.ubloxM8orAbove) { - ubloxSendPowerMode(); - } else { - gpsData.state_position++; - } - break; - case UBLOX_MSG_NAV_PVT: //Enable NAV-PVT Messages - if (gpsData.ubloxM9orAbove) { - ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_PVT_UART1, 1); - } else if (gpsData.ubloxM7orAbove) { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_PVT, 1); - } else { - gpsData.state_position++; - } - break; - // if NAV-PVT is enabled, we don't need the older nav messages - case UBLOX_MSG_SOL: - if (gpsData.ubloxM9orAbove) { - // SOL is deprecated above M8 - gpsData.state_position++; - } else if (gpsData.ubloxM7orAbove) { - // use NAV-PVT, so don't use NAV-SOL - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SOL, 0); - } else { - // Only use NAV-SOL below M7 - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SOL, 1); - } - break; - case UBLOX_MSG_POSLLH: - if (gpsData.ubloxM7orAbove) { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_POSLLH, 0); - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_POSLLH, 1); - } - break; - case UBLOX_MSG_STATUS: - if (gpsData.ubloxM7orAbove) { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_STATUS, 0); - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_STATUS, 1); - } - break; - case UBLOX_MSG_VELNED: - if (gpsData.ubloxM7orAbove) { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_VELNED, 0); - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_VELNED, 1); - } - break; - case UBLOX_MSG_DOP: - // nav-pvt has what we need and is available M7 and above - if (gpsData.ubloxM9orAbove) { - ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_DOP_UART1, 0); - } else if (gpsData.ubloxM7orAbove) { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_DOP, 0); - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_NAV_DOP, 1); - } - break; - case UBLOX_SAT_INFO: - // enable by default, turned off when armed and receiving data to reduce in-flight traffic - setSatInfoMessageRate(5); - break; - case UBLOX_SET_NAV_RATE: - // set the nav solution rate to the user's configured update rate - gpsData.updateRateHz = gpsConfig()->gps_update_rate_hz; - ubloxSetNavRate(gpsData.updateRateHz, 1, 1); - break; - case UBLOX_MSG_CFG_GNSS: - if ((gpsConfig()->sbasMode == SBAS_NONE) || (gpsConfig()->gps_ublox_use_galileo)) { - ubloxSendPollMessage(MSG_CFG_GNSS); // poll messages wait for ACK - } else { - gpsData.state_position++; - } - break; - case UBLOX_CONFIG_COMPLETE: - gpsSetState(GPS_STATE_RECEIVING_DATA); - break; - // TO DO: (separate PR) add steps that remove I2C or SPI data on ValSet aware units, eg - // ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_I2C, 0); - // ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_SPI, 0); - default: - break; - } - } - - // check the ackState after changing CONFIG state, or every iteration while not UBLOX_ACK_IDLE - switch (gpsData.ackState) { - case UBLOX_ACK_IDLE: - break; - case UBLOX_ACK_WAITING: - if (cmp32(gpsData.now, gpsData.lastMessageSent) > UBLOX_ACK_TIMEOUT_MS){ - // give up, treat it like receiving ack - gpsData.ackState = UBLOX_ACK_GOT_ACK; - } - break; - case UBLOX_ACK_GOT_ACK: - // move forward one position, and clear the ack state + case UBLOX_SET_SBAS: + ubloxSetSbas(); + break; + case UBLOX_SET_PMS: + if (gpsData.ubloxM8orAbove) { + ubloxSendPowerMode(); + } else { gpsData.state_position++; - gpsData.ackState = UBLOX_ACK_IDLE; - break; - case UBLOX_ACK_GOT_NACK: - // this is the tricky bit - // and we absolutely must get the unit type right - if (gpsData.state_position == UBLOX_DETECT_UNIT) { - gpsSetState(GPS_STATE_CONFIGURE); - gpsData.ackState = UBLOX_ACK_IDLE; - } else { - // otherwise, for testing: just ignore nacks - gpsData.state_position++; - gpsData.ackState = UBLOX_ACK_IDLE; - } - break; - default: - break; + } + break; + case UBLOX_MSG_NAV_PVT: //Enable NAV-PVT Messages + if (gpsData.ubloxM9orAbove) { + ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_PVT_UART1, 1); + } else if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_PVT, 1); + } else { + gpsData.state_position++; + } + break; + // if NAV-PVT is enabled, we don't need the older nav messages + case UBLOX_MSG_SOL: + if (gpsData.ubloxM9orAbove) { + // SOL is deprecated above M8 + gpsData.state_position++; + } else if (gpsData.ubloxM7orAbove) { + // use NAV-PVT, so don't use NAV-SOL + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SOL, 0); + } else { + // Only use NAV-SOL below M7 + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SOL, 1); + } + break; + case UBLOX_MSG_POSLLH: + if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_POSLLH, 0); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_POSLLH, 1); + } + break; + case UBLOX_MSG_STATUS: + if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_STATUS, 0); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_STATUS, 1); + } + break; + case UBLOX_MSG_VELNED: + if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_VELNED, 0); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_VELNED, 1); + } + break; + case UBLOX_MSG_DOP: + // nav-pvt has what we need and is available M7 and above + if (gpsData.ubloxM9orAbove) { + ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_DOP_UART1, 0); + } else if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_DOP, 0); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_DOP, 1); + } + break; + case UBLOX_SAT_INFO: + // enable by default, turned off when armed and receiving data to reduce in-flight traffic + setSatInfoMessageRate(5); + break; + case UBLOX_SET_NAV_RATE: + // set the nav solution rate to the user's configured update rate + gpsData.updateRateHz = gpsConfig()->gps_update_rate_hz; + ubloxSetNavRate(gpsData.updateRateHz, 1, 1); + break; + case UBLOX_MSG_CFG_GNSS: + if ((gpsConfig()->sbasMode == SBAS_NONE) || (gpsConfig()->gps_ublox_use_galileo)) { + ubloxSendPollMessage(MSG_CFG_GNSS); // poll messages wait for ACK + } else { + gpsData.state_position++; + } + break; + case UBLOX_CONFIG_COMPLETE: + gpsSetState(GPS_STATE_RECEIVING_DATA); + break; + // TO DO: (separate PR) add steps that remove I2C or SPI data on ValSet aware units, eg + // ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_I2C, 0); + // ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_SPI, 0); + default: + break; } + } + + // check the ackState after changing CONFIG state, or every iteration while not UBLOX_ACK_IDLE + switch (gpsData.ackState) { + case UBLOX_ACK_IDLE: + break; + case UBLOX_ACK_WAITING: + if (cmp32(gpsData.now, gpsData.lastMessageSent) > UBLOX_ACK_TIMEOUT_MS){ + // give up, treat it like receiving ack + gpsData.ackState = UBLOX_ACK_GOT_ACK; + } + break; + case UBLOX_ACK_GOT_ACK: + // move forward one position, and clear the ack state + gpsData.state_position++; + gpsData.ackState = UBLOX_ACK_IDLE; + break; + case UBLOX_ACK_GOT_NACK: + // this is the tricky bit + // and we absolutely must get the unit type right + if (gpsData.state_position == UBLOX_DETECT_UNIT) { + gpsSetState(GPS_STATE_CONFIGURE); + gpsData.ackState = UBLOX_ACK_IDLE; + } else { + // otherwise, for testing: just ignore nacks + gpsData.state_position++; + gpsData.ackState = UBLOX_ACK_IDLE; + } + break; + default: + break; + } } } #endif // USE_GPS_UBLOX @@ -1538,7 +1541,8 @@ static void gpsNewData(uint16_t c) } #ifdef USE_GPS_UBLOX -static ubloxVersion_e ubloxParseVersion(const uint32_t version) { +static ubloxVersion_e ubloxParseVersion(const uint32_t version) +{ for (size_t i = 0; i < ARRAYLEN(ubloxVersionMap); ++i) { if (version == ubloxVersionMap[i].hw) { return (ubloxVersion_e) i; @@ -1687,113 +1691,113 @@ static void parseFieldNmea(gpsDataNmea_t *data, char *str, uint8_t gpsFrame, uin switch (gpsFrame) { - case FRAME_GGA: //************* GPGGA FRAME parsing - switch (idx) { - case 1: - data->time = ((uint8_t)(str[5] - '0') * 10 + (uint8_t)(str[7] - '0')) * 100; - break; - case 2: - data->latitude = GPS_coord_to_degrees(str); - break; - case 3: - if (str[0] == 'S') data->latitude *= -1; - break; - case 4: - data->longitude = GPS_coord_to_degrees(str); - break; - case 5: - if (str[0] == 'W') data->longitude *= -1; - break; - case 6: - gpsSetFixState(str[0] > '0'); - break; - case 7: - data->numSat = grab_fields(str, 0); - break; - case 9: - data->altitudeCm = grab_fields(str, 1) * 10; // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10 - break; - } + case FRAME_GGA: //************* GPGGA FRAME parsing + switch (idx) { + case 1: + data->time = ((uint8_t)(str[5] - '0') * 10 + (uint8_t)(str[7] - '0')) * 100; + break; + case 2: + data->latitude = GPS_coord_to_degrees(str); + break; + case 3: + if (str[0] == 'S') data->latitude *= -1; + break; + case 4: + data->longitude = GPS_coord_to_degrees(str); + break; + case 5: + if (str[0] == 'W') data->longitude *= -1; + break; + case 6: + gpsSetFixState(str[0] > '0'); + break; + case 7: + data->numSat = grab_fields(str, 0); + break; + case 9: + data->altitudeCm = grab_fields(str, 1) * 10; // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10 + break; + } + break; + + case FRAME_RMC: //************* GPRMC FRAME parsing + switch (idx) { + case 1: + data->time = grab_fields(str, 2); // UTC time hhmmss.ss + break; + case 7: + data->speed = ((grab_fields(str, 1) * 5144L) / 1000L); // speed in cm/s added by Mis + break; + case 8: + data->ground_course = (grab_fields(str, 1)); // ground course deg * 10 + break; + case 9: + data->date = grab_fields(str, 0); // date dd/mm/yy + break; + } + break; + + case FRAME_GSV: + switch (idx) { + /*case 1: + // Total number of messages of this type in this cycle + break; */ + case 2: + // Message number + svMessageNum = grab_fields(str, 0); + break; + case 3: + // Total number of SVs visible + GPS_numCh = MIN(grab_fields(str, 0), GPS_SV_MAXSATS_LEGACY); + break; + } + if (idx < 4) break; - case FRAME_RMC: //************* GPRMC FRAME parsing - switch (idx) { - case 1: - data->time = grab_fields(str, 2); // UTC time hhmmss.ss - break; - case 7: - data->speed = ((grab_fields(str, 1) * 5144L) / 1000L); // speed in cm/s added by Mis - break; - case 8: - data->ground_course = (grab_fields(str, 1)); // ground course deg * 10 - break; - case 9: - data->date = grab_fields(str, 0); // date dd/mm/yy - break; - } + svPacketIdx = (idx - 4) / 4 + 1; // satellite number in packet, 1-4 + svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number + svSatParam = idx - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite + + if (svSatNum > GPS_SV_MAXSATS_LEGACY) break; - case FRAME_GSV: - switch (idx) { - /*case 1: - // Total number of messages of this type in this cycle - break; */ - case 2: - // Message number - svMessageNum = grab_fields(str, 0); - break; - case 3: - // Total number of SVs visible - GPS_numCh = MIN(grab_fields(str, 0), GPS_SV_MAXSATS_LEGACY); - break; - } - if (idx < 4) - break; - - svPacketIdx = (idx - 4) / 4 + 1; // satellite number in packet, 1-4 - svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number - svSatParam = idx - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite - - if (svSatNum > GPS_SV_MAXSATS_LEGACY) - break; - - switch (svSatParam) { - case 1: - // SV PRN number - GPS_svinfo[svSatNum - 1].chn = svSatNum; - GPS_svinfo[svSatNum - 1].svid = grab_fields(str, 0); - break; - /*case 2: - // Elevation, in degrees, 90 maximum - break; - case 3: - // Azimuth, degrees from True North, 000 through 359 - break; */ - case 4: - // SNR, 00 through 99 dB (null when not tracking) - GPS_svinfo[svSatNum - 1].cno = grab_fields(str, 0); - GPS_svinfo[svSatNum - 1].quality = 0; // only used by ublox - break; - } + switch (svSatParam) { + case 1: + // SV PRN number + GPS_svinfo[svSatNum - 1].chn = svSatNum; + GPS_svinfo[svSatNum - 1].svid = grab_fields(str, 0); + break; + /*case 2: + // Elevation, in degrees, 90 maximum + break; + case 3: + // Azimuth, degrees from True North, 000 through 359 + break; */ + case 4: + // SNR, 00 through 99 dB (null when not tracking) + GPS_svinfo[svSatNum - 1].cno = grab_fields(str, 0); + GPS_svinfo[svSatNum - 1].quality = 0; // only used by ublox + break; + } #ifdef USE_DASHBOARD - dashboardGpsNavSvInfoRcvCount++; + dashboardGpsNavSvInfoRcvCount++; #endif - break; + break; - case FRAME_GSA: - switch (idx) { - case 15: - data->pdop = grab_fields(str, 2); // pDOP * 100 - break; - case 16: - data->hdop = grab_fields(str, 2); // hDOP * 100 - break; - case 17: - data->vdop = grab_fields(str, 2); // vDOP * 100 - break; - } + case FRAME_GSA: + switch (idx) { + case 15: + data->pdop = grab_fields(str, 2); // pDOP * 100 break; + case 16: + data->hdop = grab_fields(str, 2); // hDOP * 100 + break; + case 17: + data->vdop = grab_fields(str, 2); // vDOP * 100 + break; + } + break; } } @@ -1803,55 +1807,55 @@ static bool writeGpsSolutionNmea(gpsSolutionData_t *sol, const gpsDataNmea_t *da const uint32_t msInTenSeconds = 10000; switch (gpsFrame) { - case FRAME_GGA: + case FRAME_GGA: #ifdef USE_DASHBOARD - *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_GGA; + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_GGA; #endif - if (STATE(GPS_FIX)) { - sol->llh.lat = data->latitude; - sol->llh.lon = data->longitude; - sol->numSat = data->numSat; - sol->llh.altCm = data->altitudeCm; - } - navDeltaTimeMs = (msInTenSeconds + data->time - gpsData.lastNavSolTs) % msInTenSeconds; - gpsData.lastNavSolTs = data->time; - sol->navIntervalMs = constrain(navDeltaTimeMs, 50, 2500); - // return only one true statement to trigger one "newGpsDataReady" flag per GPS loop - return true; + if (STATE(GPS_FIX)) { + sol->llh.lat = data->latitude; + sol->llh.lon = data->longitude; + sol->numSat = data->numSat; + sol->llh.altCm = data->altitudeCm; + } + navDeltaTimeMs = (msInTenSeconds + data->time - gpsData.lastNavSolTs) % msInTenSeconds; + gpsData.lastNavSolTs = data->time; + sol->navIntervalMs = constrain(navDeltaTimeMs, 50, 2500); + // return only one true statement to trigger one "newGpsDataReady" flag per GPS loop + return true; - case FRAME_GSA: + case FRAME_GSA: #ifdef USE_DASHBOARD - *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_GSA; + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_GSA; #endif - sol->dop.pdop = data->pdop; - sol->dop.hdop = data->hdop; - sol->dop.vdop = data->vdop; - return false; + sol->dop.pdop = data->pdop; + sol->dop.hdop = data->hdop; + sol->dop.vdop = data->vdop; + return false; - case FRAME_RMC: + case FRAME_RMC: #ifdef USE_DASHBOARD - *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_RMC; + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_RMC; #endif - sol->groundSpeed = data->speed; - sol->groundCourse = data->ground_course; + sol->groundSpeed = data->speed; + sol->groundCourse = data->ground_course; #ifdef USE_RTC_TIME - // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid - if(!rtcHasTime() && data->date != 0 && data->time != 0) { - dateTime_t temp_time; - temp_time.year = (data->date % 100) + 2000; - temp_time.month = (data->date / 100) % 100; - temp_time.day = (data->date / 10000) % 100; - temp_time.hours = (data->time / 1000000) % 100; - temp_time.minutes = (data->time / 10000) % 100; - temp_time.seconds = (data->time / 100) % 100; - temp_time.millis = (data->time & 100) * 10; - rtcSetDateTime(&temp_time); - } + // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid + if (!rtcHasTime() && data->date != 0 && data->time != 0) { + dateTime_t temp_time; + temp_time.year = (data->date % 100) + 2000; + temp_time.month = (data->date / 100) % 100; + temp_time.day = (data->date / 10000) % 100; + temp_time.hours = (data->time / 1000000) % 100; + temp_time.minutes = (data->time / 10000) % 100; + temp_time.seconds = (data->time / 100) % 100; + temp_time.millis = (data->time & 100) * 10; + rtcSetDateTime(&temp_time); + } #endif - return false; + return false; - default: - return false; + default: + return false; } } @@ -1865,68 +1869,68 @@ static bool gpsNewFrameNMEA(char c) switch (c) { - case '$': - param = 0; - offset = 0; - parity = 0; - break; + case '$': + param = 0; + offset = 0; + parity = 0; + break; - case ',': - case '*': - string[offset] = 0; - if (param == 0) { // frame identification (5 chars, e.g. "GPGGA", "GNGGA", "GLGGA", ...) - gps_frame = NO_FRAME; - if (strcmp(&string[2], "GGA") == 0) { - gps_frame = FRAME_GGA; - } else if (strcmp(&string[2], "RMC") == 0) { - gps_frame = FRAME_RMC; - } else if (strcmp(&string[2], "GSV") == 0) { - gps_frame = FRAME_GSV; - } else if (strcmp(&string[2], "GSA") == 0) { - gps_frame = FRAME_GSA; - } + case ',': + case '*': + string[offset] = 0; + if (param == 0) { // frame identification (5 chars, e.g. "GPGGA", "GNGGA", "GLGGA", ...) + gps_frame = NO_FRAME; + if (strcmp(&string[2], "GGA") == 0) { + gps_frame = FRAME_GGA; + } else if (strcmp(&string[2], "RMC") == 0) { + gps_frame = FRAME_RMC; + } else if (strcmp(&string[2], "GSV") == 0) { + gps_frame = FRAME_GSV; + } else if (strcmp(&string[2], "GSA") == 0) { + gps_frame = FRAME_GSA; } + } - // parse string and write data into gps_msg - parseFieldNmea(&gps_msg, string, gps_frame, param); + // parse string and write data into gps_msg + parseFieldNmea(&gps_msg, string, gps_frame, param); - param++; - offset = 0; - if (c == '*') - checksum_param = 1; - else - parity ^= c; - break; + param++; + offset = 0; + if (c == '*') + checksum_param = 1; + else + parity ^= c; + break; - case '\r': - case '\n': - if (checksum_param) { //parity checksum + case '\r': + case '\n': + if (checksum_param) { //parity checksum #ifdef USE_DASHBOARD - shiftPacketLog(); + shiftPacketLog(); #endif - uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); - if (checksum == parity) { + uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); + if (checksum == parity) { #ifdef USE_DASHBOARD - *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_IGNORED; - dashboardGpsPacketCount++; -#endif - receivedNavMessage = writeGpsSolutionNmea(&gpsSol, &gps_msg, gps_frame); // // write gps_msg into gpsSol - } -#ifdef USE_DASHBOARD - else { - *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_ERROR; - } + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_IGNORED; + dashboardGpsPacketCount++; #endif + receivedNavMessage = writeGpsSolutionNmea(&gpsSol, &gps_msg, gps_frame); // // write gps_msg into gpsSol } - checksum_param = 0; - break; +#ifdef USE_DASHBOARD + else { + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_ERROR; + } +#endif + } + checksum_param = 0; + break; - default: - if (offset < 15) - string[offset++] = c; - if (!checksum_param) - parity ^= c; - break; + default: + if (offset < 15) + string[offset++] = c; + if (!checksum_param) + parity ^= c; + break; } return receivedNavMessage; @@ -2387,112 +2391,52 @@ static bool gpsNewFrameUBLOX(uint8_t data) bool newPositionDataReceived = false; switch (ubxFrameParseState) { - case UBX_PARSE_PREAMBLE_SYNC_1: - if (PREAMBLE1 == data) { - // Might be a new UBX message, go on to look for next preamble byte. - ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; - break; - } - // Not a new UBX message, stay in this state for the next incoming byte. + case UBX_PARSE_PREAMBLE_SYNC_1: + if (PREAMBLE1 == data) { + // Might be a new UBX message, go on to look for next preamble byte. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; break; - case UBX_PARSE_PREAMBLE_SYNC_2: - if (PREAMBLE2 == data) { - // Matches the two-byte preamble, seems to be a legit message, go on to process the rest of the message. - ubxFrameParseState = UBX_PARSE_MESSAGE_CLASS; - break; - } - // False start, if this byte is not a preamble 1, restart new message parsing. - // If this byte is a preamble 1, we might have gotten two in a row, so stay here and look for preamble 2 again. - if (PREAMBLE1 != data) { - ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; - } + } + // Not a new UBX message, stay in this state for the next incoming byte. + break; + case UBX_PARSE_PREAMBLE_SYNC_2: + if (PREAMBLE2 == data) { + // Matches the two-byte preamble, seems to be a legit message, go on to process the rest of the message. + ubxFrameParseState = UBX_PARSE_MESSAGE_CLASS; break; - case UBX_PARSE_MESSAGE_CLASS: - ubxRcvMsgChecksumB = ubxRcvMsgChecksumA = data; // Reset & start the checksum A & B accumulators. - ubxRcvMsgClass = data; - ubxFrameParseState = UBX_PARSE_MESSAGE_ID; + } + // False start, if this byte is not a preamble 1, restart new message parsing. + // If this byte is a preamble 1, we might have gotten two in a row, so stay here and look for preamble 2 again. + if (PREAMBLE1 != data) { + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; + } + break; + case UBX_PARSE_MESSAGE_CLASS: + ubxRcvMsgChecksumB = ubxRcvMsgChecksumA = data; // Reset & start the checksum A & B accumulators. + ubxRcvMsgClass = data; + ubxFrameParseState = UBX_PARSE_MESSAGE_ID; + break; + case UBX_PARSE_MESSAGE_ID: + ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. + ubxRcvMsgID = data; + ubxFrameParseState = UBX_PARSE_PAYLOAD_LENGTH_LSB; + break; + case UBX_PARSE_PAYLOAD_LENGTH_LSB: + ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); + ubxRcvMsgPayloadLength = data; // Payload length LSB. + ubxFrameParseState = UBX_PARSE_PAYLOAD_LENGTH_MSB; + break; + case UBX_PARSE_PAYLOAD_LENGTH_MSB: + ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. + ubxRcvMsgPayloadLength += (uint16_t)(data << 8); //Payload length MSB. + if (ubxRcvMsgPayloadLength == 0) { + // No payload for this message, skip to checksum checking. + ubxFrameParseState = UBX_PARSE_CHECKSUM_A; break; - case UBX_PARSE_MESSAGE_ID: - ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. - ubxRcvMsgID = data; - ubxFrameParseState = UBX_PARSE_PAYLOAD_LENGTH_LSB; - break; - case UBX_PARSE_PAYLOAD_LENGTH_LSB: - ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); - ubxRcvMsgPayloadLength = data; // Payload length LSB. - ubxFrameParseState = UBX_PARSE_PAYLOAD_LENGTH_MSB; - break; - case UBX_PARSE_PAYLOAD_LENGTH_MSB: - ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. - ubxRcvMsgPayloadLength += (uint16_t)(data << 8); //Payload length MSB. - if (ubxRcvMsgPayloadLength == 0) { - // No payload for this message, skip to checksum checking. - ubxFrameParseState = UBX_PARSE_CHECKSUM_A; - break; - } - if (ubxRcvMsgPayloadLength > UBLOX_MAX_PAYLOAD_SANITY_SIZE) { - // Payload length is not reasonable, treat as a bad packet, restart new message parsing. - // Note that we do not parse the rest of the message, better to leave it and look for a new message. -#ifdef USE_DASHBOARD - logErrorToPacketLog(); -#endif - if (PREAMBLE1 == data) { - // If this byte is a preamble 1 value, it might be a new message, so look for preamble 2 instead of starting over. - ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; - } else { - ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; - } - break; - } - // Payload length seems legit, go on to receive the payload content. - ubxFrameParsePayloadCounter = 0; - ubxFrameParseState = UBX_PARSE_PAYLOAD_CONTENT; - break; - case UBX_PARSE_PAYLOAD_CONTENT: - ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. - if (ubxFrameParsePayloadCounter < UBLOX_PAYLOAD_SIZE) { - // Only add bytes to the buffer if we haven't reached the max supported payload size. - // Note that we still read & checksum every byte so the checksum calculates correctly. - ubxRcvMsgPayload.rawBytes[ubxFrameParsePayloadCounter] = data; - } - if (++ubxFrameParsePayloadCounter >= ubxRcvMsgPayloadLength) { - // All bytes for payload length processed. - ubxFrameParseState = UBX_PARSE_CHECKSUM_A; - break; - } - // More payload content left, stay in this state. - break; - case UBX_PARSE_CHECKSUM_A: - if (ubxRcvMsgChecksumA == data) { - // Checksum A matches, go on to checksum B. - ubxFrameParseState = UBX_PARSE_CHECKSUM_B; - break; - } - // Bad checksum A, restart new message parsing. - // Note that we do not parse checksum B, new message processing will handle skipping it if needed. -#ifdef USE_DASHBOARD - logErrorToPacketLog(); -#endif - if (PREAMBLE1 == data) { - // If this byte is a preamble 1 value, it might be a new message, so look for preamble 2 instead of starting over. - ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; - } else { - ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; - } - break; - case UBX_PARSE_CHECKSUM_B: - if (ubxRcvMsgChecksumB == data) { - // Checksum B also matches, successfully received a new full packet! -#ifdef USE_DASHBOARD - dashboardGpsPacketCount++; // Packet counter used by dashboard device. - shiftPacketLog(); // Make space for message handling to add the message type char to the dashboard device packet log. -#endif - // Handle the parsed message. Note this is a questionable inverted call dependency, but something for a later refactoring. - newPositionDataReceived = UBLOX_parse_gps(); // True only when we have new position data from the parsed message. - ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; // Restart new message parsing. - break; - } - // Bad checksum B, restart new message parsing. + } + if (ubxRcvMsgPayloadLength > UBLOX_MAX_PAYLOAD_SANITY_SIZE) { + // Payload length is not reasonable, treat as a bad packet, restart new message parsing. + // Note that we do not parse the rest of the message, better to leave it and look for a new message. #ifdef USE_DASHBOARD logErrorToPacketLog(); #endif @@ -2504,6 +2448,66 @@ static bool gpsNewFrameUBLOX(uint8_t data) } break; } + // Payload length seems legit, go on to receive the payload content. + ubxFrameParsePayloadCounter = 0; + ubxFrameParseState = UBX_PARSE_PAYLOAD_CONTENT; + break; + case UBX_PARSE_PAYLOAD_CONTENT: + ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. + if (ubxFrameParsePayloadCounter < UBLOX_PAYLOAD_SIZE) { + // Only add bytes to the buffer if we haven't reached the max supported payload size. + // Note that we still read & checksum every byte so the checksum calculates correctly. + ubxRcvMsgPayload.rawBytes[ubxFrameParsePayloadCounter] = data; + } + if (++ubxFrameParsePayloadCounter >= ubxRcvMsgPayloadLength) { + // All bytes for payload length processed. + ubxFrameParseState = UBX_PARSE_CHECKSUM_A; + break; + } + // More payload content left, stay in this state. + break; + case UBX_PARSE_CHECKSUM_A: + if (ubxRcvMsgChecksumA == data) { + // Checksum A matches, go on to checksum B. + ubxFrameParseState = UBX_PARSE_CHECKSUM_B; + break; + } + // Bad checksum A, restart new message parsing. + // Note that we do not parse checksum B, new message processing will handle skipping it if needed. +#ifdef USE_DASHBOARD + logErrorToPacketLog(); +#endif + if (PREAMBLE1 == data) { + // If this byte is a preamble 1 value, it might be a new message, so look for preamble 2 instead of starting over. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; + } else { + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; + } + break; + case UBX_PARSE_CHECKSUM_B: + if (ubxRcvMsgChecksumB == data) { + // Checksum B also matches, successfully received a new full packet! +#ifdef USE_DASHBOARD + dashboardGpsPacketCount++; // Packet counter used by dashboard device. + shiftPacketLog(); // Make space for message handling to add the message type char to the dashboard device packet log. +#endif + // Handle the parsed message. Note this is a questionable inverted call dependency, but something for a later refactoring. + newPositionDataReceived = UBLOX_parse_gps(); // True only when we have new position data from the parsed message. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; // Restart new message parsing. + break; + } + // Bad checksum B, restart new message parsing. +#ifdef USE_DASHBOARD + logErrorToPacketLog(); +#endif + if (PREAMBLE1 == data) { + // If this byte is a preamble 1 value, it might be a new message, so look for preamble 2 instead of starting over. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; + } else { + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; + } + break; + } // Note this function returns if UBLOX_parse_gps() found new position data, NOT whether this function successfully parsed the frame or not. return newPositionDataReceived; @@ -2567,15 +2571,13 @@ static void GPS_calculateDistanceFlown(bool initialize) if (initialize) { GPS_distanceFlownInCm = 0; - } else { - if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) { - uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed; - // Only add up movement when speed is faster than minimum threshold - if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) { - uint32_t dist; - GPS_distance_cm_bearing(&gpsSol.llh, &lastLLH, gpsConfig()->gps_use_3d_speed, &dist, NULL); - GPS_distanceFlownInCm += dist; - } + } else if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) { + uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed; + // Only add up movement when speed is faster than minimum threshold + if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) { + uint32_t dist; + GPS_distance_cm_bearing(&gpsSol.llh, &lastLLH, gpsConfig()->gps_use_3d_speed, &dist, NULL); + GPS_distanceFlownInCm += dist; } } lastLLH = gpsSol.llh; @@ -2724,4 +2726,3 @@ baudRate_e getGpsPortActualBaudRateIndex(void) } #endif // USE_GPS - From d63726f92878bb5f0adf388c30231cd2bd4e32ff Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Tue, 11 Feb 2025 06:25:04 +0000 Subject: [PATCH 19/58] Auto updated submodule references [11-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 64337752ec..5ce9d40849 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 64337752ec6af16a8663b4df48d8a9eb8fb1fbdb +Subproject commit 5ce9d4084986d511d606cce66320fc028ec9483a From b96c0b69cbe0678dcec981363203937feebb5782 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Tue, 11 Feb 2025 15:57:21 +0100 Subject: [PATCH 20/58] CLI - prevent serial overflow (#14251) use serialWriteBufBlocking for cli, busy-waiting if TX buffer is full --- src/main/cli/cli.c | 2 +- src/main/drivers/serial.c | 12 ++++++++++++ src/main/drivers/serial.h | 1 + 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index a619f06a83..40e080bdd3 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -6896,7 +6896,7 @@ void cliEnter(serialPort_t *serialPort, bool interactive) setPrintfSerialPort(cliPort); } - bufWriterInit(&cliWriterDesc, cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort); + bufWriterInit(&cliWriterDesc, cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufBlockingShim, serialPort); cliErrorWriter = cliWriter = &cliWriterDesc; if (interactive) { diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index 6fae5d7e59..d8bf92474a 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -129,8 +129,20 @@ void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) serialEndWrite(instance); } +void serialWriteBufBlocking(serialPort_t *instance, const uint8_t *data, int count) +{ + while (serialTxBytesFree(instance) < (uint32_t)count) /* NOP */; + serialWriteBuf(instance, data, count); +} + void serialWriteBufShim(void *instance, const uint8_t *data, int count) { serialWriteBuf((serialPort_t *)instance, data, count); } +void serialWriteBufBlockingShim(void *instance, const uint8_t *data, int count) +{ + serialWriteBufBlocking(instance, data, count); +} + + diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index f5f21bb74e..ce9a8ca1f9 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -150,5 +150,6 @@ uint32_t serialGetBaudRate(serialPort_t *instance); // A shim that adapts the bufWriter API to the serialWriteBuf() API. void serialWriteBufShim(void *instance, const uint8_t *data, int count); +void serialWriteBufBlockingShim(void *instance, const uint8_t *data, int count); void serialBeginWrite(serialPort_t *instance); void serialEndWrite(serialPort_t *instance); From eae7e350029a783e8c9c18d55b94c9ed1c8afc18 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Fri, 14 Feb 2025 06:25:04 +0000 Subject: [PATCH 21/58] Auto updated submodule references [14-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 5ce9d40849..cab072104b 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 5ce9d4084986d511d606cce66320fc028ec9483a +Subproject commit cab072104b28b92a7b6c2946c4de1e76648e570f From b6cfe2e2074ddae4bb9455dc2bca7470164add65 Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Fri, 14 Feb 2025 10:03:28 +0100 Subject: [PATCH 22/58] Add SPI_DEVICE_4 to AT32 targets (#14255) --- src/platform/AT32/target/AT32F435G/target.h | 1 + src/platform/AT32/target/AT32F435M/target.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/platform/AT32/target/AT32F435G/target.h b/src/platform/AT32/target/AT32F435G/target.h index 76a4e039dc..b76a523cd4 100644 --- a/src/platform/AT32/target/AT32F435G/target.h +++ b/src/platform/AT32/target/AT32F435G/target.h @@ -60,6 +60,7 @@ #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 #define USE_SPI_DEVICE_3 +#define USE_SPI_DEVICE_4 #define USE_SPI_DMA_ENABLE_LATE #define USE_EXTI diff --git a/src/platform/AT32/target/AT32F435M/target.h b/src/platform/AT32/target/AT32F435M/target.h index 3429a10876..fb52e7fcea 100644 --- a/src/platform/AT32/target/AT32F435M/target.h +++ b/src/platform/AT32/target/AT32F435M/target.h @@ -60,6 +60,7 @@ #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 #define USE_SPI_DEVICE_3 +#define USE_SPI_DEVICE_4 #define USE_SPI_DMA_ENABLE_LATE #define USE_EXTI From 1db7c1184dd29e86b049e528f82a7e6ef3af5073 Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Fri, 14 Feb 2025 16:35:38 +0100 Subject: [PATCH 23/58] Reduce required motors for MIXER_CUSTOM_AIRPLANE (#14257) Update CUSTOM_AIRPLANE mixer to require one motor --- src/main/flight/mixer_init.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index be226230f8..e1d0eafe39 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -268,7 +268,7 @@ const mixer_t mixers[] = { { 1, true, NULL }, // MIXER_SINGLECOPTER { 4, false, mixerAtail4 }, // MIXER_ATAIL4 { 0, false, NULL }, // MIXER_CUSTOM - { 2, true, NULL }, // MIXER_CUSTOM_AIRPLANE + { 1, true, NULL }, // MIXER_CUSTOM_AIRPLANE { 3, true, NULL }, // MIXER_CUSTOM_TRI { 4, false, mixerQuadX1234 }, // MIXER_QUADX_1234 { 8, false, mixerOctoX8P }, // MIXER_OCTOX8P From 66ce15c74e9b1172fa6619769490b23c4c35c6c4 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Sat, 15 Feb 2025 06:25:03 +0000 Subject: [PATCH 24/58] Auto updated submodule references [15-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index cab072104b..ff6095ae17 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit cab072104b28b92a7b6c2946c4de1e76648e570f +Subproject commit ff6095ae177dc7f729dd827fdd9584bf28995cec From 95b43ec175b774d5362dd099afdddf0983a6b8b1 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Mon, 17 Feb 2025 06:25:03 +0000 Subject: [PATCH 25/58] Auto updated submodule references [17-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index ff6095ae17..bd712ef4cc 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit ff6095ae177dc7f729dd827fdd9584bf28995cec +Subproject commit bd712ef4cc19d400f715aabd9b96777739ae00fc From 7789ecafe3aa58c29a47da8f023bc891788cd468 Mon Sep 17 00:00:00 2001 From: kensherman <17434103+kensherman@users.noreply.github.com> Date: Mon, 17 Feb 2025 14:30:35 -0600 Subject: [PATCH 26/58] increasing number of msp ports (#14263) * increasing number of msp ports * Adding custom define and bumping to 6 ports * reverting msp ports to allow to be bumped at build time * Update src/main/msp/msp_serial.h Co-authored-by: Mark Haslinghuis * reset src/config --------- Co-authored-by: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Co-authored-by: Mark Haslinghuis --- src/main/msp/msp_serial.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index a080408fc7..315e003060 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -27,7 +27,9 @@ #include "msp/msp.h" // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 3 MSP ports. +#ifndef MAX_MSP_PORT_COUNT #define MAX_MSP_PORT_COUNT 3 +#endif typedef enum { PORT_IDLE, From 1b36ccc063c1544c4f3d2b135f548f04a90fcc61 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Tue, 18 Feb 2025 06:25:04 +0000 Subject: [PATCH 27/58] Auto updated submodule references [18-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index bd712ef4cc..651512d6e3 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit bd712ef4cc19d400f715aabd9b96777739ae00fc +Subproject commit 651512d6e33e346aed88a58e0f5f2e6025cc2592 From 094d36bb6f86c3a037538c15d3b2ceb381d85474 Mon Sep 17 00:00:00 2001 From: crteensy Date: Tue, 18 Feb 2025 20:16:04 +0100 Subject: [PATCH 28/58] STM32G4: Add option to use external 26 MHz clock (#14261) --- src/main/fc/init.c | 2 +- src/platform/STM32/startup/system_stm32g4xx.c | 16 +++++++++++++--- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/fc/init.c b/src/main/fc/init.c index bc0726fbbc..d48b5d6128 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -485,7 +485,7 @@ void init(void) #if defined(STM32F4) || defined(STM32G4) || defined(APM32F4) // F4 has non-8MHz boards - // G4 for Betaflight allow 24 or 27MHz oscillator + // G4 for Betaflight allow 8, 16, 24, 26 or 27MHz oscillator systemClockSetHSEValue(systemConfig()->hseMhz * 1000000U); #endif diff --git a/src/platform/STM32/startup/system_stm32g4xx.c b/src/platform/STM32/startup/system_stm32g4xx.c index 9874d2d1f1..5690987942 100644 --- a/src/platform/STM32/startup/system_stm32g4xx.c +++ b/src/platform/STM32/startup/system_stm32g4xx.c @@ -225,6 +225,7 @@ void Error_Handler(void) // Target frequencies for cpu_overclock (Levels 0 through 3) uint16_t sysclkSeries8[] = { 168, 192, 216, 240 }; +uint16_t sysclkSeries26[] = { 169, 195, 221, 247 }; uint16_t sysclkSeries27[] = { 171, 198, 225, 252 }; #define OVERCLOCK_LEVELS ARRAYLEN(sysclkSeries8) @@ -247,6 +248,13 @@ static bool systemComputePLLParameters(uint8_t src, uint16_t target, int *sysclk multDiff = vcoDiff / 16 * 2; *plln = 42 + multDiff; vcoFreq = 8 * *plln; + } else if (src == 26) { + *pllm = 2; + vcoBase = 169 * 2; + vcoDiff = vcoTarget - vcoBase; + multDiff = vcoDiff / 26 * 2; + *plln = 26 + multDiff; + vcoFreq = 13 * *plln; } else if (src == 27) { *pllm = 3; vcoBase = 171 * 2; @@ -289,8 +297,10 @@ static bool systemClock_PLLConfig(int overclockLevel) pllSrc = RCC_PLLSOURCE_HSE; if (pllInput == 8 || pllInput == 16 || pllInput == 24) { targetMhz = sysclkSeries8[overclockLevel]; + } else if (pllInput == 26) { + targetMhz = sysclkSeries26[overclockLevel]; } else if (pllInput == 27) { - targetMhz = sysclkSeries8[overclockLevel]; + targetMhz = sysclkSeries27[overclockLevel]; } else { return false; } @@ -303,9 +313,9 @@ void systemClockSetHSEValue(uint32_t frequency) { uint32_t freqMhz = frequency / 1000000; - // Only supported HSE crystal/resonator is 27MHz or integer multiples of 8MHz + // Only supported HSE crystal/resonator is 27MHz, 26 MHz or integer multiples of 8MHz - if (freqMhz != 27 && (freqMhz / 8) * 8 != freqMhz) { + if (freqMhz != 27 && freqMhz != 26 && (freqMhz / 8) * 8 != freqMhz) { return; } From df8beeeea8a549db7ca7822ac6b520ad827d4d3d Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Wed, 19 Feb 2025 06:25:03 +0000 Subject: [PATCH 29/58] Auto updated submodule references [19-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 651512d6e3..7fbb6e044c 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 651512d6e33e346aed88a58e0f5f2e6025cc2592 +Subproject commit 7fbb6e044c343825d9075950273c9d5518a03d14 From c91e3d1050b60bacdf1d03d93be66517467fccb3 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Thu, 20 Feb 2025 06:25:03 +0000 Subject: [PATCH 30/58] Auto updated submodule references [20-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 7fbb6e044c..9a226da8fb 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 7fbb6e044c343825d9075950273c9d5518a03d14 +Subproject commit 9a226da8fbbcf2f17ddebf131cb668a782c18696 From 1359b40ee7aadf32a9b22feb99799633499f9b51 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sat, 22 Feb 2025 16:17:48 +0000 Subject: [PATCH 31/58] Maintain state for i2c_ev_handler() for each i2c bus (#14268) --- src/platform/STM32/bus_i2c_stm32f4xx.c | 75 ++++++++++++++------------ 1 file changed, 41 insertions(+), 34 deletions(-) diff --git a/src/platform/STM32/bus_i2c_stm32f4xx.c b/src/platform/STM32/bus_i2c_stm32f4xx.c index 34a0c02802..2d7be2dfb5 100644 --- a/src/platform/STM32/bus_i2c_stm32f4xx.c +++ b/src/platform/STM32/bus_i2c_stm32f4xx.c @@ -116,6 +116,14 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = { i2cDevice_t i2cDevice[I2CDEV_COUNT]; +// State used by event handler ISR +typedef struct { + uint8_t subaddress_sent; // flag to indicate if subaddess sent + uint8_t final_stop; // flag to indicate final bus condition + int8_t index; // index is signed -1 == send the subaddress +} i2cEvState_t; +static i2cEvState_t i2c_ev_state[I2CDEV_COUNT]; + static volatile uint16_t i2cErrorCount = 0; void I2C1_ER_IRQHandler(void) @@ -316,18 +324,17 @@ void i2c_ev_handler(I2CDevice device) { I2C_TypeDef *I2Cx = i2cDevice[device].hardware->reg; + i2cEvState_t *ev_state = &i2c_ev_state[device]; i2cState_t *state = &i2cDevice[device].state; - static uint8_t subaddress_sent, final_stop; // flag to indicate if subaddess sent, flag to indicate final bus condition - static int8_t index; // index is signed -1 == send the subaddress uint8_t SReg_1 = I2Cx->SR1; // read the status register here if (SReg_1 & I2C_SR1_SB) { // we just sent a start - EV5 in ref manual I2Cx->CR1 &= ~I2C_CR1_POS; // reset the POS bit so ACK/NACK applied to the current byte I2C_AcknowledgeConfig(I2Cx, ENABLE); // make sure ACK is on - index = 0; // reset the index - if (state->reading && (subaddress_sent || 0xFF == state->reg)) { // we have sent the subaddr - subaddress_sent = 1; // make sure this is set in case of no subaddress, so following code runs correctly + ev_state->index = 0; // reset the index + if (state->reading && (ev_state->subaddress_sent || 0xFF == state->reg)) { // we have sent the subaddr + ev_state->subaddress_sent = 1; // make sure this is set in case of no subaddress, so following code runs correctly if (state->bytes == 2) I2Cx->CR1 |= I2C_CR1_POS; // set the POS bit so NACK applied to the final byte in the two byte read I2C_Send7bitAddress(I2Cx, state->addr, I2C_Direction_Receiver); // send the address and set hardware mode @@ -335,93 +342,93 @@ void i2c_ev_handler(I2CDevice device) else { // direction is Tx, or we havent sent the sub and rep start I2C_Send7bitAddress(I2Cx, state->addr, I2C_Direction_Transmitter); // send the address and set hardware mode if (state->reg != 0xFF) // 0xFF as subaddress means it will be ignored, in Tx or Rx mode - index = -1; // send a subaddress + ev_state->index = -1; // send a subaddress } } else if (SReg_1 & I2C_SR1_ADDR) { // we just sent the address - EV6 in ref manual // Read SR1,2 to clear ADDR __DMB(); // memory fence to control hardware - if (state->bytes == 1 && state->reading && subaddress_sent) { // we are receiving 1 byte - EV6_3 + if (state->bytes == 1 && state->reading && ev_state->subaddress_sent) { // we are receiving 1 byte - EV6_3 I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK __DMB(); (void)I2Cx->SR2; // clear ADDR after ACK is turned off I2C_GenerateSTOP(I2Cx, ENABLE); // program the stop - final_stop = 1; + ev_state->final_stop = 1; I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // allow us to have an EV7 } else { // EV6 and EV6_1 (void)I2Cx->SR2; // clear the ADDR here __DMB(); - if (state->bytes == 2 && state->reading && subaddress_sent) { // rx 2 bytes - EV6_1 + if (state->bytes == 2 && state->reading && ev_state->subaddress_sent) { // rx 2 bytes - EV6_1 I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to fill } - else if (state->bytes == 3 && state->reading && subaddress_sent) // rx 3 bytes + else if (state->bytes == 3 && state->reading && ev_state->subaddress_sent) // rx 3 bytes I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // make sure RXNE disabled so we get a BTF in two bytes time else // receiving greater than three bytes, sending subaddress, or transmitting I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); } } else if (SReg_1 & I2C_SR1_BTF) { // Byte transfer finished - EV7_2, EV7_3 or EV8_2 - final_stop = 1; - if (state->reading && subaddress_sent) { // EV7_2, EV7_3 + ev_state->final_stop = 1; + if (state->reading && ev_state->subaddress_sent) { // EV7_2, EV7_3 if (state->bytes > 2) { // EV7_2 I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK - state->read_p[index++] = (uint8_t)I2Cx->DR; // read data N-2 + state->read_p[ev_state->index++] = (uint8_t)I2Cx->DR; // read data N-2 I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop - final_stop = 1; // required to fix hardware - state->read_p[index++] = (uint8_t)I2Cx->DR; // read data N - 1 + ev_state->final_stop = 1; // required to fix hardware + state->read_p[ev_state->index++] = (uint8_t)I2Cx->DR; // read data N - 1 I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // enable TXE to allow the final EV7 } else { // EV7_3 - if (final_stop) + if (ev_state->final_stop) I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop else I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start - state->read_p[index++] = (uint8_t)I2Cx->DR; // read data N - 1 - state->read_p[index++] = (uint8_t)I2Cx->DR; // read data N - index++; // to show job completed + state->read_p[ev_state->index++] = (uint8_t)I2Cx->DR; // read data N - 1 + state->read_p[ev_state->index++] = (uint8_t)I2Cx->DR; // read data N + ev_state->index++; // to show job completed } } else { // EV8_2, which may be due to a subaddress sent or a write completion - if (subaddress_sent || (state->writing)) { - if (final_stop) + if (ev_state->subaddress_sent || (state->writing)) { + if (ev_state->final_stop) I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop else I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start - index++; // to show that the job is complete + ev_state->index++; // to show that the job is complete } else { // We need to send a subaddress I2C_GenerateSTART(I2Cx, ENABLE); // program the repeated Start - subaddress_sent = 1; // this is set back to zero upon completion of the current task + ev_state->subaddress_sent = 1; // this is set back to zero upon completion of the current task } } // we must wait for the start to clear, otherwise we get constant BTF while (I2Cx->CR1 & I2C_CR1_START) {; } } else if (SReg_1 & I2C_SR1_RXNE) { // Byte received - EV7 - state->read_p[index++] = (uint8_t)I2Cx->DR; - if (state->bytes == (index + 3)) + state->read_p[ev_state->index++] = (uint8_t)I2Cx->DR; + if (state->bytes == (ev_state->index + 3)) I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush so we can get an EV7_2 - if (state->bytes == index) // We have completed a final EV7 - index++; // to show job is complete + if (state->bytes == ev_state->index) // We have completed a final EV7 + ev_state->index++; // to show job is complete } else if (SReg_1 & I2C_SR1_TXE) { // Byte transmitted EV8 / EV8_1 - if (index != -1) { // we dont have a subaddress to send - I2Cx->DR = state->write_p[index++]; - if (state->bytes == index) // we have sent all the data + if (ev_state->index != -1) { // we dont have a subaddress to send + I2Cx->DR = state->write_p[ev_state->index++]; + if (state->bytes == ev_state->index) // we have sent all the data I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush } else { - index++; + ev_state->index++; I2Cx->DR = state->reg; // send the subaddress if (state->reading || !(state->bytes)) // if receiving or sending 0 bytes, flush now I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush } } - if (index == state->bytes + 1) { // we have completed the current job - subaddress_sent = 0; // reset this here - if (final_stop) // If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF + if (ev_state->index == state->bytes + 1) { // we have completed the current job + ev_state->subaddress_sent = 0; // reset this here + if (ev_state->final_stop) // If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive state->busy = 0; } From 6948fb75e995db7060ea0c5eafecd4f8177986f8 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Sat, 22 Feb 2025 18:02:57 +0100 Subject: [PATCH 32/58] Blackbox - IMU quaternion - fix case when w is negative (#14275) quaternion w can ce negative, causing problem with unit quaternion reconstruction. Send inverted quaternion in negative-w case (q and -q are identical rotations) --- src/main/blackbox/blackbox.c | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 42d274979f..7ece9940ca 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -365,7 +365,7 @@ typedef struct blackboxMainState_s { int16_t gyroUnfilt[XYZ_AXIS_COUNT]; #ifdef USE_ACC int16_t accADC[XYZ_AXIS_COUNT]; - int16_t imuAttitudeQuaternion[XYZ_AXIS_COUNT]; + int16_t imuAttitudeQuaternion3[XYZ_AXIS_COUNT]; // only x,y,z is stored; w is always positive #endif int16_t debug[DEBUG16_VALUE_COUNT]; int16_t motor[MAX_SUPPORTED_MOTORS]; @@ -747,7 +747,7 @@ static void writeIntraframe(void) } if (testBlackboxCondition(CONDITION(ATTITUDE))) { - blackboxWriteSigned16VBArray(blackboxCurrent->imuAttitudeQuaternion, XYZ_AXIS_COUNT); + blackboxWriteSigned16VBArray(blackboxCurrent->imuAttitudeQuaternion3, XYZ_AXIS_COUNT); } #endif @@ -932,7 +932,7 @@ static void writeInterframe(void) } if (testBlackboxCondition(CONDITION(ATTITUDE))) { - blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, imuAttitudeQuaternion), XYZ_AXIS_COUNT); + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, imuAttitudeQuaternion3), XYZ_AXIS_COUNT); } #endif @@ -1250,14 +1250,22 @@ static void loadMainState(timeUs_t currentTimeUs) #if defined(USE_ACC) blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]); - STATIC_ASSERT(offsetof(quaternion_t, w) == 0, "Code expects quaternion in w, x, y, z order"); - blackboxCurrent->imuAttitudeQuaternion[i] = lrintf(imuAttitudeQuaternion.v[i + 1] * 0x7FFF); //Scale to int16 by value 0x7FFF = 2^15 - 1; Use i+1 index for x,y,z components access, [0] - w #endif #ifdef USE_MAG blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]); #endif } - +#if defined(USE_ACC) // IMU quaternion + { + // write x,y,z of IMU quaternion. Make sure that w is always positive + STATIC_ASSERT(offsetof(quaternion_t, w) == 0, "Code expects quaternion in w, x, y, z order"); + const float q_sign = imuAttitudeQuaternion.w < 0 ? -1 : 1; // invert quaternion if w is negative + const float q_scale = q_sign * 0x7FFF; // Scale to int16 by value 0x7FFF = 2^15 - 1 (-1 <= x,y,z <= 1) + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { + blackboxCurrent->imuAttitudeQuaternion3[i] = lrintf(imuAttitudeQuaternion.v[i + 1] * q_scale); // Use i+1 index for x,y,z components access, [0] - w + } + } +#endif for (int i = 0; i < 4; i++) { blackboxCurrent->rcCommand[i] = lrintf(rcCommand[i] * blackboxHighResolutionScale); } From 611ad5907ccb8529a55021c760f821248e8bd56b Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Sun, 23 Feb 2025 21:56:03 +0100 Subject: [PATCH 33/58] Update io_preinit.c (#14273) --- src/main/drivers/io_preinit.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/io_preinit.c b/src/main/drivers/io_preinit.c index 81ca43d420..98ab4fc29b 100644 --- a/src/main/drivers/io_preinit.c +++ b/src/main/drivers/io_preinit.c @@ -48,7 +48,7 @@ void ioPreinitByIO(const IO_t io, uint8_t iocfg, ioPreinitPinState_e init) IOHi(io); break; default: - // Do nothing + break; } } From c80de5c62af44d61f3e3e1c27b1bf448bacb3a52 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Tue, 25 Feb 2025 06:25:03 +0000 Subject: [PATCH 34/58] Auto updated submodule references [25-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 9a226da8fb..b4ba6f92c2 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 9a226da8fbbcf2f17ddebf131cb668a782c18696 +Subproject commit b4ba6f92c25d995e0277712c901fd644124715cf From a779ffaff70ed1bbd4eede60fc28e6e1df767a36 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Wed, 26 Feb 2025 06:25:03 +0000 Subject: [PATCH 35/58] Auto updated submodule references [26-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index b4ba6f92c2..3d55cd45bd 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit b4ba6f92c25d995e0277712c901fd644124715cf +Subproject commit 3d55cd45bdc6ba9dad631e4021c52037b8f3d127 From 84c11a5009aa0465aa5aea1e53e3f3a242a0b177 Mon Sep 17 00:00:00 2001 From: Jay Blackman Date: Thu, 27 Feb 2025 03:21:05 +1100 Subject: [PATCH 36/58] FIX: PWM motor output stopped working after #14156 (#14280) FIX: https://github.com/betaflight/betaflight/issues/14265 --- src/main/drivers/motor.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index 22f2415a11..9060316a62 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -184,7 +184,7 @@ bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isP motorProtocolFamily_e motorGetProtocolFamily(void) { switch (motorConfig()->dev.motorProtocol) { -#ifdef USE_PWMOUTPUT +#ifdef USE_PWM_OUTPUT case MOTOR_PROTOCOL_PWM : case MOTOR_PROTOCOL_ONESHOT125: case MOTOR_PROTOCOL_ONESHOT42: From a801aadf9cc1bea189c147f7a0c1f1655297e15f Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Thu, 27 Feb 2025 06:25:03 +0000 Subject: [PATCH 37/58] Auto updated submodule references [27-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 3d55cd45bd..5f4908dd85 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 3d55cd45bdc6ba9dad631e4021c52037b8f3d127 +Subproject commit 5f4908dd859e413b90d9362230f81d48727cedaa From d8cd6b743be808278115f55cb0383671fb188ce9 Mon Sep 17 00:00:00 2001 From: crteensy Date: Thu, 27 Feb 2025 13:46:26 +0100 Subject: [PATCH 38/58] Properly handle HSI if selected (#14259) * Properly handle HSI if selected In some places, PERSISTENT_OBJECT_HSE_VALUE being zero is used to imply that the MCU should be clocked from HSI, but the code doesn't really follow through and results in an invalid clock tree setup. The proposed changes should fix this. * Apply suggestions from code review Co-authored-by: Mark Haslinghuis * Update system_stm32g4xx.c HSI should now be implicitly selected when SYSTEM_HSE_MHZ is defined as 0 or omitted in the target config. USE_CLOCK_SOURCE_HSI is not necessary. * Update src/platform/STM32/startup/system_stm32g4xx.c Co-authored-by: Petr Ledvina * Update src/platform/STM32/startup/system_stm32g4xx.c Co-authored-by: Mark Haslinghuis --------- Co-authored-by: Mark Haslinghuis Co-authored-by: Petr Ledvina --- src/platform/STM32/startup/system_stm32g4xx.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/platform/STM32/startup/system_stm32g4xx.c b/src/platform/STM32/startup/system_stm32g4xx.c index 5690987942..9a35bdef64 100644 --- a/src/platform/STM32/startup/system_stm32g4xx.c +++ b/src/platform/STM32/startup/system_stm32g4xx.c @@ -118,7 +118,6 @@ void SystemInit(void) void SystemCoreClockUpdate(void) { uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE); - uint32_t tmp, pllvco, pllr, pllsource, pllm; /* Get SYSCLK source -------------------------------------------------------*/ @@ -381,10 +380,12 @@ void SystemClock_Config(void) // Initializes the CPU, AHB and APB busses clocks - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_HSI48 - |RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE; - - RCC_OscInitStruct.HSEState = RCC_HSE_ON; + const bool useHse = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE) != 0; + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_HSI48 + | RCC_OSCILLATORTYPE_LSI + | (useHse ? RCC_OSCILLATORTYPE_HSE : 0); + RCC_OscInitStruct.HSEState = useHse ? RCC_HSE_ON : RCC_HSE_OFF; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; RCC_OscInitStruct.LSIState = RCC_LSI_ON; From 0b4b1123cd822d0285a846bd7bcbabdfa3e6dc3d Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Fri, 28 Feb 2025 06:25:03 +0000 Subject: [PATCH 39/58] Auto updated submodule references [28-02-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 5f4908dd85..337bf96ddc 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 5f4908dd859e413b90d9362230f81d48727cedaa +Subproject commit 337bf96ddc627c3d87c6294fda966f01acb760e6 From 99e8dd8840877e2b30fa14756b1fd9b7fd0fdf8f Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Sat, 1 Mar 2025 07:40:02 +0100 Subject: [PATCH 40/58] Fix virtualled (#14276) --- src/main/cli/cli.c | 4 ++- src/main/cli/settings.c | 4 ++- src/main/drivers/light_led.c | 60 +++++++++++++++++------------------- src/main/drivers/light_led.h | 40 ++++++++++-------------- src/test/unit/target.h | 1 + 5 files changed, 52 insertions(+), 57 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 40e080bdd3..b9539389ec 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -5096,7 +5096,9 @@ const cliResourceValue_t resourceTable[] = { DEFW( OWNER_I2C_SCL, PG_I2C_CONFIG, i2cConfig_t, ioTagScl, I2CDEV_COUNT ), DEFW( OWNER_I2C_SDA, PG_I2C_CONFIG, i2cConfig_t, ioTagSda, I2CDEV_COUNT ), #endif - DEFA( OWNER_LED, PG_STATUS_LED_CONFIG, statusLedConfig_t, ioTags[0], STATUS_LED_NUMBER ), +#if !defined(USE_VIRTUAL_LED) + DEFA( OWNER_LED, PG_STATUS_LED_CONFIG, statusLedConfig_t, ioTags[0], STATUS_LED_COUNT ), +#endif #ifdef USE_SPEKTRUM_BIND DEFS( OWNER_RX_BIND, PG_RX_CONFIG, rxConfig_t, spektrum_bind_pin_override_ioTag ), DEFS( OWNER_RX_BIND_PLUG, PG_RX_CONFIG, rxConfig_t, spektrum_bind_plug_ioTag ), diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index cc5ef8571d..e1b9bdd91e 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1749,7 +1749,9 @@ const clivalue_t valueTable[] = { { "frsky_spi_a1_source", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_FRSKY_SPI_A1_SOURCE }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, a1Source) }, { "cc2500_spi_chip_detect", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, chipDetectEnabled) }, #endif - { "led_inversion", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) }, +#if !defined(USE_VIRTUAL_LED) + { "led_inversion", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, ((1 << STATUS_LED_COUNT) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) }, +#endif #ifdef USE_DASHBOARD { "dashboard_i2c_bus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, device) }, { "dashboard_i2c_addr", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { I2C_ADDR7_MIN, I2C_ADDR7_MAX }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, address) }, diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index e0be78bd97..ca8917bdd1 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -27,32 +27,26 @@ #include "light_led.h" -#if !(defined(UNIT_TEST) || defined(USE_VIRTUAL_LED)) +#if !defined(USE_VIRTUAL_LED) -PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); - -static IO_t leds[STATUS_LED_NUMBER]; +static IO_t leds[STATUS_LED_COUNT]; static uint8_t ledInversion = 0; -#ifndef LED0_PIN -#define LED0_PIN NONE +PG_REGISTER_WITH_RESET_TEMPLATE(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); + +PG_RESET_TEMPLATE(statusLedConfig_t, statusLedConfig, + .ioTags = { +#if STATUS_LED_COUNT > 0 && defined(LED0_PIN) + [0] = IO_TAG(LED0_PIN), #endif - -#ifndef LED1_PIN -#define LED1_PIN NONE +#if STATUS_LED_COUNT > 1 && defined(LED1_PIN) + [1] = IO_TAG(LED1_PIN), #endif - -#ifndef LED2_PIN -#define LED2_PIN NONE +#if STATUS_LED_COUNT > 2 && defined(LED2_PIN) + [2] = IO_TAG(LED2_PIN), #endif - -void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) -{ - statusLedConfig->ioTags[0] = IO_TAG(LED0_PIN); - statusLedConfig->ioTags[1] = IO_TAG(LED1_PIN); - statusLedConfig->ioTags[2] = IO_TAG(LED2_PIN); - - statusLedConfig->inversion = 0 + }, + .inversion = 0 #ifdef LED0_INVERTED | BIT(0) #endif @@ -62,35 +56,37 @@ void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) #ifdef LED2_INVERTED | BIT(2) #endif - ; -} + , +); void ledInit(const statusLedConfig_t *statusLedConfig) { ledInversion = statusLedConfig->inversion; - for (int i = 0; i < STATUS_LED_NUMBER; i++) { - if (statusLedConfig->ioTags[i]) { - leds[i] = IOGetByTag(statusLedConfig->ioTags[i]); + for (int i = 0; i < (int)ARRAYLEN(leds); i++) { + leds[i] = IOGetByTag(statusLedConfig->ioTags[i]); + if (leds[i]) { IOInit(leds[i], OWNER_LED, RESOURCE_INDEX(i)); IOConfigGPIO(leds[i], IOCFG_OUT_PP); - } else { - leds[i] = IO_NONE; } + ledSet(i, false); } - - LED0_OFF; - LED1_OFF; - LED2_OFF; } void ledToggle(int led) { + if (led < 0 || led >= (int)ARRAYLEN(leds)) { + return; + } IOToggle(leds[led]); } void ledSet(int led, bool on) { - const bool inverted = (1 << (led)) & ledInversion; + if (led < 0 || led >= (int)ARRAYLEN(leds)) { + return; + } + const bool inverted = ledInversion & (1 << led); IOWrite(leds[led], on ? inverted : !inverted); } + #endif diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 0543f0478d..8d995e7edc 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -24,31 +24,9 @@ #include "drivers/io_types.h" #include "common/utils.h" -#define STATUS_LED_NUMBER 3 - -typedef struct statusLedConfig_s { - ioTag_t ioTags[STATUS_LED_NUMBER]; - uint8_t inversion; -} statusLedConfig_t; +#define STATUS_LED_COUNT 3 // Helpful macros -#if defined(UNIT_TEST) || defined(USE_VIRTUAL_LED) - -#define LED0_TOGGLE NOOP -#define LED0_OFF NOOP -#define LED0_ON NOOP - -#define LED1_TOGGLE NOOP -#define LED1_OFF NOOP -#define LED1_ON NOOP - -#define LED2_TOGGLE NOOP -#define LED2_OFF NOOP -#define LED2_ON NOOP - -#else - -PG_DECLARE(statusLedConfig_t, statusLedConfig); #define LED0_TOGGLE ledToggle(0) #define LED0_OFF ledSet(0, false) @@ -62,6 +40,22 @@ PG_DECLARE(statusLedConfig_t, statusLedConfig); #define LED2_OFF ledSet(2, false) #define LED2_ON ledSet(2, true) +// use dummy functions for unittest +#if defined(USE_VIRTUAL_LED) + +// ledInit is missing intentionally +static inline void ledToggle(int led) { UNUSED(led); } +static inline void ledSet(int led, bool state) { UNUSED(led); UNUSED(state); } + +#else + +typedef struct statusLedConfig_s { + ioTag_t ioTags[STATUS_LED_COUNT]; + uint8_t inversion; +} statusLedConfig_t; + +PG_DECLARE(statusLedConfig_t, statusLedConfig); + void ledInit(const statusLedConfig_t *statusLedConfig); void ledToggle(int led); void ledSet(int led, bool state); diff --git a/src/test/unit/target.h b/src/test/unit/target.h index 866d9afe9a..068e96fe2b 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -58,6 +58,7 @@ #define USE_LED_STRIP_STATUS_MODE #define USE_SERVOS #define USE_TRANSPONDER +#define USE_VIRTUAL_LED #define USE_VCP #define USE_UART1 #define USE_UART2 From 9e53f4ad5b87a762b608c6d6b0f6b765e20c0cae Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Thu, 6 Mar 2025 06:25:03 +0000 Subject: [PATCH 41/58] Auto updated submodule references [06-03-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 337bf96ddc..c4353ec9c9 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 337bf96ddc627c3d87c6294fda966f01acb760e6 +Subproject commit c4353ec9c9772c7bfb14caa5a22bfd30b7109de8 From f33e1614edd57429657a35f1fc53f379e1c793ba Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Tue, 11 Mar 2025 17:07:43 +0100 Subject: [PATCH 42/58] Fix typo in accgyro_spi_icm426xx.c (#14290) --- src/main/drivers/accgyro/accgyro_spi_icm426xx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index 5e8e4cc574..204b547c56 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c @@ -169,7 +169,7 @@ static aafConfig_t aafLUT42688[AAF_CONFIG_COUNT] = { // see table in section 5. [AAF_CONFIG_1962HZ] = { 37, 1376, 4 }, }; -// Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P +// Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42605 // actual cutoff differs slightly from those of the 42688P static aafConfig_t aafLUT42605[AAF_CONFIG_COUNT] = { // see table in section 5.3 [AAF_CONFIG_258HZ] = { 21, 440, 6 }, // actually 249 Hz From 225bb21e9c42d591400ff43aa55e32d247ad4ae9 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Thu, 13 Mar 2025 06:25:03 +0000 Subject: [PATCH 43/58] Auto updated submodule references [13-03-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index c4353ec9c9..ed54f70c18 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit c4353ec9c9772c7bfb14caa5a22bfd30b7109de8 +Subproject commit ed54f70c185e2779288fad07199cd4b533318bb7 From f5404097688d86cf0a296e1cf3811839f09909d2 Mon Sep 17 00:00:00 2001 From: fluke9 Date: Thu, 13 Mar 2025 19:30:14 +0100 Subject: [PATCH 44/58] fix: led strip not working (#14279) (#14286) * fix: led strip not working (#14279) * fix: led strip not working reworked (#14279) * Revert src/platform/STM32/target/STM32F745/target.h * Revert src/platform/STM32/target/STM32F7X2/target.h * Fix per review ledvinap --------- Co-authored-by: Mark Haslinghuis --- src/main/drivers/light_ws2811strip.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 9547eda9c9..b69b87ca58 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -60,7 +60,7 @@ #define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof(uint32_t)) DMA_RW_AXI __attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH]; #else -FAST_DATA_ZERO_INIT uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +DMA_DATA_ZERO_INIT uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #endif static ioTag_t ledStripIoTag; From a7323389051d03390a0c36a84164f9b5eac3bd7d Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Fri, 14 Mar 2025 06:25:03 +0000 Subject: [PATCH 45/58] Auto updated submodule references [14-03-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index ed54f70c18..595cf6f71a 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit ed54f70c185e2779288fad07199cd4b533318bb7 +Subproject commit 595cf6f71a16c8d411f7ece975e7a5e273c5e837 From 92c59091cee24c48d0cf142b8ee0b0b5dfc07c2e Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Fri, 14 Mar 2025 22:27:17 +0100 Subject: [PATCH 46/58] Change airborne default (#14271) --- src/main/pg/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/pg/gps.c b/src/main/pg/gps.c index 81f9e0e396..c8d5df62f8 100644 --- a/src/main/pg/gps.c +++ b/src/main/pg/gps.c @@ -41,7 +41,7 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .autoConfig = GPS_AUTOCONFIG_ON, .autoBaud = GPS_AUTOBAUD_OFF, .gps_ublox_acquire_model = UBLOX_MODEL_STATIONARY, - .gps_ublox_flight_model = UBLOX_MODEL_AIRBORNE_4G, + .gps_ublox_flight_model = UBLOX_MODEL_AIRBORNE_1G, .gps_update_rate_hz = 10, .gps_ublox_use_galileo = false, .gps_set_home_point_once = false, From 0350bd3188504ef065beaa56de273a65326e1fc0 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Sat, 15 Mar 2025 06:25:03 +0000 Subject: [PATCH 47/58] Auto updated submodule references [15-03-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 595cf6f71a..3c65ad85a6 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 595cf6f71a16c8d411f7ece975e7a5e273c5e837 +Subproject commit 3c65ad85a6c25f83944f4e491b7a5ee6644d71c6 From 7e47905132fa3eb587ba07f61eddf79e646de10c Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Tue, 18 Mar 2025 06:25:03 +0000 Subject: [PATCH 48/58] Auto updated submodule references [18-03-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 3c65ad85a6..0f78778100 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 3c65ad85a6c25f83944f4e491b7a5ee6644d71c6 +Subproject commit 0f7877810016660eb0cace94211d8077bf51ac75 From 8dafdbce41460c01dc8f552d2b2aa330949a0485 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Fri, 21 Mar 2025 06:25:03 +0000 Subject: [PATCH 49/58] Auto updated submodule references [21-03-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 0f78778100..3eedb40cf0 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 0f7877810016660eb0cace94211d8077bf51ac75 +Subproject commit 3eedb40cf07dc40c246cd0416d04464aeb7c73c6 From 9c8f84f62d75d9025c8bbda8af5664c141ef67fe Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 21 Mar 2025 17:27:40 -0500 Subject: [PATCH 50/58] Fix gimbalCmdIn.u.crc to gimbalCmdIn.u.gimbalCmd.crc (#14311) --- src/main/io/gimbal_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gimbal_control.c b/src/main/io/gimbal_control.c index 73a89f3c55..a9948abee8 100644 --- a/src/main/io/gimbal_control.c +++ b/src/main/io/gimbal_control.c @@ -288,7 +288,7 @@ void gimbalUpdate(timeUs_t currentTimeUs) if (gimbalInCount == sizeof(gimbalCmdIn.u.gimbalCmd)) { uint16_t crc = gimbalCrc((uint8_t *)&gimbalCmdIn, sizeof(gimbalCmdIn) - 2); // Only use the data if the CRC is correct - if (gimbalCmdIn.u.crc == crc) { + if (gimbalCmdIn.u.gimbalCmd.crc == crc) { gimbalCmdOut = gimbalCmdIn.u.gimbalCmd; gimbalSet(gimbalCmdIn.u.gimbalCmd.roll, gimbalCmdIn.u.gimbalCmd.pitch, gimbalCmdIn.u.gimbalCmd.yaw); serialWriteBuf(gimbalSerialPort, (uint8_t *)&gimbalCmdOut, sizeof(gimbalCmdOut)); From 3ae8056ec6178b589f2f61a499f11643defb949f Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Fri, 21 Mar 2025 23:32:42 +0100 Subject: [PATCH 51/58] Fix typos (#14315) --- src/main/flight/pos_hold_multirotor.c | 2 +- src/main/flight/pos_hold_wing.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pos_hold_multirotor.c b/src/main/flight/pos_hold_multirotor.c index ba977d8589..e811b433da 100644 --- a/src/main/flight/pos_hold_multirotor.c +++ b/src/main/flight/pos_hold_multirotor.c @@ -105,6 +105,6 @@ bool posHoldFailure(void) { return FLIGHT_MODE(POS_HOLD_MODE) && (!posHold.isControlOk || !posHold.areSensorsOk); } -#endif // USE_POS_HOLD +#endif // USE_POSITION_HOLD #endif // !USE_WING diff --git a/src/main/flight/pos_hold_wing.c b/src/main/flight/pos_hold_wing.c index fac189426f..211645565e 100644 --- a/src/main/flight/pos_hold_wing.c +++ b/src/main/flight/pos_hold_wing.c @@ -52,6 +52,6 @@ bool posHoldFailure(void) { return true; } -#endif // USE_POS_HOLD +#endif // USE_POSITION_HOLD #endif // USE_WING From bf4a1a24969f26d9a1bc87cc990e8e3877bf7726 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Mon, 24 Mar 2025 06:25:03 +0000 Subject: [PATCH 52/58] Auto updated submodule references [24-03-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 3eedb40cf0..db74441b1c 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 3eedb40cf07dc40c246cd0416d04464aeb7c73c6 +Subproject commit db74441b1cb1ed96c63aee1288516c876250495e From 28848d1324fbcb773ab043dbf9479ca318860509 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Wed, 26 Mar 2025 06:25:03 +0000 Subject: [PATCH 53/58] Auto updated submodule references [26-03-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index db74441b1c..48588b79d3 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit db74441b1cb1ed96c63aee1288516c876250495e +Subproject commit 48588b79d31428620af4d0629c57cc1894dd3542 From ad58b69c20f9f2d304a75ff0871526f3f6b848cc Mon Sep 17 00:00:00 2001 From: ke deng Date: Thu, 27 Mar 2025 05:20:53 +0800 Subject: [PATCH 54/58] Add NULL check for vTable->decodeTelemetry (#14321) --- src/main/drivers/motor.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index 9060316a62..2fa45124cb 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -109,7 +109,9 @@ void motorWriteAll(float *values) // Perform the decode of the last data received // New data will be received once the send of motor data, triggered above, completes #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) - motorDevice.vTable->decodeTelemetry(); + if (motorDevice.vTable->decodeTelemetry) { + motorDevice.vTable->decodeTelemetry(); + } #endif // Update the motor data From 06844745f63f48cb9074d2173f8290ff206ee75c Mon Sep 17 00:00:00 2001 From: AJ Date: Wed, 26 Mar 2025 22:25:24 +0100 Subject: [PATCH 55/58] fix: missing sitl motor count (#14319) - other platforms set it, sitl does not and configurator complains --- src/platform/SIMULATOR/sitl.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index 862d2835fd..2fc992cb45 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -644,14 +644,16 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, if (!device) { return false; } + + pwmMotorCount = device->count; device->vTable = &vTable; - const uint8_t motorCount = device->count; - printf("Initialized motor count %d\n", motorCount); - pwmRawPkt.motorCount = motorCount; + + printf("Initialized motor count %d\n", pwmMotorCount); + pwmRawPkt.motorCount = pwmMotorCount; idlePulse = _idlePulse; - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) { pwmMotors[motorIndex].enabled = true; } From 268952eeb54573b9ccf6c86ef2edc58a7255d068 Mon Sep 17 00:00:00 2001 From: Jay Blackman Date: Fri, 28 Mar 2025 21:31:30 +1100 Subject: [PATCH 56/58] Adding simplified timer calls for enabling ESC Serial and Soft Serial for AT32 (#14228) --- src/main/drivers/rx/expresslrs_driver.c | 55 ++++----------------- src/main/drivers/serial_escserial.c | 33 ++++--------- src/main/drivers/serial_softserial.c | 6 +-- src/main/drivers/timer.h | 9 ++++ src/platform/APM32/timer_apm32.c | 42 +++++++++++++++- src/platform/AT32/target/AT32F435G/target.h | 1 + src/platform/AT32/target/AT32F435M/target.h | 1 + src/platform/AT32/timer_at32bsp.c | 46 +++++++++++++++++ src/platform/STM32/timer_hal.c | 39 +++++++++++++++ src/platform/STM32/timer_stdperiph.c | 39 +++++++++++++++ 10 files changed, 195 insertions(+), 76 deletions(-) diff --git a/src/main/drivers/rx/expresslrs_driver.c b/src/main/drivers/rx/expresslrs_driver.c index 610c7ea286..e61d8bd033 100644 --- a/src/main/drivers/rx/expresslrs_driver.c +++ b/src/main/drivers/rx/expresslrs_driver.c @@ -101,13 +101,8 @@ void expressLrsUpdateTimerInterval(uint16_t intervalUs) timerState.intervalUs = intervalUs; expressLrsRecalculatePhaseShiftLimits(); -#ifdef USE_HAL_DRIVER timerReconfigureTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1)); - LL_TIM_SetAutoReload(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1); -#else - configTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1)); - TIM_SetAutoreload(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1); -#endif + timerSetPeriod(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1); } void expressLrsUpdatePhaseShift(int32_t newPhaseShift) @@ -140,11 +135,7 @@ static void expressLrsOnTimerUpdate(timerOvrHandlerRec_t *cbRec, captureCompare_ uint32_t adjustedPeriod = (timerState.intervalUs / TICK_TOCK_COUNT) + timerState.frequencyOffsetTicks; -#ifdef USE_HAL_DRIVER - LL_TIM_SetAutoReload(timer, adjustedPeriod - 1); -#else - TIM_SetAutoreload(timer, adjustedPeriod - 1); -#endif + timerSetPeriod(timer, adjustedPeriod - 1); expressLrsOnTimerTickISR(); @@ -154,11 +145,7 @@ static void expressLrsOnTimerUpdate(timerOvrHandlerRec_t *cbRec, captureCompare_ uint32_t adjustedPeriod = (timerState.intervalUs / TICK_TOCK_COUNT) + timerState.phaseShiftUs + timerState.frequencyOffsetTicks; -#ifdef USE_HAL_DRIVER - LL_TIM_SetAutoReload(timer, adjustedPeriod - 1); -#else - TIM_SetAutoreload(timer, adjustedPeriod - 1); -#endif + timerSetPeriod(timer, adjustedPeriod - 1); timerState.phaseShiftUs = 0; @@ -175,15 +162,8 @@ bool expressLrsTimerIsRunning(void) void expressLrsTimerStop(void) { -#ifdef USE_HAL_DRIVER - LL_TIM_DisableIT_UPDATE(timer); - LL_TIM_DisableCounter(timer); - LL_TIM_SetCounter(timer, 0); -#else - TIM_ITConfig(timer, TIM_IT_Update, DISABLE); - TIM_Cmd(timer, DISABLE); - TIM_SetCounter(timer, 0); -#endif + timerDisable(timer); + timerSetCounter(timer, 0); timerState.running = false; } @@ -191,29 +171,12 @@ void expressLrsTimerResume(void) { timerState.tickTock = TOCK; -#ifdef USE_HAL_DRIVER - LL_TIM_SetAutoReload(timer, (timerState.intervalUs / TICK_TOCK_COUNT)); - LL_TIM_SetCounter(timer, 0); - - LL_TIM_ClearFlag_UPDATE(timer); - LL_TIM_EnableIT_UPDATE(timer); -#else - TIM_SetAutoreload(timer, (timerState.intervalUs / TICK_TOCK_COUNT)); - TIM_SetCounter(timer, 0); - - TIM_ClearFlag(timer, TIM_FLAG_Update); - TIM_ITConfig(timer, TIM_IT_Update, ENABLE); -#endif + timerSetPeriod(timer, (timerState.intervalUs / TICK_TOCK_COUNT)); + timerSetCounter(timer, 0); + timerEnableInterrupt(timer); timerState.running = true; - -#ifdef USE_HAL_DRIVER - LL_TIM_EnableCounter(timer); - LL_TIM_GenerateEvent_UPDATE(timer); -#else - TIM_Cmd(timer, ENABLE); - TIM_GenerateEvent(timer, TIM_EventSource_Update); -#endif + timerEnable(timer); } void expressLrsInitialiseTimer(TIM_TypeDef *t, timerOvrHandlerRec_t *timerUpdateCb) diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index b0d7424a1b..6067dfde22 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -134,13 +134,6 @@ enum { #define STOP_BIT_MASK (1 << 0) #define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1)) -#ifdef USE_HAL_DRIVER -static void TIM_DeInit(TIM_TypeDef *tim) -{ - LL_TIM_DeInit(tim); -} -#endif - static void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) { if (escSerial->mode == PROTOCOL_KISSALL) @@ -343,7 +336,7 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 { uint32_t clock = SystemCoreClock/2; uint32_t timerPeriod; - TIM_DeInit(timerHardwarePtr->tim); + timerReset(timerHardwarePtr->tim); do { timerPeriod = clock / baud; if (isTimerPeriodTooLarge(timerPeriod)) { @@ -377,11 +370,8 @@ static void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t c // Adjust the timing so it will interrupt on the middle. // This is clobbers transmission, but it is okay because we are // always half-duplex. -#ifdef USE_HAL_DRIVER - __HAL_TIM_SetCounter(escSerial->txTimerHandle, __HAL_TIM_GetAutoreload(escSerial->txTimerHandle) / 2); -#else - TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2); -#endif + timerSetCounter(escSerial->txTimerHardware->tim, timerGetPeriod(escSerial->txTimerHardware->tim) / 2); + if (escSerial->isTransmittingData) { escSerial->transmissionErrors++; } @@ -414,7 +404,7 @@ static void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t c static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_e options) { // start bit is usually a FALLING signal - TIM_DeInit(timerHardwarePtr->tim); + timerReset(timerHardwarePtr->tim); timerReconfigureTimeBase(timerHardwarePtr->tim, 0xFFFF, SystemCoreClock / 2); timerChConfigIC(timerHardwarePtr, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); @@ -538,7 +528,7 @@ static void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t captur static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { uint32_t timerPeriod = 34; - TIM_DeInit(timerHardwarePtr->tim); + timerReset(timerHardwarePtr->tim); timerReconfigureTimeBase(timerHardwarePtr->tim, timerPeriod, MHZ_TO_HZ(1)); timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); @@ -570,12 +560,7 @@ static void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb); - //clear timer -#ifdef USE_HAL_DRIVER - __HAL_TIM_SetCounter(escSerial->rxTimerHandle, 0); -#else - TIM_SetCounter(escSerial->rxTimerHardware->tim,0); -#endif + timerSetCounter(escSerial->rxTimerHardware->tim, 0); if (capture > 40 && capture < 90) { @@ -628,7 +613,7 @@ static void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { // start bit is usually a FALLING signal - TIM_DeInit(timerHardwarePtr->tim); + timerReset(timerHardwarePtr->tim); timerReconfigureTimeBase(timerHardwarePtr->tim, 0xFFFF, MHZ_TO_HZ(1)); timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); @@ -786,11 +771,11 @@ static void closeEscSerial(escSerialPortIndex_e portIndex, uint8_t mode) if (mode != PROTOCOL_KISSALL) { escSerialInputPortDeConfig(escSerial->rxTimerHardware); timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL); - TIM_DeInit(escSerial->rxTimerHardware->tim); + timerReset(escSerial->rxTimerHardware->tim); } timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL); - TIM_DeInit(escSerial->txTimerHardware->tim); + timerReset(escSerial->txTimerHardware->tim); } static uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 13fe8bb682..0f69dee869 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -487,11 +487,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) // Synchronize the bit timing so that it will interrupt at the center // of the bit period. -#ifdef USE_HAL_DRIVER - __HAL_TIM_SetCounter(self->timerHandle, __HAL_TIM_GetAutoreload(self->timerHandle) / 2); -#else - TIM_SetCounter(self->timerHardware->tim, self->timerHardware->tim->ARR / 2); -#endif + timerSetCounter(self->timerHardware->tim, timerGetPeriod(self->timerHardware->tim) / 2); // For a mono-timer full duplex configuration, this may clobber the // transmission because the next callback to the onSerialTimerOverflow diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 28ed1872b3..40bae0689c 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -216,3 +216,12 @@ uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_ int8_t timerGetNumberByIndex(uint8_t index); int8_t timerGetTIMNumber(const TIM_TypeDef *tim); uint8_t timerLookupChannelIndex(const uint16_t channel); + +// TODO: replace TIM_TypeDef with alternate +void timerReset(TIM_TypeDef *timer); +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period); +uint32_t timerGetPeriod(TIM_TypeDef *timer); +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter); +void timerDisable(TIM_TypeDef *timer); +void timerEnable(TIM_TypeDef *timer); +void timerEnableInterrupt(TIM_TypeDef *timer); diff --git a/src/platform/APM32/timer_apm32.c b/src/platform/APM32/timer_apm32.c index b113bc5f07..12ef8f197b 100644 --- a/src/platform/APM32/timer_apm32.c +++ b/src/platform/APM32/timer_apm32.c @@ -330,8 +330,9 @@ static void timerNVICConfigure(uint8_t irq) TMR_HandleTypeDef* timerFindTimerHandle(TMR_TypeDef *tim) { uint8_t timerIndex = lookupTimerIndex(tim); - if (timerIndex >= USED_TIMER_COUNT) + if (timerIndex >= USED_TIMER_COUNT) { return NULL; + } return &timerHandle[timerIndex].Handle; } @@ -1196,4 +1197,43 @@ DAL_StatusTypeDef DMA_SetCurrDataCounter(TMR_HandleTypeDef *htim, uint32_t Chann /* Return function status */ return DAL_OK; } + +void timerReset(TIM_TypeDef *timer) +{ + DDL_TMR_DeInit(timer); +} + +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period) +{ + timer->AUTORLD = period; +} + +uint32_t timerGetPeriod(TIM_TypeDef *timer) +{ + return timer->AUTORLD; +} + +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter) +{ + timer->CNT = counter; +} + +void timerDisable(TIM_TypeDef *timer) +{ + DDL_TMR_DisableIT_UPDATE(timer); + DDL_TMR_DisableCounter(timer); +} + +void timerEnable(TIM_TypeDef *timer) +{ + DDL_TMR_EnableCounter(timer); + DDL_TMR_GenerateEvent_UPDATE(timer); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + DDL_TMR_ClearFlag_UPDATE(timer); + DDL_TMR_EnableIT_UPDATE(timer); +} + #endif diff --git a/src/platform/AT32/target/AT32F435G/target.h b/src/platform/AT32/target/AT32F435G/target.h index b76a523cd4..e9dbcebed6 100644 --- a/src/platform/AT32/target/AT32F435G/target.h +++ b/src/platform/AT32/target/AT32F435G/target.h @@ -97,5 +97,6 @@ #undef USE_RX_EXPRESSLRS // #undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #undef USE_SERIAL_4WAY_SK_BOOTLOADER +#define USE_ESCSERIAL #define FLASH_PAGE_SIZE ((uint32_t)0x0800) // 2K sectors diff --git a/src/platform/AT32/target/AT32F435M/target.h b/src/platform/AT32/target/AT32F435M/target.h index fb52e7fcea..3ee3ca1928 100644 --- a/src/platform/AT32/target/AT32F435M/target.h +++ b/src/platform/AT32/target/AT32F435M/target.h @@ -97,5 +97,6 @@ #undef USE_RX_EXPRESSLRS // #undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #undef USE_SERIAL_4WAY_SK_BOOTLOADER +#define USE_ESCSERIAL #define FLASH_PAGE_SIZE ((uint32_t)0x1000) // 4K sectors diff --git a/src/platform/AT32/timer_at32bsp.c b/src/platform/AT32/timer_at32bsp.c index f76a417152..6336ad1a2a 100644 --- a/src/platform/AT32/timer_at32bsp.c +++ b/src/platform/AT32/timer_at32bsp.c @@ -777,4 +777,50 @@ uint16_t timerGetPrescalerByDesiredHertz(tmr_type *tim, uint32_t hz) } return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1; } + +void timerReset(tmr_type *timer) +{ + ATOMIC_BLOCK(NVIC_PRIO_TIMER) { + tmr_counter_enable(timer, FALSE); + } +} + +void timerSetPeriod(tmr_type *timer, uint32_t period) +{ + tmr_period_value_set(timer, period); +} + +uint32_t timerGetPeriod(tmr_type *timer) +{ + return tmr_period_value_get(timer); +} + +void timerSetCounter(tmr_type *timer, uint32_t counter) +{ + tmr_counter_value_set(timer, counter); +} + +void timerReconfigureTimeBase(tmr_type *timer, uint16_t period, uint32_t hz) +{ + configTimeBase(timer, period, hz); +} + +void timerDisable(TIM_TypeDef *timer) +{ + tmr_interrupt_enable(timer, TMR_OVF_INT, FALSE); + tmr_counter_enable(timer, FALSE); +} + +void timerEnable(TIM_TypeDef *timer) +{ + tmr_counter_enable(timer, TRUE); + tmr_overflow_event_disable(timer, TRUE); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + tmr_flag_clear(timer, TMR_OVF_FLAG); + tmr_interrupt_enable(timer, TMR_OVF_INT, TRUE); +} + #endif diff --git a/src/platform/STM32/timer_hal.c b/src/platform/STM32/timer_hal.c index 9f68624eca..c9e9180f6c 100644 --- a/src/platform/STM32/timer_hal.c +++ b/src/platform/STM32/timer_hal.c @@ -1251,4 +1251,43 @@ HAL_StatusTypeDef DMA_SetCurrDataCounter(TIM_HandleTypeDef *htim, uint32_t Chann /* Return function status */ return HAL_OK; } + +void timerReset(TIM_TypeDef *timer) +{ + LL_TIM_DeInit(timer); +} + +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period) +{ + timer->ARR = period; +} + +uint32_t timerGetPeriod(TIM_TypeDef *timer) +{ + return timer->ARR; +} + +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter) +{ + timer->CNT = counter; +} + +void timerDisable(TIM_TypeDef *timer) +{ + LL_TIM_DisableIT_UPDATE(timer); + LL_TIM_DisableCounter(timer); +} + +void timerEnable(TIM_TypeDef *timer) +{ + LL_TIM_EnableCounter(timer); + LL_TIM_GenerateEvent_UPDATE(timer); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + LL_TIM_ClearFlag_UPDATE(timer); + LL_TIM_EnableIT_UPDATE(timer); +} + #endif diff --git a/src/platform/STM32/timer_stdperiph.c b/src/platform/STM32/timer_stdperiph.c index f12fd5547b..8e0c2bc95a 100644 --- a/src/platform/STM32/timer_stdperiph.c +++ b/src/platform/STM32/timer_stdperiph.c @@ -954,4 +954,43 @@ uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz) } return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1; } + +void timerReset(TIM_TypeDef *timer) +{ + TIM_DeInit(timer); +} + +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period) +{ + timer->ARR = period; +} + +uint32_t timerGetPeriod(TIM_TypeDef *timer) +{ + return timer->ARR; +} + +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter) +{ + timer->CNT = counter; +} + +void timerDisable(TIM_TypeDef *timer) +{ + TIM_ITConfig(timer, TIM_IT_Update, DISABLE); + TIM_Cmd(timer, DISABLE); +} + +void timerEnable(TIM_TypeDef *timer) +{ + TIM_Cmd(timer, ENABLE); + TIM_GenerateEvent(timer, TIM_EventSource_Update); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + TIM_ClearFlag(timer, TIM_FLAG_Update); + TIM_ITConfig(timer, TIM_IT_Update, ENABLE); +} + #endif From 107bab5ed3dfb1a35a9e99e9f5a430b6182d5727 Mon Sep 17 00:00:00 2001 From: "Git bot (blckmn)" Date: Tue, 1 Apr 2025 06:25:03 +0000 Subject: [PATCH 57/58] Auto updated submodule references [01-04-2025] --- src/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/config b/src/config index 48588b79d3..b606002103 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 48588b79d31428620af4d0629c57cc1894dd3542 +Subproject commit b60600210384d1da4621cc35a0632d9cca8a62b7 From 1d3c661c06e2dd88d6a508ccc1e693dabe33ee76 Mon Sep 17 00:00:00 2001 From: Jay Blackman Date: Tue, 1 Apr 2025 22:17:25 +1100 Subject: [PATCH 58/58] REFACTOR: Moving platform specific DMA code to include/platform (#14320) * REFACTOR: Moving platform specific DMA code to include/platform * Cleanup --- src/main/drivers/dma.h | 239 +----------------- .../{dma_apm32.h => include/platform/dma.h} | 20 ++ src/platform/APM32/mk/APM32F4.mk | 3 +- .../{dma_atbsp.h => include/platform/dma.h} | 9 + src/platform/AT32/mk/AT32F4.mk | 3 +- src/platform/SIMULATOR/include/platform/dma.h | 27 ++ src/platform/SIMULATOR/mk/SITL.mk | 1 + src/platform/STM32/include/platform/dma.h | 238 +++++++++++++++++ src/platform/STM32/mk/STM32F4.mk | 2 + src/platform/STM32/mk/STM32F7.mk | 1 + src/platform/STM32/mk/STM32G4.mk | 1 + src/platform/STM32/mk/STM32H5.mk | 1 + src/platform/STM32/mk/STM32H7.mk | 1 + src/test/unit/platform/dma.h | 27 ++ 14 files changed, 335 insertions(+), 238 deletions(-) rename src/platform/APM32/{dma_apm32.h => include/platform/dma.h} (78%) rename src/platform/AT32/{dma_atbsp.h => include/platform/dma.h} (94%) create mode 100644 src/platform/SIMULATOR/include/platform/dma.h create mode 100644 src/platform/STM32/include/platform/dma.h create mode 100644 src/test/unit/platform/dma.h diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index ec93c09a78..39ce91552c 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -22,13 +22,7 @@ #include "drivers/resource.h" -#if defined(USE_ATBSP_DRIVER) -#include "dma_atbsp.h" -#endif - -#if defined(APM32F4) -#include "dma_apm32.h" -#endif +#include "platform/dma.h" #define CACHE_LINE_SIZE 32 #define CACHE_LINE_MASK (CACHE_LINE_SIZE - 1) @@ -42,26 +36,13 @@ typedef struct dmaResource_s dmaResource_t; -#if defined(STM32F4) || defined(STM32F7) -#define DMA_ARCH_TYPE DMA_Stream_TypeDef -#elif defined(STM32H7) -// H7 has stream based DMA and channel based BDMA, but we ignore BDMA (for now). -#define DMA_ARCH_TYPE DMA_Stream_TypeDef -#elif defined(AT32F435) -#define DMA_ARCH_TYPE dma_channel_type -#elif defined(APM32F4) -#define DMA_ARCH_TYPE DMA_Stream_TypeDef -#else -#define DMA_ARCH_TYPE DMA_Channel_TypeDef -#endif - struct dmaChannelDescriptor_s; typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channelDescriptor); typedef struct dmaChannelDescriptor_s { DMA_TypeDef* dma; dmaResource_t *ref; -#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4) +#if PLATFORM_TRAIT_DMA_STREAM_REQUIRED uint8_t stream; #endif uint32_t channel; @@ -72,198 +53,15 @@ typedef struct dmaChannelDescriptor_s { resourceOwner_t owner; uint8_t resourceIndex; uint32_t completeFlag; -#if defined(USE_ATBSP_DRIVER) +#if PLATFORM_TRAIT_DMA_MUX_REQUIRED dmamux_channel_type *dmamux; #endif } dmaChannelDescriptor_t; #define DMA_IDENTIFIER_TO_INDEX(x) ((x) - 1) -#if defined(USE_ATBSP_DRIVER) - -#elif defined(APM32F4) -// dma_apm32.h - -#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7) - -typedef enum { - DMA_NONE = 0, - DMA1_ST0_HANDLER = 1, - DMA1_ST1_HANDLER, - DMA1_ST2_HANDLER, - DMA1_ST3_HANDLER, - DMA1_ST4_HANDLER, - DMA1_ST5_HANDLER, - DMA1_ST6_HANDLER, - DMA1_ST7_HANDLER, - DMA2_ST0_HANDLER, - DMA2_ST1_HANDLER, - DMA2_ST2_HANDLER, - DMA2_ST3_HANDLER, - DMA2_ST4_HANDLER, - DMA2_ST5_HANDLER, - DMA2_ST6_HANDLER, - DMA2_ST7_HANDLER, - DMA_LAST_HANDLER = DMA2_ST7_HANDLER -} dmaIdentifier_e; - -#define DMA_DEVICE_NO(x) ((((x)-1) / 8) + 1) -#define DMA_DEVICE_INDEX(x) ((((x)-1) % 8)) -#define DMA_OUTPUT_INDEX 0 -#define DMA_OUTPUT_STRING "DMA%d Stream %d:" -#define DMA_INPUT_STRING "DMA%d_ST%d" - -#define DEFINE_DMA_CHANNEL(d, s, f) { \ - .dma = d, \ - .ref = (dmaResource_t *)d ## _Stream ## s, \ - .stream = s, \ - .irqHandlerCallback = NULL, \ - .flagsShift = f, \ - .irqN = d ## _Stream ## s ## _IRQn, \ - .userParam = 0, \ - .owner.owner = 0, \ - .owner.resourceIndex = 0 \ - } - -#define DEFINE_DMA_IRQ_HANDLER(d, s, i) FAST_IRQ_HANDLER void DMA ## d ## _Stream ## s ## _IRQHandler(void) {\ - const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \ - dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \ - if (handler) \ - handler(&dmaDescriptors[index]); \ - } - -#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift) -#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) - -#define DMA_IT_TCIF ((uint32_t)0x00000020) -#define DMA_IT_HTIF ((uint32_t)0x00000010) -#define DMA_IT_TEIF ((uint32_t)0x00000008) -#define DMA_IT_DMEIF ((uint32_t)0x00000004) -#define DMA_IT_FEIF ((uint32_t)0x00000001) - void dmaMuxEnable(dmaIdentifier_e identifier, uint32_t dmaMuxId); -#else - -#if defined(STM32G4) - -typedef enum { - DMA_NONE = 0, - DMA1_CH1_HANDLER = 1, - DMA1_CH2_HANDLER, - DMA1_CH3_HANDLER, - DMA1_CH4_HANDLER, - DMA1_CH5_HANDLER, - DMA1_CH6_HANDLER, - DMA1_CH7_HANDLER, - DMA1_CH8_HANDLER, - DMA2_CH1_HANDLER, - DMA2_CH2_HANDLER, - DMA2_CH3_HANDLER, - DMA2_CH4_HANDLER, - DMA2_CH5_HANDLER, - DMA2_CH6_HANDLER, - DMA2_CH7_HANDLER, - DMA2_CH8_HANDLER, - DMA_LAST_HANDLER = DMA2_CH8_HANDLER -} dmaIdentifier_e; - -#define DMA_DEVICE_NO(x) ((((x)-1) / 8) + 1) -#define DMA_DEVICE_INDEX(x) ((((x)-1) % 8) + 1) - -uint32_t dmaGetChannel(const uint8_t channel); - -#else // !STM32G4 - -typedef enum { - DMA_NONE = 0, - DMA1_CH1_HANDLER = 1, - DMA1_CH2_HANDLER, - DMA1_CH3_HANDLER, - DMA1_CH4_HANDLER, - DMA1_CH5_HANDLER, - DMA1_CH6_HANDLER, - DMA1_CH7_HANDLER, - DMA_LAST_HANDLER = DMA1_CH7_HANDLER -} dmaIdentifier_e; - -#define DMA_DEVICE_NO(x) ((((x)-1) / 7) + 1) -#define DMA_DEVICE_INDEX(x) ((((x)-1) % 7) + 1) - -#endif // STM32G4 - -#define DMA_OUTPUT_INDEX 0 -#define DMA_OUTPUT_STRING "DMA%d Channel %d:" -#define DMA_INPUT_STRING "DMA%d_CH%d" - -#define DEFINE_DMA_CHANNEL(d, c, f) { \ - .dma = d, \ - .ref = (dmaResource_t *)d ## _Channel ## c, \ - .irqHandlerCallback = NULL, \ - .flagsShift = f, \ - .irqN = d ## _Channel ## c ## _IRQn, \ - .userParam = 0, \ - .owner.owner = 0, \ - .owner.resourceIndex = 0 \ - } - -#define DMA_HANDLER_CODE - -#define DEFINE_DMA_IRQ_HANDLER(d, c, i) DMA_HANDLER_CODE void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\ - const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \ - dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \ - if (handler) \ - handler(&dmaDescriptors[index]); \ - } - -#define DMA_CLEAR_FLAG(d, flag) d->dma->IFCR = (flag << d->flagsShift) -#define DMA_GET_FLAG_STATUS(d, flag) (d->dma->ISR & (flag << d->flagsShift)) - -#define DMA_IT_TCIF ((uint32_t)0x00000002) -#define DMA_IT_HTIF ((uint32_t)0x00000004) -#define DMA_IT_TEIF ((uint32_t)0x00000008) -#endif - -// Macros to avoid direct register and register bit access - -#if defined(STM32F4) || defined(STM32F7) -#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->CR & DMA_SxCR_EN) -#define REG_NDTR NDTR -#elif defined(STM32H7) -// For H7, we have to differenciate DMA1/2 and BDMA for access to the control register. -// HAL library has a macro for this, but it is extremely inefficient in that it compares -// the address against all possible values. -// Here, we just compare the address against regions of memory. -#if defined(STM32H7A3xx) || defined(STM32H7A3xxQ) -// For H7A3, if it's lower than CD_AHB2PERIPH_BASE, then it's DMA1/2 and it's stream based. -// If not, it's BDMA and it's channel based. -#define IS_DMA_ENABLED(reg) \ - ((uint32_t)(reg) < CD_AHB2PERIPH_BASE) ? \ - (((DMA_Stream_TypeDef *)(reg))->CR & DMA_SxCR_EN) : \ - (((BDMA_Channel_TypeDef *)(reg))->CCR & BDMA_CCR_EN) -#else -// For H743 and H750, if it's not in D3 peripheral area, then it's DMA1/2 and it's stream based. -// If not, it's BDMA and it's channel based. -#define IS_DMA_ENABLED(reg) \ - ((uint32_t)(reg) < D3_AHB1PERIPH_BASE) ? \ - (((DMA_Stream_TypeDef *)(reg))->CR & DMA_SxCR_EN) : \ - (((BDMA_Channel_TypeDef *)(reg))->CCR & BDMA_CCR_EN) -#endif -#elif defined(STM32G4) -#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->CCR & DMA_CCR_EN) -// Missing __HAL_DMA_SET_COUNTER in FW library V1.0.0 -#define __HAL_DMA_SET_COUNTER(__HANDLE__, __COUNTER__) ((__HANDLE__)->Instance->CNDTR = (uint16_t)(__COUNTER__)) -#elif defined(AT32F4) -#define DMA_CCR_EN 1 -#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->ctrl_bit.chen & DMA_CCR_EN) -#elif defined(APM32F4) -#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->SCFG & DMA_SCFGx_EN) -#define REG_NDTR NDATA -#else -#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->CCR & DMA_CCR_EN) -#define DMAx_SetMemoryAddress(reg, address) ((DMA_ARCH_TYPE *)(reg))->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail] -#endif - dmaIdentifier_e dmaAllocate(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex); void dmaEnable(dmaIdentifier_e identifier); void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam); @@ -273,34 +71,3 @@ const resourceOwner_t *dmaGetOwner(dmaIdentifier_e identifier); dmaChannelDescriptor_t* dmaGetDescriptorByIdentifier(const dmaIdentifier_e identifier); uint32_t dmaGetChannel(const uint8_t channel); -// -// Wrapper macros to cast dmaResource_t back into DMA_ARCH_TYPE -// - -#if defined(USE_HAL_DRIVER) - -// We actually need these LL case only - -#define xLL_EX_DMA_DeInit(dmaResource) LL_EX_DMA_DeInit((DMA_ARCH_TYPE *)(dmaResource)) -#define xLL_EX_DMA_Init(dmaResource, initstruct) LL_EX_DMA_Init((DMA_ARCH_TYPE *)(dmaResource), initstruct) -#define xLL_EX_DMA_DisableResource(dmaResource) LL_EX_DMA_DisableResource((DMA_ARCH_TYPE *)(dmaResource)) -#define xLL_EX_DMA_EnableResource(dmaResource) LL_EX_DMA_EnableResource((DMA_ARCH_TYPE *)(dmaResource)) -#define xLL_EX_DMA_GetDataLength(dmaResource) LL_EX_DMA_GetDataLength((DMA_ARCH_TYPE *)(dmaResource)) -#define xLL_EX_DMA_SetDataLength(dmaResource, length) LL_EX_DMA_SetDataLength((DMA_ARCH_TYPE *)(dmaResource), length) -#define xLL_EX_DMA_EnableIT_TC(dmaResource) LL_EX_DMA_EnableIT_TC((DMA_ARCH_TYPE *)(dmaResource)) - -#elif defined(USE_ATBSP_DRIVER) - -#else - -#define xDMA_Init(dmaResource, initStruct) DMA_Init((DMA_ARCH_TYPE *)(dmaResource), initStruct) -#define xDMA_DeInit(dmaResource) DMA_DeInit((DMA_ARCH_TYPE *)(dmaResource)) -#define xDMA_Cmd(dmaResource, newState) DMA_Cmd((DMA_ARCH_TYPE *)(dmaResource), newState) -#define xDMA_ITConfig(dmaResource, flags, newState) DMA_ITConfig((DMA_ARCH_TYPE *)(dmaResource), flags, newState) -#define xDMA_GetCurrDataCounter(dmaResource) DMA_GetCurrDataCounter((DMA_ARCH_TYPE *)(dmaResource)) -#define xDMA_SetCurrDataCounter(dmaResource, count) DMA_SetCurrDataCounter((DMA_ARCH_TYPE *)(dmaResource), count) -#define xDMA_GetFlagStatus(dmaResource, flags) DMA_GetFlagStatus((DMA_ARCH_TYPE *)(dmaResource), flags) -#define xDMA_ClearFlag(dmaResource, flags) DMA_ClearFlag((DMA_ARCH_TYPE *)(dmaResource), flags) -#define xDMA_MemoryTargetConfig(dmaResource, address, target) DMA_MemoryTargetConfig((DMA_ARCH_TYPE *)(dmaResource), address, target) - -#endif diff --git a/src/platform/APM32/dma_apm32.h b/src/platform/APM32/include/platform/dma.h similarity index 78% rename from src/platform/APM32/dma_apm32.h rename to src/platform/APM32/include/platform/dma.h index a719782180..0b20062250 100644 --- a/src/platform/APM32/dma_apm32.h +++ b/src/platform/APM32/include/platform/dma.h @@ -24,6 +24,12 @@ #include "platform.h" #include "drivers/resource.h" +#if defined(APM32F4) +#define PLATFORM_TRAIT_DMA_STREAM_REQUIRED 1 +#endif + +#define DMA_ARCH_TYPE DMA_Stream_TypeDef + typedef enum { DMA_NONE = 0, DMA1_ST0_HANDLER = 1, @@ -88,3 +94,17 @@ typedef enum { #define DMA_IT_FEIF ((uint32_t)0x00000001) void dmaMuxEnable(dmaIdentifier_e identifier, uint32_t dmaMuxId); + +#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->SCFG & DMA_SCFGx_EN) +#define REG_NDTR NDATA + +#if defined(USE_HAL_DRIVER) +// We actually need these LL case only +#define xLL_EX_DMA_DeInit(dmaResource) LL_EX_DMA_DeInit((DMA_ARCH_TYPE *)(dmaResource)) +#define xLL_EX_DMA_Init(dmaResource, initstruct) LL_EX_DMA_Init((DMA_ARCH_TYPE *)(dmaResource), initstruct) +#define xLL_EX_DMA_DisableResource(dmaResource) LL_EX_DMA_DisableResource((DMA_ARCH_TYPE *)(dmaResource)) +#define xLL_EX_DMA_EnableResource(dmaResource) LL_EX_DMA_EnableResource((DMA_ARCH_TYPE *)(dmaResource)) +#define xLL_EX_DMA_GetDataLength(dmaResource) LL_EX_DMA_GetDataLength((DMA_ARCH_TYPE *)(dmaResource)) +#define xLL_EX_DMA_SetDataLength(dmaResource, length) LL_EX_DMA_SetDataLength((DMA_ARCH_TYPE *)(dmaResource), length) +#define xLL_EX_DMA_EnableIT_TC(dmaResource) LL_EX_DMA_EnableIT_TC((DMA_ARCH_TYPE *)(dmaResource)) +#endif diff --git a/src/platform/APM32/mk/APM32F4.mk b/src/platform/APM32/mk/APM32F4.mk index b82d82cf56..c0b9a27c91 100644 --- a/src/platform/APM32/mk/APM32F4.mk +++ b/src/platform/APM32/mk/APM32F4.mk @@ -111,8 +111,9 @@ DEVICE_STDPERIPH_SRC := \ VPATH := $(VPATH):$(LIB_MAIN_DIR)/APM32F4/Libraries/Device/Geehy/APM32F4xx INCLUDE_DIRS += \ - $(TARGET_PLATFORM_DIR)/startup \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ + $(TARGET_PLATFORM_DIR)/startup \ $(PLATFORM_DIR)/common/stm32 \ $(STDPERIPH_DIR)/Include \ $(LIB_MAIN_DIR)/$(USBCORE_DIR)/Inc \ diff --git a/src/platform/AT32/dma_atbsp.h b/src/platform/AT32/include/platform/dma.h similarity index 94% rename from src/platform/AT32/dma_atbsp.h rename to src/platform/AT32/include/platform/dma.h index 1cf03115c1..adb08d3843 100644 --- a/src/platform/AT32/dma_atbsp.h +++ b/src/platform/AT32/include/platform/dma.h @@ -24,6 +24,12 @@ #include "platform.h" #include "drivers/resource.h" +#if defined(USE_ATBSP_DRIVER) +#define PLATFORM_TRAIT_DMA_MUX_REQUIRED 1 +#endif + +#define DMA_ARCH_TYPE dma_channel_type + typedef enum { DMA_NONE = 0, DMA1_CH1_HANDLER = 1, @@ -90,3 +96,6 @@ void dmaMuxEnable(dmaIdentifier_e identifier, uint32_t dmaMuxId); #define xDMA_SetCurrDataCounter(dmaResource, count) dma_data_number_set((DMA_ARCH_TYPE *)(dmaResource), count) #define xDMA_GetFlagStatus(dmaResource, flags) dma_flag_get((DMA_ARCH_TYPE *)(dmaResource), flags) #define xDMA_ClearFlag(dmaResource, flags) dma_flag_clear((DMA_ARCH_TYPE *)(dmaResource), flags) + +#define DMA_CCR_EN 1 +#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->ctrl_bit.chen & DMA_CCR_EN) diff --git a/src/platform/AT32/mk/AT32F4.mk b/src/platform/AT32/mk/AT32F4.mk index 2a0149017e..ec377078aa 100644 --- a/src/platform/AT32/mk/AT32F4.mk +++ b/src/platform/AT32/mk/AT32F4.mk @@ -61,8 +61,9 @@ VCP_INCLUDES = \ DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC) INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(TARGET_PLATFORM_DIR)/startup \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ + $(TARGET_PLATFORM_DIR)/startup \ $(PLATFORM_DIR)/common/stm32 \ $(STDPERIPH_DIR)/inc \ $(CMSIS_DIR)/cm4/core_support \ diff --git a/src/platform/SIMULATOR/include/platform/dma.h b/src/platform/SIMULATOR/include/platform/dma.h new file mode 100644 index 0000000000..f199deeed6 --- /dev/null +++ b/src/platform/SIMULATOR/include/platform/dma.h @@ -0,0 +1,27 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software 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. + * + * Betaflight 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 this software. + * + * If not, see . + */ + + #pragma once + + typedef enum { + DMA_NONE = 0, + DMA_LAST_HANDLER = DMA_NONE +} dmaIdentifier_e; diff --git a/src/platform/SIMULATOR/mk/SITL.mk b/src/platform/SIMULATOR/mk/SITL.mk index 6a2d8e8b9d..001255826e 100644 --- a/src/platform/SIMULATOR/mk/SITL.mk +++ b/src/platform/SIMULATOR/mk/SITL.mk @@ -2,6 +2,7 @@ INCLUDE_DIRS := \ $(INCLUDE_DIRS) \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ $(LIB_MAIN_DIR)/dyad MCU_COMMON_SRC := \ diff --git a/src/platform/STM32/include/platform/dma.h b/src/platform/STM32/include/platform/dma.h new file mode 100644 index 0000000000..fbd9f5f1cb --- /dev/null +++ b/src/platform/STM32/include/platform/dma.h @@ -0,0 +1,238 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software 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. + * + * Betaflight 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 this software. + * + * If not, see . + */ + +#pragma once + +#include "platform.h" + +#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4) +#define PLATFORM_TRAIT_DMA_STREAM_REQUIRED 1 +#endif + +#if defined(STM32F4) || defined(STM32F7) +#define DMA_ARCH_TYPE DMA_Stream_TypeDef +#elif defined(STM32H7) +// H7 has stream based DMA and channel based BDMA, but we ignore BDMA (for now). +#define DMA_ARCH_TYPE DMA_Stream_TypeDef +#else +#define DMA_ARCH_TYPE DMA_Channel_TypeDef +#endif + +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) + +typedef enum { + DMA_NONE = 0, + DMA1_ST0_HANDLER = 1, + DMA1_ST1_HANDLER, + DMA1_ST2_HANDLER, + DMA1_ST3_HANDLER, + DMA1_ST4_HANDLER, + DMA1_ST5_HANDLER, + DMA1_ST6_HANDLER, + DMA1_ST7_HANDLER, + DMA2_ST0_HANDLER, + DMA2_ST1_HANDLER, + DMA2_ST2_HANDLER, + DMA2_ST3_HANDLER, + DMA2_ST4_HANDLER, + DMA2_ST5_HANDLER, + DMA2_ST6_HANDLER, + DMA2_ST7_HANDLER, + DMA_LAST_HANDLER = DMA2_ST7_HANDLER +} dmaIdentifier_e; + +#define DMA_DEVICE_NO(x) ((((x)-1) / 8) + 1) +#define DMA_DEVICE_INDEX(x) ((((x)-1) % 8)) +#define DMA_OUTPUT_INDEX 0 +#define DMA_OUTPUT_STRING "DMA%d Stream %d:" +#define DMA_INPUT_STRING "DMA%d_ST%d" + +#define DEFINE_DMA_CHANNEL(d, s, f) { \ + .dma = d, \ + .ref = (dmaResource_t *)d ## _Stream ## s, \ + .stream = s, \ + .irqHandlerCallback = NULL, \ + .flagsShift = f, \ + .irqN = d ## _Stream ## s ## _IRQn, \ + .userParam = 0, \ + .owner.owner = 0, \ + .owner.resourceIndex = 0 \ + } + +#define DEFINE_DMA_IRQ_HANDLER(d, s, i) FAST_IRQ_HANDLER void DMA ## d ## _Stream ## s ## _IRQHandler(void) {\ + const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \ + dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \ + if (handler) \ + handler(&dmaDescriptors[index]); \ + } + +#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift) +#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) + +#define DMA_IT_TCIF ((uint32_t)0x00000020) +#define DMA_IT_HTIF ((uint32_t)0x00000010) +#define DMA_IT_TEIF ((uint32_t)0x00000008) +#define DMA_IT_DMEIF ((uint32_t)0x00000004) +#define DMA_IT_FEIF ((uint32_t)0x00000001) + +void dmaMuxEnable(dmaIdentifier_e identifier, uint32_t dmaMuxId); + +#else + +#if defined(STM32G4) + +typedef enum { + DMA_NONE = 0, + DMA1_CH1_HANDLER = 1, + DMA1_CH2_HANDLER, + DMA1_CH3_HANDLER, + DMA1_CH4_HANDLER, + DMA1_CH5_HANDLER, + DMA1_CH6_HANDLER, + DMA1_CH7_HANDLER, + DMA1_CH8_HANDLER, + DMA2_CH1_HANDLER, + DMA2_CH2_HANDLER, + DMA2_CH3_HANDLER, + DMA2_CH4_HANDLER, + DMA2_CH5_HANDLER, + DMA2_CH6_HANDLER, + DMA2_CH7_HANDLER, + DMA2_CH8_HANDLER, + DMA_LAST_HANDLER = DMA2_CH8_HANDLER +} dmaIdentifier_e; + +#define DMA_DEVICE_NO(x) ((((x)-1) / 8) + 1) +#define DMA_DEVICE_INDEX(x) ((((x)-1) % 8) + 1) + +uint32_t dmaGetChannel(const uint8_t channel); + +#else // !STM32G4 + +typedef enum { + DMA_NONE = 0, + DMA1_CH1_HANDLER = 1, + DMA1_CH2_HANDLER, + DMA1_CH3_HANDLER, + DMA1_CH4_HANDLER, + DMA1_CH5_HANDLER, + DMA1_CH6_HANDLER, + DMA1_CH7_HANDLER, + DMA_LAST_HANDLER = DMA1_CH7_HANDLER +} dmaIdentifier_e; + +#define DMA_DEVICE_NO(x) ((((x)-1) / 7) + 1) +#define DMA_DEVICE_INDEX(x) ((((x)-1) % 7) + 1) + +#endif // STM32G4 + +#define DMA_OUTPUT_INDEX 0 +#define DMA_OUTPUT_STRING "DMA%d Channel %d:" +#define DMA_INPUT_STRING "DMA%d_CH%d" + +#define DEFINE_DMA_CHANNEL(d, c, f) { \ + .dma = d, \ + .ref = (dmaResource_t *)d ## _Channel ## c, \ + .irqHandlerCallback = NULL, \ + .flagsShift = f, \ + .irqN = d ## _Channel ## c ## _IRQn, \ + .userParam = 0, \ + .owner.owner = 0, \ + .owner.resourceIndex = 0 \ + } + +#define DMA_HANDLER_CODE + +#define DEFINE_DMA_IRQ_HANDLER(d, c, i) DMA_HANDLER_CODE void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\ + const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \ + dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \ + if (handler) \ + handler(&dmaDescriptors[index]); \ + } + +#define DMA_CLEAR_FLAG(d, flag) d->dma->IFCR = (flag << d->flagsShift) +#define DMA_GET_FLAG_STATUS(d, flag) (d->dma->ISR & (flag << d->flagsShift)) + +#define DMA_IT_TCIF ((uint32_t)0x00000002) +#define DMA_IT_HTIF ((uint32_t)0x00000004) +#define DMA_IT_TEIF ((uint32_t)0x00000008) + +#endif + +// Macros to avoid direct register and register bit access + +#if defined(STM32F4) || defined(STM32F7) +#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->CR & DMA_SxCR_EN) +#define REG_NDTR NDTR +#elif defined(STM32H7) +// For H7, we have to differenciate DMA1/2 and BDMA for access to the control register. +// HAL library has a macro for this, but it is extremely inefficient in that it compares +// the address against all possible values. +// Here, we just compare the address against regions of memory. +#if defined(STM32H7A3xx) || defined(STM32H7A3xxQ) +// For H7A3, if it's lower than CD_AHB2PERIPH_BASE, then it's DMA1/2 and it's stream based. +// If not, it's BDMA and it's channel based. +#define IS_DMA_ENABLED(reg) \ + ((uint32_t)(reg) < CD_AHB2PERIPH_BASE) ? \ + (((DMA_Stream_TypeDef *)(reg))->CR & DMA_SxCR_EN) : \ + (((BDMA_Channel_TypeDef *)(reg))->CCR & BDMA_CCR_EN) +#else +// For H743 and H750, if it's not in D3 peripheral area, then it's DMA1/2 and it's stream based. +// If not, it's BDMA and it's channel based. +#define IS_DMA_ENABLED(reg) \ + ((uint32_t)(reg) < D3_AHB1PERIPH_BASE) ? \ + (((DMA_Stream_TypeDef *)(reg))->CR & DMA_SxCR_EN) : \ + (((BDMA_Channel_TypeDef *)(reg))->CCR & BDMA_CCR_EN) +#endif +#elif defined(STM32G4) +#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->CCR & DMA_CCR_EN) +// Missing __HAL_DMA_SET_COUNTER in FW library V1.0.0 +#define __HAL_DMA_SET_COUNTER(__HANDLE__, __COUNTER__) ((__HANDLE__)->Instance->CNDTR = (uint16_t)(__COUNTER__)) +#else +#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->CCR & DMA_CCR_EN) +#define DMAx_SetMemoryAddress(reg, address) ((DMA_ARCH_TYPE *)(reg))->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail] +#endif + +#if defined(USE_HAL_DRIVER) + +// We actually need these LL case only + +#define xLL_EX_DMA_DeInit(dmaResource) LL_EX_DMA_DeInit((DMA_ARCH_TYPE *)(dmaResource)) +#define xLL_EX_DMA_Init(dmaResource, initstruct) LL_EX_DMA_Init((DMA_ARCH_TYPE *)(dmaResource), initstruct) +#define xLL_EX_DMA_DisableResource(dmaResource) LL_EX_DMA_DisableResource((DMA_ARCH_TYPE *)(dmaResource)) +#define xLL_EX_DMA_EnableResource(dmaResource) LL_EX_DMA_EnableResource((DMA_ARCH_TYPE *)(dmaResource)) +#define xLL_EX_DMA_GetDataLength(dmaResource) LL_EX_DMA_GetDataLength((DMA_ARCH_TYPE *)(dmaResource)) +#define xLL_EX_DMA_SetDataLength(dmaResource, length) LL_EX_DMA_SetDataLength((DMA_ARCH_TYPE *)(dmaResource), length) +#define xLL_EX_DMA_EnableIT_TC(dmaResource) LL_EX_DMA_EnableIT_TC((DMA_ARCH_TYPE *)(dmaResource)) + +#else + +#define xDMA_Init(dmaResource, initStruct) DMA_Init((DMA_ARCH_TYPE *)(dmaResource), initStruct) +#define xDMA_DeInit(dmaResource) DMA_DeInit((DMA_ARCH_TYPE *)(dmaResource)) +#define xDMA_Cmd(dmaResource, newState) DMA_Cmd((DMA_ARCH_TYPE *)(dmaResource), newState) +#define xDMA_ITConfig(dmaResource, flags, newState) DMA_ITConfig((DMA_ARCH_TYPE *)(dmaResource), flags, newState) +#define xDMA_GetCurrDataCounter(dmaResource) DMA_GetCurrDataCounter((DMA_ARCH_TYPE *)(dmaResource)) +#define xDMA_SetCurrDataCounter(dmaResource, count) DMA_SetCurrDataCounter((DMA_ARCH_TYPE *)(dmaResource), count) +#define xDMA_GetFlagStatus(dmaResource, flags) DMA_GetFlagStatus((DMA_ARCH_TYPE *)(dmaResource), flags) +#define xDMA_ClearFlag(dmaResource, flags) DMA_ClearFlag((DMA_ARCH_TYPE *)(dmaResource), flags) +#define xDMA_MemoryTargetConfig(dmaResource, address, target) DMA_MemoryTargetConfig((DMA_ARCH_TYPE *)(dmaResource), address, target) + +#endif diff --git a/src/platform/STM32/mk/STM32F4.mk b/src/platform/STM32/mk/STM32F4.mk index e41e3db33b..aa4b1d6249 100644 --- a/src/platform/STM32/mk/STM32F4.mk +++ b/src/platform/STM32/mk/STM32F4.mk @@ -117,6 +117,7 @@ CMSIS_SRC := INCLUDE_DIRS := \ $(INCLUDE_DIRS) \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ $(TARGET_PLATFORM_DIR)/startup \ $(STDPERIPH_DIR)/Inc \ $(LIB_MAIN_DIR)/$(USBCORE_DIR)/Inc \ @@ -132,6 +133,7 @@ CMSIS_SRC := \ INCLUDE_DIRS := \ $(INCLUDE_DIRS) \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ $(TARGET_PLATFORM_DIR)/startup \ $(STDPERIPH_DIR)/inc \ $(LIB_MAIN_DIR)/$(USBOTG_DIR)/inc \ diff --git a/src/platform/STM32/mk/STM32F7.mk b/src/platform/STM32/mk/STM32F7.mk index f68fd69525..90c3291cec 100644 --- a/src/platform/STM32/mk/STM32F7.mk +++ b/src/platform/STM32/mk/STM32F7.mk @@ -85,6 +85,7 @@ VPATH := $(VPATH):$(CMSIS_DIR)/Include:$(CMSIS_DIR)/Device/ST/STM32F7x CMSIS_SRC := INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ $(TARGET_PLATFORM_DIR)/startup \ $(STDPERIPH_DIR)/Inc \ $(LIB_MAIN_DIR)/$(USBCORE_DIR)/Inc \ diff --git a/src/platform/STM32/mk/STM32G4.mk b/src/platform/STM32/mk/STM32G4.mk index 38cf7cdbdd..0706aff8fd 100644 --- a/src/platform/STM32/mk/STM32G4.mk +++ b/src/platform/STM32/mk/STM32G4.mk @@ -78,6 +78,7 @@ CMSIS_SRC := INCLUDE_DIRS := \ $(INCLUDE_DIRS) \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ $(TARGET_PLATFORM_DIR)/startup \ $(STDPERIPH_DIR)/Inc \ $(LIB_MAIN_DIR)/$(USBCORE_DIR)/Inc \ diff --git a/src/platform/STM32/mk/STM32H5.mk b/src/platform/STM32/mk/STM32H5.mk index 13c5a59cb3..a9080ba2ed 100644 --- a/src/platform/STM32/mk/STM32H5.mk +++ b/src/platform/STM32/mk/STM32H5.mk @@ -87,6 +87,7 @@ VPATH := $(VPATH):$(CMSIS_DIR)/Include:$(CMSIS_DIR)/Device/ST/STM32H5x CMSIS_SRC := INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ $(TARGET_PLATFORM_DIR)/startup \ $(STDPERIPH_DIR)/Inc \ $(LIB_MAIN_DIR)/$(USBCORE_DIR)/Inc \ diff --git a/src/platform/STM32/mk/STM32H7.mk b/src/platform/STM32/mk/STM32H7.mk index edcd2c27b8..7d192ff057 100644 --- a/src/platform/STM32/mk/STM32H7.mk +++ b/src/platform/STM32/mk/STM32H7.mk @@ -91,6 +91,7 @@ VPATH := $(VPATH):$(CMSIS_DIR)/Include:$(CMSIS_DIR)/Device/ST/STM32H7x CMSIS_SRC := INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ $(TARGET_PLATFORM_DIR)/startup \ $(STDPERIPH_DIR)/Inc \ $(LIB_MAIN_DIR)/$(USBCORE_DIR)/Inc \ diff --git a/src/test/unit/platform/dma.h b/src/test/unit/platform/dma.h new file mode 100644 index 0000000000..f199deeed6 --- /dev/null +++ b/src/test/unit/platform/dma.h @@ -0,0 +1,27 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software 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. + * + * Betaflight 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 this software. + * + * If not, see . + */ + + #pragma once + + typedef enum { + DMA_NONE = 0, + DMA_LAST_HANDLER = DMA_NONE +} dmaIdentifier_e;