Skip to content

Instantly share code, notes, and snippets.

@nerdCopter

nerdCopter/diff_robertb-hesp-performance-with-branch_411.diff

Created November 20, 2022 17:17
Show Gist options
  • Save nerdCopter/91270c6a1d45a653efc8082d33aadd69 to your computer and use it in GitHub Desktop.
Save nerdCopter/91270c6a1d45a653efc8082d33aadd69 to your computer and use it in GitHub Desktop.
Download ZIP
diff_robertb-hesp-performance-with-branch_411.diff
Raw
diff_robertb-hesp-performance-with-branch_411.diff
index e498012a5..9fd743b22
--- a/make/mcu/STM32F3.mk
+++ b/make/mcu/STM32F3.mk
@@ -87,9 +87,10 @@ MCU_COMMON_SRC = \
drivers/pwm_output_dshot.c \
drivers/pwm_output_dshot_shared.c \
drivers/serial_uart_stdperiph.c \
drivers/serial_uart_stm32f30x.c \
drivers/system_stm32f30x.c \
- drivers/timer_stm32f30x.c
+ drivers/timer_stm32f30x.c \
+ drivers/persistent.c
DSP_LIB := $(ROOT)/lib/main/CMSIS/DSP
DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM4
!🟥^^^^^^^^^^^^^^^^^ UNKNOWN skipped ^^^^^^^^^^^^^^^^^^
index 6d8f71568..e3c747612
--- a/make/mcu/STM32F7.mk
+++ b/make/mcu/STM32F7.mk
@@ -12,12 +12,10 @@ CMSIS_DIR := $(ROOT)/lib/main/CMSIS
#STDPERIPH
STDPERIPH_DIR = $(ROOT)/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver
STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/Src/*.c))
EXCLUDES = stm32f7xx_hal_can.c \
stm32f7xx_hal_cec.c \
- stm32f7xx_hal_crc.c \
- stm32f7xx_hal_crc_ex.c \
stm32f7xx_hal_cryp.c \
stm32f7xx_hal_cryp_ex.c \
stm32f7xx_hal_dcmi.c \
stm32f7xx_hal_dcmi_ex.c \
stm32f7xx_hal_dfsdm.c \
!🟥^^^^^^^^^^^^^^^^^ done -- found required dutring compile phased ^^^^^^^^^^^^^^^^^^
index 89d3e97f5..c88e3fc53
--- a/src/link/stm32_flash.ld
+++ b/src/link/stm32_flash.ld
@@ -51,10 +51,23 @@ SECTIONS
. = ALIGN(4);
_etext = .; /* define a global symbols at end of code */
} >FLASH
+ /* Critical program code goes into CCM RAM */
+ /* Copy specific fast-executing code to CCM RAM */
+ ccm_code = LOADADDR(.ccm_code);
+ .ccm_code :
+ {
+ . = ALIGN(4);
+ ccm_code_start = .;
+ *(.ccm_code)
+ *(.ccm_code*)
+ . = ALIGN(4);
+ ccm_code_end = .;
+ } >CCM AT >FLASH
+
.ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
.ARM : {
__exidx_start = .;
*(.ARM.exidx*)
!🟥^^^^^^^^^^^^^^^^^ UNKNOWN skipped ^^^^^^^^^^^^^^^^^^
index f226f4142..4db818d31
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -91,6 +91,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"BARO",
"GPS_RESCUE_THROTTLE_PID",
"DYN_IDLE",
"FF_LIMIT",
"FF_INTERPOLATED",
+ "DYN_LPF2",
};
!🟥^^^^^^^^^^^^^^^^^ Out of Scope - skipped ^^^^^^^^^^^^^^^^^^
index 2a3f4e993..3b63d72f0
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -107,9 +107,10 @@ typedef enum {
DEBUG_BARO,
DEBUG_GPS_RESCUE_THROTTLE_PID,
DEBUG_DYN_IDLE,
DEBUG_FF_LIMIT,
DEBUG_FF_INTERPOLATED,
+ DEBUG_DYN_LPF2,
DEBUG_COUNT
} debugType_e;
extern const char * const debugModeNames[DEBUG_COUNT];
!🟥^^^^^^^^^^^^^^^^^ Out of Scope - skipped ^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ done -- git checkout file ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
!🟥 ↓↓↓↓↓↓↓↓ git checkout robertb-hesp-performancef3-master -- src/main/cli/cli.c ↓↓↓↓↓↓↓↓
index b55bca0f5..17fe0e58b
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -69,10 +69,11 @@ bool cliMode = false;
#include "drivers/pwm_output_dshot_shared.h"
#include "drivers/camera_control.h"
#include "drivers/compass/compass.h"
#include "drivers/display.h"
#include "drivers/dma.h"
+#include "drivers/dma_spi.h"
#include "drivers/flash.h"
#include "drivers/inverter.h"
#include "drivers/io.h"
#include "drivers/io_impl.h"
#include "drivers/light_led.h"
@@ -172,10 +173,14 @@ bool cliMode = false;
#include "telemetry/frsky_hub.h"
#include "telemetry/telemetry.h"
#include "cli.h"
+#ifdef USE_GYRO_IMUF9001
+#include "drivers/accgyro/accgyro_imuf9001.h"
+#endif
+
static serialPort_t *cliPort = NULL;
#ifdef STM32F1
#define CLI_IN_BUFFER_SIZE 128
#else
@@ -212,10 +217,19 @@ static bool signatureUpdated = false;
#endif // USE_BOARD_INFO
static const char* const emptyName = "-";
static const char* const emptyString = "";
+#ifdef USE_GYRO_IMUF9001
+#define IMUF_CUSTOM_BUFF_LENGTH 26000
+static uint8_t imuf_custom_buff[IMUF_CUSTOM_BUFF_LENGTH];
+static uint32_t imuf_buff_ptr = 0;
+static uint32_t imuf_checksum = 0;
+static int imuf_bin_safe = 0;
+
+#endif
+
#if !defined(USE_CUSTOM_DEFAULTS)
#define CUSTOM_DEFAULTS_START ((char*)0)
#define CUSTOM_DEFAULTS_END ((char *)0)
#else
extern char __custom_defaults_start;
@@ -3390,10 +3404,68 @@ void cliRxBind(char *cmdline){
#endif
}
}
#endif
+static void hex2byte(char *string, uint8_t *output)
+{
+ char tempBuff[3];
+ tempBuff[0] = string[0];
+ tempBuff[1] = string[1];
+ tempBuff[2] = 0;
+ *output = (uint8_t)strtol(tempBuff, NULL, 16);
+}
+
+
+#ifdef MSP_OVER_CLI
+sbuf_t buft;
+uint8_t bufPtr[256];
+
+void cliMsp(char *cmdline){
+ int len = strlen(cmdline);
+ if (len == 0) {
+ cliPrintLine("No MSP command present");
+
+ return;
+ } else {
+ uint8_t mspCommand = atoi(cmdline);
+ uint8_t start = 2;
+ if (mspCommand > 99) {
+ start = 4;
+ } else if (mspCommand > 9) {
+ start= 3;
+ }
+ uint8_t inBuff[len];
+ uint8_t output;
+ for (int i = 0; i < len; i++) {
+ hex2byte(&cmdline[(i*2) + start], &output);
+ inBuff[i] = output;
+ }
+ sbuf_t inBuf = {.ptr = inBuff, .end = &inBuff[len-1]};
+ //TODO need to fill inPtr with the rest of the bytes from the command line
+
+ buft.ptr = buft.end = bufPtr;
+ if (mspCommonProcessOutCommand(mspCommand, &buft, NULL) || mspProcessOutCommand(mspCommand, &buft)
+ || mspCommonProcessInCommand(mspCommand, &inBuf, NULL) > -1 || mspProcessInCommand(mspCommand, &inBuf) > -1)
+ {
+
+ bufWriterAppend(cliWriter, '.'); //"." is success
+ bufWriterAppend(cliWriter, mspCommand); //msp command sent
+ bufWriterAppend(cliWriter, inBuf.ptr - inBuf.end); //msp command sent
+ bufWriterAppend(cliWriter, buft.ptr - buft.end); //number of chars
+
+ while (buft.end <= buft.ptr)
+ bufWriterAppend(cliWriter, *(buft.end)++); //send data
+ }
+ else
+ {
+ bufWriterAppend(cliWriter, '!'); //"!" is failure
+ }
+ }
+}
+#endif
+
static void printMap(dumpFlags_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig, const char *headingStr)
{
bool equalsDefault = true;
char buf[16];
char bufDefault[16];
@@ -3544,10 +3616,120 @@ static void cliExit(char *cmdline)
// incase a motor was left running during motortest, clear it here
mixerResetDisarmedMotors();
cliReboot();
}
+
+#ifdef USE_GYRO_IMUF9001
+
+
+static void cliImufBootloaderMode(char *cmdline)
+{
+ (void)(cmdline);
+ if(imufBootloader())
+ {
+ cliPrintLine("BOOTLOADER");
+ }
+ else
+ {
+ cliPrintLine("FAIL");
+ }
+}
+
+
+static void cliImufLoadBin(char *cmdline)
+{
+ #define TEMP_BUFF 256
+ uint32_t dataSize;
+ uint8_t output;
+ uint8_t dataBuff[TEMP_BUFF] = {0,};
+ uint32_t x;
+
+ if(cmdline[0] == '!')
+ {
+ imuf_bin_safe = 1;
+ imuf_buff_ptr = 0;
+ imuf_checksum = 0;
+ memset(imuf_custom_buff, 0, IMUF_CUSTOM_BUFF_LENGTH);
+ cliPrintLine("SUCCESS");
+ }
+ else if(cmdline[0] == '.')
+ {
+ cliPrintLinef("%d", imuf_buff_ptr);
+ }
+ else if(cmdline[0] == 'c')
+ {
+ cliPrintLinef("%d", imuf_checksum);
+ }
+ else if(cmdline[0] == 'l')
+ {
+ if (imuf_bin_safe)
+ {
+ //get the datasize
+ hex2byte(&cmdline[1], &output);
+ dataSize = ((output & 0xff) << 0 );
+ hex2byte(&cmdline[3], &output);
+ dataSize += ((output & 0xff) << 8 );
+ hex2byte(&cmdline[5], &output);
+ dataSize += ((output & 0xff) << 16);
+ hex2byte(&cmdline[7], &output);
+ dataSize += ((output & 0xff) << 24);
+
+ if(dataSize < TEMP_BUFF)
+ {
+ //fill the temp buffer
+ for(x=0; x< dataSize; x++)
+ {
+ hex2byte(&cmdline[(x*2)+9], &output);
+ dataBuff[x] = output;
+ imuf_checksum += output;
+ //cliPrintLinef("out:%d:%d:%d:%d", dataSize, x, (x*2)+9, output, checksum);
+ }
+ if ( (imuf_buff_ptr+dataSize) < IMUF_CUSTOM_BUFF_LENGTH )
+ {
+ memcpy(imuf_custom_buff+imuf_buff_ptr, dataBuff, dataSize);
+ imuf_buff_ptr += dataSize;
+ cliPrintLine("LOADED");
+ }
+ else
+ {
+ cliPrintLine("WOAH!");
+ }
+ }
+ else
+ {
+ cliPrintLine("CRAP!");
+ }
+ }
+ else
+ {
+ cliPrintLine("PFFFT!");
+ }
+ }
+}
+
+static void cliImufFlashBin(char *cmdline)
+{
+ (void)(cmdline);
+ if (imufUpdate(imuf_custom_buff, imuf_buff_ptr))
+ {
+ cliPrintLine("SUCCESS");
+ bufWriterFlush(cliWriter);
+ delay(5000);
+
+ *cliBuffer = '\0';
+ bufferIndex = 0;
+ cliMode = 0;
+ // incase a motor was left running during motortest, clear it here
+ mixerResetDisarmedMotors();
+ cliReboot();
+
+ cliWriter = NULL;
+ }
+}
+#endif
+
#ifdef USE_GPS
static void cliGpsPassthrough(char *cmdline)
{
UNUSED(cmdline);
@@ -4625,11 +4807,11 @@ static void cliStatus(char *cmdline)
cliPrintLinef("Config size: %d, Max available config: %d", getEEPROMConfigSize(), getEEPROMStorageSize());
// Sensors
-#if defined(USE_SENSOR_NAMES)
+#if defined(USE_SENSOR_NAMES) && !defined(USE_GYRO_IMUF9001)
const uint32_t detectedSensorsMask = sensorsMask();
for (uint32_t i = 0; ; i++) {
if (sensorTypeNames[i] == NULL) {
break;
}
@@ -4647,10 +4829,16 @@ static void cliStatus(char *cmdline)
}
#endif
}
}
cliPrintLinefeed();
+#else
+ #if defined(USE_GYRO_IMUF9001)
+ UNUSED(sensorHardwareNames);
+ UNUSED(sensorTypeNames);
+ cliPrintf(" | IMU-F Version: %lu", imufCurrentVersion);
+ #endif
#endif /* USE_SENSOR_NAMES */
// Uptime and wall clock
cliPrintf("System Uptime: %d seconds", millis() / 1000);
@@ -4781,10 +4969,14 @@ static void cliVersion(char *cmdline)
cliPrintLinef(" / FEATURE CUT LEVEL %d", FEATURE_CUT_LEVEL);
#else
cliPrintLinefeed();
#endif
+#ifdef USE_GYRO_IMUF9001
+ cliPrintLinef("# IMU-F Version: %lu", imufCurrentVersion);
+#endif
+
#ifdef USE_UNIFIED_TARGET
cliPrint("# ");
#ifdef USE_BOARD_INFO
if (strlen(getManufacturerId())) {
cliPrintf("manufacturer_id: %s ", getManufacturerId());
@@ -6237,10 +6429,14 @@ typedef struct {
name, \
method \
}
#endif
+#ifdef USE_GYRO_IMUF9001
+static void cliReportImufErrors(char *cmdline);
+#endif
+
static void cliHelp(char *cmdline);
// should be sorted a..z for bsearch()
const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", "<index> <unused> <range channel> <start> <end> <function> <select channel> [<center> <scale>]", cliAdjustmentRange),
@@ -6317,10 +6513,15 @@ const clicmd_t cmdTable[] = {
#endif
#if defined(USE_GYRO_REGISTER_DUMP) && !defined(SIMULATOR_BUILD)
CLI_COMMAND_DEF("gyroregisters", "dump gyro config registers contents", NULL, cliDumpGyroRegisters),
#endif
CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
+#ifdef USE_GYRO_IMUF9001
+ CLI_COMMAND_DEF("imufbootloader", NULL, NULL, cliImufBootloaderMode),
+ CLI_COMMAND_DEF("imufloadbin", NULL, NULL, cliImufLoadBin),
+ CLI_COMMAND_DEF("imufflashbin", NULL, NULL, cliImufFlashBin),
+#endif
#ifdef USE_LED_STRIP_STATUS_MODE
CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed),
#endif
#if defined(USE_BOARD_INFO)
CLI_COMMAND_DEF("manufacturer_id", "get / set the id of the board manufacturer", "[manufacturer id]", cliManufacturerId),
@@ -6351,10 +6552,13 @@ const clicmd_t cmdTable[] = {
#ifdef USE_RC_SMOOTHING_FILTER
CLI_COMMAND_DEF("rc_smoothing_info", "show rc_smoothing operational settings", NULL, cliRcSmoothing),
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_RESOURCE_MGMT
CLI_COMMAND_DEF("resource", "show/set resources", "<> | <resource name> <index> [<pin>|none] | show [all]", cliResource),
+#endif
+#ifdef USE_GYRO_IMUF9001
+ CLI_COMMAND_DEF("reportimuferrors", "report imu-f comm errors", NULL, cliReportImufErrors),
#endif
CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFailsafe),
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
#ifdef USE_SDCARD
@@ -6399,10 +6603,19 @@ const clicmd_t cmdTable[] = {
#ifdef USE_VTX_TABLE
CLI_COMMAND_DEF("vtxtable", "vtx frequency table", "<band> <bandname> <bandletter> [FACTORY|CUSTOM] <freq> ... <freq>\r\n", cliVtxTable),
#endif
};
+#ifdef USE_GYRO_IMUF9001
+static void cliReportImufErrors(char *cmdline)
+{
+ UNUSED(cmdline);
+ cliPrintf("Current Comm Errors: %lu", crcErrorCount);
+ cliPrintLinefeed();
+}
+#endif
+
static void cliHelp(char *cmdline)
{
UNUSED(cmdline);
for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) {
!🟥^^^^^^^^^^^ git checkout ^^^ cli.c ^^^^^^^^^^^^^^^^
!🟥 git checkout robertb-hesp-performancef3-master -- src/main/cli/cli.c
!🟥^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ skipping this section ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 4a73815d3..61e891260
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -340,10 +340,15 @@ static const char * const lookupOverclock[] = {
"192MHZ", "216MHZ", "240MHZ"
#elif defined(STM32F411xE)
"108MHZ", "120MHZ"
#elif defined(STM32F7)
"240MHZ"
+#elif defined(STM32F3)
+ "80MHZ", "88MHZ", "96MHZ", "104MHZ", "112MHZ", "120MHZ", "128MHZ",
+#ifdef USE_VCP
+ "80MHZ_VCP", "88MHZ_VCP", "96MHZ_VCP", "104MHZ_VCP", "112MHZ_VCP", "120MHZ_VCP", "128MHZ_VCP",
+#endif
#endif
};
#endif
#ifdef USE_LED_STRIP
@@ -630,10 +635,13 @@ const clivalue_t valueTable[] = {
#endif
#ifdef USE_YAW_SPIN_RECOVERY
{ "yaw_spin_recovery", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_recovery) },
{ "yaw_spin_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 500, 1950 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) },
#endif
+#ifdef USE_DYN_LPF2
+ { "dynlpf2_enable", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dynlpf2_enable) },
+#endif
#ifdef USE_MULTI_GYRO
{ "gyro_to_use", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif
#if defined(USE_GYRO_DATA_ANALYSE)
@@ -955,10 +963,11 @@ const clivalue_t valueTable[] = {
{ "gps_rescue_use_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
#endif
#endif
#endif
+ { "i_decay", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 6 }, PG_PID_PROFILE, offsetof(pidProfile_t, i_decay) },
{ "deadband", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) },
{ "yaw_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) },
{ "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) },
// PG_PID_CONFIG
@@ -998,10 +1007,11 @@ const clivalue_t valueTable[] = {
{ "crash_delay", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_delay) },
{ "crash_recovery_angle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 5, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_angle) },
{ "crash_recovery_rate", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 50, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_rate) },
{ "crash_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_limit_yaw) },
{ "crash_recovery", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) },
+ { "crash_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_relax) },
{ "iterm_rotation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) },
#if defined(USE_ITERM_RELAX)
{ "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
{ "iterm_relax_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
!🟥^^^^^^^^^^^^^^^^ SKIPPING in entirety ^^^^^^^^^^^^^^^^^^^^^^^
index 13d089c1f..ba5f23685
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -71,10 +71,14 @@ static uint16_t tempPidF[3];
static uint8_t tmpRateProfileIndex;
static uint8_t rateProfileIndex;
static char rateProfileIndexString[MAX_RATE_PROFILE_NAME_LENGTH + 5];
static controlRateConfig_t rateProfile;
+static const char * const cms_offOnLabels[] = {
+ "OFF", "ON"
+};
+
!🟥^^^^^^^^^^^^^^^ done - required ^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ skipped nonsense ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
static const char * const osdTableThrottleLimitType[] = {
"OFF", "SCALE", "CLIP"
};
#if defined(USE_GYRO_DATA_ANALYSE) && defined(USE_EXTENDED_CMS_MENUS)
@@ -297,10 +301,15 @@ static uint8_t cmsx_launchControlMode;
static uint8_t cmsx_launchControlAllowTriggerReset;
static uint8_t cmsx_launchControlThrottlePercent;
static uint8_t cmsx_launchControlAngleLimit;
static uint8_t cmsx_launchControlGain;
+#ifndef USE_GYRO_IMUF9001
+static uint16_t gyroConfig_gyro_filter_q;
+static uint16_t gyroConfig_gyro_filter_r;
+#endif
+
static long cmsx_launchControlOnEnter(void)
{
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
cmsx_launchControlMode = pidProfile->launchControlMode;
@@ -334,11 +343,16 @@ static const OSD_Entry cmsx_menuLaunchControlEntries[] = {
{ "ALLOW RESET", OME_Bool, NULL, &cmsx_launchControlAllowTriggerReset, 0 },
{ "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePercent, 0, LAUNCH_CONTROL_THROTTLE_TRIGGER_MAX, 1 } , 0 },
{ "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit, 0, 80, 1 } , 0 },
{ "ITERM GAIN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain, 0, 200, 1 } , 0 },
- { "BACK", OME_Back, NULL, NULL, 0 },
+#ifndef USE_GYRO_IMUF9001
+ { "KALMAN Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_filter_q, 0, 16000, 1 }, 0 },
+ { "KALMAN R", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_filter_r, 0, 16000, 1 }, 0 },
+#endif
+
+{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
};
!🟥^^^^^^^^^^^^^^^^ skipped -- nonsensical ^^^^^^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ done ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
static CMS_Menu cmsx_menuLaunchControl = {
#ifdef CMS_MENU_DEBUG
@@ -371,10 +385,65 @@ static uint8_t cmsx_d_min_advance;
static uint8_t cmsx_iterm_relax;
static uint8_t cmsx_iterm_relax_type;
static uint8_t cmsx_iterm_relax_cutoff;
#endif
+//
+// SPRING Imuf
+//
+
+#if defined(USE_GYRO_IMUF9001)
+static uint16_t gyroConfig_imuf_roll_q;
+static uint16_t gyroConfig_imuf_pitch_q;
+static uint16_t gyroConfig_imuf_yaw_q;
+static uint16_t gyroConfig_imuf_w;
+static uint16_t gyroConfig_imuf_pitch_lpf_cutoff_hz;
+static uint16_t gyroConfig_imuf_roll_lpf_cutoff_hz;
+static uint16_t gyroConfig_imuf_yaw_lpf_cutoff_hz;
+static uint8_t gyroConfig_imuf_roll_af;
+static uint8_t gyroConfig_imuf_pitch_af;
+static uint8_t gyroConfig_imuf_yaw_af;
+#endif
+
+#if defined(USE_GYRO_IMUF9001)
+static long cmsx_menuImuf_onEnter(void)
+{
+ gyroConfig_imuf_roll_q = gyroConfig()->imuf_roll_q;
+ gyroConfig_imuf_pitch_q = gyroConfig()->imuf_pitch_q;
+ gyroConfig_imuf_yaw_q = gyroConfig()->imuf_yaw_q;
+ gyroConfig_imuf_w = gyroConfig()->imuf_w;
+ gyroConfig_imuf_pitch_lpf_cutoff_hz = gyroConfig()->imuf_pitch_lpf_cutoff_hz;
+ gyroConfig_imuf_roll_lpf_cutoff_hz = gyroConfig()->imuf_roll_lpf_cutoff_hz;
+ gyroConfig_imuf_yaw_lpf_cutoff_hz = gyroConfig()->imuf_yaw_lpf_cutoff_hz;
+ gyroConfig_imuf_roll_af = gyroConfigMutable()->imuf_roll_af;
+ gyroConfig_imuf_pitch_af = gyroConfigMutable()->imuf_pitch_af;
+ gyroConfig_imuf_yaw_af = gyroConfigMutable()->imuf_yaw_af;
+
+ return 0;
+}
+#endif
+#if defined(USE_GYRO_IMUF9001)
+static long cmsx_menuImuf_onExit(const OSD_Entry *self)
+{
+ UNUSED(self);
+
+ gyroConfigMutable()->imuf_roll_q = gyroConfig_imuf_roll_q;
+ gyroConfigMutable()->imuf_pitch_q = gyroConfig_imuf_pitch_q;
+ gyroConfigMutable()->imuf_yaw_q = gyroConfig_imuf_yaw_q;
+ gyroConfigMutable()->imuf_w = gyroConfig_imuf_w;
+ gyroConfigMutable()->imuf_roll_lpf_cutoff_hz = gyroConfig_imuf_roll_lpf_cutoff_hz;
+ gyroConfigMutable()->imuf_pitch_lpf_cutoff_hz = gyroConfig_imuf_pitch_lpf_cutoff_hz;
+ gyroConfigMutable()->imuf_yaw_lpf_cutoff_hz = gyroConfig_imuf_yaw_lpf_cutoff_hz;
+ gyroConfigMutable()->imuf_roll_af = gyroConfig_imuf_roll_af;
+ gyroConfigMutable()->imuf_pitch_af = gyroConfig_imuf_pitch_af;
+ gyroConfigMutable()->imuf_yaw_af = gyroConfig_imuf_yaw_af;
+
+ return 0;
+}
+#endif
+
+
static long cmsx_profileOtherOnEnter(void)
{
setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
@@ -483,10 +552,44 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
};
+#if defined(USE_GYRO_IMUF9001)
+static OSD_Entry cmsx_menuImufEntries[] =
+{
+ { "-- SPRING IMU-F --", OME_Label, NULL, NULL, 0 },
+ { "-- CHANGES REQUIRE REBOOT --", OME_Label, NULL, NULL, 0 },
+ { "IMUF W", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_w, 0, 300, 1 }, 0 },
+ { "ROLL Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q, 0, 16000, 50 }, 0 },
+ { "PITCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_q, 0, 16000, 50 }, 0 },
+ { "YAW Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_q, 0, 16000, 50 }, 0 },
+ { "ROLL LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_lpf_cutoff_hz, 0, 450, 1 }, 0 },
+ { "PITCH LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_lpf_cutoff_hz, 0, 450, 1 }, 0 },
+ { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_lpf_cutoff_hz, 0, 450, 1 }, 0 },
+ { "ROLL AF", OME_TAB, NULL, &(OSD_TAB_t) { &gyroConfig_imuf_roll_af, 0, cms_offOnLabels}, 0 },
+ { "PITCH AF", OME_TAB, NULL, &(OSD_TAB_t) { &gyroConfig_imuf_pitch_af, 0, cms_offOnLabels}, 0 },
+ { "YAW AF", OME_TAB, NULL, &(OSD_TAB_t) { &gyroConfig_imuf_yaw_af, 0, cms_offOnLabels}, 0 },
+
+ { "BACK", OME_Back, NULL, NULL, 0},
+ { "SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVEREBOOT, 0},
+ { NULL, OME_END, NULL, NULL, 0 }
+};
+#endif
+
+#if defined(USE_GYRO_IMUF9001)
+static CMS_Menu cmsx_menuImuf = {
+#ifdef CMS_MENU_DEBUG
+ .GUARD_text = "XIMUF",
+ .GUARD_type = OME_MENU,
+#endif
+ .onEnter = cmsx_menuImuf_onEnter,
+ .onExit = cmsx_menuImuf_onExit,
+ .entries = cmsx_menuImufEntries,
+};
+#endif
+
static CMS_Menu cmsx_menuProfileOther = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XPROFOTHER",
.GUARD_type = OME_MENU,
#endif
!🟥 ^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ skiping DynLPF2 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
@@ -502,20 +605,27 @@ static uint16_t gyroConfig_gyro_soft_notch_hz_1;
static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
static uint16_t gyroConfig_gyro_soft_notch_hz_2;
static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
static uint8_t gyroConfig_gyro_to_use;
+#ifdef USE_DYN_LPF2
+static uint8_t gyroConfig_dynlpf2_dynlpf2_enable;
+#endif
+
static long cmsx_menuGyro_onEnter(void)
{
gyroConfig_gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
gyroConfig_gyro_lowpass2_hz = gyroConfig()->gyro_lowpass2_hz;
gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use;
+#ifdef USE_DYN_LPF2
+ gyroConfig_dynlpf2_dynlpf2_enable = gyroConfig()->dynlpf2_enable;
+#endif
return 0;
}
static long cmsx_menuGyro_onExit(const OSD_Entry *self)
{
@@ -527,10 +637,13 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self)
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use;
+#ifdef USE_DYN_LPF2
+ gyroConfigMutable()->dynlpf2_enable = gyroConfig_dynlpf2_dynlpf2_enable;
+#endif
return 0;
}
static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
{
@@ -546,10 +659,13 @@ static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
#ifdef USE_MULTI_GYRO
{ "GYRO TO USE", OME_TAB, NULL, &(OSD_TAB_t) { &gyroConfig_gyro_to_use, 2, osdTableGyroToUse}, REBOOT_REQUIRED },
#endif
+#ifdef USE_DYN_LPF2
+ { "DLPF2 ENABLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_dynlpf2_dynlpf2_enable, 0, 1, 1 }, 0 },
+#endif
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
};
static CMS_Menu cmsx_menuFilterGlobal = {
!🟥^^^^^^^^^^^^^^^ SKIPPING DYN_LPF2 ^^^^^^^^^^^^^^^^^
@@ -792,10 +908,15 @@ static const OSD_Entry cmsx_menuImuEntries[] =
{"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, CONTROL_RATE_PROFILE_COUNT, 1}, 0},
{"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0},
{"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0},
+
+#if defined(USE_GYRO_IMUF9001)
+ {"IMUF", OME_Submenu, cmsMenuChange, &cmsx_menuImuf, 0},
+#endif
+
#if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
{"DYN FILT", OME_Submenu, cmsMenuChange, &cmsx_menuDynFilt, 0},
#endif
#ifdef USE_EXTENDED_CMS_MENUS
!🟥^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ skipping DYNLPF2 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 000000000..e742e48d7
--- /dev/null
+++ b/src/main/common/dynLpf2.c
@@ -0,0 +1,140 @@
+
+#include <stdbool.h>
+
+#include "platform.h"
+#include "math.h"
+
+#include "dynLpf2.h"
+
+#include "build/debug.h"
+
+#include "common/filter.h"
+
+#include "sensors/gyro.h"
+
+#ifdef USE_DYN_LPF2
+//DEFINITIONS
+//-----------
+#define Sqr(x) ((x)*(x))
+
+//TYPES
+//-----
+typedef struct ABGF_s {
+ float a, b, g;
+ float ak_1, vk_1, xk_1;
+ // real32 dT, dT2;
+} filterStructABG;
+
+typedef enum {
+ CRITICAL_DAMPED = 0,
+ UNDER_DAMPED,
+} eABGF;
+
+
+//VARIABLES
+//---------
+static filterStructABG agbFilter[3];
+static float gyroDt;
+//________________________________________________________________________________________
+
+// Robert Bouwens
+
+// www.megamanual.com/alphabeta.htm
+// Alpha, Beta and Gamma can be considered to proportional, first and second derivative terms respectively
+// Alpha 0.5..1.5 (0.9) Beta < 1.0 (0.8) Gamma < 0.5 (0.1)
+
+
+void initABGLPF(filterStructABG *F, float alpha, eABGF ftype) {
+
+ F->xk_1 = 0.0f;
+ F->vk_1 = 0.0f;
+ F->ak_1 = 0.0f;
+
+ if (ftype == CRITICAL_DAMPED) {
+ // near critically damped F
+ const float beta = 0.8f
+ * (2.0f - Sqr(alpha) - 2.0f * sqrtf(1.0f - Sqr(alpha)))
+ / (Sqr(alpha));
+ F->a = alpha;
+ F->b = beta;
+ F->g = Sqr(beta) / (alpha * 2.0f);
+ } else {
+ const float beta = Sqr(alpha) / (2.0f - alpha); // standard, under damped beta value
+ F->a = alpha;
+ F->b = beta;
+ F->g = Sqr(beta) / (alpha * 2.0f);
+ }
+
+} // ABGInit
+
+float ABGLPF(filterStructABG *F, float input, float dT) {
+ float x0 = F->xk_1;
+ const float dT2 = Sqr(dT);
+
+ // update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
+ F->xk_1 += dT * F->vk_1 + (0.5f * dT2 * F->ak_1);
+ // update (estimated) velocity
+ F->vk_1 += dT * F->ak_1;
+ // what is our residual error (measured - estimated)
+ const float rk = input - F->xk_1;
+ // update our estimates given the residual error.
+ F->xk_1 += F->a * rk; // prediction
+ F->xk_1 += F->a * (x0 - F->xk_1); // correction
+
+ float v0 = F->vk_1;
+ F->vk_1 += (F->b / dT) * rk; // prediction
+ F->vk_1 += F->b * (v0 - F->vk_1); // correction
+
+ if (F->g != 0.0f) {
+ F->ak_1 += (F->g / (2.0f * dT2)) * rk; // prediction
+ F->ak_1 += (F->g / dT) * (v0 - F->vk_1); // correction
+ }
+
+ return F->xk_1;
+
+} // ABG
+
+
+
+//////////////////////////////
+// //
+// DYN PT1 INIT //
+// //
+//////////////////////////////
+void init_dynLpf2(void)
+{
+ const float ABGalpha = 0.5f;
+ const uint8_t ABGType = UNDER_DAMPED;
+
+ gyroDt = gyro.targetLooptime * 1e-6f;
+ initABGLPF(&agbFilter[0], ABGalpha, ABGType);
+ initABGLPF(&agbFilter[1], ABGalpha, ABGType);
+ initABGLPF(&agbFilter[2], ABGalpha, ABGType);
+}
+
+
+//////////////////////////////
+// //
+// DYN LPF2 APPLY //
+// //
+//////////////////////////////
+
+FAST_CODE float dynLpf2Apply(int axis, float input)
+{
+ float output;
+
+ // Apply filter if filter is enable.
+ if (gyroConfigMutable()->dynlpf2_enable != 0)
+ {
+ output = ABGLPF(&agbFilter[axis], input, gyroDt);
+ }
+ else
+ {
+ output = input;
+ }
+
+ return output;
+}
+
+
+#endif
!🟥^^^^^^^^^^^^^^^ SKIPPING DYN_LPF2 ^^^^^^^^^^^^^^^^^^^^^^
↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ skipping synlpf2 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 000000000..a37c2279e
--- /dev/null
+++ b/src/main/common/dynLpf2.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#define DEFAULT_DYNLPF2_ENABLE 1 //Enable DYN_LPF2 by default
+
+extern void init_dynLpf2(void);
+extern float dynLpf2Apply(int axis, float input);
!🟥^^^^^^^^^^^^^^^ SKIPPING DYN_LPF2 ^^^^^^^^^^^^^^^^^^^^^^
index e158c6052..98376d9e1
--- a/src/main/common/sensor_alignment.h
+++ b/src/main/common/sensor_alignment.h
@@ -36,11 +36,21 @@ typedef enum {
CW0_DEG_FLIP = 5, // 00,10,00 // _FLIP = 2x90 degree PITCH rotations
CW90_DEG_FLIP = 6, // 00,10,01
CW180_DEG_FLIP = 7, // 00,10,10
CW270_DEG_FLIP = 8, // 00,10,11
- ALIGN_CUSTOM = 9, // arbitrary sensor angles, e.g. for external sensors
+#ifdef USE_GYRO_IMUF9001
+ CW45_DEG = 9,
+ CW135_DEG = 10,
+ CW225_DEG = 11,
+ CW315_DEG = 12,
+ CW45_DEG_FLIP = 13,
+ CW135_DEG_FLIP = 14,
+ CW225_DEG_FLIP = 15,
+ CW315_DEG_FLIP = 16,
+#endif
+ ALIGN_CUSTOM = 17, // arbitrary sensor angles, e.g. for external sensors
} sensor_align_e;
typedef union sensorAlignment_u {
// value order is the same as axis_e
!🟥^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^
index d2f7f6ee2..32308ff8b
--- a/src/main/drivers/accgyro/accgyro.h
+++ b/src/main/drivers/accgyro/accgyro.h
@@ -51,10 +51,11 @@ typedef enum {
GYRO_ICM20602,
GYRO_ICM20608G,
GYRO_ICM20649,
GYRO_ICM20689,
GYRO_BMI160,
+ GYRO_IMUF9001,
GYRO_FAKE
} gyroHardware_e;
typedef enum {
GYRO_HARDWARE_LPF_NORMAL,
!🟥^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ done via git checkout files
index 000000000..7f903fd26
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_imuf9001.c
@@ -0,0 +1,655 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "sensors/gyro.h"
+#include "accgyro.h"
+#include "accgyro_mpu.h"
+#include "accgyro_imuf9001.h"
+
+#include "common/axis.h"
+#include "build/debug.h"
+#include "common/maths.h"
+#include "drivers/serial.h"
+#include "drivers/bus_spi.h"
+#include "drivers/dma_spi.h"
+#include "drivers/exti.h"
+#include "drivers/io.h"
+#include "drivers/light_led.h"
+#include "drivers/sensor.h"
+#include "drivers/time.h"
+#include "fc/config.h"
+#include "fc/runtime_config.h"
+
+#include "sensors/boardalignment.h"
+#include "sensors/gyro.h"
+#include "sensors/acceleration.h"
+
+#ifdef USE_HAL_F7_CRC
+//CRC stuff should really go in a separate CRC driver, but only IMUF uses it
+#include "stm32f7xx_hal.h"
+#include "drivers/system.h"
+#endif
+
+
+#ifdef USE_GYRO_IMUF9001
+
+#define MPU_INT_EXTI PB0
+
+volatile uint16_t imufCurrentVersion = IMUF_FIRMWARE_MIN_VERSION;
+FAST_RAM_ZERO_INIT volatile uint32_t isImufCalibrating;
+FAST_RAM_ZERO_INIT volatile imuFrame_t imufQuat;
+FAST_RAM_ZERO_INIT busDevice_t *imufDev;
+
+#ifdef USE_HAL_F7_CRC
+//CRC stuff should really go in a separate CRC driver, but only IMUF uses it
+FAST_RAM_ZERO_INIT CRC_HandleTypeDef CrcHandle;
+#endif
+
+void crcConfig(void)
+{
+ #ifdef USE_HAL_F7_CRC
+ __HAL_RCC_CRC_CLK_ENABLE();
+ CrcHandle.Instance = CRC;
+ CrcHandle.Init.DefaultPolynomialUse = DEFAULT_POLYNOMIAL_DISABLE;
+ CrcHandle.Init.GeneratingPolynomial = 0x04C11DB7;
+ CrcHandle.Init.CRCLength = CRC_POLYLENGTH_32B;
+ CrcHandle.InputDataFormat = CRC_INPUTDATA_FORMAT_WORDS;
+
+ HAL_CRC_Init(&CrcHandle);
+ #endif
+ //F4 std per doesn't need to be init
+ return;
+}
+
+FAST_CODE uint32_t getCrcImuf9001(uint32_t* data, uint32_t size)
+{
+ #ifdef USE_HAL_F7_CRC
+ return HAL_CRC_Calculate(&CrcHandle, data, size);
+ #else
+ CRC_ResetDR(); //reset data register
+ for(uint32_t x=0; x<size; x++ )
+ {
+ CRC_CalcCRC(data[x]);
+ }
+ return CRC_GetCRC();
+ #endif
+}
+
+FAST_CODE void appendCrcToData(uint32_t* data, uint32_t size)
+{
+ data[size] = getCrcImuf9001(data, size);;
+}
+
+FAST_CODE static void gpio_write_pin(GPIO_TypeDef * GPIOx, uint16_t GPIO_Pin, gpioState_t pinState)
+{
+ //GPIO manipulation should go into a fast GPIO driver and should be separate from the befhal
+
+ #ifdef USE_HAL_F7_CRC
+ HAL_GPIO_WritePin(GPIOx, GPIO_Pin, pinState);
+ #else
+ if (pinState == GPIO_HI)
+ {
+ GPIOx->BSRRL = (uint32_t)GPIO_Pin;
+ }
+ else
+ {
+ GPIOx->BSRRH = (uint32_t)GPIO_Pin;
+ }
+ #endif
+}
+
+void resetImuf9001(void)
+{
+ gpio_write_pin(IMUF_RST_PORT, IMUF_RST_PIN, GPIO_LO);
+ //blink
+ for(uint32_t x = 0; x<40; x++)
+ {
+ LED0_TOGGLE;
+ delay(20);
+ }
+ LED0_OFF;
+ gpio_write_pin(IMUF_RST_PORT, IMUF_RST_PIN, GPIO_HI);
+ delay(100);
+}
+
+
+#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)
+#define GPIO_GET_INDEX(__GPIOx__) (uint8_t)(((__GPIOx__) == (GPIOA))? 0U :\
+ ((__GPIOx__) == (GPIOB))? 1U :\
+ ((__GPIOx__) == (GPIOC))? 2U :\
+ ((__GPIOx__) == (GPIOD))? 3U :\
+ ((__GPIOx__) == (GPIOE))? 4U :\
+ ((__GPIOx__) == (GPIOF))? 5U :\
+ ((__GPIOx__) == (GPIOG))? 6U :\
+ ((__GPIOx__) == (GPIOH))? 7U : 8U)
+#endif
+
+void imufDeinitGpio(GPIO_TypeDef * GPIOx, uint16_t GPIO_Pin)
+{
+ uint32_t position;
+ uint32_t ioposition = 0x00U;
+ uint32_t iocurrent = 0x00U;
+ uint32_t tmp = 0x00U;
+
+ /* Configure the port pins */
+ for(position = 0U; position < 16; position++)
+ {
+ /* Get the IO position */
+ ioposition = 0x01U << position;
+ /* Get the current IO position */
+ iocurrent = (GPIO_Pin) & ioposition;
+
+ if(iocurrent == ioposition)
+ {
+ /*------------------------- GPIO Mode Configuration --------------------*/
+ /* Configure IO Direction in Input Floating Mode */
+ GPIOx->MODER &= ~(GPIO_MODER_MODER0 << (position * 2U));
+
+ /* Configure the default Alternate Function in current IO */
+ GPIOx->AFR[position >> 3U] &= ~(0xFU << ((uint32_t)(position & 0x07U) * 4U)) ;
+
+ /* Configure the default value for IO Speed */
+ GPIOx->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (position * 2U));
+
+ /* Configure the default value IO Output Type */
+ GPIOx->OTYPER &= ~(GPIO_OTYPER_OT_0 << position) ;
+
+ /* Deactivate the Pull-up and Pull-down resistor for the current IO */
+ GPIOx->PUPDR &= ~(GPIO_PUPDR_PUPDR0 << (position * 2U));
+
+ /*------------------------- EXTI Mode Configuration --------------------*/
+ tmp = SYSCFG->EXTICR[position >> 2U];
+ tmp &= (0x0FU << (4U * (position & 0x03U)));
+ if(tmp == ((uint32_t)(GPIO_GET_INDEX(GPIOx)) << (4U * (position & 0x03U))))
+ {
+ /* Configure the External Interrupt or event for the current IO */
+ tmp = 0x0FU << (4U * (position & 0x03U));
+ SYSCFG->EXTICR[position >> 2U] &= ~tmp;
+
+ /* Clear EXTI line configuration */
+ EXTI->IMR &= ~((uint32_t)iocurrent);
+ EXTI->EMR &= ~((uint32_t)iocurrent);
+
+ /* Clear Rising Falling edge configuration */
+ EXTI->RTSR &= ~((uint32_t)iocurrent);
+ EXTI->FTSR &= ~((uint32_t)iocurrent);
+ }
+ }
+ }
+}
+
+void initImuf9001(void)
+{
+ //GPIO manipulation should go into a fast GPIO driver and should be separate from the befhal
+ #ifdef USE_HAL_F7_CRC
+ HAL_GPIO_DeInit(IMUF_RST_PORT, IMUF_RST_PIN);
+ GPIO_InitTypeDef GPIO_InitStruct;
+ GPIO_InitStruct.Pin = IMUF_RST_PIN;
+ GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+
+ HAL_GPIO_Init(IMUF_RST_PORT, &GPIO_InitStruct);
+ #else
+ //GPIO_DeInit(IMUF_RST_PORT);
+ imufDeinitGpio(IMUF_RST_PORT, IMUF_RST_PIN);
+ GPIO_InitTypeDef gpioInitStruct;
+ gpioInitStruct.GPIO_Pin = IMUF_RST_PIN;
+ gpioInitStruct.GPIO_Mode = GPIO_Mode_OUT;
+ gpioInitStruct.GPIO_Speed = GPIO_Speed_50MHz;
+ gpioInitStruct.GPIO_OType = GPIO_OType_OD;
+ gpioInitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ GPIO_Init(IMUF_RST_PORT, &gpioInitStruct);
+ #endif
+
+ resetImuf9001();
+}
+
+FAST_CODE bool imufSendReceiveSpiBlocking(const busDevice_t *bus, uint8_t *dataTx, uint8_t *daRx, uint8_t length)
+{
+ spiBusTransfer(bus, dataTx, daRx, length);
+ return true;
+}
+
+FAST_CODE static int imuf9001SendReceiveCommand(const busDevice_t *gyro, gyroCommands_t commandToSend, imufCommand_t *reply, imufCommand_t *data)
+{
+
+ imufCommand_t command;
+ volatile uint32_t attempt, crcCalc;
+ int failCount = 5000;
+
+ memset(reply, 0, sizeof(command));
+
+ if (data)
+ {
+ memcpy(&command, data, sizeof(command));
+ }
+ else
+ {
+ memset(&command, 0, sizeof(command));
+ }
+
+ command.command = commandToSend;
+ command.crc = getCrcImuf9001((uint32_t *)&command, 11);;
+
+
+ while (failCount-- > 0)
+ {
+ delayMicroseconds(1000);
+ if( IORead(IOGetByTag(IO_TAG(MPU_INT_EXTI))) ) //IMU is ready to talk
+ {
+ failCount -= 100;
+ if (imufSendReceiveSpiBlocking(gyro, (uint8_t *)&command, (uint8_t *)reply, sizeof(imufCommand_t)))
+ {
+ crcCalc = getCrcImuf9001((uint32_t *)reply, 11);
+ //this is the only valid reply we'll get if we're in BL mode
+ if(crcCalc == reply->crc && (reply->command == IMUF_COMMAND_LISTENING || reply->command == BL_LISTENING)) //this tells us the IMU was listening for a command, else we need to reset synbc
+ {
+ for (attempt = 0; attempt < 100; attempt++)
+ {
+ //reset command, just waiting for reply data now
+ command.command = IMUF_COMMAND_NONE;
+ command.crc = getCrcImuf9001((uint32_t *)&command, 11);
+ if (commandToSend == BL_ERASE_ALL){
+ delay(600);
+ }
+ if(commandToSend == BL_WRITE_FIRMWARES)
+ {
+ delay(10);
+ }
+ delayMicroseconds(1000); //give pin time to set
+
+ if( IORead(IOGetByTag(IO_TAG(MPU_INT_EXTI))) ) //IMU is ready to talk
+ {
+ //reset attempts
+ attempt = 100;
+
+ delayMicroseconds(1000); //give pin time to set
+ imufSendReceiveSpiBlocking(gyro, (uint8_t *)&command, (uint8_t *)reply, sizeof(imufCommand_t));
+ crcCalc = getCrcImuf9001((uint32_t *)reply, 11);
+
+ if(crcCalc == reply->crc && reply->command == commandToSend ) //this tells us the IMU understood the last command
+ {
+ return 1;
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ return 0;
+}
+
+int imufBootloader()
+{
+
+ imufCommand_t reply;
+ imufCommand_t data;
+ memset(&data, 0, sizeof(data));
+
+ //config BL pin as output (shared with EXTI, this happens before EXTI init though)
+ //config pins
+ #ifdef USE_HAL_F7_CRC
+ HAL_GPIO_DeInit(IMUF_EXTI_PORT, IMUF_EXTI_PIN);
+ GPIO_InitTypeDef GPIO_InitStruct;
+ GPIO_InitStruct.Pin = IMUF_EXTI_PIN;
+ GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+
+ HAL_GPIO_Init(IMUF_EXTI_PORT, &GPIO_InitStruct);
+ #else
+ //GPIO_DeInit(IMUF_EXTI_PORT);
+ imufDeinitGpio(IMUF_EXTI_PORT, IMUF_EXTI_PIN);
+ GPIO_InitTypeDef gpioInitStruct;
+ gpioInitStruct.GPIO_Pin = IMUF_EXTI_PIN;
+ gpioInitStruct.GPIO_Mode = GPIO_Mode_OUT;
+ gpioInitStruct.GPIO_Speed = GPIO_Speed_50MHz;
+ gpioInitStruct.GPIO_OType = GPIO_OType_PP;
+ gpioInitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ GPIO_Init(IMUF_EXTI_PORT, &gpioInitStruct);
+ #endif
+ //config pins
+ delay(200);
+
+ gpio_write_pin(IMUF_EXTI_PORT, IMUF_EXTI_PIN, 1); //set bl pin Hi
+ initImuf9001(); //reset imuf, make three blinks
+ resetImuf9001(); //reset imuf, make three blinks
+ resetImuf9001(); //reset imuf, make three blinks
+ delay(1000); //delay 100 ms. give IMUF BL time to look for bl init pin
+ gpio_write_pin(IMUF_EXTI_PORT, IMUF_EXTI_PIN, 0); //set bl pin Lo
+
+ //config EXTI as input so we can check imuf status
+ //config pins
+ #ifdef USE_HAL_F7_CRC
+ HAL_GPIO_DeInit(IMUF_EXTI_PORT, IMUF_EXTI_PIN);
+ GPIO_InitStruct.Pin = IMUF_EXTI_PIN;
+ GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+ GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+
+ HAL_GPIO_Init(IMUF_EXTI_PORT, &GPIO_InitStruct);
+ #else
+ imufDeinitGpio(IMUF_EXTI_PORT, IMUF_EXTI_PIN);
+ gpioInitStruct.GPIO_Pin = IMUF_EXTI_PIN;
+ gpioInitStruct.GPIO_Mode = GPIO_Mode_IN;
+ gpioInitStruct.GPIO_Speed = GPIO_Speed_50MHz;
+ gpioInitStruct.GPIO_OType = GPIO_OType_PP;
+ gpioInitStruct.GPIO_PuPd = GPIO_PuPd_DOWN;
+ GPIO_Init(IMUF_EXTI_PORT, &gpioInitStruct);
+ #endif
+ //config pins
+ delay(200);
+
+ if (imuf9001SendReceiveCommand(imufDev, BL_REPORT_INFO, &reply, &data))
+ {
+ return 1;
+ }
+ else
+ {
+ return 0;
+ }
+}
+
+volatile int checkCount = 0;
+
+int imufUpdate(uint8_t *buff, uint32_t bin_length)
+{
+ imufCommand_t reply;
+ imufCommand_t data;
+ memset(&data, 0, sizeof(data));
+
+ //check if BL is active
+ if (imuf9001SendReceiveCommand(imufDev, BL_REPORT_INFO, &reply, &data))
+ {
+ //erase firmware on MCU
+ if( imuf9001SendReceiveCommand(imufDev, BL_ERASE_ALL, &reply, &data) )
+ {
+ //good data
+ if( imuf9001SendReceiveCommand(imufDev, BL_PREPARE_PROGRAM, &reply, &data) )
+ {
+
+ //blink
+ for(uint32_t x = 0; x<10; x++)
+ {
+ LED0_TOGGLE;
+ delay(200);
+ }
+ LED0_OFF;
+
+ volatile uint32_t chunk_start = (uint32_t)buff;
+ for(uint32_t x=0;x<(bin_length);x+=32)
+ {
+ data.param1 = 0x08002000+x;
+ data.param2 = (*(__IO uint32_t *)(chunk_start+x));
+ data.param3 = (*(__IO uint32_t *)(chunk_start+x+4));
+ data.param4 = (*(__IO uint32_t *)(chunk_start+x+8));
+ data.param5 = (*(__IO uint32_t *)(chunk_start+x+12));
+ data.param6 = (*(__IO uint32_t *)(chunk_start+x+16));
+ data.param7 = (*(__IO uint32_t *)(chunk_start+x+20));
+ data.param8 = (*(__IO uint32_t *)(chunk_start+x+24));
+ data.param9 = (*(__IO uint32_t *)(chunk_start+x+28));
+
+ if( imuf9001SendReceiveCommand(imufDev, BL_WRITE_FIRMWARES, &reply, &data) )
+ {
+ //continue writing
+ LED0_TOGGLE;
+ }
+ else
+ {
+ //error handler
+ for(uint32_t x = 0; x<40; x++)
+ {
+ LED0_TOGGLE;
+ delay(200);
+ }
+ LED0_OFF;
+ return 0;
+ }
+ }
+
+ if (imuf9001SendReceiveCommand(imufDev, BL_END_PROGRAM, &reply, &data) )
+ {
+ //blink
+ for (uint32_t x = 0; x<40; x++)
+ {
+ LED0_TOGGLE;
+ delay(20);
+ }
+ LED0_OFF;
+ return 1;
+ }
+ }
+ else
+ {
+ checkCount++;
+ if (checkCount > 40)
+ {
+ checkCount = 0;
+ }
+ }
+ }
+ }
+
+ return 0;
+}
+
+int imuf9001Whoami(const busDevice_t *gyro)
+{
+ imufDev = (busDevice_t *)gyro;
+ uint32_t attempt;
+ imufCommand_t reply;
+
+ for (attempt = 0; attempt < 5; attempt++)
+ {
+ if (imuf9001SendReceiveCommand(gyro, IMUF_COMMAND_REPORT_INFO, &reply, NULL))
+ {
+ imufCurrentVersion = (*(imufVersion_t *)&(reply.param1)).firmware;
+ if (imufCurrentVersion >= IMUF_FIRMWARE_MIN_VERSION) {
+ return IMUF_9001_SPI;
+ }
+ }
+ }
+ imufCurrentVersion = 9999;
+ return 0;
+}
+
+uint8_t imuf9001SpiDetect(const busDevice_t *gyro)
+{
+ static bool hardwareInitialised = false;
+ int returnCheck;
+
+ if (hardwareInitialised) {
+ return(0);
+ }
+
+ //config crc
+ crcConfig();
+
+ //config exti as input, not exti for now
+ IOInit(IOGetByTag( IO_TAG(MPU_INT_EXTI) ), OWNER_GYRO_EXTI, 0);
+ IOConfigGPIO(IOGetByTag( IO_TAG(MPU_INT_EXTI) ), IOCFG_IPD);
+
+ delayMicroseconds(100);
+
+ IOInit(gyro->busdev_u.spi.csnPin, OWNER_GYRO_CS, 0);
+ IOConfigGPIO(gyro->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
+ IOHi(gyro->busdev_u.spi.csnPin);
+
+ hardwareInitialised = true;
+
+ for (int x=0; x<3; x++)
+ {
+ if (x)
+ {
+ initImuf9001();
+ delay(200 * x);
+ }
+
+ returnCheck = imuf9001Whoami(gyro);
+ if(returnCheck)
+ {
+ return returnCheck;
+ }
+ }
+ return 0;
+}
+
+void imufSpiAccInit(accDev_t *acc)
+{
+ acc->acc_1G = 512 * 4;
+}
+
+static gyroToBoardCommMode_t VerifyAllowedCommMode(uint32_t commMode)
+{
+ switch (commMode)
+ {
+ case GTBCM_SETUP:
+ case GTBCM_GYRO_ONLY_PASSTHRU:
+ case GTBCM_GYRO_ACC_PASSTHRU:
+ case GTBCM_GYRO_ONLY_FILTER_F:
+ case GTBCM_GYRO_ACC_FILTER_F:
+ case GTBCM_GYRO_ACC_QUAT_FILTER_F:
+ return (gyroToBoardCommMode_t)commMode;
+ break;
+ default:
+ return GTBCM_DEFAULT;
+ }
+}
+
+uint16_t imufGyroAlignment(void)
+{
+ if (isBoardAlignmentStandard(boardAlignment()))
+ {
+ if(gyroConfig()->gyro_align <= 1)
+ {
+ return 0;
+ }
+ else
+ {
+ return (uint16_t)(gyroConfig()->gyro_align - 1);
+ }
+ }
+ else
+ {
+ return (uint16_t)IMU_CW0;
+ }
+}
+
+void setupImufParams(imufCommand_t * data)
+{
+ if (imufCurrentVersion < 107) {
+ //backwards compatibility for Caprica
+ data->param2 = ( (uint16_t)(gyroConfig()->imuf_rate+1) << 16 );
+ data->param3 = ( (uint16_t)gyroConfig()->imuf_pitch_q << 16 ) | (uint16_t)constrain(gyroConfig()->imuf_w, 6, 10);
+ data->param4 = ( (uint16_t)gyroConfig()->imuf_roll_q << 16 ) | (uint16_t)constrain(gyroConfig()->imuf_w, 6, 10);
+ data->param5 = ( (uint16_t)gyroConfig()->imuf_yaw_q << 16 ) | (uint16_t)constrain(gyroConfig()->imuf_w, 6, 10);
+ data->param6 = ( (uint16_t)gyroConfig()->imuf_pitch_lpf_cutoff_hz << 16) | (uint16_t)gyroConfig()->imuf_roll_lpf_cutoff_hz;
+ data->param7 = ( (uint16_t)gyroConfig()->imuf_yaw_lpf_cutoff_hz << 16) | (uint16_t)0;
+ data->param8 = ( (int16_t)boardAlignment()->rollDegrees << 16 ) | imufGyroAlignment();
+ data->param9 = ( (int16_t)boardAlignment()->yawDegrees << 16 ) | (int16_t)boardAlignment()->pitchDegrees;
+ } else {
+ //Odin contract.
+ data->param2 = ( (uint16_t)(gyroConfig()->imuf_rate+1) << 16) | (uint16_t)gyroConfig()->imuf_w;
+ data->param3 = ( (uint16_t)gyroConfig()->imuf_roll_q << 16) | (uint16_t)gyroConfig()->imuf_pitch_q;
+ data->param4 = ( (uint16_t)gyroConfig()->imuf_yaw_q << 16) | (uint16_t)gyroConfig()->imuf_roll_lpf_cutoff_hz;
+ data->param5 = ( (uint16_t)gyroConfig()->imuf_pitch_lpf_cutoff_hz << 16) | (uint16_t)gyroConfig()->imuf_yaw_lpf_cutoff_hz;
+ data->param6 = ( (uint32_t)((gyroConfig()->imuf_roll_af & 1) |
+ (gyroConfig()->imuf_pitch_af & 1) << 1 |
+ (gyroConfig()->imuf_yaw_af & 1) << 2 ));
+ data->param7 = ( (uint16_t)0 << 16) | (uint16_t)0;
+ data->param8 = ( (int16_t)boardAlignment()->rollDegrees << 16 ) | imufGyroAlignment();
+ data->param9 = ( (int16_t)boardAlignment()->yawDegrees << 16 ) | (int16_t)boardAlignment()->pitchDegrees;
+ }
+}
+
+void imufSpiGyroInit(gyroDev_t *gyro)
+{
+ uint32_t attempt = 0;
+ imufCommand_t txData;
+ imufCommand_t rxData;
+
+ rxData.param1 = VerifyAllowedCommMode(gyroConfig()->imuf_mode);
+
+ setupImufParams(&rxData);
+
+ for (attempt = 0; attempt < 10; attempt++)
+ {
+ if(attempt)
+ {
+ resetImuf9001();
+ delay(300 * attempt);
+ }
+
+ if (imuf9001SendReceiveCommand(&(gyro->bus), IMUF_COMMAND_SETUP, &txData, &rxData))
+ {
+ //enable EXTI
+ mpuGyroInit(gyro);
+ return;
+ }
+ }
+ setArmingDisabled(ARMING_DISABLED_NO_GYRO);
+}
+
+FAST_CODE bool imufReadAccData(accDev_t *acc) {
+ UNUSED(acc);
+ return true;
+}
+
+bool imufSpiAccDetect(accDev_t *acc)
+{
+ acc->initFn = imufSpiAccInit;
+ acc->readFn = imufReadAccData;
+
+ return true;
+}
+
+bool imufSpiGyroDetect(gyroDev_t *gyro)
+{
+ // MPU6500 is used as a equivalent of other gyros by some flight controllers
+ switch (gyro->mpuDetectionResult.sensor) {
+ case IMUF_9001_SPI:
+ break;
+ default:
+ return false;
+ }
+
+ gyro->initFn = imufSpiGyroInit;
+ gyro->scale = 1.0f;
+ return true;
+}
+
+FAST_CODE void imufStartCalibration(void)
+{
+ isImufCalibrating = IMUF_IS_CALIBRATING; //reset by EXTI
+}
+
+FAST_CODE void imufEndCalibration(void)
+{
+ isImufCalibrating = IMUF_NOT_CALIBRATING; //reset by EXTI
+}
+
+#endif
!🟥^^^^^^^^^^^^^ git checkout file ^^^^^^^^^^^^^^^^^^
!🟥 git checkout robertb-hesp-performancef3-master -- src/main/drivers/accgyro/accgyro_imuf9001.c
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ done via checkout file ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 000000000..4ae8ae079
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_imuf9001.h
@@ -0,0 +1,235 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "drivers/bus.h"
+
+uint8_t imuf9001SpiDetect(const busDevice_t *bus);
+
+bool imufSpiAccDetect(accDev_t *acc);
+bool imufSpiGyroDetect(gyroDev_t *gyro);
+
+void imufSpiGyroInit(gyroDev_t *gyro);
+void imufSpiAccInit(accDev_t *acc);
+
+void imufStartCalibration(void);
+void imufEndCalibration(void);
+
+#ifndef IMUF_DEFAULT_PITCH_Q
+#define IMUF_DEFAULT_PITCH_Q 3000
+#endif
+#ifndef IMUF_DEFAULT_ROLL_Q
+#define IMUF_DEFAULT_ROLL_Q 3000
+#endif
+#ifndef IMUF_DEFAULT_YAW_Q
+#define IMUF_DEFAULT_YAW_Q 3000
+#endif
+#ifndef IMUF_IMUF_W
+#define IMUF_IMUF_W 32
+#endif
+#ifndef IMUF_DEFAULT_ROLL_AF
+#define IMUF_DEFAULT_ROLL_AF 0
+#endif
+#ifndef IMUF_DEFAULT_PITCH_AF
+#define IMUF_DEFAULT_PITCH_AF 0
+#endif
+#ifndef IMUF_DEFAULT_YAW_AF
+#define IMUF_DEFAULT_YAW_AF 0
+#endif
+
+
+#define IMUF_FIRMWARE_MIN_VERSION 106
+extern volatile uint16_t imufCurrentVersion;
+typedef struct imufVersion
+{
+ uint32_t hardware;
+ uint32_t firmware;
+ uint32_t bootloader;
+ uint32_t uid1;
+ uint32_t uid2;
+ uint32_t uid3;
+} __attribute__((__packed__)) imufVersion_t;
+
+typedef struct imufCommand {
+ uint32_t command;
+ uint32_t param1;
+ uint32_t param2;
+ uint32_t param3;
+ uint32_t param4;
+ uint32_t param5;
+ uint32_t param6;
+ uint32_t param7;
+ uint32_t param8;
+ uint32_t param9;
+ uint32_t param10;
+ uint32_t crc;
+ uint32_t tail;
+} __attribute__ ((__packed__)) imufCommand_t;
+
+typedef struct imufData
+{
+ float gyroX;
+ float gyroY;
+ float gyroZ;
+ float accX;
+ float accY;
+ float accZ;
+ float tempC;
+ float quaternionW;
+ float quaternionX;
+ float quaternionY;
+ float quaternionZ;
+ uint32_t crc;
+ uint32_t tail;
+} __attribute__((__packed__)) imufData_t;
+
+typedef enum gyroCommands
+{
+ BL_ERASE_ALL = 22,
+ BL_REPORT_INFO = 24,
+ BL_WRITE_FIRMWARES = 29,
+ BL_PREPARE_PROGRAM = 30,
+ BL_END_PROGRAM = 31,
+ BL_LISTENING = 32,
+ IMUF_COMMAND_NONE = 0,
+ IMUF_COMMAND_CALIBRATE = 99,
+ IMUF_COMMAND_LISTENING = 108,
+ IMUF_COMMAND_REPORT_INFO = 121,
+ IMUF_COMMAND_SETUP = 122,
+ IMUF_COMMAND_SETPOINT = 126,
+ IMUF_COMMAND_RESTART = 127
+} gyroCommands_t;
+
+typedef struct gyroFrame
+{
+ float gyroX;
+ float gyroY;
+ float gyroZ;
+ float accelX;
+ float accelY;
+ float accelZ;
+ float temp;
+} __attribute__((__packed__)) gyroFrame_t;
+
+typedef struct imuFrame
+{
+ float w;
+ float x;
+ float y;
+ float z;
+} __attribute__((__packed__)) imuFrame_t;
+
+typedef struct imuCommFrame
+{
+ gyroFrame_t gyroFrame;
+ imuFrame_t imuFrame;
+ uint32_t tail;
+} __attribute__((__packed__)) imuCommFrame_t;
+
+typedef enum imufLoopHz
+{
+ IMUF_32000 = 0,
+ IMUF_16000 = 1,
+ IMUF_8000 = 2,
+ IMUF_4000 = 3,
+ IMUF_2000 = 4,
+ IMUF_1000 = 5,
+ IMUF_500 = 6,
+ IMUF_250 = 7,
+} imufLoopHz_t;
+
+typedef enum imufOutput
+{
+ IMUF_GYRO_OUTPUT = 1 << 0,
+ IMUF_TEMP_OUTPUT = 1 << 1,
+ IMUF_ACC_OUTPUT = 1 << 2,
+ IMUF_QUAT_OUTPUT = 1 << 3,
+} imufOutput_t;
+
+typedef enum imufOreintation
+{
+ IMU_CW0 = 0,
+ IMU_CW90 = 1,
+ IMU_CW180 = 2,
+ IMU_CW270 = 3,
+ IMU_CW0_INV = 4,
+ IMU_CW90_INV = 5,
+ IMU_CW180_INV = 6,
+ IMU_CW270_INV = 7,
+ IMU_CW45 = 8,
+ IMU_CW135 = 9,
+ IMU_CW225 = 10,
+ IMU_CW315 = 11,
+ IMU_CW45_INV = 12,
+ IMU_CW135_INV = 13,
+ IMU_CW225_INV = 14,
+ IMU_CW315_INV = 15,
+ IMU_CUSTOM = 16,
+} imufOrientation_t;
+
+typedef struct imufMode
+{
+ uint8_t command; //output Hz
+ uint8_t hz; //output Hz
+ uint8_t dataOut; //what data to send
+ uint8_t filterLevelX; //what filter level, 0% to 100% as a uint8_t
+ uint8_t filterLevelY; //what filter level, 0% to 100% as a uint8_t
+ uint8_t filterLevelZ; //what filter level, 0% to 100% as a uint8_t
+ uint8_t orientation; //what orienetation is the IMU? 0 gives raw output, if you want to use quats this must be set right
+ uint16_t rotationX; //custom orientation X, used when orientation is set to IMU_CUSTOM
+ uint16_t rotationY; //custom orientation Y, used when orientation is set to IMU_CUSTOM
+ uint16_t rotationZ; //custom orientation Z, used when orientation is set to IMU_CUSTOM
+ uint8_t param4; //future parameters
+ uint8_t param5; //future parameters
+ uint8_t param6; //future parameters
+ uint8_t param7; //future parameters
+ uint8_t param8; //future parameters
+} __attribute__((__packed__)) imufMode_t;
+
+typedef enum gyroToBoardCommMode
+{
+ GTBCM_SETUP = 53, //setup
+ GTBCM_GYRO_ONLY_PASSTHRU = 6, //no crc, gyro, 3*2 bytes
+ GTBCM_GYRO_ACC_PASSTHRU = 14, //no crc, acc, temp, gyro, 3*2, 1*2, 3*2 bytes
+ GTBCM_GYRO_ONLY_FILTER_F = 20, //gyro filtered, 3*4 bytes, 4 bytes crc
+ GTBCM_GYRO_ACC_FILTER_F = 32, //gyro filtered, acc filtered, temp, crc
+ GTBCM_GYRO_ACC_QUAT_FILTER_F = 48, //gyro filtered, acc filtered, temp, quaternions filtered, crc
+ GTBCM_DEFAULT = GTBCM_GYRO_ACC_QUAT_FILTER_F, //default mode
+} gyroToBoardCommMode_t;
+
+typedef enum imufCalibrationSteps
+{
+ IMUF_NOT_CALIBRATING = 0,
+ IMUF_IS_CALIBRATING = 1,
+ IMUF_DONE_CALIBRATING = 2
+
+} imufCalibrationSteps_t;
+
+typedef enum gpioState
+{
+ GPIO_LO = 0,
+ GPIO_HI = 1
+} gpioState_t;
+
+extern volatile imuFrame_t imufQuat;
+extern volatile uint32_t isImufCalibrating;
+
+extern void initImuf9001(void);
+extern uint32_t getCrcImuf9001(uint32_t* data, uint32_t size);
+extern int imufUpdate(uint8_t *buff, uint32_t bin_length);
+extern int imufBootloader(void);
\ No newline at end of file
!🟥^^^^^^^^^^^^^^ git checkout ^^^ files ^^^^^^^^^^^^^6
!🟥 git checkout robertb-hesp-performancef3-master -- src/main/drivers/accgyro/accgyro_imuf9001.h
!🟥^^^^^^^^^^^^^^^^^^^^^^^
index 5f0dfae6c..8cafcf6ea
--- a/src/main/drivers/accgyro/accgyro_mpu.c
+++ b/src/main/drivers/accgyro/accgyro_mpu.c
@@ -40,10 +40,16 @@
#include "drivers/nvic.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/time.h"
+#ifdef USE_DMA_SPI_DEVICE
+#include "drivers/dma_spi.h"
+#include "sensors/gyro.h"
+#include "sensors/acceleration.h"
+#endif //USE_DMA_SPI_DEVICE
+
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu3050.h"
#include "drivers/accgyro/accgyro_mpu6050.h"
#include "drivers/accgyro/accgyro_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_bmi160.h"
@@ -53,13 +59,24 @@
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
#include "drivers/accgyro/accgyro_spi_l3gd20.h"
#include "drivers/accgyro/accgyro_mpu.h"
+#ifdef USE_GYRO_IMUF9001
+#include "drivers/accgyro/accgyro_imuf9001.h"
+#include "rx/rx.h"
+#include "fc/rc.h"
+#include "fc/runtime_config.h"
+#endif //USE_GYRO_IMUF9001
+
#include "pg/pg.h"
#include "pg/gyrodev.h"
+#ifdef USE_GYRO_IMUF9001
+ imufData_t imufData;
+#endif
+
#ifndef MPU_ADDRESS
#define MPU_ADDRESS 0x68
#endif
#define MPU_INQUIRY_MASK 0x7E
!🟥^^^^^^^^^^ done ^^^^^^^^^^^^
@@ -104,10 +121,15 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
* Gyro interrupt service routine
*/
#ifdef USE_GYRO_EXTI
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
{
+#ifdef USE_DMA_SPI_DEVICE
+ //start dma read
+ (void)(cb);
+ gyroDmaSpiStartRead();
+#else
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
static uint32_t lastCalledAtUs = 0;
const uint32_t nowUs = micros();
debug[0] = (uint16_t)(nowUs - lastCalledAtUs);
lastCalledAtUs = nowUs;
@@ -116,10 +138,11 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb)
gyro->dataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
const uint32_t now2Us = micros();
debug[1] = (uint16_t)(now2Us - nowUs);
#endif
+#endif // USE_DMA_SPI_DEVICE
}
!🟥^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^
static void mpuIntExtiInit(gyroDev_t *gyro)
{
if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
@@ -133,13 +156,22 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
if (status) {
return;
}
#endif
+#if defined (STM32F7)
IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler);
+// EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING);
+#else
+ IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
+ IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
+
+ EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler);
+ EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING);
+#endif
EXTIEnable(mpuIntIO, true);
}
#endif // USE_GYRO_EXTI
bool mpuAccRead(accDev_t *acc)
!🟥^^^^^^^^^^^^^^^^^^^ done but recoded ^^^^^ really messy and nonsensical ^^^^^^^^^^^^^^^^^^^^
!🟥 i would recommend review if this is even needed.
@@ -172,10 +204,89 @@ bool mpuGyroRead(gyroDev_t *gyro)
gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
+#ifdef USE_DMA_SPI_DEVICE
+FAST_CODE bool mpuGyroDmaSpiReadStart(gyroDev_t * gyro)
+{
+ (void)(gyro); ///not used at this time
+ //no reason not to get acc and gyro data at the same time
+#ifdef USE_GYRO_IMUF9001
+ if (isImufCalibrating == IMUF_IS_CALIBRATING) //calibrating
+ {
+ //two steps
+ //step 1 is isImufCalibrating=1, this starts the calibration command and sends it to the IMU-f
+ //step 2 is isImufCalibrating=2, this sets the tx buffer back to 0 so we don't keep sending the calibration command over and over
+ memset(dmaTxBuffer, 0, sizeof(imufCommand_t)); //clear buffer
+ //set calibration command with CRC, typecast the dmaTxBuffer as imufCommand_t
+ (*(imufCommand_t *)(dmaTxBuffer)).command = IMUF_COMMAND_CALIBRATE;
+ (*(imufCommand_t *)(dmaTxBuffer)).crc = getCrcImuf9001((uint32_t *)dmaTxBuffer, 11); //typecast the dmaTxBuffer as a uint32_t array which is what the crc command needs
+ //set isImufCalibrating to step 2, which is just used so the memset to 0 runs after the calibration commmand is sent
+ isImufCalibrating = IMUF_DONE_CALIBRATING; //go to step two
+ }
+ else if (isImufCalibrating == IMUF_DONE_CALIBRATING)
+ {
+ // step 2, memset of the tx buffer has run, set isImufCalibrating to 0.
+ (*(imufCommand_t *)(dmaTxBuffer)).command = 0;
+ (*(imufCommand_t *)(dmaTxBuffer)).crc = 0; //typecast the dmaTxBuffer as a uint32_t array which is what the crc command needs
+ imufEndCalibration();
+ }
+ else
+ {
+ if (isSetpointNew) {
+ //send setpoint and arm status
+ (*(imufCommand_t *)(dmaTxBuffer)).command = IMUF_COMMAND_SETPOINT;
+ (*(imufCommand_t *)(dmaTxBuffer)).param1 = getSetpointRateInt(0);
+ (*(imufCommand_t *)(dmaTxBuffer)).param2 = getSetpointRateInt(1);
+ (*(imufCommand_t *)(dmaTxBuffer)).param3 = getSetpointRateInt(2);
+ (*(imufCommand_t *)(dmaTxBuffer)).crc = getCrcImuf9001((uint32_t *)dmaTxBuffer, 11); //typecast the dmaTxBuffer as a uint32_t array which is what the crc command needs
+ isSetpointNew = 0;
+ }
+ }
+
+ memset(dmaRxBuffer, 0, gyroConfig()->imuf_mode); //clear buffer
+ //send and receive data using SPI and DMA
+ dmaSpiTransmitReceive(dmaTxBuffer, dmaRxBuffer, gyroConfig()->imuf_mode, 0);
+#else
+ dmaTxBuffer[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
+ dmaSpiTransmitReceive(dmaTxBuffer, dmaRxBuffer, 15, 0);
+#endif // USE_GYRO_IMUF9001
+ return true;
+}
+
+FAST_CODE void mpuGyroDmaSpiReadFinish(gyroDev_t * gyro)
+{
+ //spi rx dma callback
+#ifdef USE_GYRO_IMUF9001
+ memcpy(&imufData, dmaRxBuffer, sizeof(imufData_t));
+ acc.dev.ADCRaw[X] = (int16_t)(imufData.accX * acc.dev.acc_1G);
+ acc.dev.ADCRaw[Y] = (int16_t)(imufData.accY * acc.dev.acc_1G);
+ acc.dev.ADCRaw[Z] = (int16_t)(imufData.accZ * acc.dev.acc_1G);
+ gyro->gyroADC[X] = imufData.gyroX;
+ gyro->gyroADC[Y] = imufData.gyroY;
+ gyro->gyroADC[Z] = imufData.gyroZ;
+ gyro->gyroADCRaw[X] = (int16_t)(imufData.gyroX * 16.4f);
+ gyro->gyroADCRaw[Y] = (int16_t)(imufData.gyroY * 16.4f);
+ gyro->gyroADCRaw[Z] = (int16_t)(imufData.gyroZ * 16.4f);
+ if (gyroConfig()->imuf_mode == GTBCM_GYRO_ACC_QUAT_FILTER_F) {
+ imufQuat.w = imufData.quaternionW;
+ imufQuat.x = imufData.quaternionX;
+ imufQuat.y = imufData.quaternionY;
+ imufQuat.z = imufData.quaternionZ;
+ }
+#else
+ acc.dev.ADCRaw[X] = (int16_t)((dmaRxBuffer[1] << 8) | dmaRxBuffer[2]);
+ acc.dev.ADCRaw[Y] = (int16_t)((dmaRxBuffer[3] << 8) | dmaRxBuffer[4]);
+ acc.dev.ADCRaw[Z] = (int16_t)((dmaRxBuffer[5] << 8) | dmaRxBuffer[6]);
+ gyro->gyroADCRaw[X] = (int16_t)((dmaRxBuffer[9] << 8) | dmaRxBuffer[10]);
+ gyro->gyroADCRaw[Y] = (int16_t)((dmaRxBuffer[11] << 8) | dmaRxBuffer[12]);
+ gyro->gyroADCRaw[Z] = (int16_t)((dmaRxBuffer[13] << 8) | dmaRxBuffer[14]);
+#endif // USE_GYRO_IMUF9001
+}
+#endif
+
#ifdef USE_SPI_GYRO
bool mpuGyroReadSPI(gyroDev_t *gyro)
{
static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint8_t data[7];
!🟥^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -199,10 +310,13 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
mpu6000SpiDetect,
#endif
#ifdef USE_GYRO_SPI_MPU6500
mpu6500SpiDetect, // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
#endif
+#ifdef USE_GYRO_IMUF9001
+ imuf9001SpiDetect,
+#endif
#ifdef USE_GYRO_SPI_MPU9250
mpu9250SpiDetect,
#endif
#ifdef USE_GYRO_SPI_ICM20649
icm20649SpiDetect,
!🟥^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -221,11 +335,11 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
{
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(config->spiBus));
- if (!instance || !config->csnTag) {
+ if (!instance /*|| !config->csnTag*/) {
return false;
}
spiBusSetInstance(&gyro->bus, instance);
gyro->bus.busdev_u.spi.csnPin = IOGetByTag(config->csnTag);
!🟥^^^^^^^^^^^^^^^^ WHAT ??!?!? ^^^^^^^skipping for now ^^^^^^^^^^^^^
@@ -278,11 +392,12 @@ bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
gyro->bus.bustype = BUSTYPE_I2C;
} else {
gyro->bus.bustype = config->bustype;
}
-#ifdef USE_I2C_GYRO
+#if defined(USE_I2C_GYRO)
+#if !defined(USE_DMA_SPI_DEVICE)
if (gyro->bus.bustype == BUSTYPE_I2C) {
gyro->bus.busdev_u.i2c.device = I2C_CFG_TO_DEV(config->i2cBus);
gyro->bus.busdev_u.i2c.address = config->i2cAddress ? config->i2cAddress : MPU_ADDRESS;
uint8_t sig = 0;
@@ -307,10 +422,11 @@ bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
}
return true;
}
}
#endif
+#endif
#ifdef USE_SPI_GYRO
gyro->bus.bustype = BUSTYPE_SPI;
return detectSPISensorsAndUpdateDetectionResult(gyro, config);
!🟥^^^^^^^^^^^^^^ done ^^^ changed to #ifdef USE_I2C_GYRO && !defined(USE_DMA_SPI_DEVICE) ^^^^^^^^^^^^^^^
index a90fa2f01..ea42d9fdf
--- a/src/main/drivers/accgyro/accgyro_mpu.h
+++ b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -198,10 +198,11 @@ typedef enum {
ICM_20608_SPI,
ICM_20649_SPI,
ICM_20689_SPI,
BMI_160_SPI,
L3GD20_SPI,
+ IMUF_9001_SPI,
} mpuSensor_e;
typedef enum {
MPU_HALF_RESOLUTION,
MPU_FULL_RESOLUTION
@@ -222,5 +223,10 @@ bool mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config);
uint8_t mpuGyroDLPF(struct gyroDev_s *gyro);
uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg);
struct accDev_s;
bool mpuAccRead(struct accDev_s *acc);
+
+#ifdef USE_DMA_SPI_DEVICE
+extern bool mpuGyroDmaSpiReadStart(struct gyroDev_s *gyro);
+extern void mpuGyroDmaSpiReadFinish(struct gyroDev_s *gyro);
+#endif
!🟥^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
index 45ec93683..9a9e90cfd
--- a/src/main/drivers/bus.c
+++ b/src/main/drivers/bus.c
@@ -27,10 +27,13 @@
#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
bool busWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data)
{
+#ifdef USE_DMA_SPI_DEVICE
+ return spiBusWriteRegister(busdev, reg & 0x7f, data);
+#else
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(reg);
UNUSED(data);
#endif
switch (busdev->bustype) {
@@ -48,10 +51,11 @@ bool busWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data)
return i2cBusWriteRegister(busdev, reg, data);
#endif
default:
return false;
}
+#endif
}
bool busWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
@@ -155,15 +159,21 @@ bool busBusy(const busDevice_t *busdev, bool *error)
}
}
uint8_t busReadRegister(const busDevice_t *busdev, uint8_t reg)
{
+#ifdef USE_DMA_SPI_DEVICE
+ uint8_t data;
+ busReadRegisterBuffer(busdev, reg, &data, 1);
+ return data;
+#else
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(busdev);
UNUSED(reg);
return false;
#else
uint8_t data;
busReadRegisterBuffer(busdev, reg, &data, 1);
return data;
#endif
+#endif
}
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^
index 8c2d35db8..ceb30cca6
--- a/src/main/drivers/bus_i2c_stm32f30x.c
+++ b/src/main/drivers/bus_i2c_stm32f30x.c
@@ -39,10 +39,12 @@
#define IOCFG_I2C_PU IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP)
#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL)
#define I2C_HIGHSPEED_TIMING 0x00500E30 // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4.
#define I2C_STANDARD_TIMING 0x00E0257A // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
+//#define I2C_HIGHSPEED_TIMING 0x00a0167c // 1000 khz when overclocked to 120mhz
+//#define I2C_HIGHSPEED_TIMING I2C_STANDARD_TIMING
#define I2C_GPIO_AF GPIO_AF_4
static uint32_t i2cTimeout;
!🟥^^^^^^^^^^^^^^^ skipped -- had done BUT reverted ^^^^^^^^^^^^^^
index 5f869b37f..84cefcecb
--- a/src/main/drivers/bus_spi.c
+++ b/src/main/drivers/bus_spi.c
@@ -30,10 +30,17 @@
#include "drivers/bus_spi.h"
#include "drivers/bus_spi_impl.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/rcc.h"
+#ifdef USE_DMA_SPI_DEVICE
+#ifndef GYRO_READ_TIMEOUT
+ #define GYRO_READ_TIMEOUT 20
+#endif //GYRO_READ_TIMEOUT
+#include "drivers/dma_spi.h"
+#include "drivers/time.h"
+#endif //USE_DMA_SPI_DEVICE
spiDevice_t spiDevice[SPIDEV_COUNT];
SPIDevice spiDeviceByInstance(SPI_TypeDef *instance)
{
@@ -136,14 +143,42 @@ uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance)
return spiDevice[device].errorCount;
}
bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length)
{
+#ifdef USE_DMA_SPI_DEVICE
+ if(USE_DMA_SPI_DEVICE == bus->busdev_u.spi.instance)
+ {
+ uint32_t timeoutCheck = millis();
+ memcpy(dmaTxBuffer, (uint8_t *)txData, length);
+ dmaSpiTransmitReceive(dmaTxBuffer, dmaRxBuffer, length, 1);
+ while(dmaSpiReadStatus != DMA_SPI_READ_DONE)
+ {
+ if(millis() - timeoutCheck > GYRO_READ_TIMEOUT)
+ {
+ //GYRO_READ_TIMEOUT ms max, read failed, cleanup spi and return 0
+ IOHi(bus->busdev_u.spi.csnPin);
+ #ifndef STM32F7
+ dmaSpicleanupspi();
+ #endif //STM32F7
+ return false;
+ }
+ }
+ memcpy((uint8_t *)rxData, dmaRxBuffer, length);
+ }
+ else
+ {
+ IOLo(bus->busdev_u.spi.csnPin);
+ spiTransfer(bus->busdev_u.spi.instance, txData, rxData, length);
+ IOHi(bus->busdev_u.spi.csnPin);
+ }
+#else
IOLo(bus->busdev_u.spi.csnPin);
spiTransfer(bus->busdev_u.spi.instance, txData, rxData, length);
IOHi(bus->busdev_u.spi.csnPin);
- return true;
+#endif
+return true;
}
!🟥^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^^^
uint16_t spiGetErrorCounter(SPI_TypeDef *instance)
{
SPIDevice device = spiDeviceByInstance(instance);
@@ -183,24 +218,82 @@ bool spiBusRawTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *r
return spiTransfer(bus->busdev_u.spi.instance, txData, rxData, len);
}
bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
+#ifdef USE_DMA_SPI_DEVICE
+ if(USE_DMA_SPI_DEVICE == bus->busdev_u.spi.instance)
+ {
+ uint32_t timeoutCheck = millis();
+ dmaTxBuffer[0] = reg;
+ dmaTxBuffer[1] = data;
+ dmaSpiTransmitReceive(dmaTxBuffer, dmaRxBuffer, 2, 1);
+ while(dmaSpiReadStatus != DMA_SPI_READ_DONE)
+ {
+ if(millis() - timeoutCheck > GYRO_READ_TIMEOUT)
+ {
+ //GYRO_READ_TIMEOUT ms max, read failed, cleanup spi and return 0
+ IOHi(bus->busdev_u.spi.csnPin);
+ #ifndef STM32F7
+ dmaSpicleanupspi();
+ #endif //STM32F7
+ return false;
+ }
+ }
+ }
+ else
+ {
+ IOLo(bus->busdev_u.spi.csnPin);
+ spiTransferByte(bus->busdev_u.spi.instance, reg);
+ spiTransferByte(bus->busdev_u.spi.instance, data);
+ IOHi(bus->busdev_u.spi.csnPin);
+ }
+#else
IOLo(bus->busdev_u.spi.csnPin);
spiTransferByte(bus->busdev_u.spi.instance, reg);
spiTransferByte(bus->busdev_u.spi.instance, data);
IOHi(bus->busdev_u.spi.csnPin);
+#endif
return true;
}
bool spiBusRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length)
{
+#ifdef USE_DMA_SPI_DEVICE
+ if(USE_DMA_SPI_DEVICE == bus->busdev_u.spi.instance)
+ {
+ uint32_t timeoutCheck = millis();
+ dmaTxBuffer[0] = reg | 0x80;
+ dmaSpiTransmitReceive(dmaTxBuffer, dmaRxBuffer, length+1, 1);
+ while(dmaSpiReadStatus != DMA_SPI_READ_DONE)
+ {
+ if(millis() - timeoutCheck > GYRO_READ_TIMEOUT)
+ {
+ //GYRO_READ_TIMEOUT ms max, read failed, cleanup spi and return 0
+ IOHi(bus->busdev_u.spi.csnPin);
+ #ifndef STM32F7
+ dmaSpicleanupspi();
+ #endif //STM32F7
+ return false;
+ }
+ }
+ memcpy(data, dmaRxBuffer+1, length);
+ }
+ else
+ {
+ IOLo(bus->busdev_u.spi.csnPin);
+ spiTransferByte(bus->busdev_u.spi.instance, reg);
+ spiTransfer(bus->busdev_u.spi.instance, NULL, data, length);
+ IOHi(bus->busdev_u.spi.csnPin);
+ }
+#else
IOLo(bus->busdev_u.spi.csnPin);
spiTransferByte(bus->busdev_u.spi.instance, reg);
spiTransfer(bus->busdev_u.spi.instance, NULL, data, length);
IOHi(bus->busdev_u.spi.csnPin);
+#endif
return true;
}
!🟥^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^^^
bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length)
@@ -216,17 +309,48 @@ void spiBusWriteRegisterBuffer(const busDevice_t *bus, uint8_t reg, const uint8_
IOHi(bus->busdev_u.spi.csnPin);
}
uint8_t spiBusRawReadRegister(const busDevice_t *bus, uint8_t reg)
{
+#ifdef USE_DMA_SPI_DEVICE
+ if(USE_DMA_SPI_DEVICE == bus->busdev_u.spi.instance)
+ {
+ uint32_t timeoutCheck = millis();
+ dmaTxBuffer[0] = reg | 0x80;
+ dmaSpiTransmitReceive(dmaTxBuffer, dmaRxBuffer, 2, 1);
+ while(dmaSpiReadStatus != DMA_SPI_READ_DONE)
+ {
+ if(millis() - timeoutCheck > GYRO_READ_TIMEOUT)
+ {
+ //GYRO_READ_TIMEOUT ms max, read failed, cleanup spi and return 0
+ IOHi(bus->busdev_u.spi.csnPin);
+ #ifndef STM32F7
+ dmaSpicleanupspi();
+ #endif //STM32F7
+ return 0;
+ }
+ }
+ return dmaRxBuffer[1];
+ }
+ else
+ {
+ uint8_t data;
+ IOLo(bus->busdev_u.spi.csnPin);
+ spiTransferByte(bus->busdev_u.spi.instance, reg);
+ spiTransfer(bus->busdev_u.spi.instance, NULL, &data, 1);
+ IOHi(bus->busdev_u.spi.csnPin);
+
+ return data;
+ }
+#else
uint8_t data;
IOLo(bus->busdev_u.spi.csnPin);
spiTransferByte(bus->busdev_u.spi.instance, reg);
spiTransfer(bus->busdev_u.spi.instance, NULL, &data, 1);
IOHi(bus->busdev_u.spi.csnPin);
-
return data;
+#endif
}
uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg)
{
return spiBusRawReadRegister(bus, reg | 0x80);
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^
index d815cb3a0..179d496ab
--- a/src/main/drivers/bus_spi.h
+++ b/src/main/drivers/bus_spi.h
@@ -65,10 +65,15 @@ typedef enum {
// spi_ker_ck = 100MHz
SPI_CLOCK_SLOW = 128, //00.78125 MHz
SPI_CLOCK_STANDARD = 8, //12.00000 MHz
SPI_CLOCK_FAST = 4, //25.00000 MHz
SPI_CLOCK_ULTRAFAST = 2 //50.00000 MHz
+#elif defined(USE_OVERCLOCK)
+ SPI_CLOCK_SLOW = 128, //00.56250 MHz * 1.6
+ SPI_CLOCK_STANDARD = 8, //04.50000 MHz * 1.6
+ SPI_CLOCK_FAST = 4, //9.00000 MHz * 1.6
+ SPI_CLOCK_ULTRAFAST = 2 //18.00000 MHz & 1.6
#else
SPI_CLOCK_SLOW = 128, //00.56250 MHz
SPI_CLOCK_STANDARD = 4, //09.00000 MHz
SPI_CLOCK_FAST = 2, //18.00000 MHz
SPI_CLOCK_ULTRAFAST = 2 //18.00000 MHz
!🟥^^^^^^^^^^^^^^^^^^^^^^^ done then reverted ^^^^^^^^^^^^^^^^^^^^^^^^
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index 691ea86d3..a86590724
--- a/src/main/drivers/dma.h
+++ b/src/main/drivers/dma.h
@@ -166,11 +166,17 @@ typedef enum {
.userParam = 0, \
.owner.owner = 0, \
.owner.resourceIndex = 0 \
}
-#define DEFINE_DMA_IRQ_HANDLER(d, c, i) void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\
+#if defined(USE_CCM_CODE) && defined(STM32F3)
+#define DMA_HANDLER_CODE CCM_CODE
+#else
+#define DMA_HANDLER_CODE
+#endif
+
+#define DEFINE_DMA_IRQ_HANDLER(d, c, i) DMA_HANDLER_CODE void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\
const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \
dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \
if (handler) \
handler(&dmaDescriptors[index]); \
}
!🟥^^^^^^^^^^^^^^^^^^^ done but reverted ^^^^^^^^^^^^^^^^^^^^^
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index 000000000..d475840d5
--- /dev/null
+++ b/src/main/drivers/dma_spi.c
@@ -0,0 +1,227 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+#include "dma_spi.h"
+#include "common/time.h"
+#include "sensors/gyro.h"
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+#ifdef USE_GYRO_IMUF9001
+#include "drivers/accgyro/accgyro_imuf9001.h"
+volatile uint32_t crcErrorCount = 0;
+#endif
+
+
+#ifdef USE_DMA_SPI_DEVICE
+
+volatile bool dmaSpiDeviceDataReady = false;
+volatile dma_spi_read_status_t dmaSpiReadStatus = DMA_SPI_READ_UNKNOWN;
+
+//must be static to avoid overflow/corruption by DMA
+uint8_t dmaTxBuffer[58];
+uint8_t dmaRxBuffer[58];
+
+static inline void gpio_write_pin(GPIO_TypeDef * GPIOx, uint16_t GPIO_Pin, uint32_t pinState)
+{
+ if (pinState != 0)
+ {
+ GPIOx->BSRRL = (uint32_t)GPIO_Pin;
+ }
+ else
+ {
+ GPIOx->BSRRH = (uint32_t)GPIO_Pin;
+ }
+}
+
+static inline void dmaSpiCsLo(void)
+{
+ gpio_write_pin(DMA_SPI_NSS_PORT, DMA_SPI_NSS_PIN, 0);
+}
+
+static inline void dmaSpiCsHi(void)
+{
+ gpio_write_pin(DMA_SPI_NSS_PORT, DMA_SPI_NSS_PIN, 1);
+}
+
+void dmaSpicleanupspi(void)
+{
+ //clear DMA flags
+ DMA_ClearFlag(DMA_SPI_TX_DMA_STREAM, DMA_SPI_TX_DMA_FLAG_ALL);
+ DMA_ClearFlag(DMA_SPI_RX_DMA_STREAM, DMA_SPI_RX_DMA_FLAG_ALL);
+
+ //disable DMAs
+ DMA_Cmd(DMA_SPI_TX_DMA_STREAM,DISABLE);
+ DMA_Cmd(DMA_SPI_RX_DMA_STREAM,DISABLE);
+
+ //disable SPI DMA requests
+ SPI_I2S_DMACmd(DMA_SPI_SPI, SPI_I2S_DMAReq_Tx, DISABLE);
+ SPI_I2S_DMACmd(DMA_SPI_SPI, SPI_I2S_DMAReq_Rx, DISABLE);
+
+ // Reset SPI (clears TXFIFO).
+
+ //disable SPI
+ SPI_Cmd(DMA_SPI_SPI, DISABLE);
+}
+
+void DMA_SPI_RX_DMA_HANDLER(void)
+{
+ dmaSpiCsHi();
+ dmaSpicleanupspi();
+
+ //spi rx dma callback
+ #ifdef USE_GYRO_IMUF9001
+ volatile uint32_t crc1 = ( (*(uint32_t *)(dmaRxBuffer+gyroConfig()->imuf_mode-4)) & 0xFF );
+ volatile uint32_t crc2 = ( getCrcImuf9001((uint32_t *)(dmaRxBuffer), (gyroConfig()->imuf_mode >> 2)-1) & 0xFF );
+ if(crc1 == crc2)
+ {
+ if(dmaSpiReadStatus != DMA_SPI_BLOCKING_READ_IN_PROGRESS)
+ {
+ gyroDmaSpiFinishRead();
+ }
+ dmaSpiDeviceDataReady = true;
+ }
+ else
+ {
+ if (crcErrorCount > 100000)
+ {
+ crcErrorCount = 0;
+ }
+ //error handler
+ crcErrorCount++; //check every so often and cause a failsafe is this number is above a certain amount
+ }
+ #else
+ if(dmaSpiReadStatus != DMA_SPI_BLOCKING_READ_IN_PROGRESS)
+ {
+ gyroDmaSpiFinishRead();
+ }
+ dmaSpiDeviceDataReady = true;
+ #endif
+ DMA_ClearITPendingBit(DMA_SPI_RX_DMA_STREAM, DMA_SPI_RX_DMA_FLAG_TC);
+ dmaSpiReadStatus = DMA_SPI_READ_DONE;
+}
+
+bool isDmaSpiDataReady(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
+{
+ (void)(currentTimeUs);
+ (void)(currentDeltaTimeUs);
+ return dmaSpiDeviceDataReady;
+}
+
+void dmaSpiInit(void)
+{
+ GPIO_InitTypeDef gpioInitStruct;
+ SPI_InitTypeDef spiInitStruct;
+ DMA_InitTypeDef dmaInitStruct;
+ NVIC_InitTypeDef nvicInitStruct;
+
+ //reset SPI periphreal
+ DMA_SPI_PER |= DMA_SPI_RST_MSK;
+ DMA_SPI_PER &= ~DMA_SPI_RST_MSK;
+
+ //config pins
+ gpioInitStruct.GPIO_Pin = DMA_SPI_NSS_PIN;
+ gpioInitStruct.GPIO_Mode = GPIO_Mode_OUT;
+ gpioInitStruct.GPIO_Speed = GPIO_Speed_50MHz;
+ gpioInitStruct.GPIO_OType = GPIO_OType_PP;
+ gpioInitStruct.GPIO_PuPd = GPIO_PuPd_DOWN;
+ GPIO_Init(DMA_SPI_NSS_PORT, &gpioInitStruct);
+
+ //set default CS state (high)
+ GPIO_SetBits(DMA_SPI_NSS_PORT, DMA_SPI_NSS_PIN);
+
+ gpioInitStruct.GPIO_Mode = GPIO_Mode_AF;
+ gpioInitStruct.GPIO_Pin = DMA_SPI_SCK_PIN;
+ GPIO_Init(DMA_SPI_SCK_PORT, &gpioInitStruct);
+
+ gpioInitStruct.GPIO_Pin = DMA_SPI_MISO_PIN;
+ GPIO_Init(DMA_SPI_MISO_PORT, &gpioInitStruct);
+
+ gpioInitStruct.GPIO_Pin = DMA_SPI_MOSI_PIN;
+ GPIO_Init(DMA_SPI_MOSI_PORT, &gpioInitStruct);
+
+ //set AF map
+ GPIO_PinAFConfig(DMA_SPI_SCK_PORT, DMA_SPI_SCK_PIN_SRC, DMA_SPI_SCK_AF);
+ GPIO_PinAFConfig(DMA_SPI_MISO_PORT, DMA_SPI_MISO_PIN_SRC, DMA_SPI_MISO_AF);
+ GPIO_PinAFConfig(DMA_SPI_MOSI_PORT, DMA_SPI_MOSI_PIN_SRC, DMA_SPI_MOSI_AF);
+
+ //config SPI
+ SPI_StructInit(&spiInitStruct);
+ spiInitStruct.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
+ spiInitStruct.SPI_Mode = SPI_Mode_Master;
+ spiInitStruct.SPI_DataSize = SPI_DataSize_8b;
+ spiInitStruct.SPI_CPOL = DMA_SPI_CPOL;
+ spiInitStruct.SPI_CPHA = DMA_SPI_CPHA;
+ spiInitStruct.SPI_NSS = SPI_NSS_Soft;
+ spiInitStruct.SPI_BaudRatePrescaler = DMA_SPI_BAUDRATE;
+ spiInitStruct.SPI_FirstBit = SPI_FirstBit_MSB;
+ SPI_Init(DMA_SPI_SPI, &spiInitStruct);
+
+ //set DMA to default state
+ DMA_DeInit(DMA_SPI_TX_DMA_STREAM);
+ DMA_DeInit(DMA_SPI_RX_DMA_STREAM);
+
+ DMA_StructInit(&dmaInitStruct);
+ dmaInitStruct.DMA_Channel = DMA_SPI_TX_DMA_CHANNEL;
+ dmaInitStruct.DMA_Mode = DMA_Mode_Normal;
+ dmaInitStruct.DMA_Priority = DMA_Priority_VeryHigh;
+ dmaInitStruct.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+
+ dmaInitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ dmaInitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ dmaInitStruct.DMA_PeripheralBaseAddr = (uint32_t)&DMA_SPI_SPI->DR;
+
+ dmaInitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ dmaInitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ dmaInitStruct.DMA_Memory0BaseAddr = 0; //this is set later when we fire the DMA
+
+ dmaInitStruct.DMA_BufferSize = 1; //this is set later when we fire the DMA, can't be 0
+
+ DMA_Init(DMA_SPI_TX_DMA_STREAM, &dmaInitStruct);
+
+ dmaInitStruct.DMA_Channel = DMA_SPI_RX_DMA_CHANNEL;
+ dmaInitStruct.DMA_Priority = DMA_Priority_High;
+ dmaInitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory;
+
+ DMA_Init(DMA_SPI_RX_DMA_STREAM, &dmaInitStruct);
+
+ //setup interrupt
+ nvicInitStruct.NVIC_IRQChannel = DMA_SPI_RX_DMA_IRQn;
+ nvicInitStruct.NVIC_IRQChannelPreemptionPriority = 0;
+ nvicInitStruct.NVIC_IRQChannelSubPriority = 2;
+ nvicInitStruct.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&nvicInitStruct);
+ DMA_ITConfig(DMA_SPI_RX_DMA_STREAM, DMA_IT_TC, ENABLE);
+}
+
+void dmaSpiTransmitReceive(uint8_t* txBuffer, uint8_t* rxBuffer, uint32_t size, uint32_t blockingRead)
+{
+ //set buffer size
+ DMA_SetCurrDataCounter(DMA_SPI_TX_DMA_STREAM, size);
+ DMA_SetCurrDataCounter(DMA_SPI_RX_DMA_STREAM, size);
+
+ //set buffer
+ DMA_SPI_TX_DMA_STREAM->M0AR = (uint32_t)txBuffer;
+ DMA_SPI_RX_DMA_STREAM->M0AR = (uint32_t)rxBuffer;
+
+ //enable DMA SPI streams
+ DMA_Cmd(DMA_SPI_TX_DMA_STREAM, ENABLE);
+ DMA_Cmd(DMA_SPI_RX_DMA_STREAM, ENABLE);
+
+ //enable CS
+ dmaSpiCsLo();
+
+ //enable DMA SPI requests
+ SPI_I2S_DMACmd(DMA_SPI_SPI, SPI_I2S_DMAReq_Tx, ENABLE);
+ SPI_I2S_DMACmd(DMA_SPI_SPI, SPI_I2S_DMAReq_Rx, ENABLE);
+
+ //enable and send
+ SPI_Cmd(DMA_SPI_SPI, ENABLE);
+
+ if(!blockingRead)
+ dmaSpiReadStatus = DMA_SPI_READ_IN_PROGRESS;
+ else
+ dmaSpiReadStatus = DMA_SPI_BLOCKING_READ_IN_PROGRESS;
+}
+#endif
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ git checkout file ^^^^^^^^^^^^^^^^^^^^^^^^^
!🟥^^^^^ git checkout robertb-hesp-performancef3-master -- src/main/drivers/dma_spi.c
index 000000000..ea73a365b
--- /dev/null
+++ b/src/main/drivers/dma_spi.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include "common/time.h"
+
+#ifdef USE_DMA_SPI_DEVICE
+
+typedef enum dma_spi_read_status_e {
+ DMA_SPI_READ_UNKNOWN = 0,
+ DMA_SPI_READ_IN_PROGRESS = 1,
+ DMA_SPI_READ_DONE = 2,
+ DMA_SPI_BLOCKING_READ_IN_PROGRESS = 3,
+} dma_spi_read_status_t;
+
+extern volatile uint32_t crcErrorCount;
+extern volatile dma_spi_read_status_t dmaSpiReadStatus;
+extern volatile bool dmaSpiDeviceDataReady;
+extern uint8_t dmaTxBuffer[58];
+extern uint8_t dmaRxBuffer[58];
+extern bool isDmaSpiDataReady(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
+extern void dmaSpicleanupspi(void);
+extern void dmaSpiInit(void);
+extern void dmaSpiTransmitReceive(uint8_t* txBuffer, uint8_t* rxBuffer, uint32_t size, uint32_t blockingRead);
+
+#endif
\ No newline at end of file
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ git checkout file ^^^^^^^^^^^^^^^^^^^^^^^^^
!🟥^^^^^ git checkout robertb-hesp-performancef3-master -- src/main/drivers/dma_spi.h
index 000000000..6960b7b57
--- /dev/null
+++ b/src/main/drivers/dma_spi_hal.c
@@ -0,0 +1,226 @@
+
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+#include "dma_spi.h"
+#include "common/time.h"
+#include "sensors/gyro.h"
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+#ifdef USE_GYRO_IMUF9001
+#include "drivers/accgyro/accgyro_imuf9001.h"
+FAST_RAM_ZERO_INIT volatile uint32_t crcErrorCount;
+#endif
+
+#ifdef USE_DMA_SPI_DEVICE
+
+FAST_RAM_ZERO_INIT SPI_HandleTypeDef dmaSpiHandle;
+FAST_RAM_ZERO_INIT DMA_HandleTypeDef SpiRxDmaHandle;
+FAST_RAM_ZERO_INIT DMA_HandleTypeDef SpiTxDmaHandle;
+FAST_RAM_ZERO_INIT volatile dma_spi_read_status_t dmaSpiReadStatus;
+FAST_RAM_ZERO_INIT volatile bool dmaSpiDeviceDataReady = false;
+FAST_RAM_ZERO_INIT uint8_t dmaTxBuffer[58];
+FAST_RAM_ZERO_INIT uint8_t dmaRxBuffer[58];
+
+
+FAST_CODE static inline void dmaSpiCsLo(void)
+{
+ HAL_GPIO_WritePin(DMA_SPI_NSS_PORT, DMA_SPI_NSS_PIN, 0);
+}
+
+FAST_CODE static inline void dmaSpiCsHi(void)
+{
+ HAL_GPIO_WritePin(DMA_SPI_NSS_PORT, DMA_SPI_NSS_PIN, 1);
+}
+
+
+// FAST_CODE void dmaSpicleanupspi(void)
+// {
+// }
+
+FAST_CODE_NOINLINE void DMA_SPI_TX_DMA_HANDLER(void)
+{
+ HAL_NVIC_ClearPendingIRQ(DMA_SPI_TX_DMA_IRQn);
+ HAL_DMA_IRQHandler(&SpiTxDmaHandle);
+}
+
+FAST_CODE_NOINLINE void DMA_SPI_RX_DMA_HANDLER(void)
+{
+ HAL_NVIC_ClearPendingIRQ(DMA_SPI_RX_DMA_IRQn);
+ HAL_DMA_IRQHandler(&SpiRxDmaHandle);
+
+ if (HAL_DMA_GetState(&SpiRxDmaHandle) == HAL_DMA_STATE_READY)
+ {
+ dmaSpiCsHi();
+ //spi rx dma callback
+ #ifdef USE_GYRO_IMUF9001
+ volatile uint32_t crc1 = ( (*(uint32_t *)(dmaRxBuffer+gyroConfig()->imuf_mode-4)) & 0xFF );
+ volatile uint32_t crc2 = ( getCrcImuf9001((uint32_t *)(dmaRxBuffer), (gyroConfig()->imuf_mode >> 2)-1) & 0xFF );
+ if(crc1 == crc2)
+ {
+ if(dmaSpiReadStatus != DMA_SPI_BLOCKING_READ_IN_PROGRESS)
+ {
+ gyroDmaSpiFinishRead();
+ }
+ dmaSpiDeviceDataReady = true;
+ }
+ else
+ {
+ if (crcErrorCount > 100000)
+ {
+ crcErrorCount = 0;
+ }
+ //error handler
+ crcErrorCount++; //check every so often and cause a failsafe is this number is above a certain ammount
+ }
+ #else
+ if(dmaSpiReadStatus != DMA_SPI_BLOCKING_READ_IN_PROGRESS)
+ {
+ gyroDmaSpiFinishRead();
+ }
+ dmaSpiDeviceDataReady = true;
+ #endif
+ }
+
+ dmaSpiCsHi();
+ // dmaSpicleanupspi();
+
+ dmaSpiReadStatus = DMA_SPI_READ_DONE;
+}
+
+FAST_CODE inline bool isDmaSpiDataReady(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
+{
+ (void)(currentTimeUs);
+ (void)(currentDeltaTimeUs);
+ return dmaSpiDeviceDataReady;
+}
+
+void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi)
+{
+ GPIO_InitTypeDef GPIO_InitStruct;
+
+ DMA_SPI_CLOCK_INIT_FUNC;
+
+ HAL_GPIO_WritePin(DMA_SPI_NSS_PORT, DMA_SPI_NSS_PIN, 1);
+ HAL_GPIO_DeInit(DMA_SPI_NSS_PORT, DMA_SPI_NSS_PIN);
+ GPIO_InitStruct.Pin = DMA_SPI_NSS_PIN;
+ GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+ HAL_GPIO_Init(DMA_SPI_NSS_PORT, &GPIO_InitStruct);
+ HAL_GPIO_WritePin(DMA_SPI_NSS_PORT, DMA_SPI_NSS_PIN, 1);
+
+ HAL_GPIO_DeInit(DMA_SPI_SCK_PORT, DMA_SPI_SCK_PIN);
+ HAL_GPIO_DeInit(DMA_SPI_MISO_PORT, DMA_SPI_MISO_PIN);
+ HAL_GPIO_DeInit(DMA_SPI_MOSI_PORT, DMA_SPI_MOSI_PIN);
+
+ GPIO_InitStruct.Pin = DMA_SPI_SCK_PIN;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+ GPIO_InitStruct.Alternate = DMA_SPI_SCK_AF;
+ HAL_GPIO_Init(DMA_SPI_SCK_PORT, &GPIO_InitStruct);
+
+ GPIO_InitStruct.Pin = DMA_SPI_MISO_PIN;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+ GPIO_InitStruct.Alternate = DMA_SPI_MISO_AF;
+ HAL_GPIO_Init(DMA_SPI_MISO_PORT, &GPIO_InitStruct);
+
+ GPIO_InitStruct.Pin = DMA_SPI_MOSI_PIN;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+ GPIO_InitStruct.Alternate = DMA_SPI_MOSI_AF;
+ HAL_GPIO_Init(DMA_SPI_MOSI_PORT, &GPIO_InitStruct);
+
+
+ SpiRxDmaHandle.Instance = DMA_SPI_RX_DMA_STREAM;
+ SpiRxDmaHandle.Init.Channel = DMA_SPI_RX_DMA_CHANNEL;
+ SpiRxDmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
+ SpiRxDmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+ SpiRxDmaHandle.Init.MemInc = DMA_MINC_ENABLE;
+ SpiRxDmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ SpiRxDmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ SpiRxDmaHandle.Init.Mode = DMA_NORMAL;
+ SpiRxDmaHandle.Init.Priority = DMA_PRIORITY_HIGH;
+ SpiRxDmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+
+ HAL_DMA_UnRegisterCallback(&SpiRxDmaHandle, HAL_DMA_XFER_ALL_CB_ID);
+
+ HAL_DMA_Init(&SpiRxDmaHandle);
+
+ __HAL_LINKDMA(hspi, hdmarx, SpiRxDmaHandle);
+
+ HAL_NVIC_SetPriority(DMA_SPI_RX_DMA_IRQn, DMA_SPI_DMA_RX_PRE_PRI, DMA_SPI_DMA_RX_SUB_PRI);
+ HAL_NVIC_EnableIRQ(DMA_SPI_RX_DMA_IRQn);
+
+ SpiTxDmaHandle.Instance = DMA_SPI_TX_DMA_STREAM;
+ SpiTxDmaHandle.Init.Channel = DMA_SPI_TX_DMA_CHANNEL;
+ SpiTxDmaHandle.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ SpiTxDmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+ SpiTxDmaHandle.Init.MemInc = DMA_MINC_ENABLE;
+ SpiTxDmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ SpiTxDmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ SpiTxDmaHandle.Init.Mode = DMA_NORMAL;
+ SpiTxDmaHandle.Init.Priority = DMA_PRIORITY_VERY_HIGH;
+ SpiTxDmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+
+ HAL_DMA_UnRegisterCallback(&SpiTxDmaHandle, HAL_DMA_XFER_ALL_CB_ID);
+
+ HAL_DMA_Init(&SpiTxDmaHandle);
+
+ __HAL_LINKDMA(hspi, hdmatx, SpiTxDmaHandle);
+
+ HAL_NVIC_SetPriority(DMA_SPI_TX_DMA_IRQn, DMA_SPI_DMA_TX_PRE_PRI, DMA_SPI_DMA_TX_SUB_PRI);
+ HAL_NVIC_EnableIRQ(DMA_SPI_TX_DMA_IRQn);
+
+}
+
+void dmaSpiInit(void)
+{
+ dmaSpiHandle.Instance = DMA_SPI_SPI;
+ HAL_SPI_DeInit(&dmaSpiHandle);
+
+ dmaSpiHandle.Init.Mode = SPI_MODE_MASTER;
+ dmaSpiHandle.Init.Direction = SPI_DIRECTION_2LINES;
+ dmaSpiHandle.Init.DataSize = SPI_DATASIZE_8BIT;
+ dmaSpiHandle.Init.CLKPolarity = SPI_POLARITY_LOW;
+ dmaSpiHandle.Init.CLKPhase = SPI_PHASE_1EDGE;
+ dmaSpiHandle.Init.NSS = SPI_NSS_SOFT;
+ dmaSpiHandle.Init.BaudRatePrescaler = DMA_SPI_BAUDRATE;
+ dmaSpiHandle.Init.FirstBit = SPI_FIRSTBIT_MSB;
+ dmaSpiHandle.Init.TIMode = SPI_TIMODE_DISABLE;
+ dmaSpiHandle.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+ dmaSpiHandle.Init.CRCPolynomial = 7;
+
+ HAL_SPI_Init(&dmaSpiHandle);
+
+}
+
+FAST_CODE_NOINLINE void dmaSpiTransmitReceive(uint8_t* txBuffer, uint8_t* rxBuffer, uint32_t size, uint32_t blockingRead)
+{
+
+ if (HAL_SPI_GetState(&dmaSpiHandle) == HAL_SPI_STATE_READY)
+ {
+ dmaSpiCsLo();
+ if(!blockingRead)
+ {
+ dmaSpiReadStatus = DMA_SPI_READ_IN_PROGRESS;
+ HAL_SPI_TransmitReceive_DMA(&dmaSpiHandle, txBuffer, rxBuffer, size);
+ }
+ else
+ {
+ dmaSpiReadStatus = DMA_SPI_BLOCKING_READ_IN_PROGRESS;
+ HAL_SPI_TransmitReceive(&dmaSpiHandle, txBuffer, rxBuffer, size, 40);
+ dmaSpiCsHi();
+ dmaSpiReadStatus = DMA_SPI_READ_DONE;
+ }
+ }
+}
+
+#endif // USE_HAL_DMA_SPI_DEVICE
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^ git checkout file ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
!🟥 git checkout robertb-hesp-performancef3-master -- src/main/drivers/dma_spi_hal.c
index 000000000..81b90be35
--- /dev/null
+++ b/src/main/drivers/dma_spi_hal.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include "common/time.h"
+
+#ifdef USE_DMA_SPI_DEVICE
+
+typedef enum dma_spi_read_status_e {
+ DMA_SPI_READ_UNKNOWN = 0,
+ DMA_SPI_READ_IN_PROGRESS = 1,
+ DMA_SPI_READ_DONE = 2,
+ DMA_SPI_BLOCKING_READ_IN_PROGRESS = 3,
+} dma_spi_read_status_t;
+
+extern volatile uint32_t crcErrorCount;
+extern volatile dma_spi_read_status_t dmaSpiReadStatus;
+extern volatile bool dmaSpiDeviceDataReady;
+extern uint8_t dmaTxBuffer[58];
+extern uint8_t dmaRxBuffer[58];
+extern bool isDmaSpiDataReady(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
+// extern void dmaSpicleanupspi(void);
+extern void dmaSpiInit(void);
+extern void dmaSpiTransmitReceive(uint8_t* txBuffer, uint8_t* rxBuffer, uint32_t size, uint32_t blockingRead);
+
+#endif
\ No newline at end of file
!🟥^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^^
!🟥^^^^^^^^^^ git checkout robertb-hesp-performancef3-master -- src/main/drivers/dma_spi_hal.h
index 25359bc37..a4d7936c7
--- a/src/main/drivers/dma_stm32f4xx.c
+++ b/src/main/drivers/dma_stm32f4xx.c
@@ -64,12 +64,14 @@ DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER)
+#ifndef USE_DMA_SPI_DEVICE
DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER)
-DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER)
+#endif
+DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER) // ????
DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER)
!🟥^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^ ????????????? different than f7 below ^^^^^^^^^^^^^^^^^^6
index 38a0b653d..a7d1896e0
--- a/src/main/drivers/dma_stm32f7xx.c
+++ b/src/main/drivers/dma_stm32f7xx.c
@@ -65,12 +65,14 @@ DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER)
+#ifndef USE_DMA_SPI_DEVICE
DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER)
+#endif
DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER)
!🟥^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^
index 30d7e307c..c3ed7c6e5
--- a/src/main/drivers/light_led.h
+++ b/src/main/drivers/light_led.h
@@ -48,14 +48,19 @@ PG_DECLARE(statusLedConfig_t, statusLedConfig);
#define LED2_OFF NOOP
#define LED2_ON NOOP
#else
+/* #define LED0_TOGGLE NOOP */
+/* #define LED0_OFF NOOP */
+/* #define LED0_ON NOOP */
+
#define LED0_TOGGLE ledToggle(0)
#define LED0_OFF ledSet(0, false)
#define LED0_ON ledSet(0, true)
+
#define LED1_TOGGLE ledToggle(1)
#define LED1_OFF ledSet(1, false)
#define LED1_ON ledSet(1, true)
#define LED2_TOGGLE ledToggle(2)
!🟥^^^^^^^^^^^^^^^^ nonsensical ?? ^^^^^^^^^^^^^^^^
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index 861652b68..5c3480ded
--- a/src/main/drivers/pwm_output_dshot.c
+++ b/src/main/drivers/pwm_output_dshot.c
@@ -118,11 +118,16 @@ FAST_CODE void pwmDshotSetDirectionOutput(
xDMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
}
#ifdef USE_DSHOT_TELEMETRY
-FAST_CODE static void pwmDshotSetDirectionInput(
+#if defined(STM32F3)
+CCM_CODE
+#else
+FAST_CODE
+#endif
+static void pwmDshotSetDirectionInput(
motorDmaOutput_t * const motor
)
{
DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
@@ -181,10 +186,15 @@ void pwmCompleteDshotMotorUpdate(void)
dmaMotorTimers[i].timerDmaSources = 0;
}
}
}
+#if defined(STM32F3)
+CCM_CODE
+#else
+FAST_CODE
+#endif
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
{
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
#ifdef USE_DSHOT_TELEMETRY
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index f43066928..2adb53bd6
--- a/src/main/drivers/pwm_output_dshot_hal.c
+++ b/src/main/drivers/pwm_output_dshot_hal.c
@@ -96,11 +96,11 @@ void pwmDshotSetDirectionOutput(
xLL_EX_DMA_Init(motor->dmaRef, pDmaInit);
xLL_EX_DMA_EnableIT_TC(motor->dmaRef);
}
#ifdef USE_DSHOT_TELEMETRY
-static void pwmDshotSetDirectionInput(
+FAST_CODE static void pwmDshotSetDirectionInput(
motorDmaOutput_t * const motor
)
{
LL_DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
@@ -162,11 +162,11 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(void)
dmaMotorTimers[i].timerDmaSources = 0;
}
}
}
-static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
+FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
{
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
if (!motor->isInput) {
#ifdef USE_DSHOT_TELEMETRY
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index 0fc5a6503..80ab7e041
--- a/src/main/drivers/serial_usb_vcp.c
+++ b/src/main/drivers/serial_usb_vcp.c
@@ -216,12 +216,13 @@ static const struct serialPortVTable usbVTable[] = {
}
};
serialPort_t *usbVcpOpen(void)
{
- vcpPort_t *s;
-
+ static vcpPort_t *s;
+ if (s) return (serialPort_t *)s;
+
IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0);
IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0);
#if defined(STM32F4)
usbGenerateDisconnectPulse();
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index b36c1f799..b2902e322
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -197,11 +197,11 @@ void delay(uint32_t ms)
{
while (ms--)
delayMicroseconds(1000);
}
-static void indicate(uint8_t count, uint16_t duration)
+void indicate(uint8_t count, uint16_t duration)
{
if (count) {
LED1_ON;
LED0_OFF;
@@ -251,10 +251,18 @@ void initialiseMemorySections(void)
extern uint8_t tcm_code_end;
extern uint8_t tcm_code;
memcpy(&tcm_code_start, &tcm_code, (size_t) (&tcm_code_end - &tcm_code_start));
#endif
+#ifdef USE_CCM_CODE
+ /* Load functions into RAM */
+ extern uint8_t ccm_code_start;
+ extern uint8_t ccm_code_end;
+ extern uint8_t ccm_code;
+ memcpy(&ccm_code_start, &ccm_code, (size_t) (&ccm_code_end - &ccm_code_start));
+#endif
+
#ifdef USE_FAST_RAM
/* Load FAST_RAM variable intializers into DTCM RAM */
extern uint8_t _sfastram_data;
extern uint8_t _efastram_data;
extern uint8_t _sfastram_idata;
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index 6c7691439..a5b7f16ec
--- a/src/main/drivers/system_stm32f30x.c
+++ b/src/main/drivers/system_stm32f30x.c
@@ -33,11 +33,11 @@
#define BOOT_TARGET_REGISTER ((uint32_t *)0x20009FFC)
#define DEFAULT_STACK_POINTER ((uint32_t *)0x1FFFD800)
#define SYSTEM_MEMORY_RESET_VECTOR ((uint32_t *) 0x1FFFD804)
-void SetSysClock();
+void SetSysClock(void);
void systemReset(void)
{
// Generate system reset
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
!🟥^^^^^^^^^^^^^^^^^ unknown / skipped ^^^^^^^^^^^
index 73e47962d..40a41c165
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -80,10 +80,14 @@
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "common/sensor_alignment.h"
+#ifdef USE_GYRO_IMUF9001
+#include "drivers/accgyro/accgyro_imuf9001.h"
+#endif
+
static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */
static bool rebootRequired = false; // set if a config change requires a reboot to take effect
pidProfile_t *currentPidProfile;
!🟥^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^
@@ -177,10 +181,36 @@ static void adjustFilterLimit(uint16_t *parm, uint16_t resetValue)
if (*parm > FILTER_FREQUENCY_MAX) {
*parm = resetValue;
}
}
+#ifdef USE_GYRO_IMUF9001
+int getImufRateFromGyroSyncDenom(int gyroSyncDenom){
+ switch (gyroSyncDenom) {
+ case 1:
+ return IMUF_RATE_32K;
+ break;
+ case 2:
+ default:
+ return IMUF_RATE_16K;
+ break;
+ case 4:
+ return IMUF_RATE_8K;
+ break;
+ case 8:
+ return IMUF_RATE_4K;
+ break;
+ case 16:
+ return IMUF_RATE_2K;
+ break;
+ case 32:
+ return IMUF_RATE_1K;
+ break;
+ }
+}
+#endif
+
static void validateAndFixConfig(void)
{
#if !defined(USE_QUAD_MIXER_ONLY)
// Reset unsupported mixer mode to default.
// This check will be gone when motor/servo mixers are loaded dynamically
!🟥^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^
@@ -556,10 +586,20 @@ void validateAndFixGyroConfig(void)
if (gyro.targetLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) {
featureDisable(FEATURE_DYNAMIC_FILTER);
}
#endif
+#ifdef USE_GYRO_IMUF9001
+ //keeop imuf_rate in sync with the gyro.
+ uint8_t imuf_rate = getImufRateFromGyroSyncDenom(gyroConfigMutable()->gyro_sync_denom);
+ gyroConfigMutable()->imuf_rate = imuf_rate;
+ if (imuf_rate == IMUF_RATE_32K) {
+ gyroConfigMutable()->imuf_mode = GTBCM_GYRO_ACC_FILTER_F;
+ } else {
+ gyroConfigMutable()->imuf_mode = GTBCM_DEFAULT;
+ }
+#endif
// Fix gyro filter settings to handle cases where an older configurator was used that
// allowed higher cutoff limits from previous firmware versions.
adjustFilterLimit(&gyroConfigMutable()->gyro_lowpass_hz, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&gyroConfigMutable()->gyro_lowpass2_hz, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&gyroConfigMutable()->gyro_soft_notch_hz_1, FILTER_FREQUENCY_MAX);
!🟥^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^
index 62b59d4c8..1cd77d2d6
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -913,18 +913,18 @@ bool processRx(timeUs_t currentTimeUs)
} else {
DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
}
#endif
- if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
- LED1_ON;
- // increase frequency of attitude task to reduce drift when in angle or horizon mode
- rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
- } else {
- LED1_OFF;
- rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
- }
+// if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
+// LED1_ON;
+// // increase frequency of attitude task to reduce drift when in angle or horizon mode
+// rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
+// } else {
+// LED1_OFF;
+// rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
+// }
if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
}
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index 42888336d..70e50a948
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -65,10 +65,11 @@
#include "drivers/rx/rx_pwm.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
#include "drivers/serial_softserial.h"
#include "drivers/serial_uart.h"
+#include "drivers/serial_usb_vcp.h"
#include "drivers/sdcard.h"
#include "drivers/sdio.h"
#include "drivers/sound_beeper.h"
#include "drivers/system.h"
#include "drivers/time.h"
@@ -80,10 +81,18 @@
#endif
#include "drivers/vtx_common.h"
#include "drivers/vtx_rtc6705.h"
#include "drivers/vtx_table.h"
+#ifdef USE_DMA_SPI_DEVICE
+#include "drivers/dma_spi.h"
+#endif //USE_DMA_SPI_DEVICE
+
+#ifdef USE_GYRO_IMUF9001
+#include "drivers/accgyro/accgyro_imuf9001.h"
+#endif //USE_GYRO_IMUF9001
+
#include "fc/board_info.h"
#include "fc/config.h"
#include "fc/dispatch.h"
#include "fc/init.h"
#include "fc/rc_controls.h"
@@ -221,10 +230,13 @@ static void configureSPIAndQuadSPI(void)
spiPreinit();
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1);
#endif
+#ifdef USE_DMA_SPI_DEVICE
+ dmaSpiInit();
+#endif
#ifdef USE_SPI_DEVICE_2
spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
spiInit(SPIDEV_3);
@@ -262,10 +274,17 @@ void init(void)
printfSerialInit();
#endif
systemInit();
+#ifdef USE_GYRO_IMUF9001
+
+ if (isMPUSoftReset()) {
+ // reset imuf before befhal mucks with the pins
+ initImuf9001();
+ }
+#endif
// initialize IO (needed for all IO operations)
IOInitGlobal();
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
@@ -484,13 +503,30 @@ void init(void)
// Only F4 has non-8MHz boards
systemClockSetHSEValue(systemConfig()->hseMhz * 1000000U);
#endif
#ifdef USE_OVERCLOCK
+#if defined(STM32F3) && defined(USE_VCP)
+ if (systemConfig()->cpu_overclock > OVERCLOCK_128MHZ) {
+ usbVcpOpen();
+ uint32_t us = micros();
+ bool usbConnected = false;
+ while(cmpTimeUs(micros(), us) < 1500000 && !usbConnected) {
+ usbConnected = usbVcpIsConnected() != 0;
+ }
+
+ /* void indicate(uint8_t count, uint16_t duration); */
+ /* indicate((RCC->CFGR & (0xf << 18)) >> 18, 500); */
+ if (!usbConnected) {
+ OverclockRebootIfNecessary(systemConfig()->cpu_overclock);
+ }
+ } else
+#endif
OverclockRebootIfNecessary(systemConfig()->cpu_overclock);
#endif
+
// Configure MCO output after config is stable
#ifdef USE_MCO
mcoInit(mcoConfig());
#endif
!🟥^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^
index 869458698..0a9550045
--- a/src/main/fc/rc.c
+++ b/src/main/fc/rc.c
@@ -18,10 +18,11 @@
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
+#include <string.h>
#include <math.h>
#include "platform.h"
#include "build/debug.h"
@@ -60,18 +61,23 @@ static float rawSetpoint[XYZ_AXIS_COUNT];
static float rawDeflection[XYZ_AXIS_COUNT];
static float oldRcCommand[XYZ_AXIS_COUNT];
static bool isDuplicateFrame;
#endif
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
+static volatile uint32_t setpointRateInt[3];
static float throttlePIDAttenuation;
static bool reverseMotors = false;
static applyRatesFn *applyRates;
uint16_t currentRxRefreshRate;
FAST_RAM_ZERO_INIT uint8_t interpolationChannels;
static FAST_RAM_ZERO_INIT uint32_t rcFrameNumber;
+#ifdef USE_GYRO_IMUF9001
+ volatile bool isSetpointNew;
+#endif
+
enum {
ROLL_FLAG = 1 << ROLL,
PITCH_FLAG = 1 << PITCH,
YAW_FLAG = 1 << YAW,
THROTTLE_FLAG = 1 << THROTTLE,
@@ -99,10 +105,15 @@ uint32_t getRcFrameNumber()
float getSetpointRate(int axis)
{
return setpointRate[axis];
}
+uint32_t getSetpointRateInt(int axis)
+{
+ return setpointRateInt[axis];
+}
+
float getRcDeflection(int axis)
{
return rcDeflection[axis];
}
@@ -226,10 +237,11 @@ static void calculateSetpointRate(int axis)
angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
}
// Rate limit from profile (deg/sec)
setpointRate[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
+ memcpy((uint32_t*)&setpointRateInt[axis], (uint32_t*)&setpointRate[axis], sizeof(float));
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
}
static void scaleRcCommandToFpvCamAngle(void)
{
@@ -650,10 +662,13 @@ FAST_CODE void processRcCommand(void)
#pragma GCC diagnostic pop
#endif
calculateSetpointRate(axis);
}
+#ifdef USE_GYRO_IMUF9001
+ isSetpointNew = 1;
+#endif
DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
!🟥^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^
index d28f5d31a..2f531bee3
--- a/src/main/fc/rc.h
+++ b/src/main/fc/rc.h
@@ -28,14 +28,18 @@ typedef enum {
INTERPOLATION_CHANNELS_RPYT,
INTERPOLATION_CHANNELS_T,
INTERPOLATION_CHANNELS_RPT,
} interpolationChannels_e;
+#ifdef USE_GYRO_IMUF9001
+extern volatile bool isSetpointNew;
+#endif
extern uint16_t currentRxRefreshRate;
void processRcCommand(void);
float getSetpointRate(int axis);
+uint32_t getSetpointRateInt(int axis);
float getRcDeflection(int axis);
float getRcDeflectionAbs(int axis);
float getThrottlePIDAttenuation(void);
void updateRcCommands(void);
void resetYawAxis(void);
!🟥^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^
index 86f6da25d..7a2975e8b
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -72,10 +72,11 @@ typedef enum {
BOXUSER4,
BOXPIDAUDIO,
BOXACROTRAINER,
BOXVTXCONTROLDISABLE,
BOXLAUNCHCONTROL,
+ BOXWHOOPRACEMODE,
CHECKBOX_ITEM_COUNT
} boxId_e;
typedef enum {
MODELOGIC_OR = 0,
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index a1ed90677..297801c97
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -389,12 +389,17 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_STACK_CHECK] = DEFINE_TASK("STACKCHECK", NULL, NULL, taskStackCheck, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
#endif
[TASK_GYROPID] = DEFINE_TASK("PID", "GYRO", NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
#ifdef USE_ACC
+#ifdef USE_ACC_IMUF9001
[TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
- [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
+ [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(500), TASK_PRIORITY_MEDIUM),
+#else
+ [TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
+ [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
+#endif
#endif
[TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling
[TASK_DISPATCH] = DEFINE_TASK("DISPATCH", NULL, NULL, dispatchProcess, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH),
#ifdef USE_BEEPER
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ QUERSTIONABLE / SKIPPPED ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 053cdd9c1..a5cf05ba1
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -35,10 +35,11 @@
#include "pg/pg_ids.h"
#include "drivers/time.h"
#include "fc/runtime_config.h"
+#include "fc/rc_controls.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
@@ -49,10 +50,11 @@
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
+
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
#include <stdio.h>
#include <pthread.h>
static pthread_mutex_t imuUpdateLock;
@@ -112,11 +114,11 @@ quaternion offset = QUATERNION_INITIALIZE;
attitudeEulerAngles_t attitude = EULER_INITIALIZE;
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 1);
PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
- .dcm_kp = 2500, // 1.0 * 10000
+ .dcm_kp = 5000, // 1.0 * 10000
.dcm_ki = 0, // 0.003 * 10000
.small_angle = 25,
);
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){
@@ -198,11 +200,11 @@ static float invSqrt(float x)
return 1.0f / sqrtf(x);
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useAcc, float ax, float ay, float az,
- bool useMag, float mx, float my, float mz,
+ bool useMag,
bool useCOG, float courseOverGround, const float dcmKpGain)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
// Calculate general spin rate (rad/s)
@@ -226,10 +228,13 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
ez = rMat[2][2] * ez_ef;
}
#ifdef USE_MAG
// Use measured magnetic field vector
+ float mx = mag.magADC[X];
+ float my = mag.magADC[Y];
+ float mz = mag.magADC[Z];
float recipMagNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipMagNorm > 0.01f) {
// Normalise magnetometer measurement
recipMagNorm = invSqrt(recipMagNorm);
mx *= recipMagNorm;
@@ -253,13 +258,10 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
ey += rMat[2][1] * ez_ef;
ez += rMat[2][2] * ez_ef;
}
#else
UNUSED(useMag);
- UNUSED(mx);
- UNUSED(my);
- UNUSED(mz);
#endif
// Use measured acceleration vector
float recipAccNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipAccNorm > 0.01f) {
@@ -361,73 +363,73 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
// Accept accel readings only in range 0.9g - 1.1g
return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
}
-// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
-// When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence.
-// After disarming we want to quickly reestablish convergence to deal with the attitude estimation being incorrect due to a crash.
-// - wait for a 250ms period of low gyro activity to ensure the craft is not moving
-// - use a large dcmKpGain value for 500ms to allow the attitude estimate to quickly converge
-// - reset the gain back to the standard setting
-static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
+// Greg Egan
+float LPF1(float O, float N, const float K)
{
- static bool lastArmState = false;
- static timeUs_t gyroQuietPeriodTimeEnd = 0;
- static timeUs_t attitudeResetTimeEnd = 0;
- static bool attitudeResetCompleted = false;
- float ret;
- bool attitudeResetActive = false;
-
- const bool armState = ARMING_FLAG(ARMED);
-
- if (!armState) {
- if (lastArmState) { // Just disarmed; start the gyro quiet period
- gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
- attitudeResetTimeEnd = 0;
- attitudeResetCompleted = false;
- }
+ return (O + (N - O) * K);
+} // LPF1
- // If gyro activity exceeds the threshold then restart the quiet period.
- // Also, if the attitude reset has been complete and there is subsequent gyro activity then
- // start the reset cycle again. This addresses the case where the pilot rights the craft after a crash.
- if ((attitudeResetTimeEnd > 0) || (gyroQuietPeriodTimeEnd > 0) || attitudeResetCompleted) {
- if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT)
- || (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT)
- || (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT)
- || (!useAcc)) {
-
- gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
- attitudeResetTimeEnd = 0;
- }
- }
- if (attitudeResetTimeEnd > 0) { // Resetting the attitude estimation
- if (currentTimeUs >= attitudeResetTimeEnd) {
- gyroQuietPeriodTimeEnd = 0;
- attitudeResetTimeEnd = 0;
- attitudeResetCompleted = true;
- } else {
- attitudeResetActive = true;
- }
- } else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) {
- // Start the high gain period to bring the estimation into convergence
- attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME;
- gyroQuietPeriodTimeEnd = 0;
- }
- }
- lastArmState = armState;
+#define SQR(x) ((x)*(x))
+#define accConfidenceDecay (0.9f)
+
+float CalculateAccConfidence(float AccMagPeak)
+{
+ // Gaussian decay in accelerometer value belief
+ static float accMag = 1.0f;
+
+ accMag = LPF1(accMag, AccMagPeak, 0.1f);
+
+ return constrainf(1.0f - (accConfidenceDecay * sqrtf(fabsf(accMag - 1.0f))), 0.0f, 1.0f);
+} // CalculateAccConfidence
- if (attitudeResetActive) {
- ret = ATTITUDE_RESET_KP_GAIN;
- } else {
- ret = imuRuntimeConfig.dcm_kp;
- if (!armState) {
- ret = ret * 10.0f; // Scale the kP to generally converge faster when disarmed.
- }
- }
- return ret;
+static void yawPitchRollToMatrixII(float mat[3][3], float delta[3])
+{
+ //Precompute sines and cosines of Euler angles
+/**
+ const float su = sin_approx(angles[0]);
+ const float cu = cos_approx(angles[0]);
+ const float sv = sin_approx(angles[1]);
+ const float cv = cos_approx(angles[1]);
+ const float sw = sin_approx(angles[2]);
+ const float cw = cos_approx(angles[2]);
+
+ rMat[0][0] = cv * cw;
+ rMat[0][1] = su * sv * cw - cu * sw;
+ rMat[0][2] = su * sw + cu * sv * cw;
+ rMat[1][0] = cv * sw;
+ rMat[1][1] = cu * cw + su * sv * sw;
+ rMat[1][2] = cu * sv * sw - su * cw;
+ rMat[2][0] = -sv;
+ rMat[2][1] = su * cv;
+ rMat[2][2] = cu * cv;
+ */
+ const float cosx = cosf(delta[ROLL]);
+ const float sinx = sinf(delta[ROLL]);
+ const float cosy = cosf(delta[PITCH]);
+ const float siny = sinf(delta[PITCH]);
+ const float cosz = cosf(delta[YAW]);
+ const float sinz = sinf(delta[YAW]);
+
+ const float coszcosx = cosz * cosx;
+ const float coszcosy = cosz * cosy;
+ const float sinzcosx = sinz * cosx;
+ const float coszsinx = sinx * cosz;
+ const float sinzsinx = sinx * sinz;
+
+ mat[0][0] = coszcosy;
+ mat[0][1] = -cosy * sinz;
+ mat[0][2] = siny;
+ mat[1][0] = sinzcosx + (coszsinx * siny);
+ mat[1][1] = coszcosx - (sinzsinx * siny);
+ mat[1][2] = -sinx * cosy;
+ mat[2][0] = (sinzsinx) - (coszcosx * siny);
+ mat[2][1] = (coszsinx) + (sinzcosx * siny);
+ mat[2][2] = cosy * cosx;
}
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{
static timeUs_t previousIMUUpdateTime;
@@ -491,15 +493,19 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
if (accGetAccumulationAverage(accAverage)) {
useAcc = imuIsAccelerometerHealthy(accAverage);
}
+ const float accMag = sqrtf(SQR(accAverage[X]) + SQR(accAverage[Y]) + SQR(accAverage[Z]));
+ const float xG = accMag / acc.dev.acc_1G;
+ const float kP = imuRuntimeConfig.dcm_kp * CalculateAccConfidence(xG);
+
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
useAcc, accAverage[X], accAverage[Y], accAverage[Z],
- useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
- useCOG, courseOverGround, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
+ useMag,
+ useCOG, courseOverGround, kP);
imuUpdateEulerAngles();
#endif
}
!🟥^^^^^^^^^^^ QUESTIONABLE ^^^^^^ SKIPPED ^^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ QUERSTIONABLE / SKIPPPED ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 73c3c31d2..8a5ccccc2
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -40,10 +40,11 @@
#include "drivers/time.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"
#include "fc/rc.h"
+#include "fc/rc_modes.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
@@ -130,17 +131,22 @@ static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 13);
+//TYPES
+//-----
+
+
+
void resetPidProfile(pidProfile_t *pidProfile)
{
RESET_CONFIG(pidProfile_t, pidProfile,
.pid = {
- [PID_ROLL] = { 42, 85, 35, 90 },
- [PID_PITCH] = { 46, 90, 38, 95 },
- [PID_YAW] = { 30, 90, 0, 90 },
+ [PID_ROLL] = { 34, 60, 31, 10 },
+ [PID_PITCH] = { 36, 60, 33, 15 },
+ [PID_YAW] = { 30, 70, 0, 10 },
[PID_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0, 0 },
},
.pidSumLimit = PIDSUM_LIMIT,
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
@@ -153,19 +159,20 @@ void resetPidProfile(pidProfile_t *pidProfile)
.levelAngleLimit = 55,
.feedForwardTransition = 0,
.yawRateAccelLimit = 0,
.rateAccelLimit = 0,
.itermThrottleThreshold = 250,
- .itermAcceleratorGain = 5000,
+ .itermAcceleratorGain = 1000,
.crash_time = 500, // ms
.crash_delay = 0, // ms
.crash_recovery_angle = 10, // degrees
.crash_recovery_rate = 100, // degrees/second
.crash_dthreshold = 50, // degrees/second/second
.crash_gthreshold = 400, // degrees/second
.crash_setpoint_threshold = 350, // degrees/second
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
+ .crash_relax = CRASH_RELAX_CUTOFF_DEFAULT,
.horizon_tilt_effect = 75,
.horizon_tilt_expert_mode = false,
.crash_limit_yaw = 200,
.itermLimit = 400,
.throttle_boost = 5,
@@ -181,15 +188,15 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_gain = 0,
.abs_control_limit = 90,
.abs_control_error_limit = 20,
.abs_control_cutoff = 11,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
- .dterm_lowpass_hz = 150, // NOTE: dynamic lpf is enabled by default so this setting is actually
+ .dterm_lowpass_hz = 75, // NOTE: dynamic lpf is enabled by default so this setting is actually
// overridden and the static lowpass 1 is disabled. We can't set this
// value to 0 otherwise Configurator versions 10.4 and earlier will also
// reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
- .dterm_lowpass2_hz = 150, // second Dterm LPF ON by default
+ .dterm_lowpass2_hz = 85, // second Dterm filters the errorRate
.dterm_filter_type = FILTER_PT1,
.dterm_filter2_type = FILTER_PT1,
.dyn_lpf_dterm_min_hz = 70,
.dyn_lpf_dterm_max_hz = 170,
.launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
@@ -214,10 +221,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
.idle_max_increase = 150,
.ff_interpolate_sp = FF_INTERPOLATE_AVG,
.ff_spike_limit = 60,
.ff_max_rate_limit = 100,
.ff_boost = 15,
+ .i_decay = 3,
);
#ifndef USE_D_MIN
pidProfile->pid[PID_ROLL].D = 30;
pidProfile->pid[PID_PITCH].D = 32;
#endif
@@ -313,10 +321,13 @@ static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
static FAST_RAM_ZERO_INIT float ffBoostFactor;
static FAST_RAM_ZERO_INIT float ffSpikeLimitInverse;
+static FAST_RAM_ZERO_INIT pt1Filter_t crashRelaxPt1[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float crashRelaxSetpointThreshold;
+
float pidGetSpikeLimitInverse()
{
return ffSpikeLimitInverse;
}
@@ -338,11 +349,11 @@ void pidInitFilters(const pidProfile_t *pidProfile)
dtermLowpassApplyFn = nullFilterApply;
ptermYawLowpassApplyFn = nullFilterApply;
return;
}
- const uint32_t pidFrequencyNyquist = pidFrequency / 2; // No rounding needed
+ const uint32_t pidFrequencyNyquist = pidFrequency / 2; // No rounding needed
uint16_t dTermNotchHz;
if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
dTermNotchHz = pidProfile->dterm_notch_hz;
} else {
@@ -465,10 +476,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
ffSpikeLimitInverse = pidProfile->ff_spike_limit ? 1.0f / ((float)pidProfile->ff_spike_limit / 10.0f) : 0.0f;
+ for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+ pt1FilterInit(&crashRelaxPt1[axis], pt1FilterGain((float)pidProfile->crash_relax, dT));
+ }
}
#ifdef USE_RC_SMOOTHING_FILTER
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
{
@@ -658,10 +672,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
// adapt setpoint threshold to user changes from default cutoff value
itermRelaxSetpointThreshold = ITERM_RELAX_SETPOINT_THRESHOLD * ITERM_RELAX_CUTOFF_DEFAULT / itermRelaxCutoff;
#endif
+ crashRelaxSetpointThreshold = CRASH_RELAX_SETPOINT_THRESHOLD * CRASH_RELAX_CUTOFF_DEFAULT / pidProfile->crash_relax;
+
#ifdef USE_ACRO_TRAINER
acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
@@ -917,13 +933,19 @@ static void detectAndSetCrashRecovery(
{
// if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
// no point in trying to recover if the crash is so severe that the gyro overflows
if ((crash_recovery || FLIGHT_MODE(GPS_RESCUE_MODE)) && !gyroOverflowDetected()) {
if (ARMING_FLAG(ARMED)) {
- if (getMotorMixRange() >= 1.0f && !inCrashRecoveryMode
- && fabsf(delta) > crashDtermThreshold
- && fabsf(errorRate) > crashGyroThreshold
+ float sp = getSetpointRate(axis);
+ float setpointHpf = fabsf(sp - pt1FilterApply(&crashRelaxPt1[axis], sp));
+ float crashRelaxFactor = MAX(0, 1 - setpointHpf / crashRelaxSetpointThreshold);
+ if (axis == FD_YAW && fabsf(pidData[axis].I) == itermLimit) {
+ crashRelaxFactor = 0.0f;
+ }
+ if (!inCrashRecoveryMode
+ && crashRelaxFactor * fabsf(delta) > crashDtermThreshold
+ && crashRelaxFactor * fabsf(errorRate) > crashGyroThreshold
&& fabsf(getSetpointRate(axis)) < crashSetpointThreshold) {
if (crash_recovery == PID_CRASH_RECOVERY_DISARM) {
setArmingDisabled(ARMING_DISABLED_CRASH_DETECTED);
disarm();
} else {
@@ -1237,10 +1259,13 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
return ret;
}
#endif
+
+#define SIGN(x) ((x > 0.0f) - (x < 0.0f))
+
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
{
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
@@ -1271,11 +1296,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinActive = gyroYawSpinDetected();
#endif
const bool launchControlActive = isLaunchControlActive();
-
+ const bool whoop_racer_mode = IS_RC_MODE_ACTIVE(BOXWHOOPRACEMODE);
#if defined(USE_ACC)
const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
const bool levelModeActive = FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive;
// Keep track of when we entered a self-level mode so that we can
@@ -1303,20 +1328,20 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
if (itermWindupPointInv > 1.0f) {
dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f);
}
// Precalculate gyro deta for D-term here, this allows loop unrolling
- float gyroRateDterm[XYZ_AXIS_COUNT];
- for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
- gyroRateDterm[axis] = gyro.gyroADCf[axis];
-#ifdef USE_RPM_FILTER
- gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
-#endif
- gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRateDterm[axis]);
- gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
- gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
- }
+// float gyroRateDterm[XYZ_AXIS_COUNT];
+// for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
+// gyroRateDterm[axis] = gyro.gyroADCf[axis];
+//#ifdef USE_RPM_FILTER
+// gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
+//#endif
+// gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRateDterm[axis]);
+// gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
+// gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
+// }
rotateItermAndAxisError();
#ifdef USE_RPM_FILTER
rpmFilterUpdate();
#endif
@@ -1336,11 +1361,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
if (maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID
#if defined(USE_ACC)
- if (levelModeActive && (axis != FD_YAW)) {
+ const bool acro_on_pitch = whoop_racer_mode && axis == FD_PITCH;
+ if (levelModeActive && (axis != FD_YAW) && !acro_on_pitch) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
#endif
#ifdef USE_ACRO_TRAINER
@@ -1356,11 +1382,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#else
currentPidSetpoint = applyLaunchControl(axis, NULL);
#endif
}
#endif
-
// Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
// It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
#ifdef USE_YAW_SPIN_RECOVERY
if ((axis == FD_YAW) && yawSpinActive) {
currentPidSetpoint = 0.0f;
@@ -1368,10 +1393,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#endif // USE_YAW_SPIN_RECOVERY
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
float errorRate = currentPidSetpoint - gyroRate; // r - y
+
#if defined(USE_ACC)
handleCrashRecovery(
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate);
#endif
@@ -1407,11 +1433,26 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// if launch control is active override the iterm gains
const float Ki = launchControlActive ? launchControlKi : pidCoefficient[axis].Ki;
#else
const float Ki = pidCoefficient[axis].Ki;
#endif
- pidData[axis].I = constrainf(previousIterm + Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
+
+ // I_DECAY
+ float ITermNew = Ki * itermErrorRate * dynCi;
+ if (ITermNew != 0.0f)
+ {
+ if (SIGN(previousIterm) != SIGN(ITermNew))
+ {
+ const float newVal = ITermNew * (float)pidProfile->i_decay;
+ if (fabsf(previousIterm) > fabsf(newVal))
+ {
+ ITermNew = newVal;
+ }
+ }
+ }
+
+ pidData[axis].I = constrainf(previousIterm + ITermNew, -itermLimit, itermLimit);
// -----calculate pidSetpointDelta
float pidSetpointDelta = 0;
#ifdef USE_INTERPOLATED_SP
if (ffFromInterpolatedSetpoint) {
@@ -1436,12 +1477,16 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// Divide rate change by dT to get differential (ie dr/dt).
// dT is fixed and calculated from the target PID loop time
// This is done to avoid DTerm spikes that occur with dynamically
// calculated deltaT whenever another task causes the PID
// loop execution to be delayed.
- const float delta =
- - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidFrequency;
+ float delta = errorRate - previousGyroRateDterm[axis];
+ previousGyroRateDterm[axis] = errorRate;
+ delta *= pidFrequency;
+ delta = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], delta);
+ delta = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], delta);
+ delta = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], delta);
#if defined(USE_ACC)
if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
}
@@ -1468,11 +1513,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#endif
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor * dMinFactor;
} else {
pidData[axis].D = 0;
}
- previousGyroRateDterm[axis] = gyroRateDterm[axis];
+// previousGyroRateDterm[axis] = gyroRateDterm[axis];
// -----calculate feedforward component
#ifdef USE_ABSOLUTE_CONTROL
// include abs control correction in FF
pidSetpointDelta += setpointCorrection - oldSetpointCorrection[axis];
!🟥^^^^^^^^^^^ QUESTIONABLE ^^^^^^ SKIPPED ^^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ QUERSTIONABLE / SKIPPPED ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 691bcb7e0..c79c93fad
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -46,10 +46,13 @@
// Full iterm suppression in setpoint mode at high-passed setpoint rate > 40deg/sec
#define ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
#define ITERM_RELAX_CUTOFF_DEFAULT 20
+#define CRASH_RELAX_SETPOINT_THRESHOLD 40.0f
+#define CRASH_RELAX_CUTOFF_DEFAULT 20
+
typedef enum {
PID_ROLL,
PID_PITCH,
PID_YAW,
PID_LEVEL,
@@ -139,10 +142,11 @@ typedef struct pidProfile_s {
uint8_t feedForwardTransition; // Feed forward weight transition
uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
uint16_t itermLimit;
uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz
uint8_t crash_recovery; // off, on, on and beeps when it is in crash recovery mode
+ uint8_t crash_relax; // cutoff of high pass to attenuate error signals
uint8_t throttle_boost; // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system
uint8_t iterm_relax_type; // Specifies type of relax algorithm
uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
@@ -182,10 +186,13 @@ typedef struct pidProfile_s {
uint8_t idle_max_increase; // max integrated correction
uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
uint8_t ff_spike_limit; // FF stick extrapolation lookahead period in ms
+
+ // I_DECAY
+ uint8_t i_decay; // i-term decay
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
typedef struct pidConfig_s {
!🟥^^^^^^^^^^^ QUESTIONABLE ^^^^^^ SKIPPED ^^^^^^^^^^^^^^^^^^^^
!🟥↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ QUERSTIONABLE / SKIPPPED ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 063cf4065..0f72a218e
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -116,11 +116,16 @@ static const servoMixer_t servoMixerFlyingWing[] = {
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
};
static const servoMixer_t servoMixerTri[] = {
+#ifdef ROVER3
+ { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 60, 0 },
+ { SERVO_ELEVATOR, INPUT_STABILIZED_YAW, 100, 0, 0, 60, 0 },
+#else
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
+#endif
};
#if defined(USE_UNCOMMON_MIXERS)
static const servoMixer_t servoMixerBI[] = {
{ SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
@@ -337,12 +342,18 @@ void writeServos(void)
case MIXER_TRI:
case MIXER_CUSTOM_TRI:
// We move servo if unarmed flag set or armed
if (!(servosTricopterIsEnabledServoUnarmed() || ARMING_FLAG(ARMED))) {
servo[SERVO_RUDDER] = 0; // kill servo signal completely.
+#ifdef ROVER3
+ servo[SERVO_ELEVATOR] = 0;
+#endif
}
writeServoWithTracking(servoIndex++, SERVO_RUDDER);
+#ifdef ROVER3
+ writeServoWithTracking(servoIndex++, SERVO_ELEVATOR);
+#endif
break;
case MIXER_FLYING_WING:
writeServoWithTracking(servoIndex++, SERVO_FLAPPERON_1);
writeServoWithTracking(servoIndex++, SERVO_FLAPPERON_2);
!🟥^^^^^^^^^^^ QUESTIONABLE ^^^^^^ SKIPPED ^^^^^^^^^^^^^^^^^^^^
!🟥↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ Skipped ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index a6779a094..6013d9df3
--- a/src/main/msp/msp_box.c
+++ b/src/main/msp/msp_box.c
@@ -94,10 +94,11 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOXPARALYZE, "PARALYZE", 45 },
{ BOXGPSRESCUE, "GPS RESCUE", 46 },
{ BOXACROTRAINER, "ACRO TRAINER", 47 },
{ BOXVTXCONTROLDISABLE, "DISABLE VTX CONTROL", 48},
{ BOXLAUNCHCONTROL, "LAUNCH CONTROL", 49 },
+ { BOXWHOOPRACEMODE, "WHOOP RACEMODE", 50 },
};
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
static boxBitmask_t activeBoxIds;
@@ -302,10 +303,11 @@ void initActiveBoxIds(void)
#endif // USE_ACRO_TRAINER
#ifdef USE_LAUNCH_CONTROL
BME(BOXLAUNCHCONTROL);
#endif
+ BME(BOXWHOOPRACEMODE);
#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++)
if (bitArrayGet(&ena, boxId)
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ Skipped ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 896324c3f..f69260988
--- a/src/main/pg/sdcard.c
+++ b/src/main/pg/sdcard.c
@@ -64,13 +64,13 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config)
#endif
#ifndef USE_DMA_SPEC
#ifdef USE_SDCARD_SPI
#if defined(SDCARD_DMA_STREAM_TX_FULL)
- config->dmaIdentifier = (uint8_t)dmaGetIdentifier(SDCARD_DMA_STREAM_TX_FULL);
+ config->dmaIdentifier = (uint8_t)dmaGetIdentifier((dmaResource_t*)SDCARD_DMA_STREAM_TX_FULL);
#elif defined(SDCARD_DMA_CHANNEL_TX)
- config->dmaIdentifier = (uint8_t)dmaGetIdentifier(SDCARD_DMA_CHANNEL_TX);
+ config->dmaIdentifier = (uint8_t)dmaGetIdentifier((dmaResource_t*)SDCARD_DMA_CHANNEL_TX);
#endif
#endif
#endif // !USE_DMA_SPEC
}
#endif
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ SKIPPPED ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index a57f8294e..7a3f4bde6
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -30,10 +30,11 @@
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "common/utils.h"
+#include "common/dynLpf2.h"
#include "config/config_reset.h"
#include "config/feature.h"
#include "drivers/accgyro/accgyro.h"
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ SKIPPPED ^^^^^^^^^^^^^^^^^^^^^^
@@ -63,10 +64,14 @@
#ifdef USE_ACC_MMA8452
#include "drivers/accgyro_legacy/accgyro_mma845x.h"
#endif
+#ifdef USE_ACC_IMUF9001
+#include "drivers/accgyro/accgyro_imuf9001.h"
+#endif //USE_ACC_IMUF9001
+
#include "drivers/bus_spi.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^66
!🟥↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ Skipped ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
@@ -114,11 +119,11 @@ void accResetFlightDynamicsTrims(void)
}
void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
{
RESET_CONFIG_2(accelerometerConfig_t, instance,
- .acc_lpf_hz = 10,
+ .acc_lpf_hz = 120,
.acc_hardware = ACC_DEFAULT,
.acc_high_fsr = false,
);
resetRollAndPitchTrims(&instance->accelerometerTrims);
resetFlightDynamicsTrims(&instance->accZero);
@@ -137,11 +142,11 @@ static int accumulatedMeasurementCount;
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static flightDynamicsTrims_t *accelerationTrims;
static uint16_t accLpfCutHz = 0;
-static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
+static pt1Filter_t accFilter[XYZ_AXIS_COUNT];
bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware = ACC_NONE;
@@ -258,11 +263,20 @@ retry:
break;
}
FALLTHROUGH;
#endif
!🟥^^^^^^^^^^^^^^^^^ skipped -- he replaced biquad with pt1 ^^^^^^^^^^^^^^^^^
-#ifdef USE_ACC_SPI_ICM20689
+#ifdef USE_ACC_IMUF9001
+ case ACC_IMUF9001:
+ if (imufSpiAccDetect(dev)) {
+ accHardware = ACC_IMUF9001;
+ break;
+ }
+ FALLTHROUGH;
+#endif
+
+ #ifdef USE_ACC_SPI_ICM20689
case ACC_ICM20689:
if (icm20689SpiAccDetect(dev)) {
accHardware = ACC_ICM20689;
break;
}
!🟥^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^
@@ -307,10 +321,16 @@ retry:
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
return true;
}
+
+int accGetSamplingIntervall(void)
+{
+ return acc.accSamplingInterval;
+}
+
bool accInit(uint32_t gyroSamplingInverval)
{
memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings
acc.dev.bus = *gyroSensorBus();
@@ -349,15 +369,24 @@ bool accInit(uint32_t gyroSamplingInverval)
break;
case 1000:
default:
acc.accSamplingInterval = 1000;
}
- if (accLpfCutHz) {
- for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
+
+#ifdef USE_ACC_IMUF9001
+ acc.accSamplingInterval = 500;
+#endif
+
+ if (accLpfCutHz)
+ {
+ const float gain = pt1FilterGain(accLpfCutHz, acc.accSamplingInterval * 1e-6f);
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
+ {
+ pt1FilterInit(&accFilter[axis], gain);
}
}
+
return true;
}
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
!🟥^^^^^^^^^^^^^^^^^^^^ did the ifdef ONLY ^^^^^^^^^^^^^^^^^^^^^^^^
!🟥^^^^^^^^^^^^^^^^^^^^↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ Skipping for now ^^^^^^^^^^^^^^^^^^^^↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
!🟥^^^^^^^^^^^^^^^^^^^^↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ crap gotta do the PT1??? ^^^^^^^^^^^^^^^^^^^^↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
!🟥^^^^^^^^^^^^^^^^^^^^↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ Skipping for now ^^^^^^^^^^^^^^^^^^^^↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
@@ -485,11 +514,12 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
acc.accADC[axis] = acc.dev.ADCRaw[axis];
}
if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- acc.accADC[axis] = biquadFilterApply(&accFilter[axis], acc.accADC[axis]);
+ acc.accADC[axis] = pt1FilterApply(&accFilter[axis], acc.accADC[axis]);
+ //acc.accADC[axis] = dynLpf2ApplyAcc(axis, acc.accADC[axis]);
}
}
if (acc.dev.accAlign == ALIGN_CUSTOM) {
alignSensorViaMatrix(acc.accADC, &acc.dev.rotationMatrix);
@@ -537,13 +567,16 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
}
void accInitFilters(void)
{
accLpfCutHz = accelerometerConfig()->acc_lpf_hz;
- if (acc.accSamplingInterval) {
- for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
+ if (acc.accSamplingInterval)
+ {
+ const float gain = pt1FilterGain(accLpfCutHz, acc.accSamplingInterval * 1e-6f);
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
+ {
+ pt1FilterInit(&accFilter[axis], gain);
}
}
}
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
!🟥^^^^^^^^^^^^^^^^^ skipped -- he replaced biquad with pt1 ^^^^^^^^^^^^^^^^^
index 4ad1b1642..965d16bda
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -41,10 +41,11 @@ typedef enum {
ACC_ICM20602,
ACC_ICM20608G,
ACC_ICM20649,
ACC_ICM20689,
ACC_BMI160,
+ ACC_IMUF9001,
ACC_FAKE
} accelerationSensor_e;
typedef struct acc_s {
accDev_t dev;
@@ -85,5 +86,8 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims);
bool accGetAccumulationAverage(float *accumulation);
union flightDynamicsTrims_u;
void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse);
void accInitFilters(void);
void applyAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
+
+int accGetSamplingIntervall(void);
+
!🟥^^^^^^^^^^^^^^^^^^^^ done BUT skipped the removal of biQuad into PT1
index de312b070..abeda83da
--- a/src/main/sensors/boardalignment.c
+++ b/src/main/sensors/boardalignment.c
@@ -41,11 +41,11 @@ static bool standardBoardAlignment = true; // board orientation correction
static fp_rotationMatrix_t boardRotation;
// no template required since defaults are zero
PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0);
-static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment)
+bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment)
{
return !boardAlignment->rollDegrees && !boardAlignment->pitchDegrees && !boardAlignment->yawDegrees;
}
void initBoardAlignment(const boardAlignment_t *boardAlignment)
!🟥^^^^^^^^^^^^^ done -- found necessary during compile phase ^^^^^^^^^^^
index e2be2ba84..f9c9bfebd
--- a/src/main/sensors/boardalignment.h
+++ b/src/main/sensors/boardalignment.h
@@ -35,5 +35,6 @@ PG_DECLARE(boardAlignment_t, boardAlignment);
void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* rotationMatrix);
void alignSensorViaRotation(float *dest, uint8_t rotation);
void initBoardAlignment(const boardAlignment_t *boardAlignment);
+bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment);
!🟥^^^^^^^^^^^^^ done -- found necessary during compile phase ^^^^^^^^^^^
index 2eae9232f..983a584c2
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -29,10 +29,11 @@
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
+#include "common/dynLpf2.h"
!🟥^^^^^ skip ^^^^^^
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
@@ -49,10 +50,15 @@
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
+#ifdef USE_GYRO_IMUF9001
+#include "drivers/accgyro/accgyro_imuf9001.h"
+#include "drivers/time.h"
+#endif //USE_GYRO_IMUF9001
+
!🟥^^^^^ done ^^^^^^
#ifdef USE_GYRO_L3GD20
#include "drivers/accgyro/accgyro_spi_l3gd20.h"
#endif
#ifdef USE_GYRO_L3G4200D
@@ -77,10 +83,11 @@
#include "scheduler/scheduler.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
+#include "sensors/gyro.h"
!🟥^^^^^^^ done but reverted -- duplicate ^^^^^^^^^^^^
#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))
#define USE_GYRO_SLEW_LIMITER
#endif
@@ -194,10 +201,30 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM;
gyroConfig->dyn_notch_width_percent = 8;
gyroConfig->dyn_notch_q = 120;
gyroConfig->dyn_notch_min_hz = 150;
gyroConfig->gyro_filter_debug_axis = FD_ROLL;
+#ifdef USE_DYN_LPF2
+ gyroConfig->dynlpf2_enable = DEFAULT_DYNLPF2_ENABLE;
+#endif
+
!🟥^^^^^^^^^^^^ skip ^^^^^^^^^^
!🟥↓↓↓↓↓↓↓↓ done ↓↓↓↓↓↓↓↓
+#if defined(USE_GYRO_IMUF9001)
+ gyroConfig->imuf_mode = GTBCM_DEFAULT;
+ gyroConfig->imuf_rate = IMUF_RATE_16K;
+ gyroConfig->imuf_roll_q = IMUF_DEFAULT_ROLL_Q;
+ gyroConfig->imuf_pitch_q = IMUF_DEFAULT_PITCH_Q;
+ gyroConfig->imuf_yaw_q = IMUF_DEFAULT_YAW_Q;
+ gyroConfig->imuf_w = IMUF_DEFAULT_W;
+ gyroConfig->imuf_roll_lpf_cutoff_hz = IMUF_DEFAULT_LPF_HZ;
+ gyroConfig->imuf_pitch_lpf_cutoff_hz = IMUF_DEFAULT_LPF_HZ;
+ gyroConfig->imuf_yaw_lpf_cutoff_hz = IMUF_DEFAULT_LPF_HZ;
+ gyroConfig->imuf_roll_af = IMUF_DEFAULT_ROLL_AF;
+ gyroConfig->imuf_pitch_af = IMUF_DEFAULT_PITCH_AF;
+ gyroConfig->imuf_yaw_af = IMUF_DEFAULT_YAW_AF;
+ gyroConfig->gyro_offset_yaw = 0;
+ gyroConfig->gyro_align = ALIGN_DEFAULT;
+#endif
}
#ifdef USE_MULTI_GYRO
#define ACTIVE_GYRO ((gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyroSensor2 : &gyroSensor1)
#else
@@ -328,10 +355,19 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
break;
}
FALLTHROUGH;
#endif
+#ifdef USE_GYRO_IMUF9001
+ case GYRO_IMUF9001:
+ if (imufSpiGyroDetect(dev)) {
+ gyroHardware = GYRO_IMUF9001;
+ break;
+ }
+ FALLTHROUGH;
+#endif //USE_GYRO_IMUF9001
+
#ifdef USE_GYRO_SPI_ICM20689
case GYRO_ICM20689:
if (icm20689SpiGyroDetect(dev)) {
gyroHardware = GYRO_ICM20689;
break;
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^
@@ -370,21 +406,23 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
}
static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
{
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
- || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
+ || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) \
+ || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_IMUF9001)
mpuPreInit(config);
#else
UNUSED(config);
#endif
}
static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
{
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
- || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20)
+ || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) \
+ || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_IMUF9001)
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
#if !defined(USE_FAKE_GYRO) // Allow resorting to fake accgyro if defined
if (!gyroFound) {
!🟥^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^
@@ -427,10 +465,11 @@ static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
case GYRO_L3GD20:
case GYRO_BMI160:
case GYRO_MPU6000:
case GYRO_MPU6500:
case GYRO_MPU9250:
+ case GYRO_IMUF9001:
gyroSensor->gyroDev.gyroHasOverflowProtection = true;
break;
case GYRO_ICM20601:
case GYRO_ICM20602:
@@ -741,10 +780,15 @@ void gyroInitFilters(void)
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
gyro_lowpass_hz = gyroConfig()->dyn_lpf_gyro_min_hz;
}
#endif
!🟥 ^^^^^^^^^^ done ^^^^^^^^^^^^
+
+#ifdef USE_DYN_LPF2
+ init_dynLpf2();
+#endif
+
gyroInitLowpassFilterLpf(
FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type,
gyro_lowpass_hz
);
!🟥^^^^^^^^^^^^^^^^^^^ skipped dynLPF2 ^^^^^^^^^^
@@ -806,10 +850,13 @@ static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibrati
return gyroCalibration->cyclesRemaining == gyroCalculateCalibratingCycles();
}
static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
{
+#ifdef USE_GYRO_IMUF9001
+ imufStartCalibration();
+#endif
#if defined(USE_FAKE_GYRO) && !defined(UNIT_TEST)
if (gyroSensor->gyroDev.gyroHardware == GYRO_FAKE) {
gyroSensor->calibration.cyclesRemaining = 0;
return;
}
@@ -819,10 +866,13 @@ static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
void gyroStartCalibration(bool isFirstArmingCalibration)
{
if (!(isFirstArmingCalibration && firstArmingCalibrationWasStarted)) {
gyroSetCalibrationCycles(&gyroSensor1);
+#ifdef USE_GYRO_IMUF9001
+ imufStartCalibration();
+#endif
#ifdef USE_MULTI_GYRO
gyroSetCalibrationCycles(&gyroSensor2);
#endif
if (isFirstArmingCalibration) {
@@ -1003,38 +1053,74 @@ static FAST_CODE void checkForYawSpin(timeUs_t currentTimeUs)
}
#endif // USE_YAW_SPIN_RECOVERY
static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor)
{
+#ifndef USE_DMA_SPI_DEVICE
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
return;
}
+#endif
gyroSensor->gyroDev.dataReady = false;
+#ifdef USE_GYRO_IMUF9001
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
+ DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroSensor->gyroDev.gyroADC[axis]));
+ if (!overflowDetected) {
+ // integrate using trapezium rule to avoid bias
+ accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroSensor->gyroDev.gyroADC[axis]) * gyro.targetLooptime;
+ gyroPrevious[axis] = gyroSensor->gyroDev.gyroADC[axis];
+ }
+ }
+ if (!isGyroSensorCalibrationComplete(gyroSensor)) {
+ performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
+ // Reset gyro values to zero to prevent other code from using uncalibrated data
+ gyroSensor->gyroDev.gyroADC[X] = 0.0f;
+ gyroSensor->gyroDev.gyroADC[Y] = 0.0f;
+ gyroSensor->gyroDev.gyroADC[Z] = 0.0f;
+ // still calibrating, so no need to further process gyro data
+ }
+#else
if (isGyroSensorCalibrationComplete(gyroSensor)) {
// move 16-bit gyro data into 32-bit variables to avoid overflows in calculations
#if defined(USE_GYRO_SLEW_LIMITER)
gyroSensor->gyroDev.gyroADC[X] = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Y] = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y];
gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z];
#else
gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y];
gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
#endif
if (gyroSensor->gyroDev.gyroAlign == ALIGN_CUSTOM) {
alignSensorViaMatrix(gyroSensor->gyroDev.gyroADC, &gyroSensor->gyroDev.rotationMatrix);
} else {
alignSensorViaRotation(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
}
} else {
performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
}
+#endif
}
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^
+#ifdef USE_DMA_SPI_DEVICE
+FAST_CODE_NOINLINE void gyroDmaSpiFinishRead(void)
+{
+ //called by dma callback
+ mpuGyroDmaSpiReadFinish(&gyroSensor1.gyroDev);
+}
+
+FAST_CODE_NOINLINE void gyroDmaSpiStartRead(void)
+{
+ //called by exti
+ mpuGyroDmaSpiReadStart(&gyroSensor1.gyroDev);
+}
+#endif
+
#define GYRO_FILTER_FUNCTION_NAME filterGyro
#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
#include "gyro_filter_impl.c"
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET
@@ -1050,13 +1136,19 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
switch (gyroToUse) {
case GYRO_CONFIG_USE_GYRO_1:
gyroUpdateSensor(&gyroSensor1);
if (isGyroSensorCalibrationComplete(&gyroSensor1)) {
+#ifdef USE_GYRO_IMUF9001
+ gyro.gyroADC[X] = gyroSensor1.gyroDev.gyroADC[X];
+ gyro.gyroADC[Y] = gyroSensor1.gyroDev.gyroADC[Y];
+ gyro.gyroADC[Z] = gyroSensor1.gyroDev.gyroADC[Z];
+#else
gyro.gyroADC[X] = gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale;
gyro.gyroADC[Y] = gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale;
gyro.gyroADC[Z] = gyroSensor1.gyroDev.gyroADC[Z] * gyroSensor1.gyroDev.scale;
+#endif
}
break;
#ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2:
gyroUpdateSensor(&gyroSensor2);
@@ -1201,11 +1293,15 @@ int16_t gyroGetTemperature(void)
return gyroSensorTemperature;
}
int16_t gyroRateDps(int axis)
{
+#if defined(USE_GYRO_IMUF9001)
+ return lrintf(gyro.gyroADCf[axis]);
+#else
return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale);
+#endif
}
bool gyroOverflowDetected(void)
{
#ifdef USE_GYRO_OVERFLOW_CHECK
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^^^^^^^^^6
index fc8351173..12f3df113
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -34,10 +34,21 @@
#include "pg/pg.h"
#define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling)
+#if defined(USE_GYRO_IMUF9001)
+typedef enum {
+ IMUF_RATE_32K = 0,
+ IMUF_RATE_16K = 1,
+ IMUF_RATE_8K = 2,
+ IMUF_RATE_4K = 3,
+ IMUF_RATE_2K = 4,
+ IMUF_RATE_1K = 5
+} imufRate_e;
+#endif
+
typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
} gyroLowpassFilter_t;
!🟥^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^
@@ -150,10 +161,28 @@ typedef struct gyroConfig_s {
uint8_t dyn_notch_range; // ignore any FFT bin below this threshold
uint8_t dyn_notch_width_percent;
uint16_t dyn_notch_q;
uint16_t dyn_notch_min_hz;
uint8_t gyro_filter_debug_axis;
+#ifdef USE_DYN_LPF2
+ uint8_t dynlpf2_enable;
+#endif
!🟥^^^^^^^^^^^^^^^^^ skipped ^^^^^^^^^^^^^^^^^
+#if defined(USE_GYRO_IMUF9001)
+ uint16_t imuf_mode;
+ uint16_t imuf_rate;
+ uint16_t imuf_pitch_q;
+ uint16_t imuf_roll_q;
+ uint16_t imuf_yaw_q;
+ uint16_t imuf_w;
+ uint16_t imuf_pitch_lpf_cutoff_hz;
+ uint16_t imuf_roll_lpf_cutoff_hz;
+ uint16_t imuf_yaw_lpf_cutoff_hz;
+ uint8_t imuf_pitch_af;
+ uint8_t imuf_roll_af;
+ uint8_t imuf_yaw_af;
+ uint8_t gyro_align; // gyro alignment
+#endif
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);
void gyroPreInit(void);
@@ -178,5 +207,11 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
gyroDetectionFlags_t getGyroDetectionFlags(void);
#ifdef USE_DYN_LPF
float dynThrottle(float throttle);
void dynLpfGyroUpdate(float throttle);
#endif
+
+#ifdef USE_DMA_SPI_DEVICE
+void gyroDmaSpiFinishRead(void);
+void gyroDmaSpiStartRead(void);
+#endif
+
!🟥^^^^^^^^^^^^^^^^^^^^^ done ^^^^^^^^^^^^^^^^^^
index 8f7ab80f6..487ce29b1
--- a/src/main/sensors/gyro_filter_impl.c
+++ b/src/main/sensors/gyro_filter_impl.c
@@ -41,10 +41,11 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
#ifdef USE_RPM_FILTER
gyroADCf = rpmFilterGyro(axis, gyroADCf);
#endif
+ gyroADCf = dynLpf2Apply(axis, gyroADCf);
// apply static notch filters and software lowpass filters
gyroADCf = gyro.notchFilter1ApplyFn((filter_t *)&gyro.notchFilter1[axis], gyroADCf);
gyroADCf = gyro.notchFilter2ApplyFn((filter_t *)&gyro.notchFilter2[axis], gyroADCf);
gyroADCf = gyro.lowpassFilterApplyFn((filter_t *)&gyro.lowpassFilter[axis], gyroADCf);
!🟥^^^^^^^^^^^^^^^^^^^^ skipped ^^^^^^^^^^^^^^^
index e1387cbc0..8724745ae
--- a/src/main/startup/startup_stm32f30x_md_gcc.S
+++ b/src/main/startup/startup_stm32f30x_md_gcc.S
@@ -75,10 +75,13 @@ Reset_Handler:
ldr r2, [r0, #0] // HJI 11/9/2012
str r0, [r0, #0] // HJI 11/9/2012
cmp r2, r1 // HJI 11/9/2012
beq Reboot_Loader // HJI 11/9/2012
+ bl persistentObjectInit
+
+
/* Copy the data segment initializers from flash to SRAM */
movs r1, #0
b LoopCopyDataInit
CopyDataInit:
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
index c6c2504d9..f822b62ea
--- a/src/main/startup/stm32f7xx_hal_conf.h
+++ b/src/main/startup/stm32f7xx_hal_conf.h
@@ -50,11 +50,11 @@
#define HAL_MODULE_ENABLED
#define HAL_ADC_MODULE_ENABLED
/* #define HAL_CAN_MODULE_ENABLED */
/* #define HAL_CEC_MODULE_ENABLED */
-/* #define HAL_CRC_MODULE_ENABLED */
+#define HAL_CRC_MODULE_ENABLED
/* #define HAL_CRYP_MODULE_ENABLED */
#define HAL_DAC_MODULE_ENABLED
/* #define HAL_DCMI_MODULE_ENABLED */
/* #define HAL_DMA2D_MODULE_ENABLED */
/* #define HAL_ETH_MODULE_ENABLED */
!🟥^^^^^^^^^^^^^^^^^^^^^^^^^ done --- found required during compile phase ^^^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓ skipping ↓↓↓↓↓↓↓↓ system_stm32f30x.c ↓↓↓↓↓↓↓↓
index 1e76980d0..5bbb69397
--- a/src/main/startup/system_stm32f30x.c
+++ b/src/main/startup/system_stm32f30x.c
@@ -100,10 +100,11 @@
*/
#include "platform.h"
#include "stm32f30x.h"
+#include "drivers/persistent.h"
#include "drivers/system.h"
uint32_t hse_value = HSE_VALUE;
/**
@@ -286,10 +287,37 @@ void SystemCoreClockUpdate (void)
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK clock frequency */
SystemCoreClock >>= tmp;
}
+#ifdef USE_OVERCLOCK
+
+const uint32_t ocPllMul[] = {
+ RCC_CFGR_PLLMULL9,
+ RCC_CFGR_PLLMULL10,
+ RCC_CFGR_PLLMULL11,
+ RCC_CFGR_PLLMULL12,
+ RCC_CFGR_PLLMULL13,
+ RCC_CFGR_PLLMULL14,
+ RCC_CFGR_PLLMULL15,
+ RCC_CFGR_PLLMULL16 };
+
+
+void OverclockRebootIfNecessary(uint32_t level)
+{
+ uint32_t overClock = level;
+ bool vcp = level >= 8;
+ if (vcp) overClock -= 7;
+ if ((RCC->CFGR & (0xf << 18)) != ocPllMul[overClock]) {
+ persistentObjectWrite(PERSISTENT_OBJECT_OVERCLOCK_LEVEL, level);
+ __disable_irq();
+ NVIC_SystemReset();
+ }
+}
+
+#endif
+
/**
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings
* @note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function).
@@ -302,10 +330,20 @@ void SetSysClock(void)
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
+ uint32_t overClock = persistentObjectRead(PERSISTENT_OBJECT_OVERCLOCK_LEVEL);
+ bool vcp = overClock > OVERCLOCK_128MHZ;
+ if (vcp) overClock -= OVERCLOCK_128MHZ;
+
+ // Reset overclock so USB device can be attached at next boot
+ if (vcp) {
+ persistentObjectWrite(PERSISTENT_OBJECT_OVERCLOCK_LEVEL,0);
+ }
+
+
/* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/
/* Enable HSE */
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
/* Wait till HSE is ready and if Time out is reached exit */
@@ -325,11 +363,11 @@ void SetSysClock(void)
}
if (HSEStatus == (uint32_t)0x01)
{
/* Enable Prefetch Buffer and set Flash Latency */
- FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1;
+ FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)(FLASH_ACR_LATENCY_1);
/* HCLK = SYSCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 1 */
@@ -343,11 +381,15 @@ void SetSysClock(void)
if (HSE_VALUE == 12000000) {
RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL6);
}
else {
+#ifdef USE_OVERCLOCK
+ RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | ocPllMul[overClock]);
+#else
RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9);
+#endif
}
/* Enable PLL */
RCC->CR |= RCC_CR_PLLON;
@@ -362,10 +404,14 @@ void SetSysClock(void)
/* Wait till PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL)
{
}
+
+ RCC->CFGR2 &= (uint32_t)0xFFFFFFF0;
+
+ SystemCoreClockUpdate();
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
!🟥 ↓↓↓↓↓↓↓↓ skipping ↓↓↓↓↓↓↓↓ system_stm32f30x.h ↓↓↓↓↓↓↓↓
index b14d1a565..a08ddfe7e
--- a/src/main/startup/system_stm32f30x.h
+++ b/src/main/startup/system_stm32f30x.h
@@ -53,11 +53,17 @@ extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Cloc
* @{
*/
extern void SystemInit(void);
extern void SystemCoreClockUpdate(void);
+extern void OverclockRebootIfNecessary(uint32_t level);
+
+#define OVERCLOCK_128MHZ 7
+
+
+
/**
* @}
*/
#ifdef __cplusplus
!🟥^^^^^^^^^^^^^^^^^ seems to be F3 performance edition specific ^^^^^^^^^^^^^^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓ done via git checkout files ↓↓↓↓↓↓↓↓
index 000000000..908717998
--- /dev/null
+++ b/src/main/target/HELIOSPRING/config.c
@@ -0,0 +1,57 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "config/config_eeprom.h"
+#include "drivers/pwm_output.h"
+#include "common/filter.h"
+#include "sensors/acceleration.h"
+#include "sensors/gyro.h"
+#include "sensors/battery.h"
+#include "telemetry/telemetry.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "fc/config.h"
+#include "fc/rc.h"
+#include "fc/rc_controls.h"
+#include "rx/rx.h"
+#include "drivers/pwm_output.h"
+#include "pg/motor.h"
+
+
+void targetConfiguration(void) {
+ telemetryConfigMutable()->halfDuplex = false;
+ voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = VBAT_SCALE;
+ rxConfigMutable()->rcInterpolation = RC_SMOOTHING_MANUAL;
+ rxConfigMutable()->rcInterpolationInterval = 14;
+ rxConfigMutable()->rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT;
+ motorConfigMutable()->dev.motorPwmProtocol = PWM_TYPE_DSHOT300;
+// gyroConfigMutable()->gyro_sync_denom = 1; // 8kHz gyro
+// pidConfigMutable()->pid_process_denom = 1; // 8kHz PID
+ gyroConfigMutable()->gyro_sync_denom = 2; // 16KHZ GYRO
+ pidConfigMutable()->pid_process_denom = 1; // 16KHZ PID
+ systemConfigMutable()->cpu_overclock = 1; //192MHz makes Multishot run a little better because of maths.
+
+// for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) {
+// pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
+// pidProfile->dterm_notch_cutoff = 0;
+// }
+}
+
index 000000000..e4f0434fa
--- /dev/null
+++ b/src/main/target/HELIOSPRING/target.c
@@ -0,0 +1,40 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include <platform.h>
+#include "drivers/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+
+ DEF_TIM(TIM4, CH1, PB6, TIM_USE_ANY, 0, 0), // CAMERA_CONTROL_PIN
+ DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0),
+
+ // Motors
+ DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S1_OUT D1_ST7
+ DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // S2_OUT D1_ST2
+ DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1), // S3_OUT D1_ST6
+ DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S4_OUT D1_ST1
+
+ // LED strip
+ DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // D1_ST0
+};
index 000000000..a9f157c3f
--- /dev/null
+++ b/src/main/target/HELIOSPRING/target.h
@@ -0,0 +1,232 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "HESP"
+#define USBD_PRODUCT_STRING "HELIOSPRING"
+
+//#define MSP_OVER_CLI
+//#define USE_DYN_LPF
+
+#define LED0_PIN PB7
+
+#define USE_BEEPER
+#define BEEPER_PIN PC15
+#define BEEPER_INVERTED
+
+
+#define USE_GYRO
+#define USE_ACC
+#define GYRO_1_SPI_INSTANCE SPI1
+#define GYRO_1_CS_PIN PB1
+#define GYRO_1_ALIGN CW0_DEG
+#define GYRO_1_EXTI_PIN PB0
+
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
+
+#define DEFAULT_ATTITUDE_UPDATE_INTERVAL 1000
+#define DEFAULT_ACC_SAMPLE_INTERVAL 1000
+
+#define USE_FAST_SPI_DRIVER
+#define USE_GYRO_IMUF9001
+#define USE_QUAT_IMUF9001
+#define USE_ACC_IMUF9001
+#define IMUF9001_CS_PIN PB1
+#define IMUF9001_RST_PIN PA4
+#define IMUF9001_SPI_INSTANCE SPI1
+#define USE_EXTI
+
+#define USE_MPU_DATA_READY_SIGNAL
+
+#define ENABLE_DSHOT_DMAR DSHOT_DMAR_ON
+
+#define FLASH_CS_PIN PC14
+#define FLASH_SPI_INSTANCE SPI3
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+
+#define USE_VCP
+
+#define VBUS_SENSING_PIN PC5
+
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_INSTANCE SPI3
+#define MAX7456_SPI_CS_PIN PA15
+#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz
+#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST)
+
+#define USE_UART1
+#define UART1_RX_PIN PA10
+#define UART1_TX_PIN PA9
+
+#define USE_UART2
+#define UART2_RX_PIN PA3
+#define UART2_TX_PIN PA2
+
+#define USE_UART3
+#define UART3_RX_PIN PB11
+#define UART3_TX_PIN PB10
+
+#define USE_UART4
+#define UART4_RX_PIN PC11
+//#define UART4_TX_PIN NONE // defaults to the vbat adc pin
+
+#define USE_UART5
+#define UART5_RX_PIN PD2
+#define UART5_TX_PIN PC12
+
+#define SERIAL_PORT_COUNT 6 //VCP, USART1, USART2, USART3, UART4, USART5
+
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_NSS_PIN PB1
+#define SPI1_SCK_PIN PB3
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+
+#define USE_DMA_SPI_DEVICE SPI1
+
+#define DMA_SPI_NSS_PIN_SRC GPIO_PinSource1
+#define DMA_SPI_NSS_PIN GPIO_Pin_1
+#define DMA_SPI_NSS_PORT GPIOB
+#define DMA_SPI_NSS_AF GPIO_AF_SPI1
+#define DMA_SPI_SCK_PIN_SRC GPIO_PinSource3
+#define DMA_SPI_SCK_PIN GPIO_Pin_3
+#define DMA_SPI_SCK_PORT GPIOB
+#define DMA_SPI_SCK_AF GPIO_AF_SPI1
+#define DMA_SPI_MISO_PIN_SRC GPIO_PinSource6
+#define DMA_SPI_MISO_PIN GPIO_Pin_6
+#define DMA_SPI_MISO_PORT GPIOA
+#define DMA_SPI_MISO_AF GPIO_AF_SPI1
+#define DMA_SPI_MOSI_PIN_SRC GPIO_PinSource7
+#define DMA_SPI_MOSI_PIN GPIO_Pin_7
+#define DMA_SPI_MOSI_PORT GPIOA
+#define DMA_SPI_MOSI_AF GPIO_AF_SPI1
+#define DMA_SPI_RST_MSK RCC_APB2RSTR_SPI1RST
+#define DMA_SPI_PER RCC->APB2RSTR
+
+#define DMA_SPI_SPI SPI1
+#define DMA_SPI_BAUDRATE SPI_BaudRatePrescaler_4
+#define DMA_SPI_CPOL SPI_CPOL_Low
+#define DMA_SPI_CPHA SPI_CPHA_1Edge
+
+#define DMA_SPI_DMA DMA2
+#define DMA_SPI_TX_DMA_STREAM DMA2_Stream3
+#define DMA_SPI_RX_DMA_STREAM DMA2_Stream2
+#define DMA_SPI_TX_DMA_CHANNEL DMA_Channel_3
+#define DMA_SPI_RX_DMA_CHANNEL DMA_Channel_3
+#define DMA_SPI_TX_DMA_HANDLER DMA2_Stream3_IRQHandler
+#define DMA_SPI_RX_DMA_HANDLER DMA2_Stream2_IRQHandler
+#define DMA_SPI_TX_DMA_IRQn DMA2_Stream3_IRQn
+#define DMA_SPI_RX_DMA_IRQn DMA2_Stream2_IRQn
+
+#define DMA_SPI_DMA_RX_PRE_PRI 0x0E
+#define DMA_SPI_DMA_RX_SUB_PRI 0x0E
+
+#define DMA_SPI_TX_DMA_FLAG_ALL DMA_FLAG_FEIF3 | DMA_FLAG_DMEIF3 | DMA_FLAG_TEIF3 | DMA_FLAG_HTIF3 | DMA_FLAG_TCIF3
+#define DMA_SPI_TX_DMA_FLAG_GL DMA_FLAG_TCIF3
+#define DMA_SPI_TX_DMA_FLAG_TC DMA_FLAG_TCIF3
+#define DMA_SPI_RX_DMA_FLAG_ALL DMA_FLAG_FEIF2 | DMA_FLAG_DMEIF2 | DMA_FLAG_TEIF2 | DMA_FLAG_HTIF2 | DMA_FLAG_TCIF2
+#define DMA_SPI_RX_DMA_FLAG_TC DMA_FLAG_TCIF2
+#define DMA_SPI_RX_DMA_FLAG_GL DMA_FLAG_TCIF2
+
+
+#define USE_SPI_DEVICE_2
+#define SPI2_NSS_PIN PB12
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PC2
+#define SPI2_MOSI_PIN PC3
+
+#define USE_SPI_DEVICE_3
+#define SPI3_NSS_PIN PA15
+#define SPI3_SCK_PIN PC10
+#define SPI3_MISO_PIN PB4
+#define SPI3_MOSI_PIN PB5
+
+#define USE_I2C
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL NONE // PB10, UART3_TX
+#define I2C2_SDA NONE // PB11, UART3_RX
+#define I2C_DEVICE (I2CDEV_2)
+
+#define USE_TARGET_CONFIG
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_SBUS
+#define SERIALRX_UART SERIAL_PORT_USART2
+#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_AIRMODE)
+
+#define USE_GPS
+#define USE_MAG
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_PIN NONE // (HARDARE=0,PPM)
+//#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2))
+
+#define USABLE_TIMER_CHANNEL_COUNT 7
+#define USED_TIMERS ( TIM_N(1) | TIM_N(8) | TIM_N(4) | TIM_N(12) )
+
+#define USE_ACC_IMUF9001
+#define IMUF_RST_PIN GPIO_Pin_4
+#define IMUF_RST_PORT GPIOA
+#define IMUF_EXTI_PIN GPIO_Pin_0
+#define IMUF_EXTI_PORT GPIOB
+
+#define USE_ADC
+#define ADC_INSTANCE ADC1
+#define ADC1_DMA_OPT 0 // DMA 2 Stream 0 Channel 0
+
+#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
+#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC
+#define CURRENT_METER_ADC_PIN PA1
+#define VBAT_ADC_PIN PA0
+#define CURRENT_METER_SCALE_DEFAULT 250
+#define VBAT_SCALE 109
+
+#define VBAT_SCALE 109
+
+#define CAMERA_CONTROL_PIN PB6 // define dedicated camera_osd_control pin
+
+#define IMUF_DEFAULT_PITCH_Q 3000
+#define IMUF_DEFAULT_ROLL_Q 3000
+#define IMUF_DEFAULT_YAW_Q 3000
+#define IMUF_DEFAULT_W 32
+#define IMUF_DEFAULT_LPF_HZ 120.0f
+
+#define USE_BUTTERED_PIDS true
+
+#define GYRO_1_CS_PIN PB1
+#define GYRO_1_RST_PIN PA4
+#define GYRO_1_SPI_INSTANCE SPI1
+#define USE_SPI_GYRO
+#define USE_EXTI
+#define USE_GYRO_EXTI
+#define GYRO_1_EXTI_PIN PB0
+#define GYRO_1_ALIGN CW0_DEG
+//#define ACC_1_ALIGN CW0_DEG
+#define TARGET_MANUFACTURER_IDENTIFIER "HESP"
index 000000000..ced109e6f
--- /dev/null
+++ b/src/main/target/HELIOSPRING/target.mk
@@ -0,0 +1,8 @@
+
+F405_TARGETS += $(TARGET)
+FEATURES += VCP ONBOARDFLASH
+
+TARGET_SRC = stm32f4xx_crc.c \
+ drivers/dma_spi.c \
+ drivers/accgyro/accgyro_imuf9001.c \
+ drivers/max7456.c
\ No newline at end of file
index 000000000..d4be21f6d
--- /dev/null
+++ b/src/main/target/STRIXF10/config.c
@@ -0,0 +1,57 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "config/config_eeprom.h"
+#include "drivers/pwm_output.h"
+#include "common/filter.h"
+#include "sensors/acceleration.h"
+#include "sensors/gyro.h"
+#include "sensors/battery.h"
+#include "telemetry/telemetry.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "fc/config.h"
+#include "fc/rc.h"
+#include "fc/rc_controls.h"
+#include "rx/rx.h"
+
+#include "pg/motor.h"
+
+#include "config_helper.h"
+
+
+void targetConfiguration(void) {
+ telemetryConfigMutable()->halfDuplex = false;
+ voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = VBAT_SCALE;
+ rxConfigMutable()->rcInterpolation = RC_SMOOTHING_MANUAL;
+ rxConfigMutable()->rcInterpolationInterval = 14;
+ rxConfigMutable()->rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT;
+ motorConfigMutable()->dev.motorPwmProtocol = PWM_TYPE_DSHOT600;
+ gyroConfigMutable()->gyro_sync_denom = 2; // 16KHZ GYRO
+ pidConfigMutable()->pid_process_denom = 1; // 16KHZ PID
+ systemConfigMutable()->cpu_overclock = 0; //192MHz makes Multishot run a little better because of maths.
+
+// for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) {
+// pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
+// pidProfile->dterm_notch_cutoff = 0;
+// }
+}
+
index 000000000..e4f0434fa
--- /dev/null
+++ b/src/main/target/STRIXF10/target.c
@@ -0,0 +1,40 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include <platform.h>
+#include "drivers/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+
+ DEF_TIM(TIM4, CH1, PB6, TIM_USE_ANY, 0, 0), // CAMERA_CONTROL_PIN
+ DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0),
+
+ // Motors
+ DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S1_OUT D1_ST7
+ DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // S2_OUT D1_ST2
+ DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1), // S3_OUT D1_ST6
+ DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S4_OUT D1_ST1
+
+ // LED strip
+ DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // D1_ST0
+};
index 000000000..ea68f9119
--- /dev/null
+++ b/src/main/target/STRIXF10/target.h
@@ -0,0 +1,228 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "SX10"
+#define USBD_PRODUCT_STRING "STRIX Binary F10 by Helio"
+
+#define STM32F7
+//#undef USE_DSHOT_BITBANG
+
+#define LED0_PIN PB7
+
+#define BEEPER PC15
+#define BEEPER_INVERTED
+
+#define USE_GYRO
+#define USE_ACC
+#define GYRO_1_SPI_INSTANCE SPI1
+#define GYRO_1_CS_PIN PB1
+#define GYRO_1_ALIGN CW0_DEG
+#define GYRO_1_EXTI_PIN PB0
+
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
+
+#define DEFAULT_ATTITUDE_UPDATE_INTERVAL 1000
+#define DEFAULT_ACC_SAMPLE_INTERVAL 1000
+
+#define USE_FAST_SPI_DRIVER
+#define USE_GYRO_IMUF9001
+#define USE_QUAT_IMUF9001
+#define USE_ACC_IMUF9001
+#define IMUF9001_CS_PIN PB1
+#define IMUF9001_RST_PIN PA4
+#define IMUF9001_SPI_INSTANCE SPI1
+#define USE_EXTI
+
+#define USE_MPU_DATA_READY_SIGNAL
+
+#define IMUF_RST_PIN GPIO_PIN_4
+#define IMUF_RST_PORT GPIOA
+#define IMUF_EXTI_PIN GPIO_PIN_0
+#define IMUF_EXTI_PORT GPIOB
+
+#define ENABLE_DSHOT_DMAR DSHOT_DMAR_ON
+
+#define FLASH_CS_PIN PC14
+#define M25P16_CS_PIN PC14
+#define FLASH_SPI_INSTANCE SPI3
+#define M25P16_SPI_INSTANCE SPI3
+#define USE_FLASH
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+
+#define USE_VCP
+
+#define VBUS_SENSING_PIN PC5
+
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_INSTANCE SPI3
+#define MAX7456_SPI_CS_PIN PA15
+#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz
+#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST)
+
+#define USE_UART1
+#define UART1_RX_PIN PA10
+#define UART1_TX_PIN PA9
+
+#define USE_UART2
+#define UART2_RX_PIN PA3
+#define UART2_TX_PIN PA2
+
+#define USE_UART3
+#define UART3_RX_PIN PB11
+#define UART3_TX_PIN PB10
+
+#define USE_UART4
+#define UART4_RX_PIN PC11
+#define UART4_TX_PIN NONE
+
+#define USE_UART5
+#define UART5_RX_PIN PD2
+#define UART5_TX_PIN PC12
+
+#define SERIAL_PORT_COUNT 6 //VCP, USART1, USART2, USART3, UART4, USART5
+
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_NSS_PIN PB1
+#define SPI1_SCK_PIN PB3
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+
+#define USE_HAL_F7_CRC
+
+#define USE_DMA_SPI_DEVICE SPI1
+
+#define DMA_SPI_NSS_PIN GPIO_PIN_1
+#define DMA_SPI_NSS_PORT GPIOB
+#define DMA_SPI_SCK_PIN GPIO_PIN_3
+#define DMA_SPI_SCK_PORT GPIOB
+#define DMA_SPI_SCK_AF GPIO_AF5_SPI1
+#define DMA_SPI_MISO_PIN GPIO_PIN_6
+#define DMA_SPI_MISO_PORT GPIOA
+#define DMA_SPI_MISO_AF GPIO_AF5_SPI1
+#define DMA_SPI_MOSI_PIN GPIO_PIN_7
+#define DMA_SPI_MOSI_PORT GPIOA
+#define DMA_SPI_MOSI_AF GPIO_AF5_SPI1
+
+#define DMA_SPI_SPI SPI1
+#define DMA_SPI_CLOCK_INIT_FUNC __HAL_RCC_SPI1_CLK_ENABLE()
+
+#define DMA_SPI_DMA DMA2
+#define DMA_SPI_TX_DMA_STREAM DMA2_Stream3
+#define DMA_SPI_RX_DMA_STREAM DMA2_Stream2
+#define DMA_SPI_TX_DMA_CHANNEL DMA_CHANNEL_3
+#define DMA_SPI_RX_DMA_CHANNEL DMA_CHANNEL_3
+#define DMA_SPI_TX_DMA_HANDLER DMA2_Stream3_IRQHandler
+#define DMA_SPI_RX_DMA_HANDLER DMA2_Stream2_IRQHandler
+#define DMA_SPI_TX_DMA_IRQn DMA2_Stream3_IRQn
+#define DMA_SPI_RX_DMA_IRQn DMA2_Stream2_IRQn
+
+#define DMA_SPI_DMA_RX_PRE_PRI 0x0E
+#define DMA_SPI_DMA_RX_SUB_PRI 0x0E
+#define DMA_SPI_DMA_TX_PRE_PRI 0x0D
+#define DMA_SPI_DMA_TX_SUB_PRI 0x0D
+
+#define DMA_SPI_BAUDRATE SPI_BAUDRATEPRESCALER_4
+
+#define USE_SPI_DEVICE_2
+#define SPI2_NSS_PIN PB12
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PC2
+#define SPI2_MOSI_PIN PC3
+
+#define USE_SPI_DEVICE_3
+#define SPI3_NSS_PIN PA15
+#define SPI3_SCK_PIN PC10
+#define SPI3_MISO_PIN PB4
+#define SPI3_MOSI_PIN PB5
+
+#define USE_I2C
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL NONE // PB10, UART3_TX
+#define I2C2_SDA NONE // PB11, UART3_RX
+#define I2C_DEVICE (I2CDEV_2)
+
+#define USE_TARGET_CONFIG
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_SBUS
+#define SERIALRX_UART SERIAL_PORT_USART2
+#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_AIRMODE)
+
+#define USE_GPS
+#define USE_MAG
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_PIN NONE // (HARDARE=0,PPM)
+//#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2))
+
+#define USABLE_TIMER_CHANNEL_COUNT 7
+#define USED_TIMERS ( TIM_N(1) | TIM_N(8) | TIM_N(4) | TIM_N(12) )
+
+#define IMUF_BIT_I2C_IF_DIS (1 << 4)
+
+
+#define USE_ADC
+#define ADC_INSTANCE ADC1
+#define ADC1_DMA_OPT 0 // DMA 2 Stream 0 Channel 0
+
+#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
+#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC
+#define CURRENT_METER_ADC_PIN PA1
+#define VBAT_ADC_PIN PA0
+#define CURRENT_METER_SCALE_DEFAULT 250
+#define VBAT_SCALE 109
+
+#define CAMERA_CONTROL_PIN PB6 // define dedicated camera_osd_control pin
+
+#define IMUF_DEFAULT_PITCH_Q 3000
+#define IMUF_DEFAULT_ROLL_Q 3000
+#define IMUF_DEFAULT_YAW_Q 3000
+#define IMUF_DEFAULT_W 32
+#define IMUF_DEFAULT_LPF_HZ 120.0f
+
+#define USE_BUTTERED_PIDS true
+
+#define DEFAULT_PIDS_ROLL {45, 50, 30, 0}
+#define DEFAULT_PIDS_PITCH {45, 50, 30, 0}
+#define DEFAULT_PIDS_YAW {45, 50, 30, 0}
+
+
+#define GYRO_1_CS_PIN PB1
+#define GYRO_1_RST_PIN PA4
+#define GYRO_1_SPI_INSTANCE SPI1
+#define USE_SPI_GYRO
+#define USE_EXTI
+#define USE_GYRO_EXTI
+#define GYRO_1_EXTI_PIN PB0
+#define GYRO_1_ALIGN CW0_DEG
+//#define ACC_1_ALIGN CW0_DEG
+
+#define TARGET_MANUFACTURER_IDENTIFIER "SF10"
index 000000000..c0dc166ec
--- /dev/null
+++ b/src/main/target/STRIXF10/target.mk
@@ -0,0 +1,6 @@
+F7X2RE_TARGETS += $(TARGET)
+FEATURES += VCP ONBOARDFLASH
+
+TARGET_SRC = drivers/dma_spi_hal.c \
+ drivers/accgyro/accgyro_imuf9001.c \
+ drivers/max7456.c
\ No newline at end of file
index b3f910622..225111ba7
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -26,10 +26,16 @@
#if defined(USE_VTX_RTC6705_SOFTSPI)
#define USE_VTX_RTC6705
#endif
+#ifdef STM32F3
+#undef USE_ESCSERIAL
+#undef USE_BARO
+#undef USE_RANGEFINDER
+#endif
+
#ifndef USE_DSHOT
#undef USE_ESC_SENSOR
#endif
#ifndef USE_ESC_SENSOR
!🟥 ^^^^^^^^^^^ skipped ^^^^^^^^^^^^^^
!🟥 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ skipped cpommon_pre ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
index 0a898ab89..7fed31dcb
--- a/src/main/target/common_pre.h
+++ b/src/main/target/common_pre.h
@@ -18,10 +18,12 @@
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
+#define USE_DYN_LPF2
+
#define USE_PARAMETER_GROUPS
// type conversion warnings.
// -Wconversion can be turned on to enable the process of eliminating these warnings
//#pragma GCC diagnostic warning "-Wconversion"
#pragma GCC diagnostic ignored "-Wsign-conversion"
@@ -42,10 +44,52 @@
#ifdef STM32F3
#define MINIMAL_CLI
#define USE_DSHOT
#define USE_GYRO_DATA_ANALYSE
+#define USE_DSHOT_TELEMETRY
+#define USE_DSHOT_TELEMETRY_STATS
+#define USE_RPM_FILTER
+//#define USE_DYN_IDLE
+//#define USE_ITERM_RELAX
+#define USE_RC_SMOOTHING_FILTER
+#define USE_TPA_MODE
+#define USE_CCM_CODE
+#define USE_OVERCLOCK
+#define USE_INTERPOLATED_SP
+//#define USE_ABSOLUTE_CONTROL
+#define USE_THROTTLE_BOOST
+#define USE_QUAD_MIXER_ONLY
+#define USE_VTX_TABLE
+
+#if !defined(USE_FRSKY) && !defined(USE_FLYSKY) && !defined(USE_SPEKTRUM) && !defined(USE_CRSF)
+#define USE_FRSKY
+#endif
+
+#if defined(USE_FRSKY)
+#define USE_SERIALRX_SBUS // Frsky and Futaba receivers
+#define USE_TELEMETRY_FRSKY_HUB
+#define USE_TELEMETRY_SMARTPORT
+#endif
+
+#ifdef USE_SPEKTRUM
+#define USE_SERIALRX_SPEKTRUM // SRXL, DSM2 and DSMX protocol
+#define USE_SERIALRX_SUMD // Graupner Hott protocol
+#define USE_TELEMETRY_SRXL
+#endif
+
+#ifdef USE_FLYSKY
+#define USE_SERIALRX_IBUS
+#define USE_TELEMETRY_IBUS
+#define USE_TELEMETRY_IBUS_EXTENDED
+#endif
+
+#ifdef USE_CRSF
+#define USE_SERIALRX_CRSF // Team Black Sheep Crossfire protocol
+#define USE_TELEMETRY_CRSF
+#endif
+
#endif
#ifdef STM32F4
#define USE_SRAM2
#if defined(STM32F40_41xxx)
@@ -54,11 +98,12 @@
#define USE_DSHOT
#define USE_DSHOT_BITBANG
#define USE_DSHOT_TELEMETRY
#define USE_DSHOT_TELEMETRY_STATS
#define USE_RPM_FILTER
-#define USE_DYN_IDLE
+#define USE_THROTTLE_BOOST
+//#define USE_DYN_IDLE
#define I2C3_OVERCLOCK true
#define USE_GYRO_DATA_ANALYSE
#define USE_ADC
#define USE_ADC_INTERNAL
#define USE_USB_CDC_HID
@@ -85,11 +130,11 @@
#define USE_DSHOT
#define USE_DSHOT_BITBANG
#define USE_DSHOT_TELEMETRY
#define USE_DSHOT_TELEMETRY_STATS
#define USE_RPM_FILTER
-#define USE_DYN_IDLE
+//#define USE_DYN_IDLE
#define I2C3_OVERCLOCK true
#define I2C4_OVERCLOCK true
#define USE_GYRO_DATA_ANALYSE
#define USE_OVERCLOCK
#define USE_ADC_INTERNAL
@@ -110,11 +155,11 @@
#define USE_FAST_RAM
#define USE_DSHOT
#define USE_DSHOT_TELEMETRY
#define USE_DSHOT_TELEMETRY_STATS
#define USE_RPM_FILTER
-#define USE_DYN_IDLE
+//#define USE_DYN_IDLE
#define USE_GYRO_DATA_ANALYSE
#define USE_ADC_INTERNAL
#define USE_USB_CDC_HID
#define USE_DMA_SPEC
#define USE_TIMER_MGMT
@@ -133,18 +178,25 @@
#define DEFAULT_AUX_CHANNEL_COUNT MAX_AUX_CHANNEL_COUNT
#else
#define DEFAULT_AUX_CHANNEL_COUNT 6
#endif
+
#ifdef USE_ITCM_RAM
#define FAST_CODE __attribute__((section(".tcm_code")))
#define FAST_CODE_NOINLINE NOINLINE
#else
#define FAST_CODE
#define FAST_CODE_NOINLINE
#endif // USE_ITCM_RAM
+#ifdef USE_CCM_CODE
+#define CCM_CODE __attribute__((section(".ccm_code")))
+#else
+#define CCM_CODE
+#endif
+
#ifdef USE_FAST_RAM
#define FAST_RAM_ZERO_INIT __attribute__ ((section(".fastram_bss"), aligned(4)))
#define FAST_RAM __attribute__ ((section(".fastram_data"), aligned(4)))
#else
#define FAST_RAM_ZERO_INIT
@@ -168,131 +220,133 @@
#else
#define DMA_RAM
#define DMA_RW_AXI
#endif
-#define USE_BRUSHED_ESC_AUTODETECT // Detect if brushed motors are connected and set defaults appropriately to avoid motors spinning on boot
#define USE_MOTOR
#define USE_PWM_OUTPUT
#define USE_DMA
#define USE_TIMER
-#define USE_CLI
+#ifndef STM32F3
+#define USE_BRUSHED_ESC_AUTODETECT // Detect if brushed motors are connected and set defaults appropriately to avoid motors spinning on boot
#define USE_SERIAL_PASSTHROUGH
-#define USE_TASK_STATISTICS
#define USE_GYRO_REGISTER_DUMP // Adds gyroregisters command to cli to dump configured register values
-#define USE_IMU_CALC
#define USE_PPM
-#define USE_SERIAL_RX
+
#define USE_SERIALRX_CRSF // Team Black Sheep Crossfire protocol
#define USE_SERIALRX_IBUS // FlySky and Turnigy receivers
#define USE_SERIALRX_SBUS // Frsky and Futaba receivers
#define USE_SERIALRX_SPEKTRUM // SRXL, DSM2 and DSMX protocol
#define USE_SERIALRX_SUMD // Graupner Hott protocol
+#endif
+
+#define USE_CLI
+#define USE_TASK_STATISTICS
+#define USE_IMU_CALC
+#define USE_SERIAL_RX
+
#if (FLASH_SIZE > 128)
#define PID_PROFILE_COUNT 3
#define CONTROL_RATE_PROFILE_COUNT 6
#else
#define PID_PROFILE_COUNT 2
#define CONTROL_RATE_PROFILE_COUNT 3
#endif
#if (FLASH_SIZE > 64)
-#define USE_ACRO_TRAINER
+#define USE_INTERPOLATED_SP
+#define USE_ABSOLUTE_CONTROL
+#define USE_THROTTLE_BOOST
+
#define USE_BLACKBOX
#define USE_CLI_BATCH
#define USE_RESOURCE_MGMT
#define USE_RUNAWAY_TAKEOFF // Runaway Takeoff Prevention (anti-taz)
-#define USE_SERVOS
#define USE_TELEMETRY
-#define USE_TELEMETRY_FRSKY_HUB
-#define USE_TELEMETRY_SMARTPORT
#endif
#if (FLASH_SIZE > 128)
#define USE_GYRO_OVERFLOW_CHECK
#define USE_YAW_SPIN_RECOVERY
-#define USE_DSHOT_DMAR
-#define USE_SERIALRX_FPORT // FrSky FPort
+//#define USE_DSHOT_DMAR
#define USE_TELEMETRY_CRSF
-#define USE_TELEMETRY_SRXL
+//#define USE_TELEMETRY_SRXL
+#ifdef USE_SERIALRX_SPEKTRUM
+#define USE_SPEKTRUM_BIND
+#define USE_SPEKTRUM_BIND_PLUG
+#define USE_SPEKTRUM_REAL_RSSI
+#define USE_SPEKTRUM_FAKE_RSSI
+#define USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
+#define USE_SPEKTRUM_VTX_CONTROL
+#define USE_SPEKTRUM_VTX_TELEMETRY
+#define USE_SPEKTRUM_CMS_TELEMETRY
+#define USE_PIN_PULL_UP_DOWN
+#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 12))
#define USE_CMS
#define USE_MSP_DISPLAYPORT
#define USE_MSP_OVER_TELEMETRY
#define USE_LED_STRIP
+#define USE_GYRO_LPF2
+//#define USE_DYN_LPF
+//#define USE_D_MIN
+#define USE_THROTTLE_BOOST
+//#define USE_ITERM_RELAX
+#define USE_RC_SMOOTHING_FILTER
+#define USE_TPA_MODE
+
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 11))
#define USE_VTX_COMMON
#define USE_VTX_CONTROL
#define USE_VTX_SMARTAUDIO
#define USE_VTX_TRAMP
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 10))
-#define USE_VIRTUAL_CURRENT_METER
#define USE_CAMERA_CONTROL
-#define USE_ESC_SENSOR
#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
#define USE_RCDEVICE
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 9))
-#define USE_GYRO_LPF2
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 8))
-#define USE_LAUNCH_CONTROL
-#define USE_DYN_LPF
-#define USE_D_MIN
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 7))
-#define USE_THROTTLE_BOOST
#define USE_INTEGRATED_YAW_CONTROL
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 6))
-#define USE_ITERM_RELAX
-#define USE_RC_SMOOTHING_FILTER
#define USE_THRUST_LINEARIZATION
-#define USE_TPA_MODE
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 5))
+#define USE_ESC_SENSOR
#define USE_PWM
+#define USE_VIRTUAL_CURRENT_METER
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 4))
#define USE_HUFFMAN
#define USE_PINIO
#define USE_PINIOBOX
+#define USE_LAUNCH_CONTROL
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 3))
-#ifdef USE_SERIALRX_SPEKTRUM
-#define USE_SPEKTRUM_BIND
-#define USE_SPEKTRUM_BIND_PLUG
-#define USE_SPEKTRUM_REAL_RSSI
-#define USE_SPEKTRUM_FAKE_RSSI
-#define USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
-#define USE_SPEKTRUM_VTX_CONTROL
-#define USE_SPEKTRUM_VTX_TELEMETRY
-#define USE_SPEKTRUM_CMS_TELEMETRY
-#define USE_PIN_PULL_UP_DOWN
-#endif
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 2))
-#define USE_TELEMETRY_HOTT
-#define USE_TELEMETRY_LTM
-#define USE_SERIALRX_SUMH // Graupner legacy protocol
-#define USE_SERIALRX_XBUS // JR
+#define USE_SERIALRX_FPORT // FrSky FPort
#endif
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 1))
#define USE_BOARD_INFO
#define USE_EXTENDED_CMS_MENUS
@@ -305,12 +359,23 @@
#endif
#endif // FLASH_SIZE > 128
#if (FLASH_SIZE > 256)
+#define USE_SERVOS
+#define USE_TELEMETRY_HOTT
+#define USE_TELEMETRY_LTM
+#define USE_SERIALRX_SUMH // Graupner legacy protocol
+#define USE_SERIALRX_XBUS // JR
+
+#define USE_TELEMETRY_FRSKY_HUB
+#define USE_TELEMETRY_SMARTPORT
+
+#define USE_ACRO_TRAINER
#define USE_AIRMODE_LPF
#define USE_DASHBOARD
+#define USE_DSHOT_DMAR
#define USE_GPS
#define USE_GPS_NMEA
#define USE_GPS_UBLOX
#define USE_GPS_RESCUE
#define USE_GYRO_DLPF_EXPERIMENTAL
!🟥 ^^^^^^^^^^ skipped common_pre ^^^^^^^^^^^^
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment