diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 39e6456830a..c94fca21209 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -64,7 +64,11 @@ if(NOT MACOSX) -Wno-error=maybe-uninitialized -fsingle-precision-constant ) - if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0) + include(CheckLinkerFlag OPTIONAL) + if(COMMAND check_linker_flag) + check_linker_flag(C "-Wl,--no-warn-rwx-segments" LINKER_SUPPORTS_NO_RWX_WARNING) + endif() + if(LINKER_SUPPORTS_NO_RWX_WARNING) set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-Wl,--no-warn-rwx-segments") endif() else() diff --git a/docs/Cli.md b/docs/Cli.md index e5fc9dc1541..966c5ec23ea 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -27,17 +27,17 @@ Disconnect main power, connect to cli via USB/FTDI. dump using cli ``` -profile 0 +control_profile 0 dump ``` -dump profiles using cli if you use them +dump control_profiles using cli if you use them ``` -profile 1 -dump profile -profile 2 -dump profile +control_profile 1 +dump control_profile +control_profile 2 +dump control_profile ``` copy screen output to a file and save it. @@ -100,7 +100,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0 | `osd_layout` | Get or set the layout of OSD items | | `pid` | Configurable PID controllers | | `play_sound` | ``, or none for next item | -| `profile` | Change profile | +| `control_profile` | Change profile | | `resource` | View currently used resources | | `rxrange` | Configure rx channel ranges | | `safehome` | Define safe home locations. See the [safehome documentation](Safehomes.md) for usage information. | diff --git a/docs/development/msp/format.md b/docs/development/msp/format.md index e8c6a348c07..bbe7c1fc6fa 100644 --- a/docs/development/msp/format.md +++ b/docs/development/msp/format.md @@ -40,6 +40,7 @@ **variable_len**: Optional boolean, if true, message does not have a predefined fixed length and needs appropriate handling\ **variants**: Optional special case, message has different cases of reply/request. Key/description is not a strict expression or code; just a readable condition\ **not_implemented**: Optional special case, message is not implemented (never or deprecated)\ +**replaced_by**: Optional array of MSP message names that replace this command. Present when a command is deprecated and scheduled for removal. Empty array if no replacement is needed\ **notes**: String with details of message ## Data dict fields: diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index e4c2a39993f..84d603de8e1 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -673,6 +673,7 @@ "MSP_POSITION_ESTIMATION_CONFIG": { "code": 16, "mspv": 1, + "replaced_by": ["MSP2_COMMON_SETTING"], "request": null, "reply": { "payload": [ @@ -727,6 +728,7 @@ "MSP_SET_POSITION_ESTIMATION_CONFIG": { "code": 17, "mspv": 1, + "replaced_by": ["MSP2_COMMON_SET_SETTING"], "request": { "payload": [ { @@ -2360,6 +2362,7 @@ "MSP_BLACKBOX_CONFIG": { "code": 80, "mspv": 1, + "replaced_by": ["MSP2_BLACKBOX_CONFIG"], "request": null, "reply": { "payload": [ @@ -2399,6 +2402,7 @@ "MSP_SET_BLACKBOX_CONFIG": { "code": 81, "mspv": 1, + "replaced_by": ["MSP2_SET_BLACKBOX_CONFIG"], "not_implemented": true, "request": null, "reply": null, @@ -2426,6 +2430,7 @@ "MSP_OSD_CONFIG": { "code": 84, "mspv": 1, + "replaced_by": ["MSP2_INAV_OSD_LAYOUTS", "MSP2_INAV_OSD_ALARMS", "MSP2_INAV_OSD_PREFERENCES"], "request": null, "reply": { "payload": [ @@ -2504,6 +2509,7 @@ "MSP_SET_OSD_CONFIG": { "code": 85, "mspv": 1, + "replaced_by": ["MSP2_INAV_OSD_SET_LAYOUT_ITEM", "MSP2_INAV_OSD_SET_ALARMS", "MSP2_INAV_OSD_SET_PREFERENCES"], "request": null, "reply": null, "notes": "Requires `USE_OSD`. Distinguishes formats based on the first byte. Format 1 requires at least 10 bytes. Format 2 requires 3 bytes. Triggers an OSD redraw. See `MSP2_INAV_OSD_SET_*` for more advanced control.", @@ -3134,6 +3140,7 @@ "MSP_FILTER_CONFIG": { "code": 92, "mspv": 1, + "replaced_by": ["MSP2_COMMON_SETTING"], "request": null, "reply": { "payload": [ @@ -3217,6 +3224,7 @@ "MSP_SET_FILTER_CONFIG": { "code": 93, "mspv": 1, + "replaced_by": ["MSP2_COMMON_SET_SETTING"], "request": { "payload": [ { @@ -3293,6 +3301,7 @@ "MSP_PID_ADVANCED": { "code": 94, "mspv": 1, + "replaced_by": ["MSP_INAV_PID"], "request": null, "reply": { "payload": [ @@ -3370,6 +3379,7 @@ "MSP_SET_PID_ADVANCED": { "code": 95, "mspv": 1, + "replaced_by": ["MSP_SET_INAV_PID"], "request": { "payload": [ { @@ -3599,6 +3609,7 @@ "MSP_STATUS": { "code": 101, "mspv": 1, + "replaced_by": ["MSP2_INAV_STATUS"], "request": null, "reply": { "payload": [ @@ -3914,6 +3925,7 @@ "MSP_ANALOG": { "code": 110, "mspv": 1, + "replaced_by": ["MSP2_INAV_ANALOG"], "request": null, "reply": { "payload": [ @@ -4039,6 +4051,7 @@ "MSP_MISC": { "code": 114, "mspv": 1, + "replaced_by": ["MSP2_INAV_MISC"], "request": null, "reply": { "payload": [ @@ -4168,6 +4181,7 @@ "MSP_PIDNAMES": { "code": 117, "mspv": 1, + "replaced_by": [], "request": null, "reply": { "payload": [ @@ -4644,6 +4658,7 @@ "MSP_STATUS_EX": { "code": 150, "mspv": 1, + "replaced_by": ["MSP2_INAV_STATUS"], "request": null, "reply": { "payload": [ @@ -4701,7 +4716,7 @@ } ] }, - "notes": "Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements.", + "notes": "Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. The `accCalibAxisFlags` field is not present in `MSP2_INAV_STATUS` but is available via `MSP_CALIBRATION_DATA`.", "description": "Provides extended flight controller status, including CPU load, arming flags, and calibration status, in addition to `MSP_STATUS` fields." }, "MSP_SENSOR_STATUS": { @@ -5135,6 +5150,7 @@ "MSP_SET_MISC": { "code": 207, "mspv": 1, + "replaced_by": ["MSP2_INAV_SET_MISC"], "request": { "payload": [ { @@ -5588,6 +5604,7 @@ "MSP_SERVO_MIX_RULES": { "code": 241, "mspv": 1, + "replaced_by": ["MSP2_INAV_SERVO_MIXER"], "request": null, "reply": { "payload": [ @@ -5644,6 +5661,7 @@ "MSP_SET_SERVO_MIX_RULE": { "code": 242, "mspv": 1, + "replaced_by": ["MSP2_INAV_SET_SERVO_MIXER"], "request": { "payload": [ { @@ -5803,6 +5821,7 @@ "MSP_DEBUG": { "code": 254, "mspv": 1, + "replaced_by": ["MSP2_INAV_DEBUG"], "request": null, "reply": { "payload": [ diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 993634d902d..d591e40de81 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -526,7 +526,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu } float gaussian(const float x, const float mu, const float sigma) { - return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2))); + return expf(-((x - mu) * (x - mu)) / (2.0f * sigma * sigma)); } float bellCurve(const float x, const float curveWidth) diff --git a/src/main/common/time.h b/src/main/common/time.h index 82f29860777..15fc5e2ce08 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -58,7 +58,7 @@ typedef uint32_t timeUs_t; #define MS2S(ms) ((ms) * 1e-3f) #define S2MS(s) ((s) * MILLISECS_PER_SEC) #define DS2MS(ds) ((ds) * 100) -#define HZ2S(hz) US2S(HZ2US(hz)) +#define HZ2S(hz) (1.0f / (hz)) // Use this function only to get small deltas (difference overflows at ~35 minutes) static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); } diff --git a/src/main/drivers/bus_spi_hal_ll.c b/src/main/drivers/bus_spi_hal_ll.c index fac8a82761a..ce6fbde9595 100644 --- a/src/main/drivers/bus_spi_hal_ll.c +++ b/src/main/drivers/bus_spi_hal_ll.c @@ -89,109 +89,159 @@ static const uint32_t spiDivisorMapSlow[] = { #endif #if defined(STM32H7) -static spiDevice_t spiHardwareMap[SPIDEV_COUNT] = { +#include "bus_spi_stm32h7xx.h" + +// Auto-resolve SPI AF per pin from the lookup table in bus_spi_stm32h7xx.h. +// Targets may still define individual SPI*_SCK/MISO/MOSI_AF values in target.h +// to override; explicit defines take priority via these #ifndef guards. #ifdef USE_SPI_DEVICE_1 -#if defined(SPI1_SCK_AF) || defined(SPI1_MISO_AF) || defined(SPI1_MOSI_AF) -#if !defined(SPI1_SCK_AF) || !defined(SPI1_MISO_AF) || !defined(SPI1_MOSI_AF) -#error SPI1: SCK, MISO and MOSI AFs should be defined together in target.h! +#ifndef SPI1_SCK_AF +#define SPI1_SCK_AF SPI_PIN_AF_HELPER(1, SPI1_SCK_PIN) #endif - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = SPI1_SCK_AF, .misoAF = SPI1_MISO_AF, .mosiAF = SPI1_MOSI_AF, .divisorMap = spiDivisorMapFast }, -#else - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = GPIO_AF5_SPI1, .misoAF = GPIO_AF5_SPI1, .mosiAF = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#ifndef SPI1_MISO_AF +#define SPI1_MISO_AF SPI_PIN_AF_HELPER(1, SPI1_MISO_PIN) +#endif +#ifndef SPI1_MOSI_AF +#define SPI1_MOSI_AF SPI_PIN_AF_HELPER(1, SPI1_MOSI_PIN) #endif -#else - { .dev = NULL }, // No SPI1 #endif #ifdef USE_SPI_DEVICE_2 -#if defined(SPI2_SCK_AF) || defined(SPI2_MISO_AF) || defined(SPI2_MOSI_AF) -#if !defined(SPI2_SCK_AF) || !defined(SPI2_MISO_AF) || !defined(SPI2_MOSI_AF) -#error SPI2: SCK, MISO and MOSI AFs should be defined together in target.h! +#ifndef SPI2_SCK_AF +#define SPI2_SCK_AF SPI_PIN_AF_HELPER(2, SPI2_SCK_PIN) #endif - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .sckAF = SPI2_SCK_AF, .misoAF = SPI2_MISO_AF, .mosiAF = SPI2_MOSI_AF, .divisorMap = spiDivisorMapSlow }, +#ifndef SPI2_MISO_AF +#define SPI2_MISO_AF SPI_PIN_AF_HELPER(2, SPI2_MISO_PIN) +#endif +#ifndef SPI2_MOSI_AF +#define SPI2_MOSI_AF SPI_PIN_AF_HELPER(2, SPI2_MOSI_PIN) +#endif +#endif + +#ifdef USE_SPI_DEVICE_3 +#ifndef SPI3_SCK_AF +#define SPI3_SCK_AF SPI_PIN_AF_HELPER(3, SPI3_SCK_PIN) +#endif +#ifndef SPI3_MISO_AF +#define SPI3_MISO_AF SPI_PIN_AF_HELPER(3, SPI3_MISO_PIN) +#endif +#ifndef SPI3_MOSI_AF +#define SPI3_MOSI_AF SPI_PIN_AF_HELPER(3, SPI3_MOSI_PIN) +#endif +#endif + +#ifdef USE_SPI_DEVICE_4 +#ifndef SPI4_SCK_AF +#define SPI4_SCK_AF SPI_PIN_AF_HELPER(4, SPI4_SCK_PIN) +#endif +#ifndef SPI4_MISO_AF +#define SPI4_MISO_AF SPI_PIN_AF_HELPER(4, SPI4_MISO_PIN) +#endif +#ifndef SPI4_MOSI_AF +#define SPI4_MOSI_AF SPI_PIN_AF_HELPER(4, SPI4_MOSI_PIN) +#endif +#endif + +static spiDevice_t spiHardwareMap[SPIDEV_COUNT] = { +#ifdef USE_SPI_DEVICE_1 + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = SPI1_SCK_AF, .misoAF = SPI1_MISO_AF, .mosiAF = SPI1_MOSI_AF, .divisorMap = spiDivisorMapFast }, #else - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .sckAF = GPIO_AF5_SPI2, .misoAF = GPIO_AF5_SPI2, .mosiAF = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, + { .dev = NULL }, // No SPI1 #endif + +#ifdef USE_SPI_DEVICE_2 + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .sckAF = SPI2_SCK_AF, .misoAF = SPI2_MISO_AF, .mosiAF = SPI2_MOSI_AF, .divisorMap = spiDivisorMapSlow }, #else { .dev = NULL }, // No SPI2 #endif #ifdef USE_SPI_DEVICE_3 -#if defined(SPI3_SCK_AF) || defined(SPI3_MISO_AF) || defined(SPI3_MOSI_AF) -#if !defined(SPI3_SCK_AF) || !defined(SPI3_MISO_AF) || !defined(SPI3_MOSI_AF) -#error SPI3: SCK, MISO and MOSI AFs should be defined together in target.h! -#endif { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .sckAF = SPI3_SCK_AF, .misoAF = SPI3_MISO_AF, .mosiAF = SPI3_MOSI_AF, .divisorMap = spiDivisorMapSlow }, -#else - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .sckAF = GPIO_AF6_SPI3, .misoAF = GPIO_AF6_SPI3, .mosiAF = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, -#endif #else { .dev = NULL }, // No SPI3 #endif #ifdef USE_SPI_DEVICE_4 -#if defined(SPI4_SCK_AF) || defined(SPI4_MISO_AF) || defined(SPI4_MOSI_AF) -#if !defined(SPI4_SCK_AF) || !defined(SPI4_MISO_AF) || !defined(SPI4_MOSI_AF) -#error SPI4: SCK, MISO and MOSI AFs should be defined together in target.h! -#endif { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = SPI4_SCK_AF, .misoAF = SPI4_MISO_AF, .mosiAF = SPI4_MOSI_AF, .divisorMap = spiDivisorMapSlow } -#else - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = GPIO_AF5_SPI4, .misoAF = GPIO_AF5_SPI4, .mosiAF = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } -#endif #else { .dev = NULL } // No SPI4 #endif }; -#else -static spiDevice_t spiHardwareMap[] = { +#elif defined(STM32F7) +#include "bus_spi_stm32f7xx.h" + +// Auto-resolve SPI AF per pin from the lookup table in bus_spi_stm32f7xx.h. +// Targets may still define individual SPI*_SCK/MISO/MOSI_AF values in target.h +// to override; explicit defines take priority via these #ifndef guards. #ifdef USE_SPI_DEVICE_1 -#if defined(SPI1_SCK_AF) || defined(SPI1_MISO_AF) || defined(SPI1_MOSI_AF) -#if !defined(SPI1_SCK_AF) || !defined(SPI1_MISO_AF) || !defined(SPI1_MOSI_AF) -#error SPI1: SCK, MISO and MOSI AFs should be defined together in target.h! +#ifndef SPI1_SCK_AF +#define SPI1_SCK_AF SPI_PIN_AF_HELPER(1, SPI1_SCK_PIN) #endif - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = SPI1_SCK_AF, .misoAF = SPI1_MISO_AF, .mosiAF = SPI1_MOSI_AF, .divisorMap = spiDivisorMapFast }, -#else - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = GPIO_AF5_SPI1, .misoAF = GPIO_AF5_SPI1, .mosiAF = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#ifndef SPI1_MISO_AF +#define SPI1_MISO_AF SPI_PIN_AF_HELPER(1, SPI1_MISO_PIN) +#endif +#ifndef SPI1_MOSI_AF +#define SPI1_MOSI_AF SPI_PIN_AF_HELPER(1, SPI1_MOSI_PIN) #endif -#else - { .dev = NULL }, // No SPI1 #endif #ifdef USE_SPI_DEVICE_2 -#if defined(SPI2_SCK_AF) || defined(SPI2_MISO_AF) || defined(SPI2_MOSI_AF) -#if !defined(SPI2_SCK_AF) || !defined(SPI2_MISO_AF) || !defined(SPI2_MOSI_AF) -#error SPI2: SCK, MISO and MOSI AFs should be defined together in target.h! +#ifndef SPI2_SCK_AF +#define SPI2_SCK_AF SPI_PIN_AF_HELPER(2, SPI2_SCK_PIN) #endif - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .sckAF = SPI2_SCK_AF, .misoAF = SPI2_MISO_AF, .mosiAF = SPI2_MOSI_AF, .divisorMap = spiDivisorMapSlow }, +#ifndef SPI2_MISO_AF +#define SPI2_MISO_AF SPI_PIN_AF_HELPER(2, SPI2_MISO_PIN) +#endif +#ifndef SPI2_MOSI_AF +#define SPI2_MOSI_AF SPI_PIN_AF_HELPER(2, SPI2_MOSI_PIN) +#endif +#endif + +#ifdef USE_SPI_DEVICE_3 +#ifndef SPI3_SCK_AF +#define SPI3_SCK_AF SPI_PIN_AF_HELPER(3, SPI3_SCK_PIN) +#endif +#ifndef SPI3_MISO_AF +#define SPI3_MISO_AF SPI_PIN_AF_HELPER(3, SPI3_MISO_PIN) +#endif +#ifndef SPI3_MOSI_AF +#define SPI3_MOSI_AF SPI_PIN_AF_HELPER(3, SPI3_MOSI_PIN) +#endif +#endif + +#ifdef USE_SPI_DEVICE_4 +#ifndef SPI4_SCK_AF +#define SPI4_SCK_AF SPI_PIN_AF_HELPER(4, SPI4_SCK_PIN) +#endif +#ifndef SPI4_MISO_AF +#define SPI4_MISO_AF SPI_PIN_AF_HELPER(4, SPI4_MISO_PIN) +#endif +#ifndef SPI4_MOSI_AF +#define SPI4_MOSI_AF SPI_PIN_AF_HELPER(4, SPI4_MOSI_PIN) +#endif +#endif + +static spiDevice_t spiHardwareMap[] = { +#ifdef USE_SPI_DEVICE_1 + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = SPI1_SCK_AF, .misoAF = SPI1_MISO_AF, .mosiAF = SPI1_MOSI_AF, .divisorMap = spiDivisorMapFast }, #else - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .sckAF = GPIO_AF5_SPI2, .misoAF = GPIO_AF5_SPI2, .mosiAF = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, + { .dev = NULL }, // No SPI1 #endif + +#ifdef USE_SPI_DEVICE_2 + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .sckAF = SPI2_SCK_AF, .misoAF = SPI2_MISO_AF, .mosiAF = SPI2_MOSI_AF, .divisorMap = spiDivisorMapSlow }, #else { .dev = NULL }, // No SPI2 #endif #ifdef USE_SPI_DEVICE_3 -#if defined(SPI3_SCK_AF) || defined(SPI3_MISO_AF) || defined(SPI3_MOSI_AF) -#if !defined(SPI3_SCK_AF) || !defined(SPI3_MISO_AF) || !defined(SPI3_MOSI_AF) -#error SPI3: SCK, MISO and MOSI AFs should be defined together in target.h! -#endif { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .sckAF = SPI3_SCK_AF, .misoAF = SPI3_MISO_AF, .mosiAF = SPI3_MOSI_AF, .divisorMap = spiDivisorMapSlow }, -#else - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .sckAF = GPIO_AF6_SPI3, .misoAF = GPIO_AF6_SPI3, .mosiAF = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, -#endif #else { .dev = NULL }, // No SPI3 #endif #ifdef USE_SPI_DEVICE_4 -#if defined(SPI4_SCK_AF) || defined(SPI4_MISO_AF) || defined(SPI4_MOSI_AF) -#if !defined(SPI4_SCK_AF) || !defined(SPI4_MISO_AF) || !defined(SPI4_MOSI_AF) -#error SPI3: SCK, MISO and MOSI AFs should be defined together in target.h! -#endif { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = SPI4_SCK_AF, .misoAF = SPI4_MISO_AF, .mosiAF = SPI4_MOSI_AF, .divisorMap = spiDivisorMapSlow } -#else - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = GPIO_AF5_SPI4, .misoAF = GPIO_AF5_SPI4, .mosiAF = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } -#endif #else { .dev = NULL } // No SPI4 #endif diff --git a/src/main/drivers/bus_spi_stm32f7xx.h b/src/main/drivers/bus_spi_stm32f7xx.h new file mode 100644 index 00000000000..a43938e1e45 --- /dev/null +++ b/src/main/drivers/bus_spi_stm32f7xx.h @@ -0,0 +1,80 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV 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 INAV. If not, see . + */ + +/* + * STM32F7 SPI pin alternate function lookup table. + * + * Usage: SPI_PIN_AF_HELPER(3, PB5) expands to SPI_PIN_AF_SPI3_PB5, + * which is defined below as GPIO_AF6_SPI3. + * + * AF assignments from STM32F722 datasheet (DS11853) and STM32F745 datasheet + * (DS10916), Table 9. + * + * NOTE: The F7 AF table differs from the H7 for SPI3/PB5: + * On F7, PB5 SPI3_MOSI is AF6 (the default). + * On H7, PB5 SPI3_MOSI is AF7 (an exception — see bus_spi_stm32h7xx.h). + * + * SPI1, SPI2, SPI4 pins are all AF5 with no exceptions. + * SPI3 SCK/MISO are AF6. SPI3 MOSI has pin-dependent exceptions: + * PB2 uses AF7 (not AF6). PD6 uses AF5 (not AF6). PC12 and PB5 use AF6. + */ + +#pragma once + +#include "common/utils.h" + +// Resolves to SPI_PIN_AF_SPIn_Pxy, defined below for each valid pin. +// If a pin is not in the table the build will fail with "undefined identifier", +// which is preferable to silently applying the wrong AF. +#define SPI_PIN_AF_HELPER(spi, pin) CONCAT4(SPI_PIN_AF_SPI, spi, _, pin) + +/* SPI1 — all data pins use AF5 */ +#define SPI_PIN_AF_SPI1_PA5 GPIO_AF5_SPI1 // SCK +#define SPI_PIN_AF_SPI1_PA6 GPIO_AF5_SPI1 // MISO +#define SPI_PIN_AF_SPI1_PA7 GPIO_AF5_SPI1 // MOSI +#define SPI_PIN_AF_SPI1_PB3 GPIO_AF5_SPI1 // SCK +#define SPI_PIN_AF_SPI1_PB4 GPIO_AF5_SPI1 // MISO +#define SPI_PIN_AF_SPI1_PB5 GPIO_AF5_SPI1 // MOSI + +/* SPI2 — all data pins use AF5 */ +#define SPI_PIN_AF_SPI2_PB13 GPIO_AF5_SPI2 // SCK +#define SPI_PIN_AF_SPI2_PB14 GPIO_AF5_SPI2 // MISO +#define SPI_PIN_AF_SPI2_PB15 GPIO_AF5_SPI2 // MOSI +#define SPI_PIN_AF_SPI2_PC1 GPIO_AF5_SPI2 // MOSI +#define SPI_PIN_AF_SPI2_PC2 GPIO_AF5_SPI2 // MISO +#define SPI_PIN_AF_SPI2_PC3 GPIO_AF5_SPI2 // MOSI + +/* + * SPI3 — SCK and MISO use AF6, but MOSI has pin-dependent exceptions. + * PB2 carries SPI3_MOSI on AF7 (not AF6). PD6 uses AF5. PB5 and PC12 use AF6. + */ +#define SPI_PIN_AF_SPI3_PB2 GPIO_AF7_SPI3 // MOSI — exception: AF7, not AF6 +#define SPI_PIN_AF_SPI3_PB3 GPIO_AF6_SPI3 // SCK +#define SPI_PIN_AF_SPI3_PB4 GPIO_AF6_SPI3 // MISO +#define SPI_PIN_AF_SPI3_PB5 GPIO_AF6_SPI3 // MOSI (AF6 on F7; H7 uses AF7 for this pin) +#define SPI_PIN_AF_SPI3_PC10 GPIO_AF6_SPI3 // SCK +#define SPI_PIN_AF_SPI3_PC11 GPIO_AF6_SPI3 // MISO +#define SPI_PIN_AF_SPI3_PC12 GPIO_AF6_SPI3 // MOSI +#define SPI_PIN_AF_SPI3_PD6 GPIO_AF5_SPI3 // MOSI — exception: AF5, not AF6 + +/* SPI4 — all data pins use AF5 */ +#define SPI_PIN_AF_SPI4_PE2 GPIO_AF5_SPI4 // SCK +#define SPI_PIN_AF_SPI4_PE5 GPIO_AF5_SPI4 // MISO +#define SPI_PIN_AF_SPI4_PE6 GPIO_AF5_SPI4 // MOSI +#define SPI_PIN_AF_SPI4_PE12 GPIO_AF5_SPI4 // SCK +#define SPI_PIN_AF_SPI4_PE13 GPIO_AF5_SPI4 // MISO +#define SPI_PIN_AF_SPI4_PE14 GPIO_AF5_SPI4 // MOSI diff --git a/src/main/drivers/bus_spi_stm32h7xx.h b/src/main/drivers/bus_spi_stm32h7xx.h new file mode 100644 index 00000000000..286e5859816 --- /dev/null +++ b/src/main/drivers/bus_spi_stm32h7xx.h @@ -0,0 +1,81 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV 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 INAV. If not, see . + */ + +/* + * STM32H7 SPI pin alternate function lookup table. + * + * Usage: SPI_PIN_AF_HELPER(3, PB5) expands to SPI_PIN_AF_SPI3_PB5, + * which is defined below as GPIO_AF7_SPI3. + * + * This allows bus_spi_hal_ll.c to resolve the correct AF per pin automatically, + * without requiring each target to define SPI*_SCK_AF / SPI*_MISO_AF / SPI*_MOSI_AF + * manually in target.h. Targets may still override individual values if needed. + * + * Alternate function assignments are from the STM32H743 datasheet (DS12110), + * Tables 10-14. + */ + +#pragma once + +#include "common/utils.h" + +// Resolves to SPI_PIN_AF_SPIn_Pxy, which is defined below for each valid pin. +// If a pin is not in the table the build will fail with "undefined identifier", +// which is preferable to silently applying the wrong AF. +#define SPI_PIN_AF_HELPER(spi, pin) CONCAT4(SPI_PIN_AF_SPI, spi, _, pin) + +/* SPI1 — all data pins use AF5 */ +#define SPI_PIN_AF_SPI1_PA5 GPIO_AF5_SPI1 // SCK +#define SPI_PIN_AF_SPI1_PA6 GPIO_AF5_SPI1 // MISO +#define SPI_PIN_AF_SPI1_PA7 GPIO_AF5_SPI1 // MOSI +#define SPI_PIN_AF_SPI1_PB3 GPIO_AF5_SPI1 // SCK +#define SPI_PIN_AF_SPI1_PB4 GPIO_AF5_SPI1 // MISO +#define SPI_PIN_AF_SPI1_PB5 GPIO_AF5_SPI1 // MOSI +#define SPI_PIN_AF_SPI1_PD7 GPIO_AF5_SPI1 // MOSI + +/* SPI2 — all data pins use AF5 */ +#define SPI_PIN_AF_SPI2_PA9 GPIO_AF5_SPI2 // SCK +#define SPI_PIN_AF_SPI2_PB10 GPIO_AF5_SPI2 // SCK +#define SPI_PIN_AF_SPI2_PB13 GPIO_AF5_SPI2 // SCK +#define SPI_PIN_AF_SPI2_PB14 GPIO_AF5_SPI2 // MISO +#define SPI_PIN_AF_SPI2_PB15 GPIO_AF5_SPI2 // MOSI +#define SPI_PIN_AF_SPI2_PC1 GPIO_AF5_SPI2 // MOSI +#define SPI_PIN_AF_SPI2_PC2 GPIO_AF5_SPI2 // MISO +#define SPI_PIN_AF_SPI2_PC3 GPIO_AF5_SPI2 // MOSI +#define SPI_PIN_AF_SPI2_PD3 GPIO_AF5_SPI2 // SCK + +/* + * SPI3 — SCK and MISO use AF6, but MOSI has pin-dependent exceptions. + * PB2 and PB5 carry SPI3_MOSI on AF7 (not AF6). PD6 uses AF5. + * This is the only SPI bus on STM32H743 where a single-AF fallback is wrong. + */ +#define SPI_PIN_AF_SPI3_PB2 GPIO_AF7_SPI3 // MOSI — exception: AF7, not AF6 +#define SPI_PIN_AF_SPI3_PB3 GPIO_AF6_SPI3 // SCK +#define SPI_PIN_AF_SPI3_PB4 GPIO_AF6_SPI3 // MISO +#define SPI_PIN_AF_SPI3_PB5 GPIO_AF7_SPI3 // MOSI — exception: AF7, not AF6 +#define SPI_PIN_AF_SPI3_PC10 GPIO_AF6_SPI3 // SCK +#define SPI_PIN_AF_SPI3_PC11 GPIO_AF6_SPI3 // MISO +#define SPI_PIN_AF_SPI3_PC12 GPIO_AF6_SPI3 // MOSI +#define SPI_PIN_AF_SPI3_PD6 GPIO_AF5_SPI3 // MOSI — exception: AF5, not AF6 + +/* SPI4 — all data pins use AF5 */ +#define SPI_PIN_AF_SPI4_PE2 GPIO_AF5_SPI4 // SCK +#define SPI_PIN_AF_SPI4_PE5 GPIO_AF5_SPI4 // MISO +#define SPI_PIN_AF_SPI4_PE6 GPIO_AF5_SPI4 // MOSI +#define SPI_PIN_AF_SPI4_PE12 GPIO_AF5_SPI4 // SCK +#define SPI_PIN_AF_SPI4_PE13 GPIO_AF5_SPI4 // MISO +#define SPI_PIN_AF_SPI4_PE14 GPIO_AF5_SPI4 // MOSI diff --git a/src/main/drivers/sdcard/sdcard_spi.c b/src/main/drivers/sdcard/sdcard_spi.c index 214f1fd8105..6446ec09164 100644 --- a/src/main/drivers/sdcard/sdcard_spi.c +++ b/src/main/drivers/sdcard/sdcard_spi.c @@ -62,7 +62,17 @@ static void sdcardSpi_deselect(void) // As per the SD-card spec, give the card 8 dummy clocks so it can finish its operation //spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - while (busIsBusy(sdcard.dev)) { __NOP(); } + int timeout = 100000; + while (busIsBusy(sdcard.dev)) { + if (timeout-- == 0) { + sdcard.failureCount++; + if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + } + break; + } + __NOP(); + } busDeselectDevice(sdcard.dev); } diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c index 8999cdd7197..f6b9d01f803 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c @@ -549,7 +549,13 @@ static void SD_StartBlockTransfert(uint32_t* pBuffer, uint32_t BlockSize, uint32 } pDMA->CR &= ~DMA_SxCR_EN; // Disable the Peripheral - while (pDMA->CR & DMA_SxCR_EN); + int dmaTimeout = 10000; + while ((pDMA->CR & DMA_SxCR_EN) && dmaTimeout-- > 0); + + if (pDMA->CR & DMA_SxCR_EN) { + SD_Handle.TransferError = SD_DATA_TIMEOUT; + return; + } pDMA->NDTR = (uint32_t) (BlockSize * NumberOfBlocks) / 4; // Configure DMA Stream data length pDMA->M0AR = (uint32_t) pBuffer; // Configure DMA Stream memory address @@ -772,7 +778,7 @@ SD_Error_t SD_GetCardInfo(void) SD_CardInfo.CardCapacity = (SD_CardInfo.SD_csd.DeviceSize + 1) ; SD_CardInfo.CardCapacity *= (1 << (SD_CardInfo.SD_csd.DeviceSizeMul + 2)); SD_CardInfo.CardBlockSize = 1 << (SD_CardInfo.SD_csd.RdBlockLen); - SD_CardInfo.CardCapacity *= SD_CardInfo.CardBlockSize; + SD_CardInfo.CardCapacity = SD_CardInfo.CardCapacity * SD_CardInfo.CardBlockSize / 512; // In 512 byte blocks } else if (SD_CardType == SD_HIGH_CAPACITY) { // Byte 7 @@ -1014,7 +1020,11 @@ SD_Error_t SD_HighSpeed(void) return ErrorState; } + uint32_t swTimeout = SD_DATATIMEOUT; while ((SDIO->STA & (SDIO_STA_RXOVERR | SDIO_STA_DCRCFAIL | SDIO_STA_DTIMEOUT | SDIO_STA_DBCKEND)) == 0) { + if (swTimeout-- == 0) { + break; + } if ((SDIO->STA & SDIO_STA_RXFIFOHF) != 0) { for(Count = 0; Count < 8; Count++) { *(Buffer + Count) = SDIO->FIFO; @@ -1124,7 +1134,11 @@ SD_Error_t SD_GetCardStatus(SD_CardStatus_t* pCardStatus) } // Get status data + uint32_t statTimeout = SD_DATATIMEOUT; while ((SDIO->STA & (SDIO_STA_RXOVERR | SDIO_STA_DCRCFAIL | SDIO_STA_DTIMEOUT | SDIO_STA_DBCKEND)) == 0) { + if (statTimeout-- == 0) { + break; + } if ((SDIO->STA & SDIO_STA_RXFIFOHF) != 0) { for(Count = 0; Count < 8; Count++) { Status[Count] = SDIO->FIFO; @@ -1318,7 +1332,11 @@ static SD_Error_t SD_FindSCR(uint32_t *pSCR) // Send ACMD51 SD_APP_SEND_SCR with argument as 0 if ((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_SEND_SCR | SD_CMD_RESPONSE_SHORT), 0, 1)) == SD_OK) { + uint32_t scrTimeout = SD_DATATIMEOUT; while ((SDIO->STA & (SDIO_STA_RXOVERR | SDIO_STA_DCRCFAIL | SDIO_STA_DTIMEOUT | SDIO_STA_DBCKEND)) == 0) { + if (scrTimeout-- == 0) { + break; + } if ((SDIO->STA & SDIO_STA_RXDAVL) != 0) { *(tempscr + Index) = SDIO->FIFO; Index++; diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index dc625aaa354..590a36002b2 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -111,7 +111,7 @@ void serialEndWrite(serialPort_t *instance) bool serialIsConnected(const serialPort_t *instance) { if (instance->vTable->isConnected) - instance->vTable->isConnected(instance); + return(instance->vTable->isConnected(instance)); // If API is not defined - assume connected return true; diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 7fdbad2a114..f076e0bfee2 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -51,6 +51,17 @@ USBD_HandleTypeDef USBD_Device; static vcpPort_t vcpPort; +// Track DTR (Data Terminal Ready) state - indicates if host has COM port open +// Default to true - assume connected until host explicitly clears DTR +static volatile bool cdcPortOpened = true; + +static void cdcCtrlLineStateCallback(void *context, uint16_t ctrlLineState) +{ + UNUSED(context); + // DTR is bit 0 of control line state + cdcPortOpened = (ctrlLineState & 0x01) != 0; +} + static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate) { UNUSED(instance); @@ -103,7 +114,8 @@ static uint8_t usbVcpRead(serialPort_t *instance) static bool usbVcpIsConnected(const serialPort_t *instance) { (void)instance; - return usbIsConnected() && usbIsConfigured(); + // Check USB hardware state AND whether host has opened the COM port (DTR) + return usbIsConnected() && usbIsConfigured() && cdcPortOpened; } static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count) @@ -209,6 +221,9 @@ void usbVcpInitHardware(void) IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, RESOURCE_INPUT, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, RESOURCE_OUTPUT, 0); USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); + + // Register callback for DTR state changes + CDC_SetCtrlLineStateCb(cdcCtrlLineStateCallback, NULL); #elif defined(STM32F7) || defined(STM32H7) usbGenerateDisconnectPulse(); @@ -225,7 +240,10 @@ void usbVcpInitHardware(void) /* Start Device Process */ USBD_Start(&USBD_Device); - + + // Register callback for DTR state changes + CDC_SetCtrlLineStateCb(cdcCtrlLineStateCallback, NULL); + #ifdef STM32H7 HAL_PWREx_EnableUSBVoltageDetector(); delay(100); // Cold boot failures observed without this, even when USB cable is not connected diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c index c217110842d..2ac3a558278 100644 --- a/src/main/fc/multifunction.c +++ b/src/main/fc/multifunction.c @@ -45,32 +45,29 @@ static void multiFunctionApply(multi_function_e selectedItem) switch (selectedItem) { case MULTI_FUNC_NONE: break; - case MULTI_FUNC_1: // redisplay current warnings - osdResetWarningFlags(); - break; - case MULTI_FUNC_2: // control manual emergency landing + case MULTI_FUNC_1: // control manual emergency landing checkManualEmergencyLandingControl(ARMING_FLAG(ARMED)); break; - case MULTI_FUNC_3: // toggle Safehome suspend + case MULTI_FUNC_2: // toggle Safehome suspend #if defined(USE_SAFE_HOME) if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES); } #endif break; - case MULTI_FUNC_4: // toggle RTH Trackback suspend + case MULTI_FUNC_3: // toggle RTH Trackback suspend if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK); } break; - case MULTI_FUNC_5: + case MULTI_FUNC_4: #ifdef USE_DSHOT if (STATE(MULTIROTOR)) { // toggle Turtle mode MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE); } #endif break; - case MULTI_FUNC_6: // emergency ARM + case MULTI_FUNC_5: // emergency ARM if (!ARMING_FLAG(ARMED)) { emergencyArmingUpdate(true, true); } diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h index 93265ba1200..6e60da2bdd8 100644 --- a/src/main/fc/multifunction.h +++ b/src/main/fc/multifunction.h @@ -26,6 +26,13 @@ #include +typedef struct multiFunctionWarning_s { + uint8_t osdWarningsFlags; // bitfield + bool newWarningActive; +} multiFunctionWarning_t; + +extern multiFunctionWarning_t multiFunctionWarning; + #ifdef USE_MULTI_FUNCTIONS extern uint8_t multiFunctionFlags; @@ -47,7 +54,6 @@ typedef enum { MULTI_FUNC_3, MULTI_FUNC_4, MULTI_FUNC_5, - MULTI_FUNC_6, MULTI_FUNC_END, } multi_function_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1e1932531e5..476dfe09ff7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3904,6 +3904,7 @@ groups: field: osd_switch_indicators_align_left type: bool default_value: ON + - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t headers: ["io/osd_common.h"] diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 9d2720bec09..b56d97eec87 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -425,6 +425,16 @@ void servoMixer(float dT) simulatorData.input[INPUT_STABILIZED_THROTTLE] = input[INPUT_STABILIZED_THROTTLE]; #endif + /* + * When disarmed, force throttle inputs to minimum so the mixer pipeline + * computes the correct safe servo position accounting for servo reversal + * and negative mixer weights. + */ + if (!ARMING_FLAG(ARMED)) { + input[INPUT_STABILIZED_THROTTLE] = -500; + input[INPUT_RC_THROTTLE] = -500; + } + for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = 0; } @@ -487,19 +497,6 @@ void servoMixer(float dT) servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max); } - /* - * When not armed, apply servo low position to all outputs that include a throttle or stabilizet throttle in the mix - */ - if (!ARMING_FLAG(ARMED)) { - for (int i = 0; i < servoRuleCount; i++) { - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - - if (from == INPUT_STABILIZED_THROTTLE || from == INPUT_RC_THROTTLE) { - servo[target] = motorConfig()->mincommand; - } - } - } } #define SERVO_AUTOTRIM_TIMER_MS 2000 diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 82e1a4f655d..102284723df 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -549,7 +549,7 @@ static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) if (bufferLength > 13 && (!strncmp(proto, "PROTVER=", 8) || !strncmp(proto, "PROTVER ", 8))) { proto+=8; - float ver = atof(proto); + float ver = fastA2F(proto); gpsState.swVersionMajor = (uint8_t)ver; gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 62c41683f02..8e8e5771450 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -127,6 +127,7 @@ static const modeColorIndexes_t defaultModeColors[] = { [LED_MODE_ANGLE] = {{ COLOR_CYAN, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, [LED_MODE_MAG] = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, [LED_MODE_BARO] = {{ COLOR_LIGHT_BLUE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_LOITER] = {{ COLOR_YELLOW, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, }; static const specialColorIndexes_t defaultSpecialColors[] = { @@ -423,6 +424,7 @@ static const struct { uint8_t ledMode; } flightModeToLed[] = { {HEADFREE_MODE, LED_MODE_HEADFREE}, + {NAV_POSHOLD_MODE, LED_MODE_LOITER}, {HEADING_MODE, LED_MODE_MAG}, #ifdef USE_BARO {NAV_ALTHOLD_MODE, LED_MODE_BARO}, diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 204a4661d17..2aea06cde73 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -23,7 +23,7 @@ #define LED_MAX_STRIP_LENGTH 128 #define LED_CONFIGURABLE_COLOR_COUNT 16 -#define LED_MODE_COUNT 6 +#define LED_MODE_COUNT 7 #define LED_DIRECTION_COUNT 6 #define LED_BASEFUNCTION_COUNT 8 #define LED_OVERLAY_COUNT 7 @@ -79,6 +79,7 @@ typedef enum { LED_MODE_ANGLE, LED_MODE_MAG, LED_MODE_BARO, + LED_MODE_LOITER, LED_SPECIAL } ledModeIndex_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6ad55632c17..f4fe19a4040 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -203,8 +203,9 @@ static bool fullRedraw = false; static uint8_t armState; +// Multifunction static textAttributes_t osdGetMultiFunctionMessage(char *buff); -static uint8_t osdWarningsFlags = 0; +multiFunctionWarning_t multiFunctionWarning; typedef struct osdMapData_s { uint32_t scale; @@ -6405,49 +6406,35 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter return elemAttr; } -void osdResetWarningFlags(void) +static bool osdCheckWarning(bool condition, uint8_t warningFlag) { - osdWarningsFlags = 0; -} - -static bool osdCheckWarning(bool condition, uint8_t warningFlag, uint8_t *warningsCount) -{ -#define WARNING_REDISPLAY_DURATION 5000; // milliseconds - + static timeMs_t newWarningEndTime = 0; + static uint8_t newWarningFlags = 0; // bitfield const timeMs_t currentTimeMs = millis(); - static timeMs_t warningDisplayStartTime = 0; - static timeMs_t redisplayStartTimeMs = 0; - static uint16_t osdWarningTimerDuration; - static uint8_t newWarningFlags; + /* New warnings dislayed individually for 10s with blinking after which + * all current warnings displayed without blinking on 1 second cycle */ if (condition) { // condition required to trigger warning - if (!(osdWarningsFlags & warningFlag)) { - osdWarningsFlags |= warningFlag; + if (!(multiFunctionWarning.osdWarningsFlags & warningFlag)) { // check for new warnings + multiFunctionWarning.osdWarningsFlags |= warningFlag; newWarningFlags |= warningFlag; - redisplayStartTimeMs = 0; + newWarningEndTime = currentTimeMs + 10000; + multiFunctionWarning.newWarningActive = true; } #ifdef USE_DEV_TOOLS if (systemConfig()->groundTestMode) { return true; } #endif - /* Warnings displayed in full for set time before shrinking down to alert symbol with warning count only. - * All current warnings then redisplayed for 5s on 30s rolling cycle. - * New warnings dislayed individually for 10s */ - if (currentTimeMs > redisplayStartTimeMs) { - warningDisplayStartTime = currentTimeMs; - osdWarningTimerDuration = newWarningFlags ? 10000 : WARNING_REDISPLAY_DURATION; - redisplayStartTimeMs = currentTimeMs + osdWarningTimerDuration + 30000; - } - - if (currentTimeMs - warningDisplayStartTime < osdWarningTimerDuration) { - return (newWarningFlags & warningFlag) || osdWarningTimerDuration == WARNING_REDISPLAY_DURATION; + if (currentTimeMs < newWarningEndTime) { + return (newWarningFlags & warningFlag); // filter out new warnings excluding older warnings } else { newWarningFlags = 0; + multiFunctionWarning.newWarningActive = false; } - *warningsCount += 1; - } else if (osdWarningsFlags & warningFlag) { - osdWarningsFlags &= ~warningFlag; + return true; + } else if (multiFunctionWarning.osdWarningsFlags & warningFlag) { + multiFunctionWarning.osdWarningsFlags &= ~warningFlag; } return false; @@ -6458,7 +6445,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) /* Message length limit 10 char max */ textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - static uint8_t warningsCount; const char *message = NULL; #ifdef USE_MULTI_FUNCTIONS @@ -6471,12 +6457,9 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) switch (selectedFunction) { case MULTI_FUNC_NONE: case MULTI_FUNC_1: - message = warningsCount ? "WARNINGS !" : "0 WARNINGS"; - break; - case MULTI_FUNC_2: message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND"; break; - case MULTI_FUNC_3: + case MULTI_FUNC_2: #if defined(USE_SAFE_HOME) if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME"; @@ -6485,14 +6468,14 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) #endif activeFunction++; FALLTHROUGH; - case MULTI_FUNC_4: + case MULTI_FUNC_3: if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK"; break; } activeFunction++; FALLTHROUGH; - case MULTI_FUNC_5: + case MULTI_FUNC_4: #ifdef USE_DSHOT if (STATE(MULTIROTOR)) { message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE"; @@ -6501,7 +6484,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) #endif activeFunction++; FALLTHROUGH; - case MULTI_FUNC_6: + case MULTI_FUNC_5: message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM "; break; case MULTI_FUNC_END: @@ -6524,23 +6507,30 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) #endif // MULTIFUNCTION - functions only, warnings always defined /* --- WARNINGS --- */ - const char *messages[7]; + const char *messages[8]; uint8_t messageCount = 0; bool warningCondition = false; - warningsCount = 0; uint8_t warningFlagID = 1; - // Low Battery - const batteryState_e batteryState = getBatteryState(); - warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING; - if (osdCheckWarning(warningCondition, warningFlagID, &warningsCount)) { - messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !"; + // Low Battery Voltage + const batteryState_e batteryVoltageState = checkBatteryVoltageState(); + warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING; + if (osdCheckWarning(warningCondition, warningFlagID)) { + messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT LAND" : "VBATT LOW "; } + // Low Battery Capacity + if (batteryUsesCapacityThresholds()) { + const batteryState_e batteryState = getBatteryState(); + warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING; + if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { + messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT DYING"; + } + } #if defined(USE_GPS) // GPS Fix and Failure if (feature(FEATURE_GPS)) { - if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1, &warningsCount)) { + if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1)) { bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE; messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX"; } @@ -6549,12 +6539,12 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) // RTH sanity (warning if RTH heads 200m further away from home than closest point) warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive && (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000; - if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { + if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { messages[messageCount++] = "RTH SANITY"; } // Altitude sanity (warning if significant mismatch between estimated and GPS altitude) - if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1, &warningsCount)) { + if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1)) { messages[messageCount++] = "ALT SANITY"; } #endif @@ -6563,7 +6553,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) // Magnetometer failure if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { hardwareSensorStatus_e magStatus = getHwCompassStatus(); - if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1, &warningsCount)) { + if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1)) { messages[messageCount++] = "MAG FAILED"; } } @@ -6572,7 +6562,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) #if defined(USE_PITOT) // Pitot sensor validation failure (blocked/failed pitot tube) if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) { - if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1, &warningsCount)) { + if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1)) { messages[messageCount++] = "PITOT FAIL"; } } @@ -6586,7 +6576,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) // } #ifdef USE_DEV_TOOLS - if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) { + if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1)) { messages[messageCount++] = "GRD TEST !"; } #endif @@ -6594,10 +6584,9 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) if (messageCount) { message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; // display each warning on 1s cycle strcpy(buff, message); - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (warningsCount) { - buff[0] = SYM_ALERT; - tfp_sprintf(buff + 1, "%u ", warningsCount); + if (multiFunctionWarning.newWarningActive) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } } return elemAttr; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index bbaa68f862d..88240ff84c0 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -585,8 +585,6 @@ int osdFormatVelocityStr(char* buff, int32_t vel, osd_SpeedTypes_e speedType, bo // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle); -void osdResetWarningFlags(void); - int16_t osdGetPanServoOffset(void); /** diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 7d5e213cce0..acb2a34e6dc 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -212,7 +212,7 @@ int saDacToPowerIndex(int dac) int saDbiToMw(uint16_t dbi) { - uint16_t mw = (uint16_t)pow(10.0, dbi / 10.0); + uint16_t mw = (uint16_t)roundf(powf(10.0f, (float)dbi / 10.0f)); if (dbi > 14) { // For powers greater than 25mW round up to a multiple of 50 to match expectations diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 5a3af115f9c..406cfe28ddf 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -110,8 +110,8 @@ #define MSP_CALIBRATION_DATA 14 #define MSP_SET_CALIBRATION_DATA 15 -#define MSP_POSITION_ESTIMATION_CONFIG 16 -#define MSP_SET_POSITION_ESTIMATION_CONFIG 17 +#define MSP_POSITION_ESTIMATION_CONFIG 16 //DEPRECATED in INAV 9.1 - use settings system instead. Will be removed in INAV 10.0 +#define MSP_SET_POSITION_ESTIMATION_CONFIG 17 //DEPRECATED in INAV 9.1 - use settings system instead. Will be removed in INAV 10.0 #define MSP_WP_MISSION_LOAD 18 // Load mission from NVRAM #define MSP_WP_MISSION_SAVE 19 // Save mission to NVRAM @@ -183,14 +183,14 @@ #define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card -#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings -#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings +#define MSP_BLACKBOX_CONFIG 80 //DEPRECATED in INAV 9.1, use MSP2_BLACKBOX_CONFIG. Will be removed in INAV 10.0 +#define MSP_SET_BLACKBOX_CONFIG 81 //DEPRECATED in INAV 9.1, use MSP2_SET_BLACKBOX_CONFIG. Will be removed in INAV 10.0 #define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings #define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings -#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight -#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight +#define MSP_OSD_CONFIG 84 //DEPRECATED in INAV 9.1 - use MSP2_INAV_OSD_* instead. Will be removed in INAV 10.0 +#define MSP_SET_OSD_CONFIG 85 //DEPRECATED in INAV 9.1 - use MSP2_INAV_OSD_* instead. Will be removed in INAV 10.0 #define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight #define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight @@ -202,10 +202,10 @@ #define MSP_ADVANCED_CONFIG 90 #define MSP_SET_ADVANCED_CONFIG 91 -#define MSP_FILTER_CONFIG 92 -#define MSP_SET_FILTER_CONFIG 93 -#define MSP_PID_ADVANCED 94 -#define MSP_SET_PID_ADVANCED 95 +#define MSP_FILTER_CONFIG 92 //DEPRECATED in INAV 9.1 - use settings system instead. Will be removed in INAV 10.0 +#define MSP_SET_FILTER_CONFIG 93 //DEPRECATED in INAV 9.1 - use settings system instead. Will be removed in INAV 10.0 +#define MSP_PID_ADVANCED 94 //DEPRECATED in INAV 9.1 - use MSP_INAV_PID instead. Will be removed in INAV 10.0 +#define MSP_SET_PID_ADVANCED 95 //DEPRECATED in INAV 9.1 - use MSP_SET_INAV_PID instead. Will be removed in INAV 10.0 #define MSP_SENSOR_CONFIG 96 #define MSP_SET_SENSOR_CONFIG 97 @@ -231,7 +231,7 @@ // Multwii original MSP commands // -#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number +#define MSP_STATUS 101 //DEPRECATED in INAV 9.1, use MSP2_INAV_STATUS. Will be removed in INAV 10.0 #define MSP_RAW_IMU 102 //out message 9 DOF #define MSP_SERVO 103 //out message servos #define MSP_MOTOR 104 //out message motors @@ -240,12 +240,12 @@ #define MSP_COMP_GPS 107 //out message distance home, direction home #define MSP_ATTITUDE 108 //out message 2 angles 1 heading #define MSP_ALTITUDE 109 //out message altitude, variometer -#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX +#define MSP_ANALOG 110 //DEPRECATED in INAV 9.1 - use MSP2_INAV_ANALOG instead. Will be removed in INAV 10.0 #define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID #define MSP_ACTIVEBOXES 113 //out message Active box flags (full width, more than 32 bits) -#define MSP_MISC 114 //out message powermeter trig +#define MSP_MISC 114 //DEPRECATED in INAV 9.1 - use MSP2_INAV_MISC instead. Will be removed in INAV 10.0 #define MSP_BOXNAMES 116 //out message the aux switch names -#define MSP_PIDNAMES 117 //out message the PID names +#define MSP_PIDNAMES 117 //DEPRECATED in INAV 9.1 - no replacement needed (static data). Will be removed in INAV 10.0 #define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold #define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes #define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations. @@ -263,7 +263,7 @@ #define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo #define MSP_ACC_CALIBRATION 205 //in message no param #define MSP_MAG_CALIBRATION 206 //in message no param -#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use +#define MSP_SET_MISC 207 //DEPRECATED in INAV 9.1 - use MSP2_INAV_SET_MISC instead. Will be removed in INAV 10.0 #define MSP_RESET_CONF 208 //in message no param #define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) #define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) @@ -284,19 +284,19 @@ #define MSP_RESERVE_1 251 //reserved for system usage #define MSP_RESERVE_2 252 //reserved for system usage #define MSP_DEBUGMSG 253 //out message debug string buffer -#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 +#define MSP_DEBUG 254 //DEPRECATED in INAV 9.1, use MSP2_INAV_DEBUG. Will be removed in INAV 10.0 #define MSP_V2_FRAME 255 //MSPv2 payload indicator // Additional commands that are not compatible with MultiWii -#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc +#define MSP_STATUS_EX 150 //DEPRECATED in INAV 9.1, use MSP2_INAV_STATUS (accCalibAxisFlags available via MSP_CALIBRATION_DATA). Will be removed in INAV 10.0 #define MSP_SENSOR_STATUS 151 //out message Hardware sensor status #define MSP_UID 160 //out message Unique device ID #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) #define MSP_GPSSTATISTICS 166 //out message get GPS debugging data #define MSP_ACC_TRIM 240 //out message get acc angle trim values #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values -#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration -#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration +#define MSP_SERVO_MIX_RULES 241 //DEPRECATED in INAV 9.1 - use MSP2_INAV_SERVO_MIXER instead. Will be removed in INAV 10.0 +#define MSP_SET_SERVO_MIX_RULE 242 //DEPRECATED in INAV 9.1 - use MSP2_INAV_SET_SERVO_MIXER instead. Will be removed in INAV 10.0 #define MSP_SET_PASSTHROUGH 245 //in message Sets up passthrough to different peripherals (4way interface, uart, etc...) #define MSP_RTC 246 //out message Gets the RTC clock (returns: secs(i32) millis(u16) - (0,0) if time is not known) #define MSP_SET_RTC 247 //in message Sets the RTC clock (args: secs(i32) millis(u16)) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index bb8f15dc8ba..7da742829c5 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -137,17 +137,17 @@ bool adjustMulticopterAltitudeFromRCInput(void) return true; } else { - const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500); + const uint8_t deadband = rcControlsConfig()->alt_hold_deadband; + const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, deadband); if (rcThrottleAdjustment) { /* Set velocity proportional to stick movement * Scale from altHoldThrottleRCZero to maxthrottle or minthrottle to altHoldThrottleRCZero */ - // Calculate max up or min down limit value scaled for deadband - int16_t limitValue = rcThrottleAdjustment > 0 ? getMaxThrottle() : getThrottleIdleValue(); - limitValue = applyDeadbandRescaled(limitValue - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500); + int16_t controlRange = -deadband; + controlRange += rcThrottleAdjustment > 0 ? getMaxThrottle() - altHoldThrottleRCZero : altHoldThrottleRCZero - getThrottleIdleValue(); - int16_t rcClimbRate = ABS(rcThrottleAdjustment) * navConfig()->mc.max_manual_climb_rate / limitValue; + const int16_t rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / controlRange; updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 4252b952c27..51ca98ccb97 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -581,7 +581,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) if (ctx->newFlags & EST_BARO_VALID && wBaro) { if (posEstimator.baro.updateDt) { // only update corrections once every sensor update - ctx->applyCorrections = true; + ctx->applyCorrectionsZ = true; const float dT = posEstimator.baro.updateDt; bool isAirCushionEffectDetected = false; @@ -641,7 +641,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->newEPV = posEstimator.gps.epv; } else { - ctx->applyCorrections = true; + ctx->applyCorrectionsZ = true; const float dT = posEstimator.gps.updateDt; // Altitude @@ -686,7 +686,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) ctx->newEPH = posEstimator.gps.eph; } else { - ctx->applyCorrections = true; + ctx->applyCorrectionsXY = true; const float dT = posEstimator.gps.updateDt; const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; @@ -743,6 +743,8 @@ static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) static void updateEstimatedTopic(timeUs_t currentTimeUs) { estimationContext_t ctx; + static timeMs_t lastXYSensorUpdateMs = 0; + static timeMs_t lastZSensorUpdateMs = 0; const float max_eph_epv = positionEstimationConfig()->max_eph_epv; @@ -758,14 +760,18 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) return; } - /* Calculate new degraded EPH and EPV for the case we didn't update estimation from sensors - linear degradation in max 10s */ - ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); - ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv) ? 100.0f * ctx.dt : 0.0f); + /* Calculate new degraded EPH and EPV for the case we didn't update estimation from sensors for > 200ms - linear degradation in max 10s */ + const bool XYSensorUpdateTimeout = US2MS(currentTimeUs) - lastXYSensorUpdateMs > 200; + ctx.newEPH = posEstimator.est.eph + ((posEstimator.est.eph <= max_eph_epv && XYSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f); + const bool ZSensorUpdateTimeout = US2MS(currentTimeUs) - lastZSensorUpdateMs > 200; + ctx.newEPV = posEstimator.est.epv + ((posEstimator.est.epv <= max_eph_epv && ZSensorUpdateTimeout) ? 100.0f * ctx.dt : 0.0f); ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); vectorZero(&ctx.estPosCorr); vectorZero(&ctx.estVelCorr); vectorZero(&ctx.accBiasCorr); + ctx.applyCorrectionsXY = false; + ctx.applyCorrectionsZ = false; /* AGL estimation - separate process, decouples from Z coordinate */ estimationCalculateAGL(&ctx); @@ -792,43 +798,52 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) } // Only apply corrections if new sensor update available - if (ctx.applyCorrections) { - ctx.applyCorrections = false; - - // Boost the corrections based on accWeight - vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); - vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); - - // Constrain corrections to prevent instability + if (ctx.applyCorrectionsXY || ctx.applyCorrectionsZ) { float maxUpdateDt = MAX(posEstimator.gps.updateDt, posEstimator.baro.updateDt); maxUpdateDt = MAX(maxUpdateDt, posEstimator.flow.updateDt); - const float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt; - for (uint8_t axis = 0; axis < 3; axis++) { + float correctionLimit = INAV_EST_CORR_LIMIT_VALUE * maxUpdateDt; + + uint8_t axisStart = 0; + uint8_t axisEnd = 2; + if (!ctx.applyCorrectionsXY) { + axisStart = 2; + } else if (!ctx.applyCorrectionsZ) { + axisEnd = 1; + } + + for (uint8_t axis = axisStart; axis <= axisEnd; axis++) { + // Boost the corrections based on accWeight + ctx.estPosCorr.v[axis] *= 1.0f / posEstimator.imu.accWeightFactor; + ctx.estVelCorr.v[axis] *= 1.0f / posEstimator.imu.accWeightFactor; + + // Constrain corrections to prevent instability ctx.estPosCorr.v[axis] = constrainf(ctx.estPosCorr.v[axis], -correctionLimit, correctionLimit); ctx.estVelCorr.v[axis] = constrainf(ctx.estVelCorr.v[axis], -correctionLimit, correctionLimit); - } - // Apply corrections - vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); - vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); - - /* Correct accelerometer bias */ - const float w_acc_bias = positionEstimationConfig()->w_acc_bias; - if (w_acc_bias > 0.0f) { - /* Correct accel bias */ - posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias; - posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias; - posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias; - - posEstimator.imu.accelBias.x = constrainf(posEstimator.imu.accelBias.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); - posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); - posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + // Apply corrections + posEstimator.est.pos.v[axis] += ctx.estPosCorr.v[axis]; + posEstimator.est.vel.v[axis] += ctx.estVelCorr.v[axis]; + + /* Correct accelerometer bias */ + const float w_acc_bias = positionEstimationConfig()->w_acc_bias; + if (w_acc_bias > 0.0f) { + /* Correct accel bias */ + posEstimator.imu.accelBias.v[axis] += ctx.accBiasCorr.v[axis] * w_acc_bias; + posEstimator.imu.accelBias.v[axis] = constrainf(posEstimator.imu.accelBias.v[axis], -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + } } // Reset sensor update time deltas once sensor corrections applied after sensor update posEstimator.gps.updateDt = 0.0f; posEstimator.baro.updateDt = 0.0f; posEstimator.flow.updateDt = 0.0f; + + if (ctx.applyCorrectionsXY) { + lastXYSensorUpdateMs = US2MS(currentTimeUs); + } + if (ctx.applyCorrectionsZ) { + lastZSensorUpdateMs = US2MS(currentTimeUs); + } } /* Update ground course */ @@ -860,8 +875,13 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) /* Publish position update */ if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) { + static pt1Filter_t estVelFilterState_X; + static pt1Filter_t estVelFilterState_Y; + float filteredVelX = pt1FilterApply4(&estVelFilterState_X, posEstimator.est.vel.x, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ)); + float filteredVelY = pt1FilterApply4(&estVelFilterState_Y, posEstimator.est.vel.y, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ)); + // FIXME!!!!! - updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y); + updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, filteredVelX, filteredVelY); } else { updateActualHorizontalPositionAndVelocity(false, false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0); @@ -869,9 +889,12 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) /* Publish altitude update and set altitude validity */ if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { + static pt1Filter_t estVelFilterState_Z; + float filteredVelZ = pt1FilterApply4(&estVelFilterState_Z, posEstimator.est.vel.z, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ)); + const float gpsCfEstimatedAltitudeError = STATE(GPS_FIX) ? posEstimator.gps.pos.z - posEstimator.est.pos.z : 0; navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED; - updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError); + updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, filteredVelZ, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError); } else { updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE, 0); diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c index 36325f5dc1a..5fc41f8eb78 100644 --- a/src/main/navigation/navigation_pos_estimator_flow.c +++ b/src/main/navigation/navigation_pos_estimator_flow.c @@ -124,7 +124,7 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) DEBUG_SET(DEBUG_FLOW, 2, posEstimator.est.flowCoordinates[X]); DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]); - return ctx->applyCorrections = true; + return ctx->applyCorrectionsXY = true; #else UNUSED(ctx); return false; diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 8a91afe268b..2a38155dddb 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -60,6 +60,8 @@ #define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f) #define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD (0.75f) +#define INAV_EST_VEL_F_CUT_HZ 3.0f + typedef struct { timeUs_t lastTriggeredTime; timeUs_t deltaTime; @@ -183,7 +185,8 @@ typedef struct { fpVector3_t estPosCorr; fpVector3_t estVelCorr; fpVector3_t accBiasCorr; - bool applyCorrections; + bool applyCorrectionsXY; + bool applyCorrectionsZ; } estimationContext_t; extern navigationPosEstimator_t posEstimator; diff --git a/src/main/navigation/rth_trackback.c b/src/main/navigation/rth_trackback.c index dfdcf801f7c..2d9c5e53148 100644 --- a/src/main/navigation/rth_trackback.c +++ b/src/main/navigation/rth_trackback.c @@ -153,10 +153,11 @@ bool rthTrackBackSetNewPosition(void) rth_trackback.activePointIndex = NAV_RTH_TRACKBACK_POINTS - 1; } - calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); - - if (rth_trackback.activePointIndex - rth_trackback.WrapAroundCounter == 0) { - rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1; + // Last trackback point reached when activePointIndex = WrapAroundCounter so only set position when not equal + if (rth_trackback.activePointIndex != rth_trackback.WrapAroundCounter) { + calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); + } else { + rth_trackback.activePointIndex = -1; // if not already = -1 set to -1 to end trackback next iteration } } else { setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 38d410610e1..79706c8dba7 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -288,7 +288,7 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) } break; #endif - + #if defined(USE_FAKE_BATT_SENSOR) case VOLTAGE_SENSOR_FAKE: vbat = fakeBattSensorGetVBat(); @@ -328,30 +328,32 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) batteryState_e checkBatteryVoltageState(void) { uint16_t stateVoltage = getBatteryVoltage(); - switch (batteryState) + static batteryState_e currentBatteryVoltageState = BATTERY_OK; + + switch (currentBatteryVoltageState) { case BATTERY_OK: if (stateVoltage <= (batteryWarningVoltage - VBATT_HYSTERESIS)) { - return BATTERY_WARNING; + currentBatteryVoltageState = BATTERY_WARNING; } break; case BATTERY_WARNING: if (stateVoltage <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) { - return BATTERY_CRITICAL; + currentBatteryVoltageState = BATTERY_CRITICAL; } else if (stateVoltage > (batteryWarningVoltage + VBATT_HYSTERESIS)){ - return BATTERY_OK; + currentBatteryVoltageState = BATTERY_OK; } break; case BATTERY_CRITICAL: if (stateVoltage > (batteryCriticalVoltage + VBATT_HYSTERESIS)) { - return BATTERY_WARNING; + currentBatteryVoltageState = BATTERY_WARNING; } break; default: break; } - return batteryState; + return currentBatteryVoltageState; } static void checkBatteryCapacityState(void) diff --git a/src/main/target/BRAHMA_H7/CMakeLists.txt b/src/main/target/BRAHMA_H7/CMakeLists.txt new file mode 100644 index 00000000000..afb710016ad --- /dev/null +++ b/src/main/target/BRAHMA_H7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(BRAHMA_H7) diff --git a/src/main/target/BRAHMA_H7/config.c b/src/main/target/BRAHMA_H7/config.c new file mode 100644 index 00000000000..0f1fec5a816 --- /dev/null +++ b/src/main/target/BRAHMA_H7/config.c @@ -0,0 +1,31 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/BRAHMA_H7/target.c b/src/main/target/BRAHMA_H7/target.c new file mode 100644 index 00000000000..3ffd307d90e --- /dev/null +++ b/src/main/target/BRAHMA_H7/target.c @@ -0,0 +1,53 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42605_2, DEVHW_ICM42605, ICM42605_SPI_BUS_2, ICM42605_CS_PIN_2, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN_2); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 + // DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BRAHMA_H7/target.h b/src/main/target/BRAHMA_H7/target.h new file mode 100644 index 00000000000..002626036e1 --- /dev/null +++ b/src/main/target/BRAHMA_H7/target.h @@ -0,0 +1,184 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "DMH7" +#define USBD_PRODUCT_STRING "BRAHMA_H7" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PA4 + +#define BEEPER PA15 +#define BEEPER_INVERTED + +// *************** IMU generic *********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +// *************** SPI1 IMU0 ICM42605 **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_IMU_ICM42605 + +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PC15 + +// *************** SPI4 IMU1 ICM42605 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define IMU_ICM42605_ALIGN_2 CW180_DEG +#define ICM42605_SPI_BUS_2 BUS_SPI4 +#define ICM42605_CS_PIN_2 PE11 + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SPI3 ************************ +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 +#define PINIO2_PIN PD11 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13))) +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 13 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/HAKRCH743/CMakeLists.txt b/src/main/target/HAKRCH743/CMakeLists.txt new file mode 100644 index 00000000000..eb70a0acdca --- /dev/null +++ b/src/main/target/HAKRCH743/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(HAKRCH743) diff --git a/src/main/target/HAKRCH743/config.c b/src/main/target/HAKRCH743/config.c new file mode 100644 index 00000000000..b963bfbfa59 --- /dev/null +++ b/src/main/target/HAKRCH743/config.c @@ -0,0 +1,28 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/HAKRCH743/target.c b/src/main/target/HAKRCH743/target.c new file mode 100644 index 00000000000..047eeeebd70 --- /dev/null +++ b/src/main/target/HAKRCH743/target.c @@ -0,0 +1,50 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_0, DEVHW_ICM42605, ICM42688_0_SPI_BUS, ICM42688_0_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42688_0_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_1, DEVHW_ICM42605, ICM42688_1_SPI_BUS, ICM42688_1_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_ICM42688_1_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/HAKRCH743/target.h b/src/main/target/HAKRCH743/target.h new file mode 100644 index 00000000000..e227fc1fa0b --- /dev/null +++ b/src/main/target/HAKRCH743/target.h @@ -0,0 +1,180 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "HK743" +#define USBD_PRODUCT_STRING "HAKRCH743" + +#define USE_TARGET_CONFIG + +#define USE_DSHOT_DMAR + +/*** Indicators ***/ +#define LED0 PE3 +#define LED1 PE4 +#define BEEPER PE9 +#define BEEPER_INVERTED + +/*** SPI/I2C bus ***/ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PC15 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PE2 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_SPI_DEVICE_4 +#define SPI4_NSS_PIN PC13 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +/*** IMU sensors ***/ + +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS +#define USE_SPI +#define USE_IMU_ICM42605 + +// IMU0 ICM42688P/ICM42605 +#define ICM42688_0_CS_PIN PC15 +#define ICM42688_0_SPI_BUS BUS_SPI1 +#define IMU_ICM42688_0_ALIGN CW0_DEG + + +// IMU1 ICM42688P/ICM42605 +#define ICM42688_1_CS_PIN PC13 +#define ICM42688_1_SPI_BUS BUS_SPI4 +#define IMU_ICM42688_1_ALIGN CW270_DEG + + +/*** OSD ***/ +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PE2 + + +// *** PINIO *** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 +#define PINIO2_PIN PD11 + +/*** Serial ports ***/ +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PD6 +#define UART2_TX_PIN PD5 + +#define USE_UART3 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 + +#define USE_UART4 +#define UART4_RX_PIN PB8 +#define UART4_TX_PIN PB9 + +#define USE_UART5 +#define UART5_RX_PIN PB12 +#define UART5_TX_PIN PB13 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_UART7 +#define UART7_RX_PIN PE7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 9 // VCP, UART1, UART2, UART3, UART4, UART5, UART6, UART7, UART8 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART7 + +/*** BARO & MAG ***/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_SPL06 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_ALL + +/*** ADC ***/ +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC5 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +/*** LED STRIP ***/ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 12 +#define USE_DSHOT +#define USE_ESC_SENSOR + +/*** Used pins ***/ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index bfe9b9e4f3c..65565888486 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -20,8 +20,8 @@ #include "common/time.h" #include "rx/crsf.h" -#define CRSF_MSP_RX_BUF_SIZE 128 -#define CRSF_MSP_TX_BUF_SIZE 128 +#define CRSF_MSP_RX_BUF_SIZE 512 +#define CRSF_MSP_TX_BUF_SIZE 512 void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 4c60a90e707..1589a513596 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -68,19 +68,20 @@ void initSharedMsp(void) mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer; mspPackage.requestPacket = &mspRxPacket; mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer; - mspPackage.requestPacket->buf.end = mspPackage.requestBuffer; + mspPackage.requestPacket->buf.end = mspPackage.requestBuffer + sizeof(mspRxBuffer); mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer; mspPackage.responsePacket = &mspTxPacket; mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer; - mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer + sizeof(mspTxBuffer); } static void processMspPacket(void) { mspPackage.responsePacket->cmd = 0; mspPackage.responsePacket->result = 0; - mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer; + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer + sizeof(mspTxBuffer); mspPostProcessFnPtr mspPostProcessFn = NULL; if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { @@ -217,7 +218,7 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) } sbufWriteU8(payloadBuf, status); - const uint8_t size = sbufBytesRemaining(txBuf); + const uint16_t size = sbufBytesRemaining(txBuf); if (lastRequestVersion == 1) { // MSPv1 sbufWriteU8(payloadBuf, size); sbufWriteU8(payloadBuf, mspPackage.responsePacket->cmd); @@ -231,7 +232,7 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK) | (lastRequestVersion << TELEMETRY_MSP_VER_SHIFT)); // header without 'start' flag } - const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); + const uint16_t bufferBytesRemaining = sbufBytesRemaining(txBuf); const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); uint8_t frame[payloadBytesRemaining]; diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c index dbc093fb641..7ad16084ad9 100644 --- a/src/main/vcp_hal/usbd_cdc_interface.c +++ b/src/main/vcp_hal/usbd_cdc_interface.c @@ -415,6 +415,8 @@ uint32_t CDC_Send_FreeBytes(void) * @param sendLength: Number of data to be sent (in bytes) * @retval Bytes sent */ +#define VCP_WRITE_TIMEOUT_MS 50 + uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) { #if defined(STM32H7) @@ -423,13 +425,22 @@ uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*)USBD_Device.pCDC_ClassData; #endif - while (hcdc->TxState != 0); + uint32_t start = millis(); + + while (hcdc->TxState != 0) { + if (millis() - start > VCP_WRITE_TIMEOUT_MS) { + return 0; + } + } for (uint32_t i = 0; i < sendLength; i++) { UserTxBuffer[UserTxBufPtrIn] = ptrBuffer[i]; UserTxBufPtrIn = (UserTxBufPtrIn + 1) % APP_TX_DATA_SIZE; while (CDC_Send_FreeBytes() == 0) { + if (millis() - start > VCP_WRITE_TIMEOUT_MS) { + return i; // Return partial count + } delay(1); } } diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 15c1f7e39d9..7511f617eb4 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -178,13 +178,24 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) *******************************************************************************/ uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) { - VCP_DataTx(ptrBuffer, sendLength); + if (VCP_DataTx(ptrBuffer, sendLength) != USBD_OK) { + return 0; + } return sendLength; } uint32_t CDC_Send_FreeBytes(void) { - return APP_RX_DATA_SIZE - CDC_Receive_BytesAvailable(); + // Calculate free space in APP_Rx_Buffer (outbound to host) + // Using correct circular buffer math with volatile pointers + uint32_t ptr_in = APP_Rx_ptr_in; + uint32_t ptr_out = APP_Rx_ptr_out; + + if (ptr_out > ptr_in) { + return ptr_out - ptr_in - 1; + } else { + return APP_RX_DATA_SIZE + ptr_out - ptr_in - 1; + } } /** @@ -194,18 +205,29 @@ uint32_t CDC_Send_FreeBytes(void) * @param Len: Number of data to be sent (in bytes) * @retval Result of the operation: USBD_OK if all operations are OK else VCP_FAIL */ +#define VCP_WRITE_TIMEOUT_MS 50 + static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) { + uint32_t start = millis(); + /* make sure that any paragraph end frame is not in play could just check for: USB_CDC_ZLP, but better to be safe and wait for any existing transmission to complete. */ - while (USB_Tx_State != 0); + while (USB_Tx_State != 0) { + if (millis() - start > VCP_WRITE_TIMEOUT_MS) { + return USBD_FAIL; + } + } for (uint32_t i = 0; i < Len; i++) { // Stall if the ring buffer is full while (((APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE) == APP_Rx_ptr_out) { + if (millis() - start > VCP_WRITE_TIMEOUT_MS) { + return USBD_FAIL; + } delay(1); } diff --git a/src/main/vcpf4/usbd_usr.c b/src/main/vcpf4/usbd_usr.c index 24c5f17c206..15aa47fb257 100644 --- a/src/main/vcpf4/usbd_usr.c +++ b/src/main/vcpf4/usbd_usr.c @@ -21,6 +21,7 @@ #include "usbd_usr.h" #include "usbd_ioreq.h" +#include "usbd_cdc_vcp.h" USBD_Usr_cb_TypeDef USR_cb = { @@ -102,13 +103,17 @@ void USBD_USR_DeviceDisconnected (void) /** * @brief USBD_USR_DeviceSuspended -* Displays the message on LCD on device suspend Event +* Handle device suspend event - treat as disconnect for safety. +* This helps on boards without VBUS sensing where hardware +* disconnect detection may not work. * @param None * @retval None */ void USBD_USR_DeviceSuspended(void) { - /* Users can do their application actions here for the USB-Reset */ + // Treat suspend as disconnect - prevents blocking on USB writes + // when cable is unplugged on boards without VBUS sensing + bDeviceState = UNCONNECTED; }