diff --git a/.gitignore b/.gitignore
index af249aa66c..89126f12e8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -43,7 +43,7 @@ stm32.mak
# artefacts for VS Code
/.vscode/
-# artefacts for Visual Studio
+# artefacts for Visual Studio
/.vs
# artefacts for CLion
@@ -57,5 +57,6 @@ ubuntu*.log
eeprom.bin
# config used for building targets
-/src/config
+# changed to submodule
+#/src/config
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000000..70c3f1c3ee
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,5 @@
+[submodule "config"]
+ path = src/config
+ url = https://github.com/betaflight/config.git
+ branch = master
+ ignore = dirty
diff --git a/Makefile b/Makefile
index 46c822012e..8af2116909 100644
--- a/Makefile
+++ b/Makefile
@@ -299,7 +299,6 @@ CFLAGS += $(ARCH_FLAGS) \
$(TEMPORARY_FLAGS) \
$(DEVICE_FLAGS) \
-D_GNU_SOURCE \
- -DUSE_STDPERIPH_DRIVER \
-D$(TARGET) \
$(TARGET_FLAGS) \
-D'__FORKNAME__="$(FORKNAME)"' \
diff --git a/mk/config.mk b/mk/config.mk
index ab2386da40..95c4d585f0 100644
--- a/mk/config.mk
+++ b/mk/config.mk
@@ -1,6 +1,7 @@
CONFIGS_REPO_URL ?= https://github.com/betaflight/config
-
+# handle only this directory as config submodule
+CONFIGS_SUBMODULE_DIR = src/config
BASE_CONFIGS = $(sort $(notdir $(patsubst %/,%,$(dir $(wildcard $(CONFIG_DIR)/configs/*/config.h)))))
ifneq ($(filter-out %_install test% %_clean clean% %-print %.hex %.h hex checks help configs $(BASE_TARGETS) $(BASE_CONFIGS),$(MAKECMDGOALS)),)
@@ -15,10 +16,17 @@ ifneq ($(TARGET),)
$(error TARGET or CONFIG should be specified. Not both.)
endif
-CONFIG_FILE = $(CONFIG_DIR)/configs/$(CONFIG)/config.h
-INCLUDE_DIRS += $(CONFIG_DIR)/configs/$(CONFIG)
+CONFIG_HEADER_FILE = $(CONFIG_DIR)/configs/$(CONFIG)/config.h
+CONFIG_SOURCE_FILE = $(CONFIG_DIR)/configs/$(CONFIG)/config.c
+INCLUDE_DIRS += $(CONFIG_DIR)/configs/$(CONFIG)
-ifneq ($(wildcard $(CONFIG_FILE)),)
+ifneq ($(wildcard $(CONFIG_HEADER_FILE)),)
+
+CONFIG_SRC :=
+ifneq ($(wildcard $(CONFIG_SOURCE_FILE)),)
+CONFIG_SRC += $(CONFIG_SOURCE_FILE)
+TARGET_FLAGS += -DUSE_CONFIG_SOURCE
+endif
CONFIG_REVISION := norevision
ifeq ($(shell git -C $(CONFIG_DIR) diff --shortstat),)
@@ -26,36 +34,40 @@ CONFIG_REVISION := $(shell git -C $(CONFIG_DIR) log -1 --format="%h")
CONFIG_REVISION_DEFINE := -D'__CONFIG_REVISION__="$(CONFIG_REVISION)"'
endif
-TARGET := $(shell grep " FC_TARGET_MCU" $(CONFIG_FILE) | awk '{print $$3}' )
-HSE_VALUE_MHZ := $(shell grep " SYSTEM_HSE_MHZ" $(CONFIG_FILE) | awk '{print $$3}' )
+TARGET := $(shell grep " FC_TARGET_MCU" $(CONFIG_HEADER_FILE) | awk '{print $$3}' )
+HSE_VALUE_MHZ := $(shell grep " SYSTEM_HSE_MHZ" $(CONFIG_HEADER_FILE) | awk '{print $$3}' )
ifneq ($(HSE_VALUE_MHZ),)
HSE_VALUE := $(shell echo $$(( $(HSE_VALUE_MHZ) * 1000000 )) )
endif
-GYRO_DEFINE := $(shell grep " USE_GYRO_" $(CONFIG_FILE) | awk '{print $$2}' )
+GYRO_DEFINE := $(shell grep " USE_GYRO_" $(CONFIG_HEADER_FILE) | awk '{print $$2}' )
ifeq ($(TARGET),)
-$(error No TARGET identified. Is the $(CONFIG_FILE) valid for $(CONFIG)?)
+$(error No TARGET identified. Is the $(CONFIG_HEADER_FILE) valid for $(CONFIG)?)
endif
-EXST_ADJUST_VMA := $(shell grep " FC_VMA_ADDRESS" $(CONFIG_FILE) | awk '{print $$3}' )
+EXST_ADJUST_VMA := $(shell grep " FC_VMA_ADDRESS" $(CONFIG_HEADER_FILE) | awk '{print $$3}' )
ifneq ($(EXST_ADJUST_VMA),)
EXST = yes
endif
else #exists
-$(error `$(CONFIG_FILE)` not found. Have you hydrated configuration using: 'make configs'?)
-endif #config_file exists
+$(error `$(CONFIG_HEADER_FILE)` not found. Have you hydrated configuration using: 'make configs'?)
+endif #CONFIG_HEADER_FILE exists
endif #config
.PHONY: configs
configs:
+ifeq ($(shell realpath $(CONFIG_DIR)),$(shell realpath $(CONFIGS_SUBMODULE_DIR)))
+ git submodule update --init --remote -- $(CONFIGS_SUBMODULE_DIR)
+else
ifeq ($(wildcard $(CONFIG_DIR)),)
@echo "Hydrating clone for configs: $(CONFIG_DIR)"
$(V0) git clone $(CONFIGS_REPO_URL) $(CONFIG_DIR)
else
$(V0) git -C $(CONFIG_DIR) pull origin
endif
+endif
$(BASE_CONFIGS):
@echo "Building target config $@"
diff --git a/mk/source.mk b/mk/source.mk
index 9b981c05cb..8fe65dde2f 100644
--- a/mk/source.mk
+++ b/mk/source.mk
@@ -56,8 +56,8 @@ COMMON_SRC = \
build/debug_pin.c \
build/version.c \
main.c \
- $(PG_SRC) \
common/bitarray.c \
+ common/chirp.c \
common/colorconversion.c \
common/crc.c \
common/encoding.c \
@@ -369,9 +369,8 @@ SDCARD_SRC += \
io/asyncfatfs/asyncfatfs.c \
io/asyncfatfs/fat_standard.c
-INCLUDE_DIRS := $(INCLUDE_DIRS) \
- $(FATFS_DIR)
-VPATH := $(VPATH):$(FATFS_DIR)
+INCLUDE_DIRS += $(FATFS_DIR)
+VPATH := $(VPATH):$(FATFS_DIR)
# Gyro driver files that only contain initialization and configuration code - not runtime code
SIZE_OPTIMISED_SRC += \
@@ -411,22 +410,20 @@ SPEED_OPTIMISED_SRC += \
endif
-COMMON_DEVICE_SRC = \
- $(CMSIS_SRC) \
- $(DEVICE_STDPERIPH_SRC)
+COMMON_DEVICE_SRC = $(CMSIS_SRC) $(DEVICE_STDPERIPH_SRC)
-COMMON_SRC := $(COMMON_SRC) $(COMMON_DEVICE_SRC) $(RX_SRC)
+COMMON_SRC += $(CONFIG_SRC) $(PG_SRC) $(COMMON_DEVICE_SRC) $(RX_SRC)
ifeq ($(EXST),yes)
-TARGET_FLAGS := -DUSE_EXST $(TARGET_FLAGS)
+TARGET_FLAGS += -DUSE_EXST
endif
ifeq ($(RAM_BASED),yes)
-TARGET_FLAGS := -DUSE_EXST -DCONFIG_IN_RAM -DRAMBASED $(TARGET_FLAGS)
+TARGET_FLAGS += -DUSE_EXST -DCONFIG_IN_RAM -DRAMBASED
endif
ifeq ($(SIMULATOR_BUILD),yes)
-TARGET_FLAGS := -DSIMULATOR_BUILD $(TARGET_FLAGS)
+TARGET_FLAGS += -DSIMULATOR_BUILD
endif
SPEED_OPTIMISED_SRC += \
diff --git a/src/config b/src/config
new file mode 160000
index 0000000000..3f3dd34c73
--- /dev/null
+++ b/src/config
@@ -0,0 +1 @@
+Subproject commit 3f3dd34c7368e5ec6c2eb5634ac638d706b1d4a8
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index c8f359b924..acebd8c2c2 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1642,6 +1642,17 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_YAW, "%d", currentPidProfile->pid[PID_YAW].S);
#endif // USE_WING
+#ifdef USE_CHIRP
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_LAG_FREQ_HZ, "%d", currentPidProfile->chirp_lag_freq_hz);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_LEAD_FREQ_HZ, "%d", currentPidProfile->chirp_lead_freq_hz);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_AMPLITUDE_ROLL, "%d", currentPidProfile->chirp_amplitude_roll);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_AMPLITUDE_PITCH, "%d", currentPidProfile->chirp_amplitude_pitch);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_AMPLITUDE_YAW, "%d", currentPidProfile->chirp_amplitude_yaw);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_FREQUENCY_START_DECI_HZ, "%d", currentPidProfile->chirp_frequency_start_deci_hz);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_FREQUENCY_END_DECI_HZ, "%d", currentPidProfile->chirp_frequency_end_deci_hz);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_TIME_SECONDS, "%d", currentPidProfile->chirp_time_seconds);
+#endif // USE_CHIRP
+
// End of Betaflight controller parameters
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEADBAND, "%d", rcControlsConfig()->deadband);
diff --git a/src/main/build/build_config.c b/src/main/build/build_config.c
index 70117e0407..1754fe3558 100644
--- a/src/main/build/build_config.c
+++ b/src/main/build/build_config.c
@@ -31,54 +31,12 @@
mcuTypeId_e getMcuTypeId(void)
{
-#if defined(SIMULATOR_BUILD)
- return MCU_TYPE_SIMULATOR;
-#elif defined(STM32F40_41xxx)
- return MCU_TYPE_F40X;
-#elif defined(STM32F411xE)
- return MCU_TYPE_F411;
-#elif defined(STM32F446xx)
- return MCU_TYPE_F446;
-#elif defined(STM32F722xx)
- return MCU_TYPE_F722;
-#elif defined(STM32F745xx)
- return MCU_TYPE_F745;
-#elif defined(STM32F746xx)
- return MCU_TYPE_F746;
-#elif defined(STM32F765xx)
- return MCU_TYPE_F765;
-#elif defined(STM32H750xx)
- return MCU_TYPE_H750;
-#elif defined(STM32H730xx)
- return MCU_TYPE_H730;
-#elif defined(STM32H743xx)
- switch (HAL_GetREVID()) {
- case REV_ID_Y:
- return MCU_TYPE_H743_REV_Y;
- case REV_ID_X:
- return MCU_TYPE_H743_REV_X;
- case REV_ID_V:
- return MCU_TYPE_H743_REV_V;
- default:
- return MCU_TYPE_H743_REV_UNKNOWN;
- }
-#elif defined(STM32H7A3xx) || defined(STM32H7A3xxQ)
- return MCU_TYPE_H7A3;
-#elif defined(STM32H723xx) || defined(STM32H725xx)
- return MCU_TYPE_H723_725;
-#elif defined(STM32G474xx)
- return MCU_TYPE_G474;
-#elif defined(AT32F435G)
- return MCU_TYPE_AT32F435G;
-#elif defined(AT32F435M)
- return MCU_TYPE_AT32F435M;
-#elif defined(APM32F405)
- return MCU_TYPE_APM32F405;
-#elif defined(APM32F407)
- return MCU_TYPE_APM32F407;
-#elif defined(RP2350B)
- return MCU_TYPE_RP2350B;
-#else
- return MCU_TYPE_UNKNOWN;
-#endif
+ const mcuTypeInfo_t *mcuTypeInfo = getMcuTypeInfo();
+ return mcuTypeInfo ? mcuTypeInfo->id : MCU_TYPE_UNKNOWN;
+}
+
+const char *getMcuTypeName(void)
+{
+ const mcuTypeInfo_t *mcuTypeInfo = getMcuTypeInfo();
+ return mcuTypeInfo ? mcuTypeInfo->name : "Unknown";
}
diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h
index b187d06de1..4665627e47 100644
--- a/src/main/build/build_config.h
+++ b/src/main/build/build_config.h
@@ -68,4 +68,11 @@ typedef enum {
MCU_TYPE_UNKNOWN = 255,
} mcuTypeId_e;
+typedef struct mcuTypeInfo_s {
+ mcuTypeId_e id;
+ const char *name;
+} mcuTypeInfo_t;
+
+const mcuTypeInfo_t *getMcuTypeInfo(void);
mcuTypeId_e getMcuTypeId(void);
+const char *getMcuTypeName(void);
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 3d67bae371..fb8795bd42 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -124,4 +124,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
[DEBUG_GIMBAL] = "GIMBAL",
[DEBUG_WING_SETPOINT] = "WING_SETPOINT",
[DEBUG_AUTOPILOT_POSITION] = "AUTOPILOT_POSITION",
+ [DEBUG_CHIRP] = "CHIRP",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 6e6e43a67a..b72ebdb496 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -126,6 +126,7 @@ typedef enum {
DEBUG_GIMBAL,
DEBUG_WING_SETPOINT,
DEBUG_AUTOPILOT_POSITION,
+ DEBUG_CHIRP,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 12dd19cd87..ce5dbddab7 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -279,32 +279,6 @@ static const char * const *sensorHardwareNames[] = {
};
#endif // USE_SENSOR_NAMES
-// Needs to be aligned with mcuTypeId_e
-static const char *mcuTypeNames[MCU_TYPE_COUNT] = {
- "SIMULATOR",
- "F40X",
- "F411",
- "F446",
- "F722",
- "F745",
- "F746",
- "F765",
- "H750",
- "H743 (Rev Unknown)",
- "H743 (Rev.Y)",
- "H743 (Rev.X)",
- "H743 (Rev.V)",
- "H7A3",
- "H723/H725",
- "G474",
- "H730",
- "AT32F435G",
- "APM32F405",
- "APM32F407",
- "AT32F435M",
- "RP2350B",
-};
-
static const char *configurationStates[] = {
[CONFIGURATION_STATE_UNCONFIGURED] = "UNCONFIGURED",
[CONFIGURATION_STATE_CONFIGURED] = "CONFIGURED"
@@ -4692,15 +4666,6 @@ STATIC_UNIT_TESTED void cliSet(const char *cmdName, char *cmdline)
}
}
-static const char *getMcuTypeById(mcuTypeId_e id)
-{
- if (id < ARRAYLEN(mcuTypeNames)) {
- return mcuTypeNames[id];
- } else {
- return "UNKNOWN";
- }
-}
-
static void cliStatus(const char *cmdName, char *cmdline)
{
UNUSED(cmdName);
@@ -4708,7 +4673,7 @@ static void cliStatus(const char *cmdName, char *cmdline)
// MCU type, clock, vrefint, core temperature
- cliPrintf("MCU %s Clock=%dMHz", getMcuTypeById(getMcuTypeId()), (SystemCoreClock / 1000000));
+ cliPrintf("MCU %s Clock=%dMHz", getMcuTypeName(), (SystemCoreClock / 1000000));
#if defined(STM32F4) || defined(STM32G4) || defined(APM32F4)
// Only F4 and G4 is capable of switching between HSE/HSI (for now)
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index ebc5b894e4..3db4343db9 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -160,6 +160,7 @@ const char * const lookupTableAccHardware[] = {
"BMI270",
"LSM6DSO",
"LSM6DSV16X",
+ "IIM42653",
"VIRTUAL"
};
@@ -183,6 +184,7 @@ const char * const lookupTableGyroHardware[] = {
"BMI270",
"LSM6DSO",
"LSM6DSV16X",
+ "IIM42653",
"VIRTUAL"
};
@@ -242,7 +244,7 @@ static const char * const lookupTableGyro[] = {
#ifdef USE_GPS
static const char * const lookupTableGpsProvider[] = {
- "NMEA", "UBLOX", "MSP"
+ "NMEA", "UBLOX", "MSP", "VIRTUAL"
};
static const char * const lookupTableGpsSbasMode[] = {
@@ -1261,6 +1263,17 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_HORIZON_IGNORE_STICKS, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_ignore_sticks) },
{ PARAM_NAME_HORIZON_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_delay_ms) },
+#ifdef USE_CHIRP
+ { PARAM_NAME_CHIRP_LAG_FREQ_HZ, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_lag_freq_hz) },
+ { PARAM_NAME_CHIRP_LEAD_FREQ_HZ, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_lead_freq_hz) },
+ { PARAM_NAME_CHIRP_AMPLITUDE_ROLL, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_amplitude_roll) },
+ { PARAM_NAME_CHIRP_AMPLITUDE_PITCH, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_amplitude_pitch) },
+ { PARAM_NAME_CHIRP_AMPLITUDE_YAW, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_amplitude_yaw) },
+ { PARAM_NAME_CHIRP_FREQUENCY_START_DECI_HZ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_frequency_start_deci_hz) },
+ { PARAM_NAME_CHIRP_FREQUENCY_END_DECI_HZ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 10000 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_frequency_end_deci_hz) },
+ { PARAM_NAME_CHIRP_TIME_SECONDS, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_time_seconds) },
+#endif
+
#if defined(USE_ABSOLUTE_CONTROL)
{ PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },
{ "abs_control_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_limit) },
diff --git a/src/main/common/chirp.c b/src/main/common/chirp.c
new file mode 100644
index 0000000000..20aff3db14
--- /dev/null
+++ b/src/main/common/chirp.c
@@ -0,0 +1,94 @@
+/*
+ * 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 .
+ */
+
+#include
+
+#include "platform.h"
+
+#include "maths.h"
+
+#include "chirp.h"
+
+// initialize the chirp signal generator
+// f0: start frequency in Hz
+// f1: end frequency in Hz
+// t1: signal length in seconds
+// looptimeUs: loop time in microseconds
+void chirpInit(chirp_t *chirp, const float f0, const float f1, const float t1, const uint32_t looptimeUs)
+{
+ chirp->f0 = f0;
+ chirp->Ts = looptimeUs * 1e-6f;
+ chirp->N = (uint32_t)(t1 / chirp->Ts);
+ chirp->beta = pow_approx(f1 / f0, 1.0f / t1);
+ chirp->k0 = 2.0f * M_PIf / log_approx(chirp->beta);
+ chirp->k1 = chirp->k0 * f0;
+ chirpReset(chirp);
+}
+
+// reset the chirp signal generator fully
+void chirpReset(chirp_t *chirp)
+{
+ chirp->count = 0;
+ chirp->isFinished = false;
+ chirpResetSignals(chirp);
+}
+
+// reset the chirp signal generator signals
+void chirpResetSignals(chirp_t *chirp)
+{
+ chirp->exc = 0.0f;
+ chirp->fchirp = 0.0f;
+ chirp->sinarg = 0.0f;
+}
+
+// update the chirp signal generator
+bool chirpUpdate(chirp_t *chirp)
+{
+ if (chirp->isFinished) {
+
+ return false;
+
+ } else if (chirp->count == chirp->N) {
+
+ chirp->isFinished = true;
+ chirpResetSignals(chirp);
+ return false;
+
+ } else {
+
+ chirp->fchirp = chirp->f0 * pow_approx(chirp->beta, (float)(chirp->count) * chirp->Ts);
+ chirp->sinarg = chirp->k0 * chirp->fchirp - chirp->k1;
+
+ // wrap sinarg to 0...2*pi
+ chirp->sinarg = fmodf(chirp->sinarg, 2.0f * M_PIf);
+
+ // use cosine so that the angle will oscillate around 0 (integral of gyro)
+ chirp->exc = cos_approx(chirp->sinarg);
+
+ // frequencies below 1 Hz will lead to the same angle magnitude as at 1 Hz (integral of gyro)
+ if (chirp->fchirp < 1.0f) {
+ chirp->exc = chirp->fchirp * chirp->exc;
+ }
+ chirp->count++;
+
+ return true;
+ }
+}
diff --git a/src/main/common/chirp.h b/src/main/common/chirp.h
new file mode 100644
index 0000000000..211a7c66e1
--- /dev/null
+++ b/src/main/common/chirp.h
@@ -0,0 +1,37 @@
+/*
+ * 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
+#include
+
+typedef struct chirp_s {
+ float f0, Ts, beta, k0, k1;
+ uint32_t count, N;
+ float exc, fchirp, sinarg;
+ bool isFinished;
+} chirp_t;
+
+void chirpInit(chirp_t *chirp, const float f0, const float f1, const float t1, const uint32_t looptimeUs);
+void chirpReset(chirp_t *chirp);
+void chirpResetSignals(chirp_t *chirp);
+bool chirpUpdate(chirp_t *chirp);
diff --git a/src/main/common/sensor_alignment.h b/src/main/common/sensor_alignment.h
index b1c7fd9ddf..d9186048e3 100644
--- a/src/main/common/sensor_alignment.h
+++ b/src/main/common/sensor_alignment.h
@@ -55,20 +55,25 @@ typedef union sensorAlignment_u {
};
} sensorAlignment_t;
-#define SENSOR_ALIGNMENT(ROLL, PITCH, YAW) ((sensorAlignment_t){\
- .roll = DEGREES_TO_DECIDEGREES(ROLL), \
- .pitch = DEGREES_TO_DECIDEGREES(PITCH), \
- .yaw = DEGREES_TO_DECIDEGREES(YAW), \
+#define SENSOR_ALIGNMENT(ROLL, PITCH, YAW) ((const sensorAlignment_t) { \
+ .roll = DEGREES_TO_DECIDEGREES(ROLL), \
+ .pitch = DEGREES_TO_DECIDEGREES(PITCH), \
+ .yaw = DEGREES_TO_DECIDEGREES(YAW), \
})
-#define CUSTOM_ALIGN_CW0_DEG SENSOR_ALIGNMENT( 0, 0, 0)
-#define CUSTOM_ALIGN_CW90_DEG SENSOR_ALIGNMENT( 0, 0, 90)
-#define CUSTOM_ALIGN_CW180_DEG SENSOR_ALIGNMENT( 0, 0, 180)
-#define CUSTOM_ALIGN_CW270_DEG SENSOR_ALIGNMENT( 0, 0, 270)
-#define CUSTOM_ALIGN_CW0_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 0)
-#define CUSTOM_ALIGN_CW90_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 90)
-#define CUSTOM_ALIGN_CW180_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 180)
-#define CUSTOM_ALIGN_CW270_DEG_FLIP SENSOR_ALIGNMENT( 0, 180, 270)
+#define CUSTOM_ALIGN_CW(deg) SENSOR_ALIGNMENT( 0, 0, (deg) )
+#define CUSTOM_ALIGN_CW_FLIP(deg) SENSOR_ALIGNMENT( 0, 180, (deg) )
+
+#define CUSTOM_ALIGN_CW0_DEG CUSTOM_ALIGN_CW( 0 )
+#define CUSTOM_ALIGN_CW45_DEG CUSTOM_ALIGN_CW( 45 )
+#define CUSTOM_ALIGN_CW90_DEG CUSTOM_ALIGN_CW( 90 )
+#define CUSTOM_ALIGN_CW180_DEG CUSTOM_ALIGN_CW( 180 )
+#define CUSTOM_ALIGN_CW270_DEG CUSTOM_ALIGN_CW( 270 )
+#define CUSTOM_ALIGN_CW0_DEG_FLIP CUSTOM_ALIGN_CW_FLIP( 0 )
+#define CUSTOM_ALIGN_CW45_DEG_FLIP CUSTOM_ALIGN_CW_FLIP( 45 )
+#define CUSTOM_ALIGN_CW90_DEG_FLIP CUSTOM_ALIGN_CW_FLIP( 90 )
+#define CUSTOM_ALIGN_CW180_DEG_FLIP CUSTOM_ALIGN_CW_FLIP( 180 )
+#define CUSTOM_ALIGN_CW270_DEG_FLIP CUSTOM_ALIGN_CW_FLIP( 270 )
void buildRotationMatrixFromAngles(matrix33_t *rm, const sensorAlignment_t *rpy);
void buildAlignmentFromStandardAlignment(sensorAlignment_t *rpy, sensor_align_e stdAlignment);
diff --git a/src/main/common/sensor_alignment_impl.h b/src/main/common/sensor_alignment_impl.h
index 0e8e0b832d..47b289905d 100644
--- a/src/main/common/sensor_alignment_impl.h
+++ b/src/main/common/sensor_alignment_impl.h
@@ -45,4 +45,13 @@
// [1:0] count of 90 degree rotations from 0
// [3:2] indicates 90 degree rotations on pitch
// [5:4] indicates 90 degree rotations on roll
-#define ALIGNMENT_TO_BITMASK(alignment) ((alignment - CW0_DEG) & 0x3) | (((alignment - CW0_DEG) & 0x4) << 1)
+#define ALIGNMENT_TO_BITMASK(alignment) (((alignment - CW0_DEG) & 0x3) | (((alignment - CW0_DEG) & 0x4) << 1))
+
+// build sensorAlignment_t from sensor_align_e as compile-time constant
+#define SENSOR_ALIGNMENT_FROM_STD(std_) \
+ ( ((std_) >= CW0_DEG && (std_) <= CW270_DEG_FLIP) \
+ ? SENSOR_ALIGNMENT(ALIGNMENT_ROLL_ROTATIONS(ALIGNMENT_TO_BITMASK(std_)) * 90, \
+ ALIGNMENT_PITCH_ROTATIONS(ALIGNMENT_TO_BITMASK(std_)) * 90, \
+ ALIGNMENT_YAW_ROTATIONS(ALIGNMENT_TO_BITMASK(std_)) * 90) \
+ : SENSOR_ALIGNMENT(0, 0, 0) ) \
+ /**/
diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c
index 0268b6db32..f208907cf5 100644
--- a/src/main/common/streambuf.c
+++ b/src/main/common/streambuf.c
@@ -24,6 +24,7 @@
#include "platform.h"
#include "streambuf.h"
+#include "common/maths.h"
sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
{
@@ -82,6 +83,13 @@ void sbufWriteString(sbuf_t *dst, const char *string)
sbufWriteData(dst, string, strlen(string));
}
+void sbufWritePString(sbuf_t *dst, const char *string)
+{
+ const int length = MIN((int)strlen(string), 255);
+ sbufWriteU8(dst, length);
+ sbufWriteData(dst, string, length);
+}
+
void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string)
{
sbufWriteData(dst, string, strlen(string) + 1);
diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h
index 3eff560e98..7a5b0831a7 100644
--- a/src/main/common/streambuf.h
+++ b/src/main/common/streambuf.h
@@ -39,6 +39,7 @@ void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val);
void sbufFill(sbuf_t *dst, uint8_t data, int len);
void sbufWriteData(sbuf_t *dst, const void *data, int len);
void sbufWriteString(sbuf_t *dst, const char *string);
+void sbufWritePString(sbuf_t *dst, const char *string);
void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string);
uint8_t sbufReadU8(sbuf_t *src);
diff --git a/src/main/config/config.h b/src/main/config/config.h
index 17f35ee36c..2c049f0945 100644
--- a/src/main/config/config.h
+++ b/src/main/config/config.h
@@ -77,6 +77,7 @@ bool canSoftwareSerialBeUsed(void);
void resetConfig(void);
void targetConfiguration(void);
void targetValidateConfiguration(void);
+void configTargetPreInit(void);
bool isSystemConfigured(void);
void setRebootRequired(void);
diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h
index 0c63a8258f..51a7672b1f 100644
--- a/src/main/drivers/accgyro/accgyro.h
+++ b/src/main/drivers/accgyro/accgyro.h
@@ -61,6 +61,7 @@ typedef enum {
GYRO_BMI270,
GYRO_LSM6DSO,
GYRO_LSM6DSV16X,
+ GYRO_IIM42653,
GYRO_VIRTUAL
} gyroHardware_e;
diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c
index 5e37cd5286..1f1943dc3b 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.c
+++ b/src/main/drivers/accgyro/accgyro_mpu.c
@@ -365,7 +365,7 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_ACCGYRO_BMI270
bmi270Detect,
#endif
-#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
+#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653)
icm426xxSpiDetect,
#endif
#ifdef USE_GYRO_SPI_ICM20649
diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
index eebb76e8f2..3c06a02ba2 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.h
+++ b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -45,6 +45,7 @@
#define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM42605_WHO_AM_I_CONST (0x42)
#define ICM42688P_WHO_AM_I_CONST (0x47)
+#define IIM42653_WHO_AM_I_CONST (0x56)
#define LSM6DSV16X_WHO_AM_I_CONST (0x70)
// RA = Register Address
@@ -146,6 +147,7 @@ enum gyro_fsr_e {
INV_FSR_500DPS,
INV_FSR_1000DPS,
INV_FSR_2000DPS,
+ INV_FSR_4000DPS,
NUM_GYRO_FSR
};
@@ -168,6 +170,7 @@ enum accel_fsr_e {
INV_FSR_4G,
INV_FSR_8G,
INV_FSR_16G,
+ INV_FSR_32G,
NUM_ACCEL_FSR
};
@@ -201,6 +204,7 @@ typedef enum {
ICM_20689_SPI,
ICM_42605_SPI,
ICM_42688P_SPI,
+ IIM_42653_SPI,
BMI_160_SPI,
BMI_270_SPI,
LSM6DSO_SPI,
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
index 1063ae4eef..29dabbcb98 100644
--- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
+++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
@@ -28,7 +28,7 @@
#include "platform.h"
-#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
+#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653)
#include "common/axis.h"
#include "common/utils.h"
@@ -269,6 +269,9 @@ uint8_t icm426xxSpiDetect(const extDevice_t *dev)
case ICM42688P_WHO_AM_I_CONST:
icmDetected = ICM_42688P_SPI;
break;
+ case IIM42653_WHO_AM_I_CONST:
+ icmDetected = IIM_42653_SPI;
+ break;
default:
icmDetected = MPU_NONE;
break;
@@ -286,15 +289,22 @@ uint8_t icm426xxSpiDetect(const extDevice_t *dev)
void icm426xxAccInit(accDev_t *acc)
{
- acc->acc_1G = 512 * 4;
+ switch (acc->mpuDetectionResult.sensor) {
+ case IIM_42653_SPI:
+ acc->acc_1G = 512 * 2; // Accel scale 32g (1024 LSB/g)
+ break;
+ default:
+ acc->acc_1G = 512 * 4; // Accel scale 16g (2048 LSB/g)
+ break;
+ }
}
bool icm426xxSpiAccDetect(accDev_t *acc)
{
switch (acc->mpuDetectionResult.sensor) {
case ICM_42605_SPI:
- break;
case ICM_42688P_SPI:
+ case IIM_42653_SPI:
break;
default:
return false;
@@ -386,12 +396,12 @@ void icm426xxGyroInit(gyroDev_t *gyro)
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
}
- STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
- spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (odrConfig & 0x0F));
+ // This sets the gyro/accel to the maximum FSR, depending on the chip
+ // ICM42605, ICM_42688P: 2000DPS and 16G.
+ // IIM42653: 4000DPS and 32G
+ spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG0, (0 << 5) | (odrConfig & 0x0F));
delay(15);
-
- STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
- spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (odrConfig & 0x0F));
+ spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG0, (0 << 5) | (odrConfig & 0x0F));
delay(15);
}
@@ -399,8 +409,11 @@ bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
{
switch (gyro->mpuDetectionResult.sensor) {
case ICM_42605_SPI:
- break;
case ICM_42688P_SPI:
+ gyro->scale = GYRO_SCALE_2000DPS;
+ break;
+ case IIM_42653_SPI:
+ gyro->scale = GYRO_SCALE_4000DPS;
break;
default:
return false;
@@ -409,8 +422,6 @@ bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
gyro->initFn = icm426xxGyroInit;
gyro->readFn = mpuGyroReadSPI;
- gyro->scale = GYRO_SCALE_2000DPS;
-
return true;
}
@@ -430,6 +441,7 @@ static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig
}
case ICM_42688P_SPI:
+ case IIM_42653_SPI:
default:
switch (config) {
case GYRO_HARDWARE_LPF_NORMAL:
@@ -448,4 +460,4 @@ static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig
}
}
-#endif // USE_GYRO_SPI_ICM42605 || USE_GYRO_SPI_ICM42688P
+#endif // USE_GYRO_SPI_ICM42605 || USE_GYRO_SPI_ICM42688P || USE_ACCGYRO_IIM42653
diff --git a/src/main/drivers/compass/compass_lis2mdl.c b/src/main/drivers/compass/compass_lis2mdl.c
index fb97536abd..80fb4b87ea 100644
--- a/src/main/drivers/compass/compass_lis2mdl.c
+++ b/src/main/drivers/compass/compass_lis2mdl.c
@@ -28,158 +28,103 @@
#if defined(USE_MAG_LIS2MDL)
+// LIS2MDL, IIS2MDC, LSM303AGR and LSM303AH are firmware and pin-to-pin compatible solutions
+// https://www.st.com/resource/en/design_tip/dt0131-digital-magnetometer-and-ecompass-efficient-design-tips--stmicroelectronics.pdf
+
#include "compass.h"
#include "drivers/time.h"
#include "common/axis.h"
-#define LIS2MDL_MAG_I2C_ADDRESS 0x1E
+#define LIS2MDL_MAG_I2C_ADDRESS 0x1E
-// Macros to encode/decode multi-bit values
-#define LIS2MDL_ENCODE_BITS(val, mask, shift) ((val << shift) & mask)
-#define LIS2MDL_DECODE_BITS(val, mask, shift) ((val & mask) >> shift)
+// LIS2MDL Registers
+#define LIS2MDL_ADDR_CFG_REG_A 0x60
+#define LIS2MDL_ADDR_CFG_REG_B 0x61
+#define LIS2MDL_ADDR_CFG_REG_C 0x62
+#define LIS2MDL_ADDR_STATUS_REG 0x67
+#define LIS2MDL_ADDR_OUTX_L_REG 0x68
+#define LIS2MDL_ADDR_WHO_AM_I 0x4F
-#define LIS2MDL_OFFSET_X_REG_L 0x45
-#define LIS2MDL_OFFSET_X_REG_H 0x46
-#define LIS2MDL_OFFSET_Y_REG_L 0x47
-#define LIS2MDL_OFFSET_Y_REG_H 0x48
-#define LIS2MDL_OFFSET_Z_REG_L 0x49
-#define LIS2MDL_OFFSET_Z_REG_H 0x4A
-
-#define LIS2MDL_REG_WHO_AM_I 0x4F
-#define LIS2MDL_DEVICE_ID 0x40
-
-#define LIS2MDL_CFG_REG_A 0x60
-#define LIS2MDL_CFG_REG_A_COMP_TEMP_EN 0x80
-#define LIS2MDL_CFG_REG_A_REBOOT 0x40
-#define LIS2MDL_CFG_REG_A_SOFT_RST 0x20
-#define LIS2MDL_CFG_REG_A_LP 0x10
-#define LIS2MDL_CFG_REG_A_ODR_MASK 0x0c
-#define LIS2MDL_CFG_REG_A_ODR_SHIFT 2
-#define LIS2MDL_CFG_REG_A_ODR_10 0
-#define LIS2MDL_CFG_REG_A_ODR_20 1
-#define LIS2MDL_CFG_REG_A_ODR_50 2
-#define LIS2MDL_CFG_REG_A_ODR_100 3
-#define LIS2MDL_CFG_REG_A_MD_MASK 0x03
-#define LIS2MDL_CFG_REG_A_MD_SHIFT 0
-#define LIS2MDL_CFG_REG_A_MD_CONT 0
-#define LIS2MDL_CFG_REG_A_MD_SINGLE 1
-#define LIS2MDL_CFG_REG_A_MD_IDLE 3
-
-#define LIS2MDL_CFG_REG_B 0x61
-#define LIS2MDL_CFG_REG_B_OFF_CANC_ONE_SHOT 0x10
-#define LIS2MDL_CFG_REG_B_INT_ON_DATA_OFF 0x08
-#define LIS2MDL_CFG_REG_B_SET_FREQ 0x04
-#define LIS2MDL_CFG_REG_B_OFF_CANC 0x02
-#define LIS2MDL_CFG_REG_B_LPF 0x01
-
-#define LIS2MDL_CFG_REG_C 0x62
-#define LIS2MDL_CFG_REG_C_INT_ON_PIN 0x40
-#define LIS2MDL_CFG_REG_C_I2C_DIS 0x20
-#define LIS2MDL_CFG_REG_C_BDU 0x10
-#define LIS2MDL_CFG_REG_C_BLE 0x08
-#define LIS2MDL_CFG_REG_C_4WSPI 0x04
-#define LIS2MDL_CFG_REG_C_SELF_TEST 0x02
-#define LIS2MDL_CFG_REG_C_DRDY_ON_PIN 0x01
-
-#define LIS2MDL_INT_CTRL_REG 0x63
-#define LIS2MDL_INT_CTRL_REG_XIEN 0x80
-#define LIS2MDL_INT_CTRL_REG_YIEN 0x40
-#define LIS2MDL_INT_CTRL_REG_ZIEN 0x20
-#define LIS2MDL_INT_CTRL_REG_IEA 0x04
-#define LIS2MDL_INT_CTRL_REG_IEL 0x02
-#define LIS2MDL_INT_CTRL_REG_IEN 0x01
-
-#define LIS2MDL_INT_SOURCE_REG 0x64
-#define LIS2MDL_INT_SOURCE_REG_P_TH_S_X 0x80
-#define LIS2MDL_INT_SOURCE_REG_P_TH_S_Y 0x40
-#define LIS2MDL_INT_SOURCE_REG_P_TH_S_Z 0x20
-#define LIS2MDL_INT_SOURCE_REG_N_TH_S_X 0x10
-#define LIS2MDL_INT_SOURCE_REG_N_TH_S_Y 0x08
-#define LIS2MDL_INT_SOURCE_REG_N_TH_S_Z 0x04
-#define LIS2MDL_INT_SOURCE_REG_MROI 0x02
-#define LIS2MDL_INT_SOURCE_REG_INT 0x01
-
-#define LIS2MDL_INT_THS_L_REG 0x65
-#define LIS2MDL_INT_THS_H_REG 0x66
-
-#define LIS2MDL_STATUS_REG 0x67
-#define LIS2MDL_STATUS_REG_ZXYOR 0x80
-#define LIS2MDL_STATUS_REG_ZOR 0x40
-#define LIS2MDL_STATUS_REG_YOR 0x20
-#define LIS2MDL_STATUS_REG_XOR 0x10
-#define LIS2MDL_STATUS_REG_ZXYDA 0x08
-#define LIS2MDL_STATUS_REG_ZDA 0x04
-#define LIS2MDL_STATUS_REG_YDA 0x02
-#define LIS2MDL_STATUS_REG_XDA 0x01
-
-#define LIS2MDL_OUTX_L_REG 0x68
-#define LIS2MDL_OUTX_H_REG 0x69
-#define LIS2MDL_OUTY_L_REG 0x6A
-#define LIS2MDL_OUTY_H_REG 0x6B
-#define LIS2MDL_OUTZ_L_REG 0x6C
-#define LIS2MDL_OUTZ_H_REG 0x6D
-
-#define LIS2MDL_TEMP_OUT_L_REG 0x6E
-#define LIS2MDL_TEMP_OUT_H_REG 0x6F
-
-static bool lis2mdlRead(magDev_t * mag, int16_t *magData)
-{
- static uint8_t buf[6];
- static bool pendingRead = true;
-
- extDevice_t *dev = &mag->dev;
-
- if (pendingRead) {
- if (busReadRegisterBufferStart(dev, LIS2MDL_OUTX_L_REG, buf, sizeof(buf))) {
- pendingRead = false;
- }
- return false;
- }
-
- magData[X] = (int16_t)(buf[1] << 8 | buf[0]);
- magData[Y] = (int16_t)(buf[3] << 8 | buf[2]);
- magData[Z] = (int16_t)(buf[5] << 8 | buf[4]);
-
- pendingRead = true;
-
- return true;
-}
+// LIS2MDL Definitions
+#define LIS2MDL_WHO_AM_I 0x40
+#define LIS2MDL_STATUS_REG_READY 0x0F
+#define CFGA_MD_CONTINUOUS (0 << 0)
+#define CFGA_ODR_100 ((1 << 3) | (1 << 2))
+#define CFGA_COMP_TEMP_EN (1 << 7)
+#define CFGB_OFF_CANC (1 << 1)
+#define CFGC_BDU (1 << 4)
static bool lis2mdlInit(magDev_t *mag)
{
+ bool ack = true;
extDevice_t *dev = &mag->dev;
busDeviceRegister(dev);
- busWriteRegister(dev, LIS2MDL_CFG_REG_A,
- LIS2MDL_CFG_REG_A_COMP_TEMP_EN |
- LIS2MDL_ENCODE_BITS(LIS2MDL_CFG_REG_A_ODR_100, LIS2MDL_CFG_REG_A_ODR_MASK, LIS2MDL_CFG_REG_A_ODR_SHIFT) |
- LIS2MDL_ENCODE_BITS(LIS2MDL_CFG_REG_A_MD_CONT, LIS2MDL_CFG_REG_A_MD_MASK, LIS2MDL_CFG_REG_A_MD_SHIFT));
+ ack = ack && busWriteRegister(dev, LIS2MDL_ADDR_CFG_REG_A, CFGA_MD_CONTINUOUS | CFGA_ODR_100 | CFGA_COMP_TEMP_EN);
+ ack = ack && busWriteRegister(dev, LIS2MDL_ADDR_CFG_REG_B, CFGB_OFF_CANC);
+ ack = ack && busWriteRegister(dev, LIS2MDL_ADDR_CFG_REG_C, CFGC_BDU);
- delay(100);
+ if (!ack) {
+ return false;
+ }
+
+ mag->magOdrHz = 100;
+ return true;
+}
+
+static bool lis2mdlRead(magDev_t *mag, int16_t *magData)
+{
+ uint8_t status = 0;
+ uint8_t buf[6];
+
+ 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))) {
+ return false;
+ }
+
+ // Sensitivity is +/- 50,000 milligauss, 16bit
+ // e.g. gauss = val * (100.f / 65.535f)
+
+ int16_t x = (int16_t)(buf[1] << 8 | buf[0]);
+ int16_t y = (int16_t)(buf[3] << 8 | buf[2]);
+ int16_t z = (int16_t)(buf[5] << 8 | buf[4]);
+
+ // adapt LIS2MDL left-handed frame to common sensor axis orientation (match LIS3MDL)
+ // pin 1 mark becomes +X -Y
+ magData[X] = -x;
+ magData[Y] = y;
+ magData[Z] = z;
return true;
}
-bool lis2mdlDetect(magDev_t * mag)
+bool lis2mdlDetect(magDev_t *mag)
{
extDevice_t *dev = &mag->dev;
- uint8_t sig = 0;
-
if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
dev->busType_u.i2c.address = LIS2MDL_MAG_I2C_ADDRESS;
}
- bool ack = busReadRegisterBuffer(&mag->dev, LIS2MDL_REG_WHO_AM_I, &sig, 1);
+ uint8_t whoami;
+ bool ack = busReadRegisterBuffer(dev, LIS2MDL_ADDR_WHO_AM_I, &whoami, sizeof(whoami));
- if (!ack || sig != LIS2MDL_DEVICE_ID) {
- return false;
+ if (ack && whoami == LIS2MDL_WHO_AM_I) {
+ mag->init = lis2mdlInit;
+ mag->read = lis2mdlRead;
+ return true;
}
- mag->init = lis2mdlInit;
- mag->read = lis2mdlRead;
-
- return true;
+ return false;
}
#endif
diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index d9b90bf387..fc2ce74709 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -93,6 +93,12 @@ int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, co
{
instance->posX = x + strlen(text);
instance->posY = y;
+
+ if (strlen(text) == 0) {
+ // No point sending a message to do nothing
+ return 0;
+ }
+
return instance->vTable->writeString(instance, x, y, attr, text);
}
diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c
index ea772a6cb0..6fae5d7e59 100644
--- a/src/main/drivers/serial.c
+++ b/src/main/drivers/serial.c
@@ -76,7 +76,10 @@ uint8_t serialRead(serialPort_t *instance)
void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{
- instance->vTable->serialSetBaudRate(instance, baudRate);
+ //vTable->serialSetBaudRate is NULL for SIMULATOR_BUILD, because the TCP port is used
+ if (instance->vTable->serialSetBaudRate != NULL) {
+ instance->vTable->serialSetBaudRate(instance, baudRate);
+ }
}
bool isSerialTransmitBufferEmpty(const serialPort_t *instance)
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index 4e99137a6e..fb5c5291e3 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -490,7 +490,7 @@ UART_IRQHandler(UART, 9, UARTDEV_9) // UART9 Rx/Tx IRQ Handler
#endif
#ifdef USE_UART10
-UART_IRQHandler(UART, 10, UARTDEV_10) // UART10 Rx/Tx IRQ Handler
+UART_IRQHandler(USART, 10, UARTDEV_10) // UART10 Rx/Tx IRQ Handler
#endif
#ifdef USE_LPUART1
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 889204938b..75bd3100d0 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -1092,6 +1092,16 @@ void processRxModes(timeUs_t currentTimeUs)
}
#endif
+#ifdef USE_CHIRP
+ if (IS_RC_MODE_ACTIVE(BOXCHIRP) && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(GPS_RESCUE_MODE)) {
+ if (!FLIGHT_MODE(CHIRP_MODE)) {
+ ENABLE_FLIGHT_MODE(CHIRP_MODE);
+ }
+ } else {
+ DISABLE_FLIGHT_MODE(CHIRP_MODE);
+ }
+#endif
+
if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE | HORIZON_MODE)) {
LED1_ON;
// increase frequency of attitude task to reduce drift when in angle or horizon mode
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index e6e90800fe..becf43fa18 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -278,15 +278,15 @@ void init(void)
// initialize IO (needed for all IO operations)
IOInitGlobal();
-#ifdef USE_HARDWARE_REVISION_DETECTION
- detectHardwareRevision();
-#endif
-
#if defined(USE_TARGET_CONFIG)
// Call once before the config is loaded for any target specific configuration required to support loading the config
targetConfiguration();
#endif
+#if defined(USE_CONFIG_TARGET_PREINIT)
+ configTargetPreInit();
+#endif
+
enum {
FLASH_INIT_ATTEMPTED = (1 << 0),
SD_INIT_ATTEMPTED = (1 << 1),
diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h
index 43624e06ea..e677b6fe26 100644
--- a/src/main/fc/parameter_names.h
+++ b/src/main/fc/parameter_names.h
@@ -192,6 +192,15 @@
#define PARAM_NAME_HORIZON_IGNORE_STICKS "horizon_ignore_sticks"
#define PARAM_NAME_HORIZON_DELAY_MS "horizon_delay_ms"
+#define PARAM_NAME_CHIRP_LAG_FREQ_HZ "chirp_lag_freq_hz"
+#define PARAM_NAME_CHIRP_LEAD_FREQ_HZ "chirp_lead_freq_hz"
+#define PARAM_NAME_CHIRP_AMPLITUDE_ROLL "chirp_amplitude_roll"
+#define PARAM_NAME_CHIRP_AMPLITUDE_PITCH "chirp_amplitude_pitch"
+#define PARAM_NAME_CHIRP_AMPLITUDE_YAW "chirp_amplitude_yaw"
+#define PARAM_NAME_CHIRP_FREQUENCY_START_DECI_HZ "chirp_frequency_start_deci_hz"
+#define PARAM_NAME_CHIRP_FREQUENCY_END_DECI_HZ "chirp_frequency_end_deci_hz"
+#define PARAM_NAME_CHIRP_TIME_SECONDS "chirp_time_seconds"
+
#ifdef USE_GPS
#define PARAM_NAME_GPS_PROVIDER "gps_provider"
#define PARAM_NAME_GPS_SBAS_MODE "gps_sbas_mode"
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 04bd4f2f3a..13838c1c23 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -35,6 +35,7 @@ typedef enum {
BOXMAG,
BOXALTHOLD,
BOXHEADFREE,
+ BOXCHIRP,
BOXPASSTHRU,
BOXFAILSAFE,
BOXPOSHOLD,
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index 9d3512ca41..a1df79b426 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -87,7 +87,7 @@ typedef enum {
// GPS_HOME_MODE = (1 << 4),
POS_HOLD_MODE = (1 << 5),
HEADFREE_MODE = (1 << 6),
-// UNUSED_MODE = (1 << 7), // old autotune
+ CHIRP_MODE = (1 << 7), // old autotune
PASSTHRU_MODE = (1 << 8),
// RANGEFINDER_MODE= (1 << 9),
FAILSAFE_MODE = (1 << 10),
@@ -109,6 +109,7 @@ extern uint16_t flightModeFlags;
[BOXALTHOLD] = LOG2(ALT_HOLD_MODE), \
[BOXPOSHOLD] = LOG2(POS_HOLD_MODE), \
[BOXHEADFREE] = LOG2(HEADFREE_MODE), \
+ [BOXCHIRP] = LOG2(CHIRP_MODE), \
[BOXPASSTHRU] = LOG2(PASSTHRU_MODE), \
[BOXFAILSAFE] = LOG2(FAILSAFE_MODE), \
[BOXGPSRESCUE] = LOG2(GPS_RESCUE_MODE), \
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 014052cf56..894658f368 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -603,8 +603,14 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
UNUSED(canUseGPSHeading);
UNUSED(imuCalcKpGain);
UNUSED(imuCalcMagErr);
-
UNUSED(currentTimeUs);
+
+#if defined(USE_GPS)
+ UNUSED(imuComputeQuaternionFromRPY);
+ UNUSED(imuDebug_GPS_RESCUE_HEADING);
+ UNUSED(imuCalcCourseErr);
+ UNUSED(imuCalcGroundspeedGain);
+#endif
}
#else
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index b0fa8fc43b..071c1feaf2 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -260,6 +260,14 @@ void resetPidProfile(pidProfile_t *pidProfile)
.tpa_speed_pitch_offset = 0,
.yaw_type = YAW_TYPE_RUDDER,
.angle_pitch_offset = 0,
+ .chirp_lag_freq_hz = 3,
+ .chirp_lead_freq_hz = 30,
+ .chirp_amplitude_roll = 230,
+ .chirp_amplitude_pitch = 230,
+ .chirp_amplitude_yaw = 180,
+ .chirp_frequency_start_deci_hz = 2,
+ .chirp_frequency_end_deci_hz = 6000,
+ .chirp_time_seconds = 20,
);
}
@@ -1200,9 +1208,49 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
disarmOnImpact();
}
+#ifdef USE_CHIRP
+
+ static int chirpAxis = 0;
+ static bool shouldChirpAxisToggle = false;
+
+ float chirp = 0.0f;
+ float sinarg = 0.0f;
+ if (FLIGHT_MODE(CHIRP_MODE)) {
+ shouldChirpAxisToggle = true; // advance chirp axis on next !CHIRP_MODE
+ // update chirp signal
+ if (chirpUpdate(&pidRuntime.chirp)) {
+ chirp = pidRuntime.chirp.exc;
+ sinarg = pidRuntime.chirp.sinarg;
+ }
+ } else {
+ if (shouldChirpAxisToggle) {
+ // toggle chirp signal logic and increment to next axis for next run
+ shouldChirpAxisToggle = false;
+ chirpAxis = (++chirpAxis > FD_YAW) ? 0 : chirpAxis;
+ // reset chirp signal generator
+ chirpReset(&pidRuntime.chirp);
+ }
+ }
+
+ // input / excitation shaping
+ float chirpFiltered = phaseCompApply(&pidRuntime.chirpFilter, chirp);
+
+ // ToDo: check if this can be reconstructed offline for rotating filter and if so, remove the debug
+ // fit (0...2*pi) into int16_t (-32768 to 32767)
+ DEBUG_SET(DEBUG_CHIRP, 0, lrintf(5.0e3f * sinarg));
+
+#endif // USE_CHIRP
+
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
+#ifdef USE_CHIRP
+ float currentChirp = 0.0f;
+ if(axis == chirpAxis){
+ currentChirp = pidRuntime.chirpAmplitude[axis] * chirpFiltered;
+ }
+#endif // USE_CHIRP
+
float currentPidSetpoint = getSetpointRate(axis);
if (pidRuntime.maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
@@ -1263,6 +1311,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
+#ifdef USE_CHIRP
+ currentPidSetpoint += currentChirp;
+#endif // USE_CHIRP
float errorRate = currentPidSetpoint - gyroRate; // r - y
#if defined(USE_ACC)
handleCrashRecovery(
@@ -1596,3 +1647,10 @@ float pidGetPidFrequency(void)
{
return pidRuntime.pidFrequency;
}
+
+#ifdef USE_CHIRP
+bool pidChirpIsFinished(void)
+{
+ return pidRuntime.chirp.isFinished;
+}
+#endif
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index f1d077bf6b..8c217dd03c 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -23,6 +23,7 @@
#include
#include "common/axis.h"
+#include "common/chirp.h"
#include "common/filter.h"
#include "common/pwl.h"
#include "common/time.h"
@@ -322,6 +323,15 @@ typedef struct pidProfile_s {
int16_t tpa_speed_pitch_offset; // For wings: pitch offset in degrees*10 for craft speed estimation
uint8_t yaw_type; // For wings: type of yaw (rudder or differential thrust)
int16_t angle_pitch_offset; // For wings: pitch offset for angle modes; in decidegrees; positive values tilting the wing down
+
+ uint8_t chirp_lag_freq_hz; // leadlag1Filter cutoff/pole to shape the excitation signal
+ uint8_t chirp_lead_freq_hz; // leadlag1Filter cutoff/zero
+ uint16_t chirp_amplitude_roll; // amplitude roll in degree/second
+ uint16_t chirp_amplitude_pitch; // amplitude pitch in degree/second
+ uint16_t chirp_amplitude_yaw; // amplitude yaw in degree/second
+ uint16_t chirp_frequency_start_deci_hz; // start frequency in units of 0.1 hz
+ uint16_t chirp_frequency_end_deci_hz; // end frequency in units of 0.1 hz
+ uint8_t chirp_time_seconds; // excitation time
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -529,6 +539,17 @@ typedef struct pidRuntime_s {
float tpaCurvePwl_yValues[TPA_CURVE_PWL_SIZE];
tpaCurveType_t tpaCurveType;
#endif // USE_ADVANCED_TPA
+
+#ifdef USE_CHIRP
+ chirp_t chirp;
+ phaseComp_t chirpFilter;
+ float chirpLagFreqHz;
+ float chirpLeadFreqHz;
+ float chirpAmplitude[3];
+ float chirpFrequencyStartHz;
+ float chirpFrequencyEndHz;
+ float chirpTimeSeconds;
+#endif // USE_CHIRP
} pidRuntime_t;
extern pidRuntime_t pidRuntime;
@@ -585,3 +606,6 @@ float pidGetDT();
float pidGetPidFrequency();
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);
+#ifdef USE_CHIRP
+bool pidChirpIsFinished();
+#endif
diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c
index e8ab4ab91f..d8dc0484a2 100644
--- a/src/main/flight/pid_init.c
+++ b/src/main/flight/pid_init.c
@@ -317,6 +317,14 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pidRuntime.angleYawSetpoint = 0.0f;
#endif
+#ifdef USE_CHIRP
+ const float alpha = pidRuntime.chirpLeadFreqHz / pidRuntime.chirpLagFreqHz;
+ const float centerFreqHz = pidRuntime.chirpLagFreqHz * sqrtf(alpha);
+ const float centerPhaseDeg = asinf( (1.0f - alpha) / (1.0f + alpha) ) / RAD;
+ phaseCompInit(&pidRuntime.chirpFilter, centerFreqHz, centerPhaseDeg, targetPidLooptime);
+ chirpInit(&pidRuntime.chirp, pidRuntime.chirpFrequencyStartHz, pidRuntime.chirpFrequencyEndHz, pidRuntime.chirpTimeSeconds, targetPidLooptime);
+#endif
+
pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT));
#ifdef USE_WING
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -411,6 +419,17 @@ void pidInitConfig(const pidProfile_t *pidProfile)
pidRuntime.horizonDelayMs = pidProfile->horizon_delay_ms;
#endif
+#ifdef USE_CHIRP
+ pidRuntime.chirpLagFreqHz = pidProfile->chirp_lag_freq_hz;
+ pidRuntime.chirpLeadFreqHz = pidProfile->chirp_lead_freq_hz;
+ pidRuntime.chirpAmplitude[FD_ROLL] = pidProfile->chirp_amplitude_roll;
+ pidRuntime.chirpAmplitude[FD_PITCH] = pidProfile->chirp_amplitude_pitch;
+ pidRuntime.chirpAmplitude[FD_YAW]= pidProfile->chirp_amplitude_yaw;
+ pidRuntime.chirpFrequencyStartHz = pidProfile->chirp_frequency_start_deci_hz / 10.0f;
+ pidRuntime.chirpFrequencyEndHz = pidProfile->chirp_frequency_end_deci_hz / 10.0f;
+ pidRuntime.chirpTimeSeconds = pidProfile->chirp_time_seconds;
+#endif
+
pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.antiGravityGain = pidProfile->anti_gravity_gain;
diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c
index feb71ec1b3..a95cab2555 100644
--- a/src/main/flight/rpm_filter.c
+++ b/src/main/flight/rpm_filter.c
@@ -121,6 +121,9 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
return;
}
+ const float dtCompensation = schedulerGetCycleTimeMultiplier();
+ const float correctedLooptime = rpmFilter.looptimeUs * dtCompensation;
+
// update RPM notches
for (int i = 0; i < notchUpdatesPerIteration; i++) {
@@ -143,7 +146,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
weight *= rpmFilter.weights[harmonicIndex];
// update notch
- biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight);
+ biquadFilterUpdate(template, frequencyHz, correctedLooptime, rpmFilter.q, FILTER_NOTCH, weight);
// copy notch properties to corresponding notches on PITCH and YAW
for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 29a80ec3d4..4584360d07 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -45,7 +45,10 @@
#ifdef USE_DASHBOARD
#include "io/dashboard.h"
#endif
+
#include "io/gps.h"
+#include "io/gps_virtual.h"
+
#include "io/serial.h"
#include "config/config.h"
@@ -407,7 +410,7 @@ void gpsInit(void)
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
gpsSetState(GPS_STATE_UNKNOWN);
- if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured
+ if (gpsConfig()->provider == GPS_MSP || gpsConfig()->provider == GPS_VIRTUAL) { // no serial ports used when GPS_MSP or GPS_VIRTUAL is configured
gpsSetState(GPS_STATE_INITIALIZED);
return;
}
@@ -1337,6 +1340,38 @@ static void calculateNavInterval (void)
gpsSol.navIntervalMs = constrain(navDeltaTimeMs, 50, 2500);
}
+#if defined(USE_VIRTUAL_GPS)
+static void updateVirtualGPS(void)
+{
+ const uint32_t updateInterval = 100; // 100ms 10Hz update time interval
+ static uint32_t nextUpdateTime = 0;
+
+ if (cmp32(gpsData.now, nextUpdateTime) > 0) {
+ if (gpsData.state == GPS_STATE_INITIALIZED) {
+ gpsSetState(GPS_STATE_RECEIVING_DATA);
+ }
+
+ getVirtualGPS(&gpsSol);
+ gpsSol.time = gpsData.now;
+
+ gpsData.lastNavMessage = gpsData.now;
+ sensorsSet(SENSOR_GPS);
+
+ if (gpsSol.numSat > 3) {
+ gpsSetFixState(GPS_FIX);
+ } else {
+ gpsSetFixState(0);
+ }
+ GPS_update ^= GPS_DIRECT_TICK;
+
+ calculateNavInterval();
+ onGpsNewData();
+
+ nextUpdateTime = gpsData.now + updateInterval;
+ }
+}
+#endif
+
void gpsUpdate(timeUs_t currentTimeUs)
{
static timeDelta_t gpsStateDurationFractionUs[GPS_STATE_COUNT];
@@ -1344,7 +1379,12 @@ void gpsUpdate(timeUs_t currentTimeUs)
gpsState_e gpsCurrentState = gpsData.state;
gpsData.now = millis();
- if (gpsPort) {
+ switch (gpsConfig()->provider) {
+ case GPS_UBLOX:
+ case GPS_NMEA:
+ if (!gpsPort) {
+ break;
+ }
DEBUG_SET(DEBUG_GPS_CONNECTION, 7, serialRxBytesWaiting(gpsPort));
static uint8_t wait = 0;
static bool isFast = false;
@@ -1368,7 +1408,9 @@ void gpsUpdate(timeUs_t currentTimeUs)
isFast = false;
rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE));
}
- } else if (gpsConfig()->provider == GPS_MSP) {
+ break;
+
+ case GPS_MSP:
if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
if (gpsData.state == GPS_STATE_INITIALIZED) {
gpsSetState(GPS_STATE_RECEIVING_DATA);
@@ -1391,45 +1433,51 @@ void gpsUpdate(timeUs_t currentTimeUs)
gpsSetState(GPS_STATE_LOST_COMMUNICATION);
}
}
+ break;
+#if defined(USE_VIRTUAL_GPS)
+ case GPS_VIRTUAL:
+ updateVirtualGPS();
+ break;
+#endif
}
switch (gpsData.state) {
- case GPS_STATE_UNKNOWN:
- case GPS_STATE_INITIALIZED:
- break;
+ case GPS_STATE_UNKNOWN:
+ case GPS_STATE_INITIALIZED:
+ break;
- case GPS_STATE_DETECT_BAUD:
- case GPS_STATE_CHANGE_BAUD:
- case GPS_STATE_CONFIGURE:
- gpsConfigureHardware();
- break;
+ case GPS_STATE_DETECT_BAUD:
+ case GPS_STATE_CHANGE_BAUD:
+ case GPS_STATE_CONFIGURE:
+ gpsConfigureHardware();
+ break;
- case GPS_STATE_LOST_COMMUNICATION:
- gpsData.timeouts++;
- // previously we would attempt a different baud rate here if gps auto-baud was enabled. that code has been removed.
- gpsSol.numSat = 0;
- DISABLE_STATE(GPS_FIX);
- gpsSetState(GPS_STATE_DETECT_BAUD);
- break;
+ case GPS_STATE_LOST_COMMUNICATION:
+ gpsData.timeouts++;
+ // previously we would attempt a different baud rate here if gps auto-baud was enabled. that code has been removed.
+ gpsSol.numSat = 0;
+ DISABLE_STATE(GPS_FIX);
+ gpsSetState(GPS_STATE_DETECT_BAUD);
+ break;
- case GPS_STATE_RECEIVING_DATA:
+ case GPS_STATE_RECEIVING_DATA:
#ifdef USE_GPS_UBLOX
- if (gpsConfig()->provider != GPS_MSP) {
- if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) {
- // when we are connected up, and get a 3D fix, enable the 'flight' fix model
- if (!gpsData.ubloxUsingFlightModel && STATE(GPS_FIX)) {
- gpsData.ubloxUsingFlightModel = true;
- ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model);
- }
+ if (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_NMEA) { // TODO Send ublox message to nmea GPS?
+ if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) {
+ // when we are connected up, and get a 3D fix, enable the 'flight' fix model
+ if (!gpsData.ubloxUsingFlightModel && STATE(GPS_FIX)) {
+ gpsData.ubloxUsingFlightModel = true;
+ ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model);
}
}
+ }
#endif
- DEBUG_SET(DEBUG_GPS_CONNECTION, 2, gpsData.now - gpsData.lastNavMessage); // time since last Nav data, updated each GPS task interval
- // check for no data/gps timeout/cable disconnection etc
- if (cmp32(gpsData.now, gpsData.lastNavMessage) > GPS_TIMEOUT_MS) {
- gpsSetState(GPS_STATE_LOST_COMMUNICATION);
- }
- break;
+ DEBUG_SET(DEBUG_GPS_CONNECTION, 2, gpsData.now - gpsData.lastNavMessage); // time since last Nav data, updated each GPS task interval
+ // check for no data/gps timeout/cable disconnection etc
+ if (cmp32(gpsData.now, gpsData.lastNavMessage) > GPS_TIMEOUT_MS) {
+ gpsSetState(GPS_STATE_LOST_COMMUNICATION);
+ }
+ break;
}
DEBUG_SET(DEBUG_GPS_CONNECTION, 4, (gpsData.state * 100 + gpsData.state_position));
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index c80b9e8f46..fb88128900 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -177,7 +177,8 @@ typedef enum {
typedef enum {
GPS_NMEA = 0,
GPS_UBLOX,
- GPS_MSP
+ GPS_MSP,
+ GPS_VIRTUAL,
} gpsProvider_e;
typedef enum {
diff --git a/src/main/io/gps_virtual.c b/src/main/io/gps_virtual.c
new file mode 100644
index 0000000000..3f65fca8f2
--- /dev/null
+++ b/src/main/io/gps_virtual.c
@@ -0,0 +1,52 @@
+/*
+ * 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 .
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_VIRTUAL_GPS
+
+#include "io/gps_virtual.h"
+
+static gpsSolutionData_t gpsVirtualData;
+
+void setVirtualGPS(double latitude, double longitude, double altiutude, double speed, double speed3D, double course)
+{
+ gpsVirtualData.numSat = 12; // satellites_in_view
+ gpsVirtualData.acc.hAcc = 500; // horizontal_pos_accuracy - convert cm to mm
+ gpsVirtualData.acc.vAcc = 500; // vertical_pos_accuracy - convert cm to mm
+ gpsVirtualData.acc.sAcc = 400; // horizontal_vel_accuracy - convert cm to mm
+ gpsVirtualData.dop.pdop = 10; // hdop in 4.4 and earlier, pdop in 4.5 and above
+ gpsVirtualData.llh.lon = (int32_t)(longitude * GPS_DEGREES_DIVIDER);
+ gpsVirtualData.llh.lat = (int32_t)(latitude * GPS_DEGREES_DIVIDER);
+ gpsVirtualData.llh.altCm = (int32_t)(altiutude * 100.0); // alt, cm
+ gpsVirtualData.groundSpeed = (uint16_t)(speed * 100.0); // cm/sec
+ gpsVirtualData.speed3d = (uint16_t)(speed3D * 100.0); // cm/sec
+ gpsVirtualData.groundCourse = (uint16_t)(course * 10.0); // decidegrees
+}
+
+void getVirtualGPS(gpsSolutionData_t *gpsSolData)
+{
+ *gpsSolData = gpsVirtualData;
+}
+#endif
diff --git a/src/main/io/gps_virtual.h b/src/main/io/gps_virtual.h
new file mode 100644
index 0000000000..6f53b7c4a5
--- /dev/null
+++ b/src/main/io/gps_virtual.h
@@ -0,0 +1,26 @@
+/*
+ * 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 "io/gps.h"
+void setVirtualGPS(double latitude, double longitude, double altiutude, double velocity, double velocity3D, double course);
+void getVirtualGPS(gpsSolutionData_t *gpsSolData);
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 891a5cf7da..4b83a4708c 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -409,8 +409,7 @@ void mspReboot(dispatchEntry_t* self)
mspRebootFn(NULL);
}
-dispatchEntry_t mspRebootEntry =
-{
+dispatchEntry_t mspRebootEntry = {
mspReboot, 0, NULL, false
};
@@ -433,8 +432,7 @@ void writeReadEeprom(dispatchEntry_t* self)
#endif
}
-dispatchEntry_t writeReadEepromEntry =
-{
+dispatchEntry_t writeReadEepromEntry = {
writeReadEeprom, 0, NULL, false
};
@@ -643,8 +641,13 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
break;
- case MSP_BOARD_INFO:
- {
+ case MSP2_MCU_INFO: {
+ sbufWriteU8(dst, getMcuTypeId());
+ sbufWritePString(dst, getMcuTypeName());
+ break;
+ }
+
+ case MSP_BOARD_INFO: {
sbufWriteData(dst, systemConfig()->boardIdentifier, BOARD_IDENTIFIER_LENGTH);
#ifdef USE_HARDWARE_REVISION_DETECTION
sbufWriteU16(dst, hardwareRevision);
@@ -682,19 +685,14 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
sbufWriteU8(dst, targetCapabilities);
// Target name with explicit length
- sbufWriteU8(dst, strlen(targetName));
- sbufWriteData(dst, targetName, strlen(targetName));
+ sbufWritePString(dst, targetName);
#if defined(USE_BOARD_INFO)
// Board name with explicit length
- char *value = getBoardName();
- sbufWriteU8(dst, strlen(value));
- sbufWriteString(dst, value);
+ sbufWritePString(dst, getBoardName());
// Manufacturer id with explicit length
- value = getManufacturerId();
- sbufWriteU8(dst, strlen(value));
- sbufWriteString(dst, value);
+ sbufWritePString(dst, getManufacturerId());
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
@@ -845,28 +843,27 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
break;
}
- case MSP_VOLTAGE_METER_CONFIG:
- {
- // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
- // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has
- // different configuration requirements.
- STATIC_ASSERT(VOLTAGE_SENSOR_ADC_VBAT == 0, VOLTAGE_SENSOR_ADC_VBAT_incorrect); // VOLTAGE_SENSOR_ADC_VBAT should be the first index
- sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload
- for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
- const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes
- sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length
+ case MSP_VOLTAGE_METER_CONFIG: {
+ // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
+ // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has
+ // different configuration requirements.
+ STATIC_ASSERT(VOLTAGE_SENSOR_ADC_VBAT == 0, VOLTAGE_SENSOR_ADC_VBAT_incorrect); // VOLTAGE_SENSOR_ADC_VBAT should be the first index
+ sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload
+ for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
+ const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes
+ sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length
- sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor
- sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for
+ sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor
+ sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for
- sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale);
- sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval);
- sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier);
- }
- // if we had any other voltage sensors, this is where we would output any needed configuration
+ sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale);
+ sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval);
+ sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier);
}
-
+ // if we had any other voltage sensors, this is where we would output any needed configuration
break;
+ }
+
case MSP_CURRENT_METER_CONFIG: {
// the ADC and VIRTUAL sensors have the same configuration requirements, however this API reflects
// that this situation may change and allows us to support configuration of any current sensor with
@@ -1085,78 +1082,75 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
switch (cmdMSP) {
case MSP_STATUS_EX:
- case MSP_STATUS:
- {
- boxBitmask_t flightModeFlags;
- const int flagBits = packFlightModeFlags(&flightModeFlags);
+ case MSP_STATUS: {
+ boxBitmask_t flightModeFlags;
+ const int flagBits = packFlightModeFlags(&flightModeFlags);
- sbufWriteU16(dst, getTaskDeltaTimeUs(TASK_PID));
+ sbufWriteU16(dst, getTaskDeltaTimeUs(TASK_PID));
#ifdef USE_I2C
- sbufWriteU16(dst, i2cGetErrorCounter());
+ sbufWriteU16(dst, i2cGetErrorCounter());
#else
- sbufWriteU16(dst, 0);
+ 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);
- sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits
- sbufWriteU8(dst, getCurrentPidProfileIndex());
- sbufWriteU16(dst, constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE));
- if (cmdMSP == MSP_STATUS_EX) {
- sbufWriteU8(dst, PID_PROFILE_COUNT);
- sbufWriteU8(dst, getCurrentControlRateProfileIndex());
- } else { // MSP_STATUS
- sbufWriteU16(dst, 0); // gyro cycle time
- }
+ 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);
+ sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits
+ sbufWriteU8(dst, getCurrentPidProfileIndex());
+ sbufWriteU16(dst, constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE));
+ if (cmdMSP == MSP_STATUS_EX) {
+ sbufWriteU8(dst, PID_PROFILE_COUNT);
+ sbufWriteU8(dst, getCurrentControlRateProfileIndex());
+ } else { // MSP_STATUS
+ sbufWriteU16(dst, 0); // gyro cycle time
+ }
- // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
- // header is emited even when all bits fit into 32 bits to allow future extension
- int byteCount = (flagBits - 32 + 7) / 8; // 32 already stored, round up
- byteCount = constrain(byteCount, 0, 15); // limit to 16 bytes (128 bits)
- sbufWriteU8(dst, byteCount);
- sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount);
+ // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
+ // header is emited even when all bits fit into 32 bits to allow future extension
+ int byteCount = (flagBits - 32 + 7) / 8; // 32 already stored, round up
+ byteCount = constrain(byteCount, 0, 15); // limit to 16 bytes (128 bits)
+ sbufWriteU8(dst, byteCount);
+ sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount);
- // Write arming disable flags
- // 1 byte, flag count
- sbufWriteU8(dst, ARMING_DISABLE_FLAGS_COUNT);
- // 4 bytes, flags
- const uint32_t armingDisableFlags = getArmingDisableFlags();
- sbufWriteU32(dst, armingDisableFlags);
+ // Write arming disable flags
+ // 1 byte, flag count
+ sbufWriteU8(dst, ARMING_DISABLE_FLAGS_COUNT);
+ // 4 bytes, flags
+ const uint32_t armingDisableFlags = getArmingDisableFlags();
+ sbufWriteU32(dst, armingDisableFlags);
- // config state flags - bits to indicate the state of the configuration, reboot required, etc.
- // other flags can be added as needed
- sbufWriteU8(dst, (getRebootRequired() << 0));
+ // config state flags - bits to indicate the state of the configuration, reboot required, etc.
+ // other flags can be added as needed
+ sbufWriteU8(dst, (getRebootRequired() << 0));
- // Added in API version 1.46
- // Write CPU temp
+ // Added in API version 1.46
+ // Write CPU temp
#ifdef USE_ADC_INTERNAL
- sbufWriteU16(dst, getCoreTemperatureCelsius());
+ sbufWriteU16(dst, getCoreTemperatureCelsius());
+#else
+ sbufWriteU16(dst, 0);
+#endif
+ break;
+ }
+
+ case MSP_RAW_IMU: {
+ for (int i = 0; i < 3; i++) {
+#if defined(USE_ACC)
+ sbufWriteU16(dst, lrintf(acc.accADC.v[i]));
+#else
+ sbufWriteU16(dst, 0);
+#endif
+ }
+ for (int i = 0; i < 3; i++) {
+ sbufWriteU16(dst, gyroRateDps(i));
+ }
+ for (int i = 0; i < 3; i++) {
+#if defined(USE_MAG)
+ sbufWriteU16(dst, lrintf(mag.magADC.v[i]));
#else
sbufWriteU16(dst, 0);
#endif
}
break;
-
- case MSP_RAW_IMU:
- {
-
- for (int i = 0; i < 3; i++) {
-#if defined(USE_ACC)
- sbufWriteU16(dst, lrintf(acc.accADC.v[i]));
-#else
- sbufWriteU16(dst, 0);
-#endif
- }
- for (int i = 0; i < 3; i++) {
- sbufWriteU16(dst, gyroRateDps(i));
- }
- for (int i = 0; i < 3; i++) {
-#if defined(USE_MAG)
- sbufWriteU16(dst, lrintf(mag.magADC.v[i]));
-#else
- sbufWriteU16(dst, 0);
-#endif
- }
- }
- break;
+ }
case MSP_NAME:
sbufWriteString(dst, pilotConfig()->craftName);
@@ -1203,7 +1197,6 @@ case MSP_NAME:
sbufWriteU16(dst, 0);
#endif
}
-
break;
// Added in API version 1.42
@@ -1285,32 +1278,29 @@ case MSP_NAME:
break;
#ifdef USE_VTX_COMMON
- case MSP2_GET_VTX_DEVICE_STATUS:
- {
- const vtxDevice_t *vtxDevice = vtxCommonDevice();
- vtxCommonSerializeDeviceStatus(vtxDevice, dst);
- }
+ case MSP2_GET_VTX_DEVICE_STATUS: {
+ const vtxDevice_t *vtxDevice = vtxCommonDevice();
+ vtxCommonSerializeDeviceStatus(vtxDevice, dst);
break;
+ }
#endif
#ifdef USE_OSD
- case MSP2_GET_OSD_WARNINGS:
- {
- bool isBlinking;
- uint8_t displayAttr;
- char warningsBuffer[OSD_WARNINGS_MAX_SIZE + 1];
+ case MSP2_GET_OSD_WARNINGS: {
+ bool isBlinking;
+ uint8_t displayAttr;
+ char warningsBuffer[OSD_WARNINGS_MAX_SIZE + 1];
- renderOsdWarning(warningsBuffer, &isBlinking, &displayAttr);
- const uint8_t warningsLen = strlen(warningsBuffer);
+ renderOsdWarning(warningsBuffer, &isBlinking, &displayAttr);
- if (isBlinking) {
- displayAttr |= DISPLAYPORT_BLINK;
- }
- sbufWriteU8(dst, displayAttr); // see displayPortSeverity_e
- sbufWriteU8(dst, warningsLen); // length byte followed by the actual characters
- sbufWriteData(dst, warningsBuffer, warningsLen);
- break;
+ if (isBlinking) {
+ displayAttr |= DISPLAYPORT_BLINK;
}
+ sbufWriteU8(dst, displayAttr); // see displayPortSeverity_e
+ sbufWritePString(dst, warningsBuffer);
+
+ break;
+ }
#endif
case MSP_RC:
@@ -1687,6 +1677,7 @@ case MSP_NAME:
sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
}
break;
+
case MSP2_COMMON_SERIAL_CONFIG: {
uint8_t count = 0;
for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
@@ -1876,6 +1867,7 @@ case MSP_NAME:
#endif
break;
}
+
case MSP_ADVANCED_CONFIG:
sbufWriteU8(dst, 1); // was gyroConfig()->gyro_sync_denom - removed in API 1.43
sbufWriteU8(dst, pidConfig()->pid_process_denom);
@@ -1894,9 +1886,9 @@ case MSP_NAME:
//Added in MSP API 1.42
sbufWriteU8(dst, systemConfig()->debug_mode);
sbufWriteU8(dst, DEBUG_COUNT);
-
break;
- case MSP_FILTER_CONFIG :
+
+ case MSP_FILTER_CONFIG:
sbufWriteU8(dst, gyroConfig()->gyro_lpf1_static_hz);
sbufWriteU16(dst, currentPidProfile->dterm_lpf1_static_hz);
sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz);
@@ -1963,8 +1955,8 @@ case MSP_NAME:
#else
sbufWriteU8(dst, 0);
#endif
-
break;
+
case MSP_PID_ADVANCED:
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
@@ -2147,44 +2139,43 @@ case MSP_NAME:
break;
#if defined(USE_VTX_COMMON)
- case MSP_VTX_CONFIG:
- {
- const vtxDevice_t *vtxDevice = vtxCommonDevice();
- unsigned vtxStatus = 0;
- vtxDevType_e vtxType = VTXDEV_UNKNOWN;
- uint8_t deviceIsReady = 0;
- if (vtxDevice) {
- vtxCommonGetStatus(vtxDevice, &vtxStatus);
- vtxType = vtxCommonGetDeviceType(vtxDevice);
- deviceIsReady = vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0;
- }
- sbufWriteU8(dst, vtxType);
- sbufWriteU8(dst, vtxSettingsConfig()->band);
- sbufWriteU8(dst, vtxSettingsConfig()->channel);
- sbufWriteU8(dst, vtxSettingsConfig()->power);
- sbufWriteU8(dst, (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0);
- sbufWriteU16(dst, vtxSettingsConfig()->freq);
- sbufWriteU8(dst, deviceIsReady);
- sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
+ case MSP_VTX_CONFIG: {
+ const vtxDevice_t *vtxDevice = vtxCommonDevice();
+ unsigned vtxStatus = 0;
+ vtxDevType_e vtxType = VTXDEV_UNKNOWN;
+ uint8_t deviceIsReady = 0;
+ if (vtxDevice) {
+ vtxCommonGetStatus(vtxDevice, &vtxStatus);
+ vtxType = vtxCommonGetDeviceType(vtxDevice);
+ deviceIsReady = vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0;
+ }
+ sbufWriteU8(dst, vtxType);
+ sbufWriteU8(dst, vtxSettingsConfig()->band);
+ sbufWriteU8(dst, vtxSettingsConfig()->channel);
+ sbufWriteU8(dst, vtxSettingsConfig()->power);
+ sbufWriteU8(dst, (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0);
+ sbufWriteU16(dst, vtxSettingsConfig()->freq);
+ sbufWriteU8(dst, deviceIsReady);
+ sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
- // API version 1.42
- sbufWriteU16(dst, vtxSettingsConfig()->pitModeFreq);
+ // API version 1.42
+ sbufWriteU16(dst, vtxSettingsConfig()->pitModeFreq);
#ifdef USE_VTX_TABLE
- sbufWriteU8(dst, 1); // vtxtable is available
- sbufWriteU8(dst, vtxTableConfig()->bands);
- sbufWriteU8(dst, vtxTableConfig()->channels);
- sbufWriteU8(dst, vtxTableConfig()->powerLevels);
+ sbufWriteU8(dst, 1); // vtxtable is available
+ sbufWriteU8(dst, vtxTableConfig()->bands);
+ sbufWriteU8(dst, vtxTableConfig()->channels);
+ sbufWriteU8(dst, vtxTableConfig()->powerLevels);
#else
- sbufWriteU8(dst, 0);
- sbufWriteU8(dst, 0);
- sbufWriteU8(dst, 0);
- sbufWriteU8(dst, 0);
+ sbufWriteU8(dst, 0);
+ sbufWriteU8(dst, 0);
+ sbufWriteU8(dst, 0);
+ sbufWriteU8(dst, 0);
#endif
#ifdef USE_VTX_MSP
- setMspVtxDeviceStatusReady(srcDesc);
+ setMspVtxDeviceStatusReady(srcDesc);
#endif
- }
break;
+ }
#endif
case MSP_TX_INFO:
@@ -2199,25 +2190,24 @@ case MSP_NAME:
rtcDateTimeIsSet = RTC_NOT_SUPPORTED;
#endif
sbufWriteU8(dst, rtcDateTimeIsSet);
-
break;
+
#ifdef USE_RTC_TIME
- case MSP_RTC:
- {
- dateTime_t dt;
- if (rtcGetDateTime(&dt)) {
- sbufWriteU16(dst, dt.year);
- sbufWriteU8(dst, dt.month);
- sbufWriteU8(dst, dt.day);
- sbufWriteU8(dst, dt.hours);
- sbufWriteU8(dst, dt.minutes);
- sbufWriteU8(dst, dt.seconds);
- sbufWriteU16(dst, dt.millis);
- }
+ case MSP_RTC: {
+ dateTime_t dt;
+ if (rtcGetDateTime(&dt)) {
+ sbufWriteU16(dst, dt.year);
+ sbufWriteU8(dst, dt.month);
+ sbufWriteU8(dst, dt.day);
+ sbufWriteU8(dst, dt.hours);
+ sbufWriteU8(dst, dt.minutes);
+ sbufWriteU8(dst, dt.seconds);
+ sbufWriteU16(dst, dt.millis);
}
-
break;
+ }
#endif
+
default:
unsupportedCommand = true;
}
@@ -2629,12 +2619,9 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
if (!textVar) return MSP_RESULT_ERROR;
- const uint8_t textLength = strlen(textVar);
-
// type byte, then length byte followed by the actual characters
sbufWriteU8(dst, textType);
- sbufWriteU8(dst, textLength);
- sbufWriteData(dst, textVar, textLength);
+ sbufWritePString(dst, textVar);
}
break;
#ifdef USE_LED_STRIP
diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c
index 7477a5337d..e816db02d9 100644
--- a/src/main/msp/msp_box.c
+++ b/src/main/msp/msp_box.c
@@ -101,6 +101,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 52},
{ .boxId = BOXREADY, .boxName = "READY", .permanentId = 53},
{ .boxId = BOXLAPTIMERRESET, .boxName = "LAP TIMER RESET", .permanentId = 54},
+ { .boxId = BOXCHIRP, .boxName = "CHIRP", .permanentId = 55}
};
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -365,6 +366,10 @@ void initActiveBoxIds(void)
BME(BOXLAPTIMERRESET);
#endif
+#if defined(USE_CHIRP)
+ BME(BOXCHIRP);
+#endif
+
#undef BME
// check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++)
diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h
index 1114ebdf34..da4027a3cf 100644
--- a/src/main/msp/msp_protocol.h
+++ b/src/main/msp/msp_protocol.h
@@ -55,15 +55,11 @@
#pragma once
-/* Protocol numbers used both by the wire format, config system, and
- field setters.
-*/
-
+// Protocol numbers used both by the wire format, config system, and field setters.
#define MSP_PROTOCOL_VERSION 0
-#define API_VERSION_MAJOR 1 // increment when major changes are made
-#define API_VERSION_MINOR 47 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
-
+#define API_VERSION_MAJOR 1
+#define API_VERSION_MINOR 47
#define API_VERSION_LENGTH 2
#define MULTIWII_IDENTIFIER "MWII";
@@ -95,258 +91,188 @@
#define CAP_NAVCAP ((uint32_t)1 << 4)
#define CAP_EXTAUX ((uint32_t)1 << 5)
-#define MSP_API_VERSION 1 //out message
-#define MSP_FC_VARIANT 2 //out message
-#define MSP_FC_VERSION 3 //out message
-#define MSP_BOARD_INFO 4 //out message
-#define MSP_BUILD_INFO 5 //out message
+#define MSP_API_VERSION 1 // out message: Get API version
+#define MSP_FC_VARIANT 2 // out message: Get flight controller variant
+#define MSP_FC_VERSION 3 // out message: Get flight controller version
+#define MSP_BOARD_INFO 4 // out message: Get board information
+#define MSP_BUILD_INFO 5 // out message: Get build information
-#define MSP_NAME 10 //out message Returns user set board name - betaflight
-#define MSP_SET_NAME 11 //in message Sets board name - betaflight
+#define MSP_NAME 10 // out message: Returns user set board name - betaflight
+#define MSP_SET_NAME 11 // in message: Sets board name - betaflight
-//
-// MSP commands for Cleanflight original features
-//
-#define MSP_BATTERY_CONFIG 32
-#define MSP_SET_BATTERY_CONFIG 33
+// Cleanflight original features (32-62)
+#define MSP_BATTERY_CONFIG 32 // out message: Get battery configuration
+#define MSP_SET_BATTERY_CONFIG 33 // in message: Set battery configuration
+#define MSP_MODE_RANGES 34 // out message: Returns all mode ranges
+#define MSP_SET_MODE_RANGE 35 // in message: Sets a single mode range
+#define MSP_FEATURE_CONFIG 36 // out message: Get feature configuration
+#define MSP_SET_FEATURE_CONFIG 37 // in message: Set feature configuration
+#define MSP_BOARD_ALIGNMENT_CONFIG 38 // out message: Get board alignment configuration
+#define MSP_SET_BOARD_ALIGNMENT_CONFIG 39 // in message: Set board alignment configuration
+#define MSP_CURRENT_METER_CONFIG 40 // out message: Get current meter configuration
+#define MSP_SET_CURRENT_METER_CONFIG 41 // in message: Set current meter configuration
+#define MSP_MIXER_CONFIG 42 // out message: Get mixer configuration
+#define MSP_SET_MIXER_CONFIG 43 // in message: Set mixer configuration
+#define MSP_RX_CONFIG 44 // out message: Get RX configuration
+#define MSP_SET_RX_CONFIG 45 // in message: Set RX configuration
+#define MSP_LED_COLORS 46 // out message: Get LED colors
+#define MSP_SET_LED_COLORS 47 // in message: Set LED colors
+#define MSP_LED_STRIP_CONFIG 48 // out message: Get LED strip configuration
+#define MSP_SET_LED_STRIP_CONFIG 49 // in message: Set LED strip configuration
+#define MSP_RSSI_CONFIG 50 // out message: Get RSSI configuration
+#define MSP_SET_RSSI_CONFIG 51 // in message: Set RSSI configuration
+#define MSP_ADJUSTMENT_RANGES 52 // out message: Get adjustment ranges
+#define MSP_SET_ADJUSTMENT_RANGE 53 // in message: Set adjustment range
+#define MSP_CF_SERIAL_CONFIG 54 // out message: Get Cleanflight serial configuration
+#define MSP_SET_CF_SERIAL_CONFIG 55 // in message: Set Cleanflight serial configuration
+#define MSP_VOLTAGE_METER_CONFIG 56 // out message: Get voltage meter configuration
+#define MSP_SET_VOLTAGE_METER_CONFIG 57 // in message: Set voltage meter configuration
+#define MSP_SONAR_ALTITUDE 58 // out message: Get sonar altitude [cm]
+#define MSP_PID_CONTROLLER 59 // out message: Get PID controller
+#define MSP_SET_PID_CONTROLLER 60 // in message: Set PID controller
+#define MSP_ARMING_CONFIG 61 // out message: Get arming configuration
+#define MSP_SET_ARMING_CONFIG 62 // in message: Set arming configuration
-#define MSP_MODE_RANGES 34 //out message Returns all mode ranges
-#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
+// Baseflight MSP commands (64-89)
+#define MSP_RX_MAP 64 // out message: Get RX map (also returns number of channels total)
+#define MSP_SET_RX_MAP 65 // in message: Set RX map, numchannels to set comes from MSP_RX_MAP
+#define MSP_REBOOT 68 // in message: Reboot settings
+#define MSP_DATAFLASH_SUMMARY 70 // out message: Get description of dataflash chip
+#define MSP_DATAFLASH_READ 71 // out message: Get content of dataflash chip
+#define MSP_DATAFLASH_ERASE 72 // in message: Erase dataflash chip
+#define MSP_FAILSAFE_CONFIG 75 // out message: Get failsafe settings
+#define MSP_SET_FAILSAFE_CONFIG 76 // in message: Set failsafe settings
+#define MSP_RXFAIL_CONFIG 77 // out message: Get RX failsafe settings
+#define MSP_SET_RXFAIL_CONFIG 78 // in message: Set RX failsafe settings
+#define MSP_SDCARD_SUMMARY 79 // out message: Get SD card state
+#define MSP_BLACKBOX_CONFIG 80 // out message: Get blackbox settings
+#define MSP_SET_BLACKBOX_CONFIG 81 // in message: Set blackbox settings
+#define MSP_TRANSPONDER_CONFIG 82 // out message: Get transponder settings
+#define MSP_SET_TRANSPONDER_CONFIG 83 // in message: Set transponder settings
+#define MSP_OSD_CONFIG 84 // out message: Get OSD settings
+#define MSP_SET_OSD_CONFIG 85 // in message: Set OSD settings
+#define MSP_OSD_CHAR_READ 86 // out message: Get OSD characters
+#define MSP_OSD_CHAR_WRITE 87 // in message: Set OSD characters
+#define MSP_VTX_CONFIG 88 // out message: Get VTX settings
+#define MSP_SET_VTX_CONFIG 89 // in message: Set VTX settings
-#define MSP_FEATURE_CONFIG 36
-#define MSP_SET_FEATURE_CONFIG 37
+// Betaflight Additional Commands (90-99)
+#define MSP_ADVANCED_CONFIG 90 // out message: Get advanced configuration
+#define MSP_SET_ADVANCED_CONFIG 91 // in message: Set advanced configuration
+#define MSP_FILTER_CONFIG 92 // out message: Get filter configuration
+#define MSP_SET_FILTER_CONFIG 93 // in message: Set filter configuration
+#define MSP_PID_ADVANCED 94 // out message: Get advanced PID settings
+#define MSP_SET_PID_ADVANCED 95 // in message: Set advanced PID settings
+#define MSP_SENSOR_CONFIG 96 // out message: Get sensor configuration
+#define MSP_SET_SENSOR_CONFIG 97 // in message: Set sensor configuration
+#define MSP_CAMERA_CONTROL 98 // in/out message: Camera control
+#define MSP_SET_ARMING_DISABLED 99 // in message: Enable/disable arming
-#define MSP_BOARD_ALIGNMENT_CONFIG 38
-#define MSP_SET_BOARD_ALIGNMENT_CONFIG 39
+// Multiwii original MSP commands (101-139)
+#define MSP_STATUS 101 // out message: Cycletime & errors_count & sensor present & box activation & current setting number
+#define MSP_RAW_IMU 102 // out message: 9 DOF
+#define MSP_SERVO 103 // out message: Servos
+#define MSP_MOTOR 104 // out message: Motors
+#define MSP_RC 105 // out message: RC channels and more
+#define MSP_RAW_GPS 106 // out message: Fix, numsat, lat, lon, alt, speed, ground course
+#define MSP_COMP_GPS 107 // out message: Distance home, direction home
+#define MSP_ATTITUDE 108 // out message: 2 angles 1 heading
+#define MSP_ALTITUDE 109 // out message: Altitude, variometer
+#define MSP_ANALOG 110 // out message: Vbat, powermetersum, rssi if available on RX
+#define MSP_RC_TUNING 111 // out message: RC rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
+#define MSP_PID 112 // out message: P I D coeff (9 are used currently)
+#define MSP_BOXNAMES 116 // out message: The aux switch names
+#define MSP_PIDNAMES 117 // out message: The PID names
+#define MSP_WP 118 // out message: Get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
+#define MSP_BOXIDS 119 // out message: Get the permanent IDs associated to BOXes
+#define MSP_SERVO_CONFIGURATIONS 120 // out message: All servo configurations
+#define MSP_NAV_STATUS 121 // out message: Returns navigation status
+#define MSP_NAV_CONFIG 122 // out message: Returns navigation parameters
+#define MSP_MOTOR_3D_CONFIG 124 // out message: Settings needed for reversible ESCs
+#define MSP_RC_DEADBAND 125 // out message: Deadbands for yaw alt pitch roll
+#define MSP_SENSOR_ALIGNMENT 126 // out message: Orientation of acc,gyro,mag
+#define MSP_LED_STRIP_MODECOLOR 127 // out message: Get LED strip mode_color settings
+#define MSP_VOLTAGE_METERS 128 // out message: Voltage (per meter)
+#define MSP_CURRENT_METERS 129 // out message: Amperage (per meter)
+#define MSP_BATTERY_STATE 130 // out message: Connected/Disconnected, Voltage, Current Used
+#define MSP_MOTOR_CONFIG 131 // out message: Motor configuration (min/max throttle, etc)
+#define MSP_GPS_CONFIG 132 // out message: GPS configuration
+#define MSP_COMPASS_CONFIG 133 // out message: Compass configuration
+#define MSP_ESC_SENSOR_DATA 134 // out message: Extra ESC data from 32-Bit ESCs (Temperature, RPM)
+#define MSP_GPS_RESCUE 135 // out message: GPS Rescue angle, returnAltitude, descentDistance, groundSpeed, sanityChecks and minSats
+#define MSP_GPS_RESCUE_PIDS 136 // out message: GPS Rescue throttleP and velocity PIDS + yaw P
+#define MSP_VTXTABLE_BAND 137 // out message: VTX table band/channel data
+#define MSP_VTXTABLE_POWERLEVEL 138 // out message: VTX table powerLevel data
+#define MSP_MOTOR_TELEMETRY 139 // out message: Per-motor telemetry data (RPM, packet stats, ESC temp, etc.)
-#define MSP_CURRENT_METER_CONFIG 40
-#define MSP_SET_CURRENT_METER_CONFIG 41
+// Simplified tuning commands (140-145)
+#define MSP_SIMPLIFIED_TUNING 140 // out message: Get simplified tuning values and enabled state
+#define MSP_SET_SIMPLIFIED_TUNING 141 // in message: Set simplified tuning positions and apply calculated tuning
+#define MSP_CALCULATE_SIMPLIFIED_PID 142 // out message: Calculate PID values based on sliders without saving
+#define MSP_CALCULATE_SIMPLIFIED_GYRO 143 // out message: Calculate gyro filter values based on sliders without saving
+#define MSP_CALCULATE_SIMPLIFIED_DTERM 144 // out message: Calculate D term filter values based on sliders without saving
+#define MSP_VALIDATE_SIMPLIFIED_TUNING 145 // out message: Returns array of true/false showing which simplified tuning groups match values
-#define MSP_MIXER_CONFIG 42
-#define MSP_SET_MIXER_CONFIG 43
+// Additional non-MultiWii commands (150-166)
+#define MSP_STATUS_EX 150 // out message: Cycletime, errors_count, CPU load, sensor present etc
+#define MSP_UID 160 // out message: Unique device ID
+#define MSP_GPSSVINFO 164 // out message: Get Signal Strength (only U-Blox)
+#define MSP_GPSSTATISTICS 166 // out message: Get GPS debugging data
-#define MSP_RX_CONFIG 44
-#define MSP_SET_RX_CONFIG 45
+// OSD specific commands (180-189)
+#define MSP_OSD_VIDEO_CONFIG 180 // out message: Get OSD video settings
+#define MSP_SET_OSD_VIDEO_CONFIG 181 // in message: Set OSD video settings
+#define MSP_DISPLAYPORT 182 // out message: External OSD displayport mode
+#define MSP_COPY_PROFILE 183 // in message: Copy settings between profiles
+#define MSP_BEEPER_CONFIG 184 // out message: Get beeper configuration
+#define MSP_SET_BEEPER_CONFIG 185 // in message: Set beeper configuration
+#define MSP_SET_TX_INFO 186 // in message: Set runtime information from TX lua scripts
+#define MSP_TX_INFO 187 // out message: Get runtime information for TX lua scripts
+#define MSP_SET_OSD_CANVAS 188 // in message: Set OSD canvas size COLSxROWS
+#define MSP_OSD_CANVAS 189 // out message: Get OSD canvas size COLSxROWS
-#define MSP_LED_COLORS 46
-#define MSP_SET_LED_COLORS 47
+// Set commands (200-229)
+#define MSP_SET_RAW_RC 200 // in message: 8 rc chan
+#define MSP_SET_RAW_GPS 201 // in message: Fix, numsat, lat, lon, alt, speed
+#define MSP_SET_PID 202 // in message: P I D coeff (9 are used currently)
+#define MSP_SET_RC_TUNING 204 // in message: RC rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
+#define MSP_ACC_CALIBRATION 205 // in message: No param - calibrate accelerometer
+#define MSP_MAG_CALIBRATION 206 // in message: No param - calibrate magnetometer
+#define MSP_RESET_CONF 208 // in message: No param - reset settings
+#define MSP_SET_WP 209 // in message: Sets a given WP (WP#,lat, lon, alt, flags)
+#define MSP_SELECT_SETTING 210 // in message: Select setting number (0-2)
+#define MSP_SET_HEADING 211 // in message: Define a new heading hold direction
+#define MSP_SET_SERVO_CONFIGURATION 212 // in message: Servo settings
+#define MSP_SET_MOTOR 214 // in message: PropBalance function
+#define MSP_SET_NAV_CONFIG 215 // in message: Sets nav config parameters
+#define MSP_SET_MOTOR_3D_CONFIG 217 // in message: Settings needed for reversible ESCs
+#define MSP_SET_RC_DEADBAND 218 // in message: Deadbands for yaw alt pitch roll
+#define MSP_SET_RESET_CURR_PID 219 // in message: Reset current PID profile to defaults
+#define MSP_SET_SENSOR_ALIGNMENT 220 // in message: Set the orientation of acc,gyro,mag
+#define MSP_SET_LED_STRIP_MODECOLOR 221 // in message: Set LED strip mode_color settings
+#define MSP_SET_MOTOR_CONFIG 222 // in message: Motor configuration (min/max throttle, etc)
+#define MSP_SET_GPS_CONFIG 223 // in message: GPS configuration
+#define MSP_SET_COMPASS_CONFIG 224 // in message: Compass configuration
+#define MSP_SET_GPS_RESCUE 225 // in message: Set GPS Rescue parameters
+#define MSP_SET_GPS_RESCUE_PIDS 226 // in message: Set GPS Rescue PID values
+#define MSP_SET_VTXTABLE_BAND 227 // in message: Set vtxTable band/channel data
+#define MSP_SET_VTXTABLE_POWERLEVEL 228 // in message: Set vtxTable powerLevel data
-#define MSP_LED_STRIP_CONFIG 48
-#define MSP_SET_LED_STRIP_CONFIG 49
-
-#define MSP_RSSI_CONFIG 50
-#define MSP_SET_RSSI_CONFIG 51
-
-#define MSP_ADJUSTMENT_RANGES 52
-#define MSP_SET_ADJUSTMENT_RANGE 53
-
-// private - only to be used by the configurator, the commands are likely to change
-#define MSP_CF_SERIAL_CONFIG 54
-#define MSP_SET_CF_SERIAL_CONFIG 55
-
-#define MSP_VOLTAGE_METER_CONFIG 56
-#define MSP_SET_VOLTAGE_METER_CONFIG 57
-
-#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm]
-
-#define MSP_PID_CONTROLLER 59
-#define MSP_SET_PID_CONTROLLER 60
-
-#define MSP_ARMING_CONFIG 61
-#define MSP_SET_ARMING_CONFIG 62
-
-//
-// Baseflight MSP commands (if enabled they exist in Cleanflight)
-//
-#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total)
-#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP
-
-// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead.
-// DEPRECATED - #define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere
-// DEPRECATED - #define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save
-
-#define MSP_REBOOT 68 //in message reboot settings
-
-// Use MSP_BUILD_INFO instead
-// DEPRECATED - #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
-
-#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip
-#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip
-#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip
-
-// No-longer needed
-// DEPRECATED - #define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter // DEPRECATED
-// DEPRECATED - #define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter // DEPRECATED
-
-#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings
-#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings
-
-#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings
-#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings
-
-#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card
-
-#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings
-#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings
-
-#define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings
-#define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings
-
-#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight
-#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight
-
-#define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight
-#define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight
-
-#define MSP_VTX_CONFIG 88 //out message Get vtx settings - betaflight
-#define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight
-
-// Betaflight Additional Commands
-#define MSP_ADVANCED_CONFIG 90
-#define MSP_SET_ADVANCED_CONFIG 91
-
-#define MSP_FILTER_CONFIG 92
-#define MSP_SET_FILTER_CONFIG 93
-
-#define MSP_PID_ADVANCED 94
-#define MSP_SET_PID_ADVANCED 95
-
-#define MSP_SENSOR_CONFIG 96
-#define MSP_SET_SENSOR_CONFIG 97
-
-#define MSP_CAMERA_CONTROL 98
-
-#define MSP_SET_ARMING_DISABLED 99
-
-//
-// OSD specific
-//
-#define MSP_OSD_VIDEO_CONFIG 180
-#define MSP_SET_OSD_VIDEO_CONFIG 181
-
-// External OSD displayport mode messages
-#define MSP_DISPLAYPORT 182
-
-#define MSP_COPY_PROFILE 183
-
-#define MSP_BEEPER_CONFIG 184
-#define MSP_SET_BEEPER_CONFIG 185
-
-#define MSP_SET_TX_INFO 186 // in message Used to send runtime information from TX lua scripts to the firmware
-#define MSP_TX_INFO 187 // out message Used by TX lua scripts to read information from the firmware
-
-#define MSP_SET_OSD_CANVAS 188 // in message Set osd canvas size COLSxROWS
-#define MSP_OSD_CANVAS 189 // out message Get osd canvas size COLSxROWS
-
-//
-// Multwii original MSP commands
-//
-
-// See MSP_API_VERSION and MSP_MIXER_CONFIG
-//DEPRECATED - #define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
-
-#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
-#define MSP_RAW_IMU 102 //out message 9 DOF
-#define MSP_SERVO 103 //out message servos
-#define MSP_MOTOR 104 //out message motors
-#define MSP_RC 105 //out message rc channels and more
-#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
-#define MSP_COMP_GPS 107 //out message distance home, direction home
-#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
-#define MSP_ALTITUDE 109 //out message altitude, variometer
-#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
-#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
-#define MSP_PID 112 //out message P I D coeff (9 are used currently)
-// Legacy Multiicommand that was never used.
-//DEPRECATED - #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
-// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead.
-//DEPRECATED - #define MSP_MISC 114 //out message powermeter trig
-// Legacy Multiicommand that was never used and always wrong
-//DEPRECATED - #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
-#define MSP_BOXNAMES 116 //out message the aux switch names
-#define MSP_PIDNAMES 117 //out message the PID names
-#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
-#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
-#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations.
-#define MSP_NAV_STATUS 121 //out message Returns navigation status
-#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
-#define MSP_MOTOR_3D_CONFIG 124 //out message Settings needed for reversible ESCs
-#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll
-#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag
-#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings
-#define MSP_VOLTAGE_METERS 128 //out message Voltage (per meter)
-#define MSP_CURRENT_METERS 129 //out message Amperage (per meter)
-#define MSP_BATTERY_STATE 130 //out message Connected/Disconnected, Voltage, Current Used
-#define MSP_MOTOR_CONFIG 131 //out message Motor configuration (min/max throttle, etc)
-#define MSP_GPS_CONFIG 132 //out message GPS configuration
-#define MSP_COMPASS_CONFIG 133 //out message Compass configuration
-#define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM)
-#define MSP_GPS_RESCUE 135 //out message GPS Rescue angle, returnAltitude, descentDistance, groundSpeed, sanityChecks and minSats
-#define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescue throttleP and velocity PIDS + yaw P
-#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data
-#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data
-#define MSP_MOTOR_TELEMETRY 139 //out message Per-motor telemetry data (RPM, packet stats, ESC temp, etc.)
-
-#define MSP_SIMPLIFIED_TUNING 140 //out message Simplified tuning values and enabled state
-#define MSP_SET_SIMPLIFIED_TUNING 141 //in message Set simplified tuning positions and apply the calculated tuning
-#define MSP_CALCULATE_SIMPLIFIED_PID 142 //out message Requests calculations of PID values based on sliders. Sends the calculated values back. But don't save anything to the firmware
-#define MSP_CALCULATE_SIMPLIFIED_GYRO 143 //out message Requests calculations of gyro filter values based on sliders. Sends the calculated values back. But don't save anything to the firmware
-#define MSP_CALCULATE_SIMPLIFIED_DTERM 144 //out message Requests calculations of gyro filter values based on sliders. Sends the calculated values back. But don't save anything to the firmware
-#define MSP_VALIDATE_SIMPLIFIED_TUNING 145 //out message Returns an array of true/false showing which simpligfied tuning groups are matching with value and which are not
-
-#define MSP_SET_RAW_RC 200 //in message 8 rc chan
-#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
-#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
-// Legacy multiiwii command that was never used.
-//DEPRECATED - #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
-#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
-#define MSP_ACC_CALIBRATION 205 //in message no param
-#define MSP_MAG_CALIBRATION 206 //in message no param
-// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead.
-//DEPRECATED - #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
-#define MSP_RESET_CONF 208 //in message no param
-#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
-#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
-#define MSP_SET_HEADING 211 //in message define a new heading hold direction
-#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings
-#define MSP_SET_MOTOR 214 //in message PropBalance function
-#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
-#define MSP_SET_MOTOR_3D_CONFIG 217 //in message Settings needed for reversible ESCs
-#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll
-#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults
-#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag
-#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings
-#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc)
-#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration
-#define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration
-#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescue angle, returnAltitude, descentDistance, groundSpeed and sanityChecks
-#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescue throttleP and velocity PIDS + yaw P
-#define MSP_SET_VTXTABLE_BAND 227 //in message set vtxTable band/channel data (one band at a time)
-#define MSP_SET_VTXTABLE_POWERLEVEL 228 //in message set vtxTable powerLevel data (one powerLevel at a time)
-
-// #define MSP_BIND 240 //in message no param
-// #define MSP_ALARMS 242
-
-#define MSP_EEPROM_WRITE 250 //in message no param
-#define MSP_RESERVE_1 251 //reserved for system usage
-#define MSP_RESERVE_2 252 //reserved for system usage
-#define MSP_DEBUGMSG 253 //out message debug string buffer
-#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
-#define MSP_V2_FRAME 255 //MSPv2 payload indicator
-
-// Additional commands that are not compatible with MultiWii
-#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, CPU temperature, sensor present etc
-#define MSP_UID 160 //out message Unique device ID
-#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
-#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data
-#define MSP_MULTIPLE_MSP 230 //out message request multiple MSPs in one request - limit is the TX buffer; returns each MSP in the order they were requested starting with length of MSP; MSPs with input arguments are not supported
-#define MSP_MODE_RANGES_EXTRA 238 //out message Reads the extra mode range data
-#define MSP_ACC_TRIM 240 //out message get acc angle trim values
-#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
-#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
-#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
-#define MSP_SET_PASSTHROUGH 245 //in message Sets up passthrough to different peripherals (4way interface, uart, etc...)
-#define MSP_SET_RTC 246 //in message Sets the RTC clock
-#define MSP_RTC 247 //out message Gets the RTC clock
-#define MSP_SET_BOARD_INFO 248 //in message Sets the board information for this board
-#define MSP_SET_SIGNATURE 249 //in message Sets the signature of the board and serial number
+// Multiple MSP and special commands (230-255)
+#define MSP_MULTIPLE_MSP 230 // out message: Request multiple MSPs in one request
+#define MSP_MODE_RANGES_EXTRA 238 // out message: Extra mode range data
+#define MSP_SET_ACC_TRIM 239 // in message: Set acc angle trim values
+#define MSP_ACC_TRIM 240 // out message: Get acc angle trim values
+#define MSP_SERVO_MIX_RULES 241 // out message: Get servo mixer configuration
+#define MSP_SET_SERVO_MIX_RULE 242 // in message: Set servo mixer configuration
+#define MSP_SET_PASSTHROUGH 245 // in message: Set passthrough to peripherals
+#define MSP_SET_RTC 246 // in message: Set the RTC clock
+#define MSP_RTC 247 // out message: Get the RTC clock
+#define MSP_SET_BOARD_INFO 248 // in message: Set the board information
+#define MSP_SET_SIGNATURE 249 // in message: Set the signature of the board and serial number
+#define MSP_EEPROM_WRITE 250 // in message: Write settings to EEPROM
+#define MSP_RESERVE_1 251 // reserved for system usage
+#define MSP_RESERVE_2 252 // reserved for system usage
+#define MSP_DEBUGMSG 253 // out message: debug string buffer
+#define MSP_DEBUG 254 // out message: debug1,debug2,debug3,debug4
+#define MSP_V2_FRAME 255 // MSPv2 payload indicator
diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h
index beefb27f2f..7a4a6c5758 100644
--- a/src/main/msp/msp_protocol_v2_betaflight.h
+++ b/src/main/msp/msp_protocol_v2_betaflight.h
@@ -30,6 +30,7 @@
#define MSP2_SET_LED_STRIP_CONFIG_VALUES 0x3009
#define MSP2_SENSOR_CONFIG_ACTIVE 0x300A
#define MSP2_SENSOR_OPTICALFLOW 0x300B
+#define MSP2_MCU_INFO 0x300C
// MSP2_SET_TEXT and MSP2_GET_TEXT variable types
#define MSP2TEXT_PILOT_NAME 1
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
index 23ddd5c9e4..99cc5484f4 100644
--- a/src/main/osd/osd_elements.c
+++ b/src/main/osd/osd_elements.c
@@ -1074,16 +1074,21 @@ static void osdElementFlymode(osdElementParms_t *element)
strcpy(element->buff, "HEAD");
} else if (FLIGHT_MODE(PASSTHRU_MODE)) {
strcpy(element->buff, "PASS");
- } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
- strcpy(element->buff, "ALTH");
} else if (FLIGHT_MODE(POS_HOLD_MODE)) {
strcpy(element->buff, "POSH");
+ } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
+ strcpy(element->buff, "ALTH");
} else if (FLIGHT_MODE(ANGLE_MODE)) {
strcpy(element->buff, "ANGL");
} else if (FLIGHT_MODE(HORIZON_MODE)) {
strcpy(element->buff, "HOR ");
} else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
strcpy(element->buff, "ATRN");
+#ifdef USE_CHIRP
+ // the additional check for pidChirpIsFinished() is to have visual feedback for user that don't have warnings enabled in their googles
+ } else if (FLIGHT_MODE(CHIRP_MODE) && !pidChirpIsFinished()) {
+#endif
+ strcpy(element->buff, "CHIR");
} else if (isAirmodeEnabled()) {
strcpy(element->buff, "AIR ");
} else {
@@ -2123,6 +2128,9 @@ void osdAddActiveElements(void)
static bool osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
{
+ // By default mark the element as rendered in case it's in the off blink state
+ activeElement.rendered = true;
+
if (!osdElementDrawFunction[item]) {
// Element has no drawing function
return true;
@@ -2143,7 +2151,6 @@ static bool osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
activeElement.buff = elementBuff;
activeElement.osdDisplayPort = osdDisplayPort;
activeElement.drawElement = true;
- activeElement.rendered = true;
activeElement.attr = DISPLAYPORT_SEVERITY_NORMAL;
// Call the element drawing function
@@ -2267,14 +2274,13 @@ bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort)
// Only advance to the next element if rendering is complete
if (osdDrawSingleElement(osdDisplayPort, item)) {
// If rendering is complete then advance to the next element
- if (activeElement.rendered) {
- // Prepare to render the background of the next element
- backgroundRendered = false;
- if (++activeElementNumber >= activeOsdElementCount) {
- activeElementNumber = 0;
- return false;
- }
+ // Prepare to render the background of the next element
+ backgroundRendered = false;
+
+ if (++activeElementNumber >= activeOsdElementCount) {
+ activeElementNumber = 0;
+ return false;
}
}
diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c
index bf6b8e1c07..3c983946b0 100644
--- a/src/main/osd/osd_warnings.c
+++ b/src/main/osd/osd_warnings.c
@@ -432,6 +432,16 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
return;
}
+#ifdef USE_CHIRP
+ // Visual info that chirp excitation is finished
+ if (pidChirpIsFinished()) {
+ tfp_sprintf(warningText, "CHIRP EXC FINISHED");
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
+ *blinking = true;
+ return;
+ }
+#endif // USE_CHIRP
+
}
#endif // USE_OSD
diff --git a/src/main/pg/gps.c b/src/main/pg/gps.c
index d7d7b202ab..81f9e0e396 100644
--- a/src/main/pg/gps.c
+++ b/src/main/pg/gps.c
@@ -32,7 +32,11 @@
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 4);
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
+#if defined(SIMULATOR_BUILD) && defined(USE_VIRTUAL_GPS)
+ .provider = GPS_VIRTUAL,
+#else
.provider = GPS_UBLOX,
+#endif
.sbasMode = SBAS_NONE,
.autoConfig = GPS_AUTOCONFIG_ON,
.autoBaud = GPS_AUTOBAUD_OFF,
diff --git a/src/main/pg/gyrodev.c b/src/main/pg/gyrodev.c
index 16e2321e8d..67d2d7d442 100644
--- a/src/main/pg/gyrodev.c
+++ b/src/main/pg/gyrodev.c
@@ -24,6 +24,7 @@
#include "platform.h"
#include "common/sensor_alignment.h"
+#include "common/sensor_alignment_impl.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
@@ -67,13 +68,67 @@
// gyro alignments
+#if defined(GYRO_1_ALIGN_ROLL) || defined(GYRO_1_ALIGN_PITCH) || defined(GYRO_1_ALIGN_YAW)
+#ifndef GYRO_1_ALIGN_ROLL
+#define GYRO_1_ALIGN_ROLL 0
+#endif
+#ifndef GYRO_1_ALIGN_PITCH
+#define GYRO_1_ALIGN_PITCH 0
+#endif
+#ifndef GYRO_1_ALIGN_YAW
+#define GYRO_1_ALIGN_YAW 0
+#endif
+#ifndef GYRO_1_CUSTOM_ALIGN
+#define GYRO_1_CUSTOM_ALIGN SENSOR_ALIGNMENT( GYRO_1_ALIGN_ROLL / 10, GYRO_1_ALIGN_PITCH / 10, GYRO_1_ALIGN_YAW / 10 )
+#else
+#error "GYRO_1_ALIGN_x and GYRO_1_CUSTOM_ALIGN are mutually exclusive"
+#endif
+#endif // GYRO_1_ALIGN_ROLL || GYRO_1_ALIGN_PITCH || GYRO_1_ALIGN_YAW
+
+#if defined(GYRO_2_ALIGN_ROLL) || defined(GYRO_2_ALIGN_PITCH) || defined(GYRO_2_ALIGN_YAW)
+#ifndef GYRO_2_ALIGN_ROLL
+#define GYRO_2_ALIGN_ROLL 0
+#endif
+#ifndef GYRO_2_ALIGN_PITCH
+#define GYRO_2_ALIGN_PITCH 0
+#endif
+#ifndef GYRO_2_ALIGN_YAW
+#define GYRO_2_ALIGN_YAW 0
+#endif
+#ifndef GYRO_2_CUSTOM_ALIGN
+#define GYRO_2_CUSTOM_ALIGN SENSOR_ALIGNMENT( GYRO_2_ALIGN_ROLL / 10, GYRO_2_ALIGN_PITCH / 10, GYRO_2_ALIGN_YAW / 10 )
+#else
+#error "GYRO_2_ALIGN_x and GYRO_2_CUSTOM_ALIGN are mutually exclusive"
+#endif
+#endif // GYRO_2_ALIGN_ROLL || GYRO_2_ALIGN_PITCH || GYRO_2_ALIGN_YAW
+
#ifndef GYRO_1_ALIGN
+#ifdef GYRO_1_CUSTOM_ALIGN
+#define GYRO_1_ALIGN ALIGN_CUSTOM
+#else
#define GYRO_1_ALIGN CW0_DEG
#endif
+#endif // GYRO_1_ALIGN
+
+#ifndef GYRO_1_CUSTOM_ALIGN
+#define GYRO_1_CUSTOM_ALIGN SENSOR_ALIGNMENT_FROM_STD(GYRO_1_ALIGN)
+#else
+STATIC_ASSERT(GYRO_1_ALIGN == ALIGN_CUSTOM, "GYRO_1_ALIGN and GYRO_1_CUSTOM_ALIGN mixed");
+#endif // GYRO_1_CUSTOM_ALIGN
#ifndef GYRO_2_ALIGN
+#ifdef GYRO_2_CUSTOM_ALIGN
+#define GYRO_2_ALIGN ALIGN_CUSTOM
+#else
#define GYRO_2_ALIGN CW0_DEG
#endif
+#endif // GYRO_2_ALIGN
+
+#ifndef GYRO_2_CUSTOM_ALIGN
+#define GYRO_2_CUSTOM_ALIGN SENSOR_ALIGNMENT_FROM_STD(GYRO_2_ALIGN)
+#else
+STATIC_ASSERT(GYRO_2_ALIGN == ALIGN_CUSTOM, "GYRO_2_ALIGN and GYRO_2_CUSTOM_ALIGN mixed");
+#endif // GYRO_2_CUSTOM_ALIGN
#if defined(USE_SPI_GYRO) && (defined(GYRO_1_SPI_INSTANCE) || defined(GYRO_2_SPI_INSTANCE))
static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, ioTag_t clkInTag, uint8_t alignment, sensorAlignment_t customAlignment)
@@ -105,32 +160,18 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDevic
void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf)
{
devconf[0].index = 0;
- sensorAlignment_t customAlignment1 = CUSTOM_ALIGN_CW0_DEG;
-#ifdef GYRO_1_CUSTOM_ALIGN
- customAlignment1 = GYRO_1_CUSTOM_ALIGN;
-#else
- buildAlignmentFromStandardAlignment(&customAlignment1, GYRO_1_ALIGN);
-#endif // GYRO_1_CUSTOM_ALIGN
-
// All multi-gyro boards use SPI based gyros.
#ifdef USE_SPI_GYRO
#ifdef GYRO_1_SPI_INSTANCE
- gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), IO_TAG(GYRO_1_CLKIN_PIN), GYRO_1_ALIGN, customAlignment1);
+ gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), IO_TAG(GYRO_1_CLKIN_PIN), GYRO_1_ALIGN, GYRO_1_CUSTOM_ALIGN);
#else
devconf[0].busType = BUS_TYPE_NONE;
#endif
#ifdef USE_MULTI_GYRO
devconf[1].index = 1;
- sensorAlignment_t customAlignment2 = CUSTOM_ALIGN_CW0_DEG;
-#ifdef GYRO_2_CUSTOM_ALIGN
- customAlignment2 = GYRO_2_CUSTOM_ALIGN;
-#else
- buildAlignmentFromStandardAlignment(&customAlignment2, GYRO_2_ALIGN);
-#endif // GYRO_2_CUSTOM_ALIGN
-
#ifdef GYRO_2_SPI_INSTANCE
// TODO: CLKIN gyro 2 on separate pin is not supported yet. need to implement it
- gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), IO_TAG(GYRO_2_CLKIN_PIN), GYRO_2_ALIGN, customAlignment2);
+ gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), IO_TAG(GYRO_2_CLKIN_PIN), GYRO_2_ALIGN, GYRO_2_CUSTOM_ALIGN);
#else
devconf[1].busType = BUS_TYPE_NONE;
#endif
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index 66a139d664..2f28e6879f 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -50,6 +50,7 @@ typedef enum {
ACC_BMI270,
ACC_LSM6DSO,
ACC_LSM6DSV16X,
+ ACC_IIM42653,
ACC_VIRTUAL
} accelerationSensor_e;
diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c
index 127ea4a80c..812ab21f6d 100644
--- a/src/main/sensors/acceleration_init.c
+++ b/src/main/sensors/acceleration_init.c
@@ -216,9 +216,10 @@ retry:
FALLTHROUGH;
#endif
-#if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P)
+#if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653)
case ACC_ICM42605:
case ACC_ICM42688P:
+ case ACC_IIM42653:
if (icm426xxSpiAccDetect(dev)) {
switch (dev->mpuDetectionResult.sensor) {
case ICM_42605_SPI:
@@ -227,6 +228,9 @@ retry:
case ICM_42688P_SPI:
accHardware = ACC_ICM42688P;
break;
+ case IIM_42653_SPI:
+ accHardware = ACC_IIM42653;
+ break;
default:
accHardware = ACC_NONE;
break;
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index f5fdcaed61..4a1bfc99d3 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -158,7 +158,11 @@ void compassPreInit(void)
#if !defined(SIMULATOR_BUILD)
bool compassDetect(magDev_t *magDev, uint8_t *alignment)
{
- *alignment = ALIGN_DEFAULT; // may be overridden if target specifies MAG_*_ALIGN
+#ifdef MAG_ALIGN
+ *alignment = MAG_ALIGN;
+#else
+ *alignment = ALIGN_DEFAULT;
+#endif
magSensor_e magHardware = MAG_NONE;
@@ -221,9 +225,6 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
}
if (hmc5883lDetect(magDev)) {
-#ifdef MAG_HMC5883_ALIGN
- *alignment = MAG_HMC5883_ALIGN;
-#endif
magHardware = MAG_HMC5883;
break;
}
@@ -237,9 +238,6 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
}
if (lis2mdlDetect(magDev)) {
-#ifdef MAG_LIS3MDL_ALIGN
- *alignment = MAG_LIS2MDL_ALIGN;
-#endif
magHardware = MAG_LIS2MDL;
break;
}
@@ -253,9 +251,6 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
}
if (lis3mdlDetect(magDev)) {
-#ifdef MAG_LIS3MDL_ALIGN
- *alignment = MAG_LIS3MDL_ALIGN;
-#endif
magHardware = MAG_LIS3MDL;
break;
}
@@ -269,9 +264,6 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
}
if (ak8975Detect(magDev)) {
-#ifdef MAG_AK8975_ALIGN
- *alignment = MAG_AK8975_ALIGN;
-#endif
magHardware = MAG_AK8975;
break;
}
@@ -290,9 +282,6 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
}
if (ak8963Detect(magDev)) {
-#ifdef MAG_AK8963_ALIGN
- *alignment = MAG_AK8963_ALIGN;
-#endif
magHardware = MAG_AK8963;
break;
}
@@ -306,9 +295,6 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
}
if (qmc5883lDetect(magDev)) {
-#ifdef MAG_QMC5883L_ALIGN
- *alignment = MAG_QMC5883L_ALIGN;
-#endif
magHardware = MAG_QMC5883;
break;
}
@@ -322,9 +308,6 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
}
if (ist8310Detect(magDev)) {
-#ifdef MAG_IST8310_ALIGN
- *alignment = MAG_IST8310_ALIGN;
-#endif
magHardware = MAG_IST8310;
break;
}
diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c
index c09d466e5a..4ebc9153e7 100644
--- a/src/main/sensors/gyro_init.c
+++ b/src/main/sensors/gyro_init.c
@@ -314,6 +314,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
case GYRO_LSM6DSO:
case GYRO_LSM6DSV16X:
case GYRO_ICM42688P:
+ case GYRO_IIM42653:
case GYRO_ICM42605:
gyroSensor->gyroDev.gyroHasOverflowProtection = true;
break;
@@ -427,9 +428,10 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH;
#endif
-#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
+#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653)
case GYRO_ICM42605:
case GYRO_ICM42688P:
+ case GYRO_IIM42653:
if (icm426xxSpiGyroDetect(dev)) {
switch (dev->mpuDetectionResult.sensor) {
case ICM_42605_SPI:
@@ -438,6 +440,9 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
case ICM_42688P_SPI:
gyroHardware = GYRO_ICM42688P;
break;
+ case IIM_42653_SPI:
+ gyroHardware = GYRO_IIM42653;
+ break;
default:
gyroHardware = GYRO_NONE;
break;
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index a69492dcf2..976ba8e7c3 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -107,7 +107,8 @@
&& !defined(USE_ACC_SPI_MPU6000) \
&& !defined(USE_ACC_SPI_MPU6500) \
&& !defined(USE_ACC_SPI_MPU9250) \
- && !defined(USE_VIRTUAL_ACC)
+ && !defined(USE_VIRTUAL_ACC) \
+ && !defined(USE_ACCGYRO_IIM42653)
#error At least one USE_ACC device definition required
#endif
@@ -126,7 +127,8 @@
&& !defined(USE_GYRO_SPI_MPU6000) \
&& !defined(USE_GYRO_SPI_MPU6500) \
&& !defined(USE_GYRO_SPI_MPU9250) \
- && !defined(USE_VIRTUAL_GYRO)
+ && !defined(USE_VIRTUAL_GYRO) \
+ && !defined(USE_ACCGYRO_IIM42653)
#error At least one USE_GYRO device definition required
#endif
@@ -466,8 +468,8 @@
// Generate USE_SPI_GYRO or USE_I2C_GYRO
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) \
- || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) \
- || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSV16X) || defined(USE_ACCGYRO_LSM6DSO)
+ || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653) \
+ || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSV16X) || defined(USE_ACCGYRO_LSM6DSO)
#ifndef USE_SPI_GYRO
#define USE_SPI_GYRO
#endif
diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
index 0db3a0d1dc..1220594f3d 100644
--- a/src/main/target/common_pre.h
+++ b/src/main/target/common_pre.h
@@ -114,6 +114,7 @@
#define USE_ACCGYRO_BMI270
#define USE_GYRO_SPI_ICM42605
#define USE_GYRO_SPI_ICM42688P
+#define USE_ACCGYRO_IIM42653
#define USE_ACC_SPI_ICM42605
#define USE_ACC_SPI_ICM42688P
#define USE_ACCGYRO_LSM6DSV16X
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index b2e48657ef..48140efae8 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -457,12 +457,14 @@ void crsfFrameFlightMode(sbuf_t *dst)
flightMode = "PASS";
} else if (FLIGHT_MODE(ANGLE_MODE)) {
flightMode = "ANGL";
- } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
- flightMode = "ALTH";
} else if (FLIGHT_MODE(POS_HOLD_MODE)) {
flightMode = "POSH";
+ } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
+ flightMode = "ALTH";
} else if (FLIGHT_MODE(HORIZON_MODE)) {
flightMode = "HOR";
+ } else if (FLIGHT_MODE(CHIRP_MODE)) {
+ flightMode = "CHIR";
} else if (isAirmodeEnabled()) {
flightMode = "AIR";
}
diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
index 733477b3ec..7322a7eb11 100644
--- a/src/main/telemetry/ltm.c
+++ b/src/main/telemetry/ltm.c
@@ -175,10 +175,10 @@ static void ltm_sframe(void)
lt_flightmode = 2;
else if (FLIGHT_MODE(HORIZON_MODE))
lt_flightmode = 3;
- else if (FLIGHT_MODE(ALT_HOLD_MODE))
- lt_flightmode = 8;
else if (FLIGHT_MODE(POS_HOLD_MODE))
lt_flightmode = 9;
+ else if (FLIGHT_MODE(ALT_HOLD_MODE))
+ lt_flightmode = 8;
else if (FLIGHT_MODE(GPS_RESCUE_MODE))
lt_flightmode = 13;
else
diff --git a/src/platform/APM32/adc_apm32f4xx.c b/src/platform/APM32/adc_apm32f4xx.c
index b9407e836b..3359b90678 100644
--- a/src/platform/APM32/adc_apm32f4xx.c
+++ b/src/platform/APM32/adc_apm32f4xx.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/apm32f4xx_ddl_ex.h b/src/platform/APM32/apm32f4xx_ddl_ex.h
index da6f28a284..187448df8d 100644
--- a/src/platform/APM32/apm32f4xx_ddl_ex.h
+++ b/src/platform/APM32/apm32f4xx_ddl_ex.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/bus_i2c_apm32.c b/src/platform/APM32/bus_i2c_apm32.c
index 338ded478d..12eb1d803a 100644
--- a/src/platform/APM32/bus_i2c_apm32.c
+++ b/src/platform/APM32/bus_i2c_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/bus_i2c_apm32_init.c b/src/platform/APM32/bus_i2c_apm32_init.c
index 9db60f71ab..12dd1227d8 100644
--- a/src/platform/APM32/bus_i2c_apm32_init.c
+++ b/src/platform/APM32/bus_i2c_apm32_init.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/bus_spi_apm32.c b/src/platform/APM32/bus_spi_apm32.c
index 8f09a3489a..43e215851f 100644
--- a/src/platform/APM32/bus_spi_apm32.c
+++ b/src/platform/APM32/bus_spi_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/dma_apm32.h b/src/platform/APM32/dma_apm32.h
index accfb132f3..a719782180 100644
--- a/src/platform/APM32/dma_apm32.h
+++ b/src/platform/APM32/dma_apm32.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/dma_apm32f4xx.c b/src/platform/APM32/dma_apm32f4xx.c
index 723838c06b..3c571a75e5 100644
--- a/src/platform/APM32/dma_apm32f4xx.c
+++ b/src/platform/APM32/dma_apm32f4xx.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/dma_reqmap_mcu.c b/src/platform/APM32/dma_reqmap_mcu.c
index 892341af71..2d36c8e795 100644
--- a/src/platform/APM32/dma_reqmap_mcu.c
+++ b/src/platform/APM32/dma_reqmap_mcu.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c
index d128aec8f2..92de301f43 100644
--- a/src/platform/APM32/dshot_bitbang.c
+++ b/src/platform/APM32/dshot_bitbang.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/dshot_bitbang_ddl.c b/src/platform/APM32/dshot_bitbang_ddl.c
index f26a21966f..c03e02ff9b 100644
--- a/src/platform/APM32/dshot_bitbang_ddl.c
+++ b/src/platform/APM32/dshot_bitbang_ddl.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/eint_apm32.c b/src/platform/APM32/eint_apm32.c
index c3e0d8bf9a..fd39b93094 100644
--- a/src/platform/APM32/eint_apm32.c
+++ b/src/platform/APM32/eint_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/io_apm32.c b/src/platform/APM32/io_apm32.c
index 1bfdefc8a7..d744117646 100644
--- a/src/platform/APM32/io_apm32.c
+++ b/src/platform/APM32/io_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/light_ws2811strip_apm32.c b/src/platform/APM32/light_ws2811strip_apm32.c
index a96d3a9bc4..676f719470 100644
--- a/src/platform/APM32/light_ws2811strip_apm32.c
+++ b/src/platform/APM32/light_ws2811strip_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/persistent_apm32.c b/src/platform/APM32/persistent_apm32.c
index 4d5caeecbd..20a367ad6f 100644
--- a/src/platform/APM32/persistent_apm32.c
+++ b/src/platform/APM32/persistent_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c
index c0f40dd948..a2d3254fe7 100644
--- a/src/platform/APM32/pwm_output_apm32.c
+++ b/src/platform/APM32/pwm_output_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/pwm_output_dshot_apm32.c b/src/platform/APM32/pwm_output_dshot_apm32.c
index 02189569f8..4025aa531c 100644
--- a/src/platform/APM32/pwm_output_dshot_apm32.c
+++ b/src/platform/APM32/pwm_output_dshot_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/rcm_apm32.c b/src/platform/APM32/rcm_apm32.c
index 9f86cf594c..96ecd59ac5 100644
--- a/src/platform/APM32/rcm_apm32.c
+++ b/src/platform/APM32/rcm_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/serial_uart_apm32.c b/src/platform/APM32/serial_uart_apm32.c
index 43344e2317..e676ca2f93 100644
--- a/src/platform/APM32/serial_uart_apm32.c
+++ b/src/platform/APM32/serial_uart_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/serial_uart_apm32f4xx.c b/src/platform/APM32/serial_uart_apm32f4xx.c
index 1dde4d9682..2c5151b62f 100644
--- a/src/platform/APM32/serial_uart_apm32f4xx.c
+++ b/src/platform/APM32/serial_uart_apm32f4xx.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/system_apm32f4xx.c b/src/platform/APM32/system_apm32f4xx.c
index 25bdeff99b..0bf5b42231 100644
--- a/src/platform/APM32/system_apm32f4xx.c
+++ b/src/platform/APM32/system_apm32f4xx.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/target/APM32F405/target.h b/src/platform/APM32/target/APM32F405/target.h
index 367ab649ad..b23694d80e 100644
--- a/src/platform/APM32/target/APM32F405/target.h
+++ b/src/platform/APM32/target/APM32F405/target.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/target/APM32F407/target.h b/src/platform/APM32/target/APM32F407/target.h
index a88cea7e6e..c529d6d366 100644
--- a/src/platform/APM32/target/APM32F407/target.h
+++ b/src/platform/APM32/target/APM32F407/target.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/timer_apm32.c b/src/platform/APM32/timer_apm32.c
index 58cb219166..df3aff9adb 100644
--- a/src/platform/APM32/timer_apm32.c
+++ b/src/platform/APM32/timer_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/timer_apm32f4xx.c b/src/platform/APM32/timer_apm32f4xx.c
index df020c0afd..708e6f3e29 100644
--- a/src/platform/APM32/timer_apm32f4xx.c
+++ b/src/platform/APM32/timer_apm32f4xx.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/timer_def.h b/src/platform/APM32/timer_def.h
index 54949a4de8..157fe314f6 100644
--- a/src/platform/APM32/timer_def.h
+++ b/src/platform/APM32/timer_def.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/transponder_ir_io_apm32.c b/src/platform/APM32/transponder_ir_io_apm32.c
index 738f88f333..799ccb8494 100644
--- a/src/platform/APM32/transponder_ir_io_apm32.c
+++ b/src/platform/APM32/transponder_ir_io_apm32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/usb/msc/usb_msc_apm32f4xx.c b/src/platform/APM32/usb/msc/usb_msc_apm32f4xx.c
index 8d0227572b..a7a0155dd2 100644
--- a/src/platform/APM32/usb/msc/usb_msc_apm32f4xx.c
+++ b/src/platform/APM32/usb/msc/usb_msc_apm32f4xx.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/APM32/usb/vcp/serial_usb_vcp.c b/src/platform/APM32/usb/vcp/serial_usb_vcp.c
index 26b44d12f3..ff7e77c1a5 100644
--- a/src/platform/APM32/usb/vcp/serial_usb_vcp.c
+++ b/src/platform/APM32/usb/vcp/serial_usb_vcp.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/adc_at32f43x.c b/src/platform/AT32/adc_at32f43x.c
index 46b3ae714c..9cc7ea6225 100644
--- a/src/platform/AT32/adc_at32f43x.c
+++ b/src/platform/AT32/adc_at32f43x.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/bus_i2c_atbsp.c b/src/platform/AT32/bus_i2c_atbsp.c
index a8d1bf6470..daa9f4bc9a 100644
--- a/src/platform/AT32/bus_i2c_atbsp.c
+++ b/src/platform/AT32/bus_i2c_atbsp.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/bus_i2c_atbsp_init.c b/src/platform/AT32/bus_i2c_atbsp_init.c
index 79582e0ada..0d3d4c2542 100644
--- a/src/platform/AT32/bus_i2c_atbsp_init.c
+++ b/src/platform/AT32/bus_i2c_atbsp_init.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/bus_spi_at32bsp.c b/src/platform/AT32/bus_spi_at32bsp.c
index b76303d5af..5c643198bb 100644
--- a/src/platform/AT32/bus_spi_at32bsp.c
+++ b/src/platform/AT32/bus_spi_at32bsp.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/dma_at32f43x.c b/src/platform/AT32/dma_at32f43x.c
index e3f3d9863c..e233713a42 100644
--- a/src/platform/AT32/dma_at32f43x.c
+++ b/src/platform/AT32/dma_at32f43x.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/dma_atbsp.h b/src/platform/AT32/dma_atbsp.h
index 49b452558e..1cf03115c1 100644
--- a/src/platform/AT32/dma_atbsp.h
+++ b/src/platform/AT32/dma_atbsp.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/dshot_bitbang.c b/src/platform/AT32/dshot_bitbang.c
index 9ea68bba43..ccff17ae91 100644
--- a/src/platform/AT32/dshot_bitbang.c
+++ b/src/platform/AT32/dshot_bitbang.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/exti_at32.c b/src/platform/AT32/exti_at32.c
index 78f40edea5..5bbe0bde50 100644
--- a/src/platform/AT32/exti_at32.c
+++ b/src/platform/AT32/exti_at32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/io_at32.c b/src/platform/AT32/io_at32.c
index 321c32f868..231f9e5e47 100644
--- a/src/platform/AT32/io_at32.c
+++ b/src/platform/AT32/io_at32.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/light_ws2811strip_at32f43x.c b/src/platform/AT32/light_ws2811strip_at32f43x.c
index 1adfd990d6..1827556d30 100644
--- a/src/platform/AT32/light_ws2811strip_at32f43x.c
+++ b/src/platform/AT32/light_ws2811strip_at32f43x.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/persistent_at32bsp.c b/src/platform/AT32/persistent_at32bsp.c
index ab497fcc23..b79c5b67ad 100644
--- a/src/platform/AT32/persistent_at32bsp.c
+++ b/src/platform/AT32/persistent_at32bsp.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c
index 62a4e4ac6b..3f67b8676c 100644
--- a/src/platform/AT32/pwm_output_at32bsp.c
+++ b/src/platform/AT32/pwm_output_at32bsp.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/pwm_output_dshot.c b/src/platform/AT32/pwm_output_dshot.c
index ddaf7ed3e9..0998db708d 100644
--- a/src/platform/AT32/pwm_output_dshot.c
+++ b/src/platform/AT32/pwm_output_dshot.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/serial_uart_at32bsp.c b/src/platform/AT32/serial_uart_at32bsp.c
index f2f6233723..0875364108 100644
--- a/src/platform/AT32/serial_uart_at32bsp.c
+++ b/src/platform/AT32/serial_uart_at32bsp.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/serial_usb_vcp_at32f4.c b/src/platform/AT32/serial_usb_vcp_at32f4.c
index 5fa490b933..f0fc0ce44d 100644
--- a/src/platform/AT32/serial_usb_vcp_at32f4.c
+++ b/src/platform/AT32/serial_usb_vcp_at32f4.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/system_at32f43x.c b/src/platform/AT32/system_at32f43x.c
index 35188ae848..8f4f7d76af 100644
--- a/src/platform/AT32/system_at32f43x.c
+++ b/src/platform/AT32/system_at32f43x.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/target/AT32F435G/target.h b/src/platform/AT32/target/AT32F435G/target.h
index 2399974ad8..76a4e039dc 100644
--- a/src/platform/AT32/target/AT32F435G/target.h
+++ b/src/platform/AT32/target/AT32F435G/target.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/target/AT32F435M/target.h b/src/platform/AT32/target/AT32F435M/target.h
index 1a25cb9f35..3429a10876 100644
--- a/src/platform/AT32/target/AT32F435M/target.h
+++ b/src/platform/AT32/target/AT32F435M/target.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/timer_at32bsp.c b/src/platform/AT32/timer_at32bsp.c
index 8aaf1b22c5..b0a584b83e 100644
--- a/src/platform/AT32/timer_at32bsp.c
+++ b/src/platform/AT32/timer_at32bsp.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
@@ -646,29 +647,29 @@ _TIM_IRQ_HANDLER(TMR7_GLOBAL_IRQHandler, 7);
#endif
#if USED_TIMERS & TIM_N(8)
-_TIM_IRQ_HANDLER(TMR8_CH_IRQnHandler, 8);
+_TIM_IRQ_HANDLER(TMR8_CH_IRQHandler, 8);
#endif
#if USED_TIMERS & TIM_N(9)
-_TIM_IRQ_HANDLER(TMR1_BRK_TMR9_IRQnHandler, 9);
+_TIM_IRQ_HANDLER(TMR1_BRK_TMR9_IRQHandler, 9);
#endif
//TODO: there may be a bug
#if USED_TIMERS & TIM_N(10)
-_TIM_IRQ_HANDLER2(TMR1_OVF_TMR10_IRQnHandler, 1,10);
+_TIM_IRQ_HANDLER2(TMR1_OVF_TMR10_IRQHandler, 1,10);
#endif
# if USED_TIMERS & TIM_N(11)
-_TIM_IRQ_HANDLER(TMR1_TRG_HALL_TMR11_IRQnHandler, 11);
+_TIM_IRQ_HANDLER(TMR1_TRG_HALL_TMR11_IRQHandler, 11);
# endif
#if USED_TIMERS & TIM_N(12)
-_TIM_IRQ_HANDLER(TMR8_BRK_TMR12_IRQnHandler, 12);
+_TIM_IRQ_HANDLER(TMR8_BRK_TMR12_IRQHandler, 12);
#endif
#if USED_TIMERS & TIM_N(13)
-_TIM_IRQ_HANDLER(TMR8_OVF_TMR13_IRQnHandler, 13);
+_TIM_IRQ_HANDLER(TMR8_OVF_TMR13_IRQHandler, 13);
#endif
#if USED_TIMERS & TIM_N(14)
-_TIM_IRQ_HANDLER(TMR8_TRG_HALL_TMR14_IRQnHandler, 14);
+_TIM_IRQ_HANDLER(TMR8_TRG_HALL_TMR14_IRQHandler, 14);
#endif
#if USED_TIMERS & TIM_N(20)
-_TIM_IRQ_HANDLER(TMR20_CH_IRQnHandler, 20);
+_TIM_IRQ_HANDLER(TMR20_CH_IRQHandler, 20);
#endif
void timerInit(void)
diff --git a/src/platform/AT32/timer_at32f43x.c b/src/platform/AT32/timer_at32f43x.c
index 8a4e39004d..13c4b99b87 100644
--- a/src/platform/AT32/timer_at32f43x.c
+++ b/src/platform/AT32/timer_at32f43x.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/AT32/usb_msc_at32f43x.c b/src/platform/AT32/usb_msc_at32f43x.c
index 42e7ec32ea..007dd07b0b 100644
--- a/src/platform/AT32/usb_msc_at32f43x.c
+++ b/src/platform/AT32/usb_msc_at32f43x.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are 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 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c
index 65752866c6..2325107ae2 100644
--- a/src/platform/SIMULATOR/sitl.c
+++ b/src/platform/SIMULATOR/sitl.c
@@ -57,6 +57,9 @@
#include "rx/rx.h"
+#include "io/gps.h"
+#include "io/gps_virtual.h"
+
#include "dyad.h"
#include "udplink.h"
@@ -180,6 +183,19 @@ void updateState(const fdm_packet* pkt)
#endif
#endif
+#if defined(USE_VIRTUAL_GPS)
+ const double longitude = pkt->position_xyz[0];
+ const double latitude = pkt->position_xyz[1];
+ const double altitude = pkt->position_xyz[2];
+ const double speed = sqrt(sq(pkt->velocity_xyz[0]) + sq(pkt->velocity_xyz[1]));
+ const double speed3D = sqrt(sq(pkt->velocity_xyz[0]) + sq(pkt->velocity_xyz[1]) + sq(pkt->velocity_xyz[2]));
+ double course = atan2(pkt->velocity_xyz[0], pkt->velocity_xyz[1]) * RAD2DEG;
+ if (course < 0.0) {
+ course += 360.0;
+ }
+ setVirtualGPS(latitude, longitude, altitude, speed, speed3D, course);
+#endif
+
#if defined(SIMULATOR_IMU_SYNC)
imuSetHasNewData(deltaSim*1e6);
imuUpdateAttitude(micros());
@@ -773,3 +789,9 @@ IO_t IOGetByTag(ioTag_t tag)
UNUSED(tag);
return NULL;
}
+
+const mcuTypeInfo_t *getMcuTypeInfo(void)
+{
+ static const mcuTypeInfo_t info = { .id = MCU_TYPE_SIMULATOR, .name = "SIMULATOR" };
+ return &info;
+}
diff --git a/src/platform/SIMULATOR/target/SITL/target.h b/src/platform/SIMULATOR/target/SITL/target.h
index 5a75501c8f..6fee3baee9 100644
--- a/src/platform/SIMULATOR/target/SITL/target.h
+++ b/src/platform/SIMULATOR/target/SITL/target.h
@@ -95,6 +95,10 @@
#define DEFAULT_RX_FEATURE FEATURE_RX_MSP
#define DEFAULT_FEATURES (FEATURE_GPS | FEATURE_TELEMETRY)
+#ifdef USE_GPS
+#define USE_VIRTUAL_GPS
+#endif
+
#define USE_PARAMETER_GROUPS
#ifndef USE_PWM_OUTPUT
@@ -241,8 +245,8 @@ typedef struct {
double imu_angular_velocity_rpy[3]; // rad/s -> range: +/- 8192; +/- 2000 deg/se
double imu_linear_acceleration_xyz[3]; // m/s/s NED, body frame -> sim 1G = 9.80665, FC 1G = 256
double imu_orientation_quat[4]; //w, x, y, z
- double velocity_xyz[3]; // m/s, earth frame
- double position_xyz[3]; // meters, NED from origin
+ double velocity_xyz[3]; // m/s, earth frame. ENU (Ve, Vn, Vup) for virtual GPS mode (USE_VIRTUAL_GPS)!
+ double position_xyz[3]; // meters, NED from origin. Longitude, Latitude, Altitude (ENU) for virtual GPS mode (USE_VIRTUAL_GPS)!
double pressure;
} fdm_packet;
diff --git a/src/platform/SIMULATOR/target/SITL/target.mk b/src/platform/SIMULATOR/target/SITL/target.mk
index c314728e48..23d7794138 100644
--- a/src/platform/SIMULATOR/target/SITL/target.mk
+++ b/src/platform/SIMULATOR/target/SITL/target.mk
@@ -6,7 +6,8 @@ TARGET_SRC = \
drivers/accgyro/accgyro_virtual.c \
drivers/barometer/barometer_virtual.c \
drivers/compass/compass_virtual.c \
- drivers/serial_tcp.c
+ drivers/serial_tcp.c \
+ io/gps_virtual.c
SIZE_OPTIMISED_SRC += \
drivers/serial_tcp.c
diff --git a/src/platform/STM32/bus_spi_ll.c b/src/platform/STM32/bus_spi_ll.c
index 77707ad260..16ae9b5980 100644
--- a/src/platform/STM32/bus_spi_ll.c
+++ b/src/platform/STM32/bus_spi_ll.c
@@ -76,13 +76,16 @@ static LL_SPI_InitTypeDef defaultInit =
static uint32_t spiDivisorToBRbits(const SPI_TypeDef *instance, uint16_t divisor)
{
-#if !defined(STM32H7)
- // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2.
-
+#if defined(STM32F7)
+ /* On F7 we run SPI2 and SPI3 (on the APB1 bus) at a division of PCLK1 which is
+ * half that of HCLK driving the APB2 bus and SPI1. So we need to reduce the
+ * division factors for SPI2/3 by a corresponding factor of 2.
+ */
if (instance == SPI2 || instance == SPI3) {
divisor /= 2; // Safe for divisor == 0 or 1
}
#else
+ // On the G4 and H7 processors the SPI busses are all derived from the same base frequency
UNUSED(instance);
#endif
diff --git a/src/platform/STM32/mk/STM32F4.mk b/src/platform/STM32/mk/STM32F4.mk
index 28bf0fecb6..9d1f404557 100644
--- a/src/platform/STM32/mk/STM32F4.mk
+++ b/src/platform/STM32/mk/STM32F4.mk
@@ -42,6 +42,8 @@ STDPERIPH_SRC = \
VPATH := $(VPATH):$(STDPERIPH_DIR)/src
endif
+DEVICE_FLAGS = -DSTM32F4
+
ifneq ($(TARGET_MCU),$(filter $(TARGET_MCU),STM32F411xE STM32F446xx))
STDPERIPH_SRC += stm32f4xx_fsmc.c
endif
@@ -141,25 +143,27 @@ INCLUDE_DIRS := \
$(CMSIS_DIR)/Core/Include \
$(LIB_MAIN_DIR)/STM32F4/Drivers/CMSIS/Device/ST/STM32F4xx \
$(TARGET_PLATFORM_DIR)/vcpf4
+
+DEVICE_FLAGS += -DUSE_STDPERIPH_DRIVER
endif
#Flags
ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant
ifeq ($(TARGET_MCU),STM32F411xE)
-DEVICE_FLAGS = -DSTM32F411xE -finline-limit=20
+DEVICE_FLAGS += -DSTM32F411xE -finline-limit=20
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f411.ld
STARTUP_SRC = STM32/startup/startup_stm32f411xe.s
MCU_FLASH_SIZE := 512
else ifeq ($(TARGET_MCU),STM32F405xx)
-DEVICE_FLAGS = -DSTM32F40_41xxx -DSTM32F405xx
+DEVICE_FLAGS += -DSTM32F40_41xxx -DSTM32F405xx
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f405.ld
STARTUP_SRC = STM32/startup/startup_stm32f40xx.s
MCU_FLASH_SIZE := 1024
else ifeq ($(TARGET_MCU),STM32F446xx)
-DEVICE_FLAGS = -DSTM32F446xx
+DEVICE_FLAGS += -DSTM32F446xx
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f446.ld
STARTUP_SRC = STM32/startup/startup_stm32f446xx.s
MCU_FLASH_SIZE := 512
@@ -167,7 +171,7 @@ MCU_FLASH_SIZE := 512
else
$(error Unknown MCU for F4 target)
endif
-DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -DSTM32
+DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE)
MCU_COMMON_SRC = \
common/stm32/system.c \
diff --git a/src/platform/STM32/sdio_h7xx.c b/src/platform/STM32/sdio_h7xx.c
index a5d1bb5aff..0a847898b0 100644
--- a/src/platform/STM32/sdio_h7xx.c
+++ b/src/platform/STM32/sdio_h7xx.c
@@ -103,12 +103,12 @@ static const sdioHardware_t sdioPinHardware[SDIODEV_COUNT] = {
{
.instance = SDMMC2,
.irqn = SDMMC2_IRQn,
- .sdioPinCK = { PINDEF(2, PC1, 9), PINDEF(2, PD6, 11) },
- .sdioPinCMD = { PINDEF(2, PA0, 9), PINDEF(2, PD7, 11) },
- .sdioPinD0 = { PINDEF(2, PB14, 9) },
- .sdioPinD1 = { PINDEF(2, PB15, 9) },
- .sdioPinD2 = { PINDEF(2, PB3, 9) },
- .sdioPinD3 = { PINDEF(2, PB4, 9) },
+ .sdioPinCK = { PINDEF(2, PC1, 9), PINDEF(2, PD6, 11) },
+ .sdioPinCMD = { PINDEF(2, PA0, 9), PINDEF(2, PD7, 11) },
+ .sdioPinD0 = { PINDEF(2, PB14, 9), },
+ .sdioPinD1 = { PINDEF(2, PB15, 9), },
+ .sdioPinD2 = { PINDEF(2, PB3, 9), PINDEF(2, PG11, 10) },
+ .sdioPinD3 = { PINDEF(2, PB4, 9), },
}
};
@@ -160,7 +160,11 @@ void sdioPinConfigure(void)
#undef SDIOFINDPIN
+#if defined(USE_SDIO_PULLUP)
+#define IOCFG_SDMMC IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
+#else
#define IOCFG_SDMMC IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
+#endif
void HAL_SD_MspInit(SD_HandleTypeDef* hsd)
{
@@ -281,11 +285,14 @@ static SD_Error_t SD_DoInit(void)
hsd1.Init.BusWide = SDMMC_BUS_WIDE_1B; // FIXME untested
}
hsd1.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE;
-#if defined(STM32H730xx)
- hsd1.Init.ClockDiv = 2; // 200Mhz / (2 * 2 ) = 50Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
-#else
- hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
+#if !defined(SDIO_CLOCK_DIV)
+# if defined(STM32H730xx)
+# define SDIO_CLOCK_DIV 2 // 200Mhz / (2 * 2 ) = 50Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
+# else
+# define SDIO_CLOCK_DIV 1 // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
+# endif
#endif
+ hsd1.Init.ClockDiv = SDIO_CLOCK_DIV;
status = HAL_SD_Init(&hsd1); // Will call HAL_SD_MspInit
if (status != HAL_OK) {
diff --git a/src/platform/common/stm32/bus_spi_pinconfig.c b/src/platform/common/stm32/bus_spi_pinconfig.c
index 10c6d76ae2..ee59d7cd24 100644
--- a/src/platform/common/stm32/bus_spi_pinconfig.c
+++ b/src/platform/common/stm32/bus_spi_pinconfig.c
@@ -184,6 +184,7 @@ const spiHardware_t spiHardware[] = {
.misoPins = {
{ DEFIO_TAG_E(PA6), GPIO_AF5_SPI1 },
{ DEFIO_TAG_E(PB4), GPIO_AF5_SPI1 },
+ { DEFIO_TAG_E(PG9), GPIO_AF5_SPI1 },
},
.mosiPins = {
{ DEFIO_TAG_E(PA7), GPIO_AF5_SPI1 },
diff --git a/src/platform/common/stm32/system.c b/src/platform/common/stm32/system.c
index 634fd07b17..d21e0ee956 100644
--- a/src/platform/common/stm32/system.c
+++ b/src/platform/common/stm32/system.c
@@ -322,3 +322,66 @@ void unusedPinsInit(void)
{
IOTraversePins(unusedPinInit);
}
+
+const mcuTypeInfo_t *getMcuTypeInfo(void)
+{
+ static const mcuTypeInfo_t info[] = {
+#if defined(STM32H743xx)
+ { .id = MCU_TYPE_H743_REV_UNKNOWN, .name = "STM32H743 (Rev Unknown)" },
+ { .id = MCU_TYPE_H743_REV_Y, .name = "STM32H743 (Rev.Y)" },
+ { .id = MCU_TYPE_H743_REV_X, .name = "STM32H743 (Rev.X)" },
+ { .id = MCU_TYPE_H743_REV_V, .name = "STM32H743 (Rev.V)" },
+#elif defined(STM32F40_41xxx)
+ { .id = MCU_TYPE_F40X, .name = "STM32F40X" },
+#elif defined(STM32F411xE)
+ { .id = MCU_TYPE_F411, .name = "STM32F411" },
+#elif defined(STM32F446xx)
+ { .id = MCU_TYPE_F446, .name = "STM32F446" },
+#elif defined(STM32F722xx)
+ { .id = MCU_TYPE_F722, .name = "STM32F722" },
+#elif defined(STM32F745xx)
+ { .id = MCU_TYPE_F745, .name = "STM32F745" },
+#elif defined(STM32F746xx)
+ { .id = MCU_TYPE_F746, .name = "STM32F746" },
+#elif defined(STM32F765xx)
+ { .id = MCU_TYPE_F765, .name = "STM32F765" },
+#elif defined(STM32H750xx)
+ { .id = MCU_TYPE_H750, .name = "STM32H750" },
+#elif defined(STM32H730xx)
+ { .id = MCU_TYPE_H730, .name = "STM32H730" },
+#elif defined(STM32H7A3xx) || defined(STM32H7A3xxQ)
+ { .id = MCU_TYPE_H7A3, .name = "STM32H7A3" },
+#elif defined(STM32H723xx) || defined(STM32H725xx)
+ { .id = MCU_TYPE_H723_725, .name = "STM32H723/H725" },
+#elif defined(STM32G474xx)
+ { .id = MCU_TYPE_G474, .name = "STM32G474" },
+#elif defined(AT32F435G)
+ { .id = MCU_TYPE_AT32F435G, .name = "AT32F435G" },
+#elif defined(AT32F435M)
+ { .id = MCU_TYPE_AT32F435M, .name = "AT32F435M" },
+#elif defined(APM32F405)
+ { .id = MCU_TYPE_APM32F405, .name = "APM32F405" },
+#elif defined(APM32F407)
+ { .id = MCU_TYPE_APM32F407, .name = "APM32F407" },
+#else
+#error MCU Type info not defined for STM (or clone)
+#endif
+ };
+ unsigned revision = 0;
+#if defined(STM32H743xx)
+ switch (HAL_GetREVID()) {
+ case REV_ID_Y:
+ revision = 1;
+ break;
+ case REV_ID_X:
+ revision = 2;
+ break;
+ case REV_ID_V:
+ revision = 3;
+ break;
+ default:
+ revision = 0;
+ }
+#endif
+ return info + revision;
+}
diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc
index c756929c8c..4b8417148d 100644
--- a/src/test/unit/cli_unittest.cc
+++ b/src/test/unit/cli_unittest.cc
@@ -342,7 +342,7 @@ void schedulerResetTaskMaxExecutionTime(taskId_e) {}
void schedulerResetCheckFunctionMaxExecutionTime(void) {}
const char * const targetName = "UNITTEST";
-const char* const buildDate = "Jan 01 2017";
+const char * const buildDate = "Jan 01 2017";
const char * const buildTime = "00:00:00";
const char * const shortGitRevision = "MASTER";
@@ -401,6 +401,7 @@ bool isModeActivationConditionConfigured(const modeActivationCondition_t *, cons
void delay(uint32_t) {}
displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; }
mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; }
+const char *getMcuTypeName(void) { return targetName; }
uint16_t getCurrentRxRateHz(void) { return 0; }
uint16_t getAverageSystemLoadPercent(void) { return 0; }
bool getRxRateValid(void) { return false; }