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
diff --git a/src/config b/src/config
index 5e33fe422e..b606002103 160000
--- a/src/config
+++ b/src/config
@@ -1 +1 @@
-Subproject commit 5e33fe422e2c1fb095db84508a9e3ebb465a80a4
+Subproject commit b60600210384d1da4621cc35a0632d9cca8a62b7
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index ba8db32289..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);
}
@@ -1441,12 +1449,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 +1718,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 +1804,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 +1843,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/cli.c b/src/main/cli/cli.c
index a619f06a83..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 ),
@@ -6896,7 +6898,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/cli/settings.c b/src/main/cli/settings.c
index 82df9227dd..e1b9bdd91e 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
@@ -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) },
@@ -1918,20 +1920,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/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
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;
}
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/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;
}
}
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/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;
diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c
index 124554928e..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
@@ -184,7 +186,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:
@@ -362,6 +364,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 +443,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..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)
@@ -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 pwmMotors[index].io;
+}
+
+bool pwmIsMotorEnabled(unsigned index)
+{
+ return pwmMotors[index].enabled;
+}
+
+bool pwmEnableMotors(void)
+{
+ /* check motors can be enabled */
+ return pwmMotorCount > 0;
+}
+
#endif
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/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/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.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);
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/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/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/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
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..e811b433da 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;
@@ -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
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));
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
-
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/main/msp/msp.c b/src/main/msp/msp.c
index df8e717cb9..025df2fc7b 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));
@@ -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;
}
@@ -1529,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);
@@ -1547,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);
@@ -1796,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
@@ -2131,7 +2132,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);
@@ -2900,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) {
@@ -2923,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);
@@ -2989,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
@@ -3350,18 +3351,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))
@@ -4147,7 +4153,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
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,
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/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,
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/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/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;
}
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;
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/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c
index 5776305d62..cf971adf97 100644
--- a/src/platform/APM32/dshot_bitbang.c
+++ b/src/platform/APM32/dshot_bitbang.c
@@ -685,6 +685,7 @@ static motorVTable_t bbVTable = {
.shutdown = bbShutdown,
.isMotorIdle = bbDshotIsMotorIdle,
.requestTelemetry = bbDshotRequestTelemetry,
+ .getMotorIO = bbGetMotorIO,
};
dshotBitbangStatus_e dshotBitbangGetStatus(void)
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/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c
index d3de5ba53a..cdec437029 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"
@@ -89,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;
}
}
}
@@ -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)
{
@@ -126,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;
}
}
@@ -158,11 +149,12 @@ static motorVTable_t motorPwmVTable = {
.updateComplete = pwmCompleteMotorUpdate,
.requestTelemetry = NULL,
.isMotorIdle = NULL,
+ .getMotorIO = pwmGetMotorIO,
};
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;
@@ -214,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
@@ -234,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;
@@ -255,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/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/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/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/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/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/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c
index 8ebc593340..82c1332cb6 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"
@@ -80,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;
}
}
}
@@ -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)
@@ -119,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;
}
}
@@ -151,11 +140,12 @@ static motorVTable_t motorPwmVTable = {
.updateComplete = pwmCompleteMotorUpdate,
.requestTelemetry = NULL,
.isMotorIdle = NULL,
+ .getMotorIO = pwmGetMotorIO,
};
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;
@@ -208,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
@@ -228,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/AT32/target/AT32F435G/target.h b/src/platform/AT32/target/AT32F435G/target.h
index 76a4e039dc..e9dbcebed6 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
@@ -96,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 3429a10876..3ee3ca1928 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
@@ -96,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/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/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c
index bd52fe2564..2fc992cb45 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,11 +544,9 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
}
}
-static motorDevice_t pwmMotorDevice; // Forward
-
pwmOutputPort_t *pwmGetMotors(void)
{
- return motors;
+ return pwmMotors;
}
static float pwmConvertFromExternal(uint16_t externalValue)
@@ -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)
@@ -657,15 +644,17 @@ 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++) {
- motors[motorIndex].enabled = true;
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) {
+ pwmMotors[motorIndex].enabled = true;
}
return true;
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;
}
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/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/platform/STM32/pwm_output_hw.c b/src/platform/STM32/pwm_output_hw.c
index 99a2a3ce69..ae8c6a21af 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"
@@ -110,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;
}
}
}
@@ -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) {
@@ -146,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;
}
}
@@ -178,11 +168,12 @@ 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)
{
- memset(motors, 0, sizeof(motors));
+ memset(pwmMotors, 0, sizeof(pwmMotors));
pwmMotorCount = device->count;
device->vTable = &motorPwmVTable;
@@ -230,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
@@ -250,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;
@@ -271,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/STM32/startup/system_stm32g4xx.c b/src/platform/STM32/startup/system_stm32g4xx.c
index 9874d2d1f1..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 -------------------------------------------------------*/
@@ -225,6 +224,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 +247,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 +296,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 +312,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;
}
@@ -371,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;
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
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 cee7aa6003..2e2186a1ac 100644
--- a/src/platform/common/stm32/dshot_dpwm.c
+++ b/src/platform/common/stm32/dshot_dpwm.c
@@ -123,7 +123,15 @@ static bool dshotPwmEnableMotors(void)
static bool dshotPwmIsMotorEnabled(unsigned index)
{
- return motors[index].enabled;
+ return pwmMotors[index].enabled;
+}
+
+static IO_t pwmDshotGetMotorIO(unsigned index)
+{
+ if (index >= dshotMotorCount) {
+ return IO_NONE;
+ }
+ return pwmMotors[index].io;
}
static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t 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)
@@ -181,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;
}
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)
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
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
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;
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