Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

Merge branch 'master' into RP2350

Browse source 
# Conflicts:
#	lib/main/tinyUSB/hw/bsp/espressif/components/led_strip/LICENSE
#	lib/main/tinyUSB/hw/bsp/samd11/boards/cynthion_d11/board.h
#	lib/main/tinyUSB/hw/mcu/dialog/da1469x/SDK_10.0.8.105/sdk/bsp/include/cmsis_compiler.h
#	lib/main/tinyUSB/hw/mcu/dialog/da1469x/SDK_10.0.8.105/sdk/bsp/include/cmsis_gcc.h
#	lib/main/tinyUSB/hw/mcu/dialog/da1469x/SDK_10.0.8.105/sdk/bsp/include/cmsis_version.h
#	lib/main/tinyUSB/hw/mcu/dialog/da1469x/SDK_10.0.8.105/sdk/bsp/include/core_cm0.h
#	lib/main/tinyUSB/hw/mcu/dialog/da1469x/SDK_10.0.8.105/sdk/bsp/include/core_cm33.h
#	lib/main/tinyUSB/hw/mcu/dialog/da1469x/SDK_10.0.8.105/sdk/bsp/include/mpu_armv8.h
#	lib/main/tinyUSB/hw/mcu/dialog/da1469x/SDK_10.0.8.105/sdk/bsp/include/system_ARMCM0.h
#	lib/main/tinyUSB/hw/mcu/dialog/da1469x/SDK_10.0.8.105/sdk/bsp/include/system_DA1469x.h
#	lib/main/tinyUSB/lib/SEGGER_RTT/Config/SEGGER_RTT_Conf.h
#	lib/main/tinyUSB/lib/SEGGER_RTT/RTT/SEGGER_RTT.c
#	lib/main/tinyUSB/lib/SEGGER_RTT/RTT/SEGGER_RTT.h
#	src/platform/PICO/pico/platform.h
#	src/platform/PICO/pico/version.h
#	src/platform/PICO/usb/usb_cdc.c
#	src/platform/PICO/usb/usb_descriptors.c
This commit is contained in:
blckmn 2025-05-29 16:15:35 +10:00
commit d87b99c7c8
48 changed files with 1746 additions and 10869 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/config

@ -1 +1 @@
Subproject commit 17892ac1bd9bc74f77870c14a2da386a1c614dd3
Subproject commit 4136fa21f2902321caad438b73505caf0c64dda3

2
src/main/cli/cli.c
View file

@ -6589,7 +6589,7 @@ const clicmd_t cmdTable[] = {
#endif
CLI_COMMAND_DEF("flash_info", "show flash chip info", NULL, cliFlashInfo),
#if defined(USE_FLASH_TOOLS) && defined(USE_FLASHFS)
CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead),
CLI_COMMAND_DEF("flash_read", NULL, "<address> <length>", cliFlashRead),
CLI_COMMAND_DEF("flash_scan", "scan flash device for errors", NULL, cliFlashVerify),
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
#endif

11
src/main/cli/settings.c
View file

@ -161,6 +161,9 @@ const char * const lookupTableAccHardware[] = {
"LSM6DSO",
"LSM6DSV16X",
"IIM42653",
"ICM45605",
"ICM45686",
"ICM40609D",
"VIRTUAL"
};
@ -185,6 +188,9 @@ const char * const lookupTableGyroHardware[] = {
"LSM6DSO",
"LSM6DSV16X",
"IIM42653",
"ICM45605",
"ICM45686",
"ICM40609D",
"VIRTUAL"
};
@ -202,7 +208,7 @@ const char * const lookupTableMagHardware[] = {
#endif
#if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)
const char * const lookupTableRangefinderHardware[] = {
"NONE", "HCSR04", "TFMINI", "TF02", "MTF01", "MTF02", "MTF01P", "MTF02P"
"NONE", "HCSR04", "TFMINI", "TF02", "MTF01", "MTF02", "MTF01P", "MTF02P", "TFNOVA"
};
#endif
#if defined(USE_SENSOR_NAMES) || defined(USE_OPTICALFLOW)
@ -1671,6 +1677,9 @@ const clivalue_t valueTable[] = {
#ifdef USE_CRAFTNAME_MSGS
{ "osd_craftname_msgs", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, osd_craftname_msgs) },
#endif //USE_CRAFTNAME_MSGS
#ifdef USE_RANGEFINDER
{ "osd_lidar_dist_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_LIDAR_DIST]) },
#endif //USE_RANGEFINDER
#endif // end of #ifdef USE_OSD
// PG_SYSTEM_CONFIG

3
src/main/cms/cms_menu_osd.c
View file

@ -175,6 +175,9 @@ const OSD_Entry menuOsdActiveElemsEntries[] =
{"CAMERA FRAME", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_CAMERA_FRAME]},
{"TOTAL FLIGHTS", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_TOTAL_FLIGHTS]},
{"AUX VALUE", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_AUX_VALUE]},
#ifdef USE_RANGEFINDER
{"LIDAR DIST", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_LIDAR_DIST]},
#endif
{"BACK", OME_Back, NULL, NULL},
{NULL, OME_END, NULL, NULL}
};

1
src/main/common/time.h
View file

@ -46,6 +46,7 @@ typedef uint32_t timeUs_t;
#define SECONDS_PER_MINUTE 60.0f
static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }
static inline timeDelta_t cmpTimeMs(timeMs_t a, timeMs_t b) { return (timeDelta_t)(a - b); }
static inline int32_t cmpTimeCycles(uint32_t a, uint32_t b) { return (int32_t)(a - b); }
#define FORMATTED_DATE_TIME_BUFSIZE 30

3
src/main/drivers/accgyro/accgyro.h
View file

@ -62,6 +62,9 @@ typedef enum {
GYRO_LSM6DSO,
GYRO_LSM6DSV16X,
GYRO_IIM42653,
GYRO_ICM45605,
GYRO_ICM45686,
GYRO_ICM40609D,
GYRO_VIRTUAL
} gyroHardware_e;

8
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -50,6 +50,7 @@
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_icm456xx.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -57,6 +58,7 @@
#include "drivers/accgyro/accgyro_spi_l3gd20.h"
#include "drivers/accgyro/accgyro_spi_lsm6dsv16x.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_spi_icm40609.h"
#include "pg/pg.h"
#include "pg/gyrodev.h"
@ -371,8 +373,14 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_GYRO_SPI_ICM20649
icm20649SpiDetect,
#endif
#if defined(USE_ACCGYRO_ICM45686) || defined(USE_ACCGYRO_ICM45605)
icm456xxSpiDetect,
#endif
#ifdef USE_GYRO_L3GD20
l3gd20Detect,
#endif
#ifdef USE_ACCGYRO_ICM40609D
icm40609SpiDetect,
#endif
NULL // Avoid an empty array
};

8
src/main/drivers/accgyro/accgyro_mpu.h
View file

@ -45,8 +45,11 @@
#define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM42605_WHO_AM_I_CONST (0x42)
#define ICM42688P_WHO_AM_I_CONST (0x47)
#define ICM45686_WHO_AM_I_CONST (0xE9)
#define ICM45605_WHO_AM_I_CONST (0xE5)
#define IIM42653_WHO_AM_I_CONST (0x56)
#define LSM6DSV16X_WHO_AM_I_CONST (0x70)
#define ICM40609_WHO_AM_I_CONST (0x3B)
// RA = Register Address
@ -209,7 +212,10 @@ typedef enum {
BMI_270_SPI,
LSM6DSO_SPI,
L3GD20_SPI,
LSM6DSV16X_SPI
LSM6DSV16X_SPI,
ICM_45605_SPI,
ICM_45686_SPI,
ICM_40609_SPI
} mpuSensor_e;
typedef enum {

764
src/main/drivers/accgyro/accgyro_spi_icm40609.c Normal file
View file

@ -0,0 +1,764 @@
/*
* This file is part of Betaflight.
*
* Betaflight is free software. You can redistribute this software
* and/or modify this software under the terms of the GNU General
* Public License as published by the Free Software Foundation,
* either version 3 of the License, or (at your option) any later
* version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "platform.h"
#if defined(USE_ACCGYRO_ICM40609D)
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_spi_icm40609.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/pwm_output.h"
#include "drivers/sensor.h"
#include "drivers/time.h"
#include "fc/runtime_config.h"
#include "sensors/gyro.h"
#include "pg/gyrodev.h"
// Datasheet: https://invensense.tdk.com/wp-content/uploads/2022/07/DS-000330-ICM-40609-D-v1.2.pdf
#define ICM40609_WHO_AM_I_REG 0x75
#define ICM40609_REG_BANK_SEL 0x76
#define ICM40609_USER_BANK_0 0x00
#define ICM40609_USER_BANK_1 0x01
#define ICM40609_USER_BANK_2 0x02
#define ICM40609_USER_BANK_3 0x03
#define ICM40609_USER_BANK_4 0x04
// Registers in BANK 0
#define ICM40609_REG_INT_CONFIG 0x14
#define ICM40609_REG_INTF_CONFIG0 0x4C
#define ICM40609_REG_PWR_MGMT0 0x4E
#define ICM40609_REG_GYRO_CONFIG0 0x4F
#define ICM40609_REG_ACCEL_CONFIG0 0x50
#define ICM40609_REG_GYRO_CONFIG1 0x51 // Bandwidth of the temperature signal DLPF
#define ICM40609_REG_GYRO_ACCEL_CONFIG0 0x52 // Bandwidth for Gyro & Accel LPF
#define ICM40609_REG_GYRO_ACCEL_CONFIG1 0x53 // Bandwidth for Gyro & Accel LPF
#define ICM40609_REG_INT_CONFIG0 0x63
#define ICM40609_REG_INT_CONFIG1 0x64
#define ICM40609_REG_INT_SOURCE0 0x65
// Registers in BANK 1
#define ICM40609_REG_GYRO_CONFIG_STATIC2 0x0B
#define ICM40609_REG_GYRO_CONFIG_STATIC3 0x0C
#define ICM40609_REG_GYRO_CONFIG_STATIC4 0x0D
#define ICM40609_REG_GYRO_CONFIG_STATIC5 0x0E
#define ICM40609_REG_GYRO_CONFIG_STATIC6 0x0F
#define ICM40609_REG_GYRO_CONFIG_STATIC7 0x10
#define ICM40609_REG_GYRO_CONFIG_STATIC8 0x11
#define ICM40609_REG_GYRO_CONFIG_STATIC9 0x12
#define ICM40609_REG_GYRO_CONFIG_STATIC10 0x13 // gyro notch filter
// Registers in BANK 2
#define ICM40609_REG_ACCEL_CONFIG_STATIC2 0x03
#define ICM40609_REG_ACCEL_CONFIG_STATIC3 0x04
#define ICM40609_REG_ACCEL_CONFIG_STATIC4 0x05
// PWR_MGMT0_REG - 0x4E
#define ICM40609_TEMP_SENSOR_ENABLED (0 << 5)
#define ICM40609_TEMP_SENSOR_DISABLED (1 << 5)
#define ICM40609_IDLE (0 << 4)
#define ICM40609_RC_ON (1 << 4)
// // PWR_MGMT0_REG - 0x4E bits [3:2]
#define ICM40609_GYRO_MODE_OFF (0 << 2)
#define ICM40609_GYRO_MODE_STANDBY (1 << 2)
#define ICM40609_GYRO_MODE_LN (3 << 2)
// // PWR_MGMT0_REG - 0x4E bits [1:0]
#define ICM40609_ACCEL_MODE_OFF (0 << 0) // Of course, this is joke, but it's so easy to check bits orders in datasheet
#define ICM40609_ACCEL_MODE_PWR_OFF (1 << 0)
#define ICM40609_ACCEL_MODE_LP (2 << 0)
#define ICM40609_ACCEL_MODE_LN (3 << 0)
// GYRO_CONFIG0_REG - 0x4F bits [7:5]
#define ICM40609_GYRO_FS_SEL_2000DPS (0 << 5) // default)
#define ICM40609_GYRO_FS_SEL_1000DPS (1 << 5)
#define ICM40609_GYRO_FS_SEL_500DPS (2 << 5)
#define ICM40609_GYRO_FS_SEL_250DPS (3 << 5)
#define ICM40609_GYRO_FS_SEL_125DPS (4 << 5)
#define ICM40609_GYRO_FS_SEL_62_5DPS (5 << 5)
#define ICM40609_GYRO_FS_SEL_31_25DPS (6 << 5)
#define ICM40609_GYRO_FS_SEL_15_625DPS (7 << 5)
// GYRO_CONFIG0_REG - 0x4F bits [3:0]
#define ICM40609_GYRO_ODR_32KHZ 0x01
#define ICM40609_GYRO_ODR_16KHZ 0x02
#define ICM40609_GYRO_ODR_8KHZ 0x03
#define ICM40609_GYRO_ODR_4KHZ 0x04
#define ICM40609_GYRO_ODR_2KHZ 0x05
#define ICM40609_GYRO_ODR_1KHZ 0x06 // default
#define ICM40609_GYRO_ODR_200HZ 0x07
#define ICM40609_GYRO_ODR_100HZ 0x08
#define ICM40609_GYRO_ODR_50HZ 0x09
#define ICM40609_GYRO_ODR_25HZ 0x0A
#define ICM40609_GYRO_ODR_12_5HZ 0x0B
#define ICM40609_GYRO_ODR_RESERVED_C 0x0C
#define ICM40609_GYRO_ODR_RESERVED_D 0x0D
#define ICM40609_GYRO_ODR_RESERVED_E 0x0E
#define ICM40609_GYRO_ODR_500HZ 0x0F
// ACCEL_CONFIG0_REG - 0x50 bits [7:5]
#define ICM40609_ACCEL_FS_SEL_16G (0 << 5)
#define ICM40609_ACCEL_FS_SEL_8G (1 << 5)
#define ICM40609_ACCEL_FS_SEL_4G (2 << 5)
#define ICM40609_ACCEL_FS_SEL_2G (3 << 5)
#define ICM40609_ACCEL_FS_SEL_1G (4 << 5)
#define ICM40609_ACCEL_FS_SEL_0_5G (5 << 5)
#define ICM40609_ACCEL_FS_SEL_0_25G (6 << 5)
#define ICM40609_ACCEL_FS_SEL_0_125G (7 << 5)
// ACCEL_CONFIG0_REG - 0x50 bits [3:0]
#define ICM40609_ACCEL_ODR_32KHZ 0x01
#define ICM40609_ACCEL_ODR_16KHZ 0x02
#define ICM40609_ACCEL_ODR_8KHZ 0x03
#define ICM40609_ACCEL_ODR_4KHZ 0x04
#define ICM40609_ACCEL_ODR_2KHZ 0x05
#define ICM40609_ACCEL_ODR_1KHZ 0x06 // 1kHz (LN mode) (default)
#define ICM40609_ACCEL_ODR_200HZ 0x07
#define ICM40609_ACCEL_ODR_100HZ 0x08
#define ICM40609_ACCEL_ODR_50HZ 0x09
#define ICM40609_ACCEL_ODR_25HZ 0x0A
#define ICM40609_ACCEL_ODR_12_5HZ 0x0B
#define ICM40609_ACCEL_ODR_500HZ 0x0F
// INT_CONFIG_REG - 0x14
#define ICM40609_INT1_MODE_PULSED (0 << 2)
#define ICM40609_INT1_MODE_LATCHED (1 << 2)
#define ICM40609_INT1_DRIVE_OPEN_DRAIN (0 << 1)
#define ICM40609_INT1_DRIVE_PUSH_PULL (1 << 1)
#define ICM40609_INT1_POLARITY_ACTIVE_LOW (0 << 0)
#define ICM40609_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
// NT_SOURCE0_REG - 0x65
#define ICM40609_UI_FSYNC_INT1_EN (1 << 6)
#define ICM40609_PLL_RDY_INT1_EN (1 << 5)
#define ICM40609_RESET_DONE_INT1_EN (1 << 4)
#define ICM40609_UI_DRDY_INT1_EN (1 << 3)
#define ICM40609_FIFO_THS_INT1_EN (1 << 2)
#define ICM40609_FIFO_FULL_INT1_EN (1 << 1)
#define ICM40609_UI_AGC_RDY_INT1_EN (1 << 0)
// INT_CONFIG0 - 0x63
#define ICM40609_UI_DRDY_INT_CLEAR_STATUS (0 << 4)
// INT_CONFIG1 - 0x64
#define ICM40609_INT_TPULSE_100US (0 << 6) // ODR < 4kHz, optional
#define ICM40609_INT_TPULSE_8US (1 << 6) // ODR ≥ 4kHz
#define ICM40609_INT_TDEASSERT_ENABLED (0 << 5)
#define ICM40609_INT_TDEASSERT_DISABLED (1 << 5)
#define ICM40609_INT_ASYNC_RESET_ENABLED (1 << 4)
#define ICM40609_INT_ASYNC_RESET_DISABLED (0 << 4)
// REG_GYRO_CONFIG1 - 0x51 bits [3:2]
#define ICM40609_GYRO_UI_FILT_ORDER_1ST (0 << 2)
#define ICM40609_GYRO_UI_FILT_ORDER_2ND (1 << 2)
#define ICM40609_GYRO_UI_FILT_ORDER_3RD (2 << 2)
// REG_GYRO_CONFIG1 - 0x51 bits [1:0]
#define ICM40609_GYRO_DEC2_M2_ORDER_3RD (2 << 0)
// REG_GYRO_ACCEL_CONFIG0 - 0x52 bits [7:4]
#define ICM40609_ACCEL_UI_FILT_BW_ODR_DIV2 (0 << 4)
#define ICM40609_ACCEL_UI_FILT_BW_ODR_DIV4 (1 << 4) // default
#define ICM40609_ACCEL_UI_FILT_BW_ODR_DIV5 (2 << 4)
#define ICM40609_ACCEL_UI_FILT_BW_ODR_DIV8 (3 << 4)
#define ICM40609_ACCEL_UI_FILT_BW_ODR_DIV10 (4 << 4)
#define ICM40609_ACCEL_UI_FILT_BW_ODR_DIV16 (5 << 4)
#define ICM40609_ACCEL_UI_FILT_BW_ODR_DIV20 (6 << 4)
#define ICM40609_ACCEL_UI_FILT_BW_ODR_DIV40 (7 << 4)
#define ICM40609_ACCEL_UI_FILT_BW_LP_TRIVIAL_400HZ_ODR (14 << 4) // Bit[7:4] - Low Latency
#define ICM40609_ACCEL_UI_FILT_BW_LP_TRIVIAL_200HZ_8XODR (15 << 4) // Bit[7:4] - Low Latency
// REG_GYRO_ACCEL_CONFIG0 - 0x52 bits [3:0]
#define ICM40609_GYRO_UI_FILT_BW_ODR_DIV2 (0 << 0)
#define ICM40609_GYRO_UI_FILT_BW_ODR_DIV4 (1 << 0) // default
#define ICM40609_GYRO_UI_FILT_BW_ODR_DIV5 (2 << 0)
#define ICM40609_GYRO_UI_FILT_BW_ODR_DIV8 (3 << 0)
#define ICM40609_GYRO_UI_FILT_BW_ODR_DIV10 (4 << 0)
#define ICM40609_GYRO_UI_FILT_BW_ODR_DIV16 (5 << 0)
#define ICM40609_GYRO_UI_FILT_BW_ODR_DIV20 (6 << 0)
#define ICM40609_GYRO_UI_FILT_BW_ODR_DIV40 (7 << 0)
#define ICM40609_GYRO_UI_FILT_BW_LP_TRIVIAL_400HZ_ODR (14 << 0) // Bit[3:0] - Low Latency
#define ICM40609_GYRO_UI_FILT_BW_LP_TRIVIAL_200HZ_8XODR (15 << 0) // Bit[3:0] - Low Latency
// REG_ACCEL_CONFIG_STATIC2 - 0x03 bit [0]
#define ICM40609_ACCEL_AAF_DIS (1 << 0)
// REG_GYRO_CONFIG_STATIC2 - 0x0B bit [1]
#define ICM40609_GYRO_AAF_DIS (1 << 1)
//REG_GYRO_CONFIG_STATIC2 - 0x0B bit [0]
#define ICM40609_GYRO_NF_DIS_BIT (1 << 0)
// GYRO_HPF_BW_IND - 0x13 bit [3:1] — High-pass filter 3dB cutoff frequency selection
#define ICM40609_GYRO_HPF_BW_IND_MASK (0x07 << 1) // bits [3:1]
// GYRO_HPF_ORD_IND [0] — High-pass filter order (1st or 2nd)
#define ICM40609_GYRO_HPF_ORD_IND_MASK (1 << 0)
// GYRO_CONFIG1 (0x51)
#define ICM40609_GYRO_UI_FILT_ORD_MASK (0x03 << 2) // bits [3:2]
#define ICM40609_GYRO_DEC2_M2_ORD_MASK (0x03 << 0) // bits [1:0]
// ACCEL_CONFIG1 (0x53)
#define ICM40609_ACCEL_UI_FILT_ORD_MASK (0x03 << 3) // bits [4:3]
#define ICM40609_ACCEL_DEC2_M2_ORD_MASK (0x03 << 1) // bits [2:1]
#define ICM40609_ACCEL_DATA_X1_UI 0x1F
#define ICM40609_GYRO_DATA_X1_UI 0x25
#define ICM40609_RESET_REG 0x4C
#define ICM40609_SOFT_RESET_VAL 0x01
#ifndef ICM40609_LOCK
// Default: 24 MHz max SPI frequency
#define ICM40609_MAX_SPI_CLK_HZ 24000000
#else
// Use the supplied value
#define ICM40609_MAX_SPI_CLK_HZ ICM40609_LOCK
#endif
#define ICM40609_AAF_PROFILE_COUNT 63
// Table 5.2 Bandwidth (Hz)
typedef struct {
uint16_t hz;
uint8_t delt;
uint16_t deltsqr;
uint8_t bitshift;
} ICM40609_AafProfile;
static const ICM40609_AafProfile aafProfiles[ICM40609_AAF_PROFILE_COUNT] = {
{ 42, 1, 1, 15 },
{ 84, 2, 4, 13 },
{ 126, 3, 9, 12 },
{ 170, 4, 16, 11 },
{ 213, 5, 25, 10 },
{ 258, 6, 36, 10 },
{ 303, 7, 49, 9 },
{ 348, 8, 64, 9 },
{ 394, 9, 81, 9 },
{ 441, 10, 100, 8 },
{ 488, 11, 122, 8 },
{ 536, 12, 144, 8 },
{ 585, 13, 170, 8 },
{ 634, 14, 196, 8 },
{ 684, 15, 224, 7 },
{ 734, 16, 256, 7 },
{ 785, 17, 288, 7 },
{ 837, 18, 324, 7 },
{ 890, 19, 360, 6 },
{ 943, 20, 400, 6 },
{ 997, 21, 440, 6 },
{ 1051, 22, 488, 6 },
{ 1107, 23, 528, 6 },
{ 1163, 24, 576, 6 },
{ 1220, 25, 624, 6 },
{ 1277, 26, 680, 6 },
{ 1336, 27, 736, 5 },
{ 1395, 28, 784, 5 },
{ 1454, 29, 848, 5 },
{ 1515, 30, 896, 5 },
{ 1577, 31, 960, 5 },
{ 1639, 32, 1024, 5 },
{ 1702, 33, 1088, 5 },
{ 1766, 34, 1152, 5 },
{ 1830, 35, 1232, 5 },
{ 1896, 36, 1296, 5 },
{ 1962, 37, 1376, 4 },
{ 2029, 38, 1440, 4 },
{ 2097, 39, 1536, 4 },
{ 2166, 40, 1600, 4 },
{ 2235, 41, 1696, 4 },
{ 2306, 42, 1760, 4 },
{ 2377, 43, 1856, 4 },
{ 2449, 44, 1952, 4 },
{ 2522, 45, 2016, 4 },
{ 2596, 46, 2112, 4 },
{ 2671, 47, 2208, 4 },
{ 2746, 48, 2304, 4 },
{ 2823, 49, 2400, 4 },
{ 2900, 50, 2496, 4 },
{ 2978, 51, 2592, 4 },
{ 3057, 52, 2720, 4 },
{ 3137, 53, 2816, 4 },
{ 3217, 54, 2944, 3 },
{ 3299, 55, 3008, 3 },
{ 3381, 56, 3136, 3 },
{ 3464, 57, 3264, 3 },
{ 3548, 58, 3392, 3 },
{ 3633, 59, 3456, 3 },
{ 3718, 60, 3584, 3 },
{ 3805, 61, 3712, 3 },
{ 3892, 62, 3840, 3 },
{ 3979, 63, 3968, 3 },
};
/*
* ICM-40609D Group Delay @DC (ODR = 8000 Hz)
*
* +-------------------+--------------------+----------+
* | Filter Order | Delay (UI_FILT_BW) | NBW (Hz) |
* +-------------------+--------------------+----------+
* | 1st order filter | 0.2 ms | 2204.6 |
* | 2nd order filter | 0.2 ms | 2204.6 |
* | 3rd order filter | 0.2 ms | 2096.3 |
* +-------------------+--------------------+----------+
*
* Note:
* Delay is independent of UI_FILT_BW when ODR = 8000Hz.
* 5.4 UI FILTER BLOCK TDK ICM-40609D Datasheet Rev 1.2 (2023)
*
* Filter order (standard DSP behavior):
* 1st order -6 dB/octave
* 2nd order -12 dB/octave
* 3rd order -18 dB/octave
* These roll-off rates are typical for LPF/HPF filters in digital signal processing (DSP).
*/
typedef enum {
ICM40609_UI_FILT_ORDER_1ST = 0,
ICM40609_UI_FILT_ORDER_2ND = 1,
ICM40609_UI_FILT_ORDER_3RD = 2
} icm40609UiFiltOrder_e;
typedef enum {
ICM40609_HPF_ORDER_1ST = 0,
ICM40609_HPF_ORDER_2ND = 1
} icm40609HpfOrder_e;
// Bandwidth selection for High-Pass Filter
// ICM40609_REG_GYRO_CONFIG_STATIC10 - 0x13 bits [3:1]
// NOTE: clarify with new datasheet, section 5.6 not found in V1.2
typedef enum {
ICM40609_HPF_BW_0 = 0,
ICM40609_HPF_BW_1 = 1,
ICM40609_HPF_BW_2 = 2,
ICM40609_HPF_BW_3 = 3,
ICM40609_HPF_BW_4 = 4,
ICM40609_HPF_BW_5 = 5,
ICM40609_HPF_BW_6 = 6,
ICM40609_HPF_BW_7 = 7,
} icm40609HpfBw_e;
// Bandwidth selection for Notch Filter GYRO_NF_BW_SEL
// ICM40609_REG_GYRO_CONFIG_STATIC10 - 0x13 bits [6:4]
typedef enum {
ICM40609_GYRO_NF_BW_1449HZ = 0,
ICM40609_GYRO_NF_BW_680HZ = 1,
ICM40609_GYRO_NF_BW_329HZ = 2,
ICM40609_GYRO_NF_BW_162HZ = 3,
ICM40609_GYRO_NF_BW_80HZ = 4,
ICM40609_GYRO_NF_BW_40HZ = 5,
ICM40609_GYRO_NF_BW_20HZ = 6,
ICM40609_GYRO_NF_BW_10HZ = 7,
} icm40609GyroNfBw_e;
typedef enum {
ICM40609_TEMP_FILT_BW_4000HZ = 0, // 4000Hz, 0.125ms latency (default)
ICM40609_TEMP_FILT_BW_170HZ = 1, // 170Hz, 1ms latency
ICM40609_TEMP_FILT_BW_82HZ = 2, // 82Hz, 2ms latency
ICM40609_TEMP_FILT_BW_40HZ = 3, // 40Hz, 4ms latency
ICM40609_TEMP_FILT_BW_20HZ = 4, // 20Hz, 8ms latency
ICM40609_TEMP_FILT_BW_10HZ = 5, // 10Hz, 16ms latency
ICM40609_TEMP_FILT_BW_5HZ = 6, // 5Hz, 32ms latency
} icm40609TempFiltBw_e;
static void icm40609SelectUserBank(const extDevice_t *dev, uint8_t bank)
{
if (bank > 4) {
return; // out of range
}
spiWriteReg(dev, ICM40609_REG_BANK_SEL, bank & 0x07); // bit [2:0]
}
static void setGyroAccPowerMode(const extDevice_t *dev, bool enable)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
spiWriteReg(dev, ICM40609_REG_PWR_MGMT0,
ICM40609_RC_ON |
ICM40609_TEMP_SENSOR_ENABLED |
(enable ? ICM40609_GYRO_MODE_LN | ICM40609_ACCEL_MODE_LN
: ICM40609_GYRO_MODE_OFF | ICM40609_ACCEL_MODE_OFF));
if (enable) {
delayMicroseconds(200);
} else {
delay(50);
}
}
static void icm40609GetAafParams(uint16_t targetHz, ICM40609_AafProfile* res)
{
uint16_t i = 0;
while (i < ICM40609_AAF_PROFILE_COUNT && targetHz > aafProfiles[i].hz) {
i++;
}
if (i < ICM40609_AAF_PROFILE_COUNT) {
*res = aafProfiles[i];
} else {
// not found - Requested frequency is higher than max available
*res = aafProfiles[ICM40609_AAF_PROFILE_COUNT - 1];
}
}
static void icm40609SetAccelAafByHz(const extDevice_t *dev, bool aafEnable, uint16_t targetHz)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_2);
if (aafEnable && targetHz > 0) {
ICM40609_AafProfile aafProfile;
icm40609GetAafParams(targetHz, &aafProfile);
uint8_t reg03 = spiReadRegMsk(dev, ICM40609_REG_ACCEL_CONFIG_STATIC2);
reg03 &= ~ICM40609_ACCEL_AAF_DIS; // Clear ACCEL_AAF_DIS to enable AAF
reg03 = (reg03 & 0x81) | (aafProfile.delt << 1); // Keep reserved bit 7, set ACCEL_AAF_DELT
spiWriteReg(dev, ICM40609_REG_ACCEL_CONFIG_STATIC2, reg03);
uint8_t reg04 = aafProfile.deltsqr & 0xFF;
uint8_t reg05 = ((aafProfile.bitshift & 0x0F) << 4) | ((aafProfile.deltsqr >> 8) & 0x0F);
spiWriteReg(dev, ICM40609_REG_ACCEL_CONFIG_STATIC3, reg04);
spiWriteReg(dev, ICM40609_REG_ACCEL_CONFIG_STATIC4, reg05);
} else {
uint8_t reg03 = spiReadRegMsk(dev, ICM40609_REG_ACCEL_CONFIG_STATIC2);
reg03 |= ICM40609_ACCEL_AAF_DIS; // Set ACCEL_AAF_DIS to disable AAF
spiWriteReg(dev, ICM40609_REG_ACCEL_CONFIG_STATIC2, reg03);
}
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
}
static void icm40609SetGyroAafByHz(const extDevice_t *dev, bool aafEnable, uint16_t targetHz)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_1);
if (aafEnable && targetHz > 0) {
ICM40609_AafProfile aafProfile;
icm40609GetAafParams(targetHz, &aafProfile);
uint8_t reg0C = aafProfile.delt & 0x3F;
uint8_t reg0D = aafProfile.deltsqr & 0xFF;
uint8_t reg0E = ((aafProfile.bitshift & 0x0F) << 4) | ((aafProfile.deltsqr >> 8) & 0x0F);
uint8_t reg0B = spiReadRegMsk(dev, ICM40609_REG_GYRO_CONFIG_STATIC2);
reg0B &= ~ICM40609_GYRO_AAF_DIS; // Clear AAF_DIS (bit1) to enable AAF
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC2, reg0B);
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC3, reg0C);
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC4, reg0D);
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC5, reg0E);
} else {
uint8_t reg0B = spiReadRegMsk(dev, ICM40609_REG_GYRO_CONFIG_STATIC2);
reg0B |= ICM40609_GYRO_AAF_DIS; // Set AAF_DIS (bit1) to disable AAF
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC2, reg0B);
}
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
}
static void icm40609SetGyroHPF(const extDevice_t *dev, bool hpfEnable, icm40609HpfBw_e hpfBwInd, icm40609HpfOrder_e hpfOrder)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_1);
uint8_t reg13 = spiReadRegMsk(dev, ICM40609_REG_GYRO_CONFIG_STATIC10);
reg13 &= ~(ICM40609_GYRO_HPF_BW_IND_MASK | ICM40609_GYRO_HPF_ORD_IND_MASK); // clear HPF bits
if (hpfEnable) {
reg13 |= (hpfBwInd << 1) | (hpfOrder << 0);
}
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC10, reg13);
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
}
static void icm40609SetGyroNotch(const extDevice_t *dev, bool notchEnable, icm40609GyroNfBw_e bwSel, float fdesiredKhz)
{
if (fdesiredKhz < 1.0f || fdesiredKhz > 3.0f) {
return; // (1kHz to 3kHz) Operating the notch filter outside this range is not supported.
}
icm40609SelectUserBank(dev, ICM40609_USER_BANK_1);
// Enable/disable Notch filter
uint8_t reg2 = spiReadRegMsk(dev, ICM40609_REG_GYRO_CONFIG_STATIC2);
if (notchEnable) {
reg2 &= ~ICM40609_GYRO_NF_DIS_BIT;
} else {
reg2 |= ICM40609_GYRO_NF_DIS_BIT; // Bypass
}
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC2, reg2);
if (notchEnable) {
// Set Bandwidth in STATIC10 (0x13)
uint8_t reg13 = spiReadRegMsk(dev, ICM40609_REG_GYRO_CONFIG_STATIC10);
reg13 &= ~(0x07 << 4);
reg13 |= (bwSel & 0x07) << 4; // GYRO_NF_BW_SEL [6:4]
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC10, reg13);
// Section 5.1.1 (v1.2) Frequency of Notch Filter (each axis)
// Calculate COSWZ and SEL based on desired frequency
float coswz = cosf(2.0f * M_PIf * fdesiredKhz / 32.0f);
uint8_t nf_coswz_lsb = 0;
uint8_t nf_coswz_msb = 0;
uint8_t nf_coswz_sel = 0;
if (fabsf(coswz) <= 0.875f) {
int16_t nf_coswz = (int16_t)roundf(coswz * 256.0f);
nf_coswz_lsb = nf_coswz & 0xFF;
nf_coswz_msb = (nf_coswz >> 8) & 0x01;
nf_coswz_sel = 0;
} else {
nf_coswz_sel = 1;
int16_t nf_coswz;
if (coswz > 0.875f) {
nf_coswz = (int16_t)roundf(8.0f * (1.0f - coswz) * 256.0f);
} else {
nf_coswz = (int16_t)roundf(8.0f * (1.0f + coswz) * 256.0f);
}
nf_coswz_lsb = nf_coswz & 0xFF;
nf_coswz_msb = (nf_coswz >> 8) & 0x01;
}
// Write NF_COSWZ[7:0] for X, Y, Z
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC6, nf_coswz_lsb); // X
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC7, nf_coswz_lsb); // Y
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC8, nf_coswz_lsb); // Z
// Write NF_COSWZ[8] and NF_COSWZ_SEL into STATIC9 (0x12)
uint8_t reg9 = 0;
reg9 |= (nf_coswz_msb << 0); // X NF_COSWZ[8]
reg9 |= (nf_coswz_msb << 1); // Y NF_COSWZ[8]
reg9 |= (nf_coswz_msb << 2); // Z NF_COSWZ[8]
reg9 |= (nf_coswz_sel << 3); // X COSWZ_SEL
reg9 |= (nf_coswz_sel << 4); // Y COSWZ_SEL
reg9 |= (nf_coswz_sel << 5); // Z COSWZ_SEL
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG_STATIC9, reg9);
}
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
}
static void icm40609SetTempFiltBw(const extDevice_t *dev, icm40609TempFiltBw_e bw)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
uint8_t reg51 = spiReadRegMsk(dev, ICM40609_REG_GYRO_CONFIG1);
reg51 &= ~(0x07 << 5);
reg51 |= (bw << 5);
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG1, reg51);
}
static void icm40609SetGyroUiFiltOrder(const extDevice_t *dev, icm40609UiFiltOrder_e order)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
uint8_t reg51 = spiReadRegMsk(dev, ICM40609_REG_GYRO_CONFIG1);
reg51 &= ~ICM40609_GYRO_UI_FILT_ORD_MASK;
reg51 |= (order << 2); // Write GYRO_UI_FILT_ORD to bits [3:2]
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG1, reg51);
}
static void icm40609SetAccelUiFiltOrder(const extDevice_t *dev, icm40609UiFiltOrder_e order)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
uint8_t reg53 = spiReadRegMsk(dev, ICM40609_REG_GYRO_ACCEL_CONFIG1);
reg53 &= ~ICM40609_ACCEL_UI_FILT_ORD_MASK;
reg53 |= (order << 3); // Write ACCEL_UI_FILT_ORD to bits [4:3]
spiWriteReg(dev, ICM40609_REG_GYRO_ACCEL_CONFIG1, reg53);
}
static void icm40609SetGyroDec2M2(const extDevice_t *dev, bool enable)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
uint8_t reg51 = spiReadRegMsk(dev, ICM40609_REG_GYRO_CONFIG1);
reg51 &= ~ICM40609_GYRO_DEC2_M2_ORD_MASK;
if (enable) {
reg51 |= (2 << 0);
}
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG1, reg51);
}
// Set endianness for sensor data (bit 4 of INTF_CONFIG0, reg 0x4C)
// true = Big Endian
// false = Little Endian
static void icm40609SetEndianess(const extDevice_t *dev, bool bigEndian)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
uint8_t reg4C = spiReadRegMsk(dev, ICM40609_REG_INTF_CONFIG0);
reg4C &= ~(1 << 4);
reg4C |= bigEndian << 4;
spiWriteReg(dev, ICM40609_REG_INTF_CONFIG0, reg4C);
}
void icm40609AccInit(accDev_t *acc)
{
acc->acc_1G = 2048; // 16g scale
acc->gyro->accSampleRateHz = 1000;
}
void icm40609GyroInit(gyroDev_t *gyro)
{
const extDevice_t *dev = &gyro->dev;
spiSetClkDivisor(dev, spiCalculateDivider(ICM40609_MAX_SPI_CLK_HZ));
mpuGyroInit(gyro);
gyro->accDataReg = ICM40609_ACCEL_DATA_X1_UI;
gyro->gyroDataReg = ICM40609_GYRO_DATA_X1_UI;
setGyroAccPowerMode(dev, false);
icm40609SetEndianess(dev, true);
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
spiWriteReg(dev, ICM40609_REG_GYRO_CONFIG0, ICM40609_GYRO_FS_SEL_2000DPS | ICM40609_GYRO_ODR_8KHZ);
gyro->scale = GYRO_SCALE_2000DPS;
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
gyro->gyroSampleRateHz = 8000;
spiWriteReg(dev, ICM40609_REG_ACCEL_CONFIG0, ICM40609_ACCEL_FS_SEL_16G | ICM40609_ACCEL_ODR_1KHZ );
icm40609SetTempFiltBw(dev, ICM40609_TEMP_FILT_BW_4000HZ); // 4000Hz, 0.125ms latency (default)
icm40609SetGyroUiFiltOrder(dev, ICM40609_UI_FILT_ORDER_3RD);
icm40609SetAccelUiFiltOrder(dev, ICM40609_UI_FILT_ORDER_3RD);
icm40609SetGyroDec2M2(dev, true);
// Set filter bandwidth: Low Latency
spiWriteReg(&gyro->dev, ICM40609_REG_GYRO_ACCEL_CONFIG0,
ICM40609_ACCEL_UI_FILT_BW_LP_TRIVIAL_200HZ_8XODR |
ICM40609_GYRO_UI_FILT_BW_LP_TRIVIAL_200HZ_8XODR);
uint16_t gyroHWLpf; // Anti-Alias Filter (AAF) in Hz
switch (gyroConfig()->gyro_hardware_lpf) {
case GYRO_HARDWARE_LPF_NORMAL:
gyroHWLpf = 213;
break;
case GYRO_HARDWARE_LPF_OPTION_1:
gyroHWLpf = 488;
break;
case GYRO_HARDWARE_LPF_OPTION_2:
gyroHWLpf = 997;
break;
#ifdef USE_GYRO_DLPF_EXPERIMENTAL
case GYRO_HARDWARE_LPF_EXPERIMENTAL:
gyroHWLpf = 1962;
break;
#endif
default:
gyroHWLpf = 213;
}
icm40609SetGyroAafByHz(dev, true, gyroHWLpf);
icm40609SetAccelAafByHz(dev, true, gyroHWLpf);
icm40609SetGyroNotch(dev, true, ICM40609_GYRO_NF_BW_1449HZ, 1.5f);
icm40609SetGyroHPF(dev, true, ICM40609_HPF_BW_1, ICM40609_HPF_ORDER_1ST);
// Enable interrupt
spiWriteReg(dev, ICM40609_REG_INT_SOURCE0, ICM40609_UI_DRDY_INT1_EN);
// Set INT1 to pulse mode, push-pull, active high
spiWriteReg(dev, ICM40609_REG_INT_CONFIG,
ICM40609_INT1_MODE_PULSED |
ICM40609_INT1_DRIVE_PUSH_PULL |
ICM40609_INT1_POLARITY_ACTIVE_HIGH);
spiWriteReg(dev, ICM40609_REG_INT_CONFIG0, ICM40609_UI_DRDY_INT_CLEAR_STATUS); // auto clear after read
// Set INT1 pulse width to 8us, deassertion enabled, async reset disabled
spiWriteReg(dev, ICM40609_REG_INT_CONFIG1,
ICM40609_INT_TPULSE_8US |
ICM40609_INT_TDEASSERT_DISABLED |
ICM40609_INT_ASYNC_RESET_DISABLED);
setGyroAccPowerMode(dev, true);
}
uint8_t icm40609SpiDetect(const extDevice_t *dev)
{
icm40609SelectUserBank(dev, ICM40609_USER_BANK_0);
spiWriteReg(dev, ICM40609_RESET_REG, ICM40609_SOFT_RESET_VAL);
delay(1);
uint8_t whoAmI = spiReadRegMsk(dev, ICM40609_WHO_AM_I_REG);
if (whoAmI == ICM40609_WHO_AM_I_CONST) {
return ICM_40609_SPI;
}
return MPU_NONE;
}
bool icm40609SpiAccDetect(accDev_t *acc)
{
if (acc->mpuDetectionResult.sensor == ICM_40609_SPI) {
acc->initFn = icm40609AccInit;
acc->readFn = mpuAccReadSPI;
return true;
}
return false;
}
bool icm40609SpiGyroDetect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor == ICM_40609_SPI) {
gyro->initFn = icm40609GyroInit;
gyro->readFn = mpuGyroReadSPI;
return true;
}
return false;
}
#endif // USE_ACCGYRO_ICM40609D

32
src/main/drivers/accgyro/accgyro_spi_icm40609.h Normal file
View file

@ -0,0 +1,32 @@
/*
* This file is part of Betaflight.
*
* Betaflight is free software. You can redistribute this software
* and/or modify this software under the terms of the GNU General
* Public License as published by the Free Software Foundation,
* either version 3 of the License, or (at your option) any later
* version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/bus.h"
void icm40609AccInit(accDev_t *acc);
void icm40609GyroInit(gyroDev_t *gyro);
uint8_t icm40609SpiDetect(const extDevice_t *dev);
bool icm40609SpiAccDetect(accDev_t *acc);
bool icm40609SpiGyroDetect(gyroDev_t *gyro);

630
src/main/drivers/accgyro/accgyro_spi_icm456xx.c Normal file
View file

@ -0,0 +1,630 @@
/*
* This file is part of Betaflight.
*
* Betaflight is free software. You can redistribute this software
* and/or modify this software under the terms of the GNU General
* Public License as published by the Free Software Foundation,
* either version 3 of the License, or (at your option) any later
* version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
#if defined(USE_ACCGYRO_ICM45686) || defined(USE_ACCGYRO_ICM45605)
#include "common/axis.h"
#include "common/utils.h"
#include "build/debug.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_spi_icm456xx.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/pwm_output.h"
#include "drivers/sensor.h"
#include "drivers/time.h"
#include "drivers/system.h"
#include "fc/runtime_config.h"
#include "sensors/gyro.h"
#include "pg/gyrodev.h"
/*
reference: https://github.com/tdk-invn-oss/motion.mcu.icm45686.driver
Datasheet: https://invensense.tdk.com/wp-content/uploads/documentation/DS-000577_ICM-45686.pdf
Datasheet: https://invensense.tdk.com/wp-content/uploads/documentation/DS-000576_ICM-45605.pdf
Note: ICM456xx has two modes of operation: Low-Power Mode Low-Noise Mode
Note: Now implemented only UI Interface with Low-Noise Mode
The following diagram shows the signal path for each mode:
The cut-off frequency of the filters is determined by the ODR setting.
Low-Noise Mode
+------+ +--------------+ +-------------+ +--------------+ +------------------+
| ADC |---->| Anti-Alias |--->| Interpolator|--->| LPF |--->| Sensor Registers |---> UI Interface
| | | Filter (AAF) | | | +->| & ODR Select | | |
+--|---+ +--------------+ +-------------+ | +--------------+ +------------------+
| |
| Low-Power Mode |
| +--------------+ |
|-------->| Notch Filter |--------------------|
| | |
| +--------------+
|
|
+--|---+ +--------------+ +------+ +------+ +------------------+
| ADC | --------> | Notch Filter | ---> | HPF | ---> | LPF | ---> | Sensor Registers | ---> AUX1 Interface
| | | | | | | | | |
+------+ +--------------+ +------+ +------+ +------------------+
The AUX1 interface default configuration can be checked by read only register IOC_PAD_SCENARIO through host interface.
By default, AUX1 interface is enabled, and default interface for AUX1 is SPI3W or I3CSM.
In Low-Noise Mode, the ADC output is sent through an Anti-Alias Filter (AAF). The AAF is an FIR filter with fixed
coefficients (not user configurable). The AAF can be enabled or disabled by the user using GYRO_SRC_CTRL and
ACCEL_SRC_CTRL.
The AUX1 signal path includes a Notch Filter. The notch filter is not user programmable. The usage of the notch
filter in the auxiliary path is recommended for sharper roll-off and for the cases where user is asynchronously
sampling the auxiliary interface data output at integer multiples of 1 kHz rate. The notch filter may be bypassed
using GYRO_OIS_M6_BYP.
The notch filter is followed by an HPF on the AUX1 signal path. HPF cut-off frequency can be selected using
GYRO_OIS_HPFBW_SEL and ACCEL_OIS_HPFBW_SEL. HPF can be bypassed using GYRO_OIS_HPF1_BYP and
ACCEL_OIS_HPF1_BYP.
The HPF is followed by LPF on the AUX1 signal path. The AUX1 LPF BW is set by register bit field
GYRO_OIS_LPF1BW_SEL and ACCEL_OIS_LPF1BW_SEL for gyroscope and accelerometer respectively. This is
followed by Full Scale Range (FSR) selection based on user configurable settings for register fields
GYRO_AUX1_FS_SEL and ACCEL_AUX1_FS_SEL. AUX1 output is fixed at 6.4kHz ODR.
*/
#define ICM456XX_REG_BANK_SEL 0x75
#define ICM456XX_BANK_0 0x00
#define ICM456XX_BANK_1 0x01
// Register map Bank 0
#define ICM456XX_WHO_AM_REGISTER 0x72
#define ICM456XX_REG_MISC2 0x7F
#define ICM456XX_INT1_CONFIG0 0x16
#define ICM456XX_INT1_CONFIG2 0x18
#define ICM456XX_INT1_STATUS0 0x19
#define ICM456XX_INT1_STATUS1 0x1A
#define ICM456XX_GYRO_CONFIG0 0x1C
#define ICM456XX_ACCEL_CONFIG0 0x1B
#define ICM456XX_PWR_MGMT0 0x10
// Register map IPREG_TOP1
#define ICM456XX_RA_SREG_CTRL 0xA267 // To access register in IPREG_TOP1, add base address 0xA200 + offset
// SREG_CTRL - 0x67
#define ICM456XX_SREG_DATA_ENDIAN_SEL_LITTLE (0 << 1)
#define ICM456XX_SREG_DATA_ENDIAN_SEL_BIG (1 << 1) // not working set SREG_CTRL regiser
// MGMT0 - 0x10 - Gyro
#define ICM456XX_GYRO_MODE_OFF (0x00 << 2)
#define ICM456XX_GYRO_MODE_STANDBY (0x01 << 2)
#define ICM456XX_GYRO_MODE_LP (0x02 << 2) // Low Power Mode
#define ICM456XX_GYRO_MODE_LN (0x03 << 2) // Low Noise Mode
// MGMT0 - 0x10 - Accel
#define ICM456XX_ACCEL_MODE_OFF (0x00)
#define ICM456XX_ACCEL_MODE_OFF2 (0x01)
#define ICM456XX_ACCEL_MODE_LP (0x02) // Low Power Mode
#define ICM456XX_ACCEL_MODE_LN (0x03) // Low Noise Mode
// INT1_CONFIG0 - 0x16
#define ICM456XX_INT1_STATUS_EN_RESET_DONE (1 << 7)
#define ICM456XX_INT1_STATUS_EN_AUX1_AGC_RDY (1 << 6)
#define ICM456XX_INT1_STATUS_EN_AP_AGC_RDY (1 << 5)
#define ICM456XX_INT1_STATUS_EN_AP_FSYNC (1 << 4)
#define ICM456XX_INT1_STATUS_EN_AUX1_DRDY (1 << 3)
#define ICM456XX_INT1_STATUS_EN_DRDY (1 << 2)
#define ICM456XX_INT1_STATUS_EN_FIFO_THS (1 << 1)
#define ICM456XX_INT1_STATUS_EN_FIFO_FULL (1 << 0)
// INT1_CONFIG2 - 0x18
#define ICM456XX_INT1_DRIVE_CIRCUIT_PP (0 << 2)
#define ICM456XX_INT1_DRIVE_CIRCUIT_OD (1 << 2)
#define ICM456XX_INT1_MODE_PULSED (0 << 1)
#define ICM456XX_INT1_MODE_LATCHED (1 << 1)
#define ICM456XX_INT1_POLARITY_ACTIVE_LOW (0 << 0)
#define ICM456XX_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
// INT1_STATUS0 - 0x19
#define ICM456XX_INT1_STATUS_RESET_DONE (1 << 7)
#define ICM456XX_INT1_STATUS_AUX1_AGC_RDY (1 << 6)
#define ICM456XX_INT1_STATUS_AP_AGC_RDY (1 << 5)
#define ICM456XX_INT1_STATUS_AP_FSYNC (1 << 4)
#define ICM456XX_INT1_STATUS_AUX1_DRDY (1 << 3)
#define ICM456XX_INT1_STATUS_DRDY (1 << 2)
#define ICM456XX_INT1_STATUS_FIFO_THS (1 << 1)
#define ICM456XX_INT1_STATUS_FIFO_FULL (1 << 0)
// REG_MISC2 - 0x7F
#define ICM456XX_SOFT_RESET (1 << 1)
#define ICM456XX_RESET_TIMEOUT_US 20000 // 20 ms
#define ICM456XX_ACCEL_DATA_X1_UI 0x00
#define ICM456XX_GYRO_DATA_X1_UI 0x06
// ACCEL_CONFIG0 - 0x1B
#define ICM456XX_ACCEL_FS_SEL_32G (0x00 << 4)
#define ICM456XX_ACCEL_FS_SEL_16G (0x01 << 4)
#define ICM456XX_ACCEL_FS_SEL_8G (0x02 << 4)
#define ICM456XX_ACCEL_FS_SEL_4G (0x03 << 4)
#define ICM456XX_ACCEL_FS_SEL_2G (0x04 << 4)
// ACCEL_CONFIG0 - 0x1B
#define ICM456XX_ACCEL_ODR_6K4_LN 0x03
#define ICM456XX_ACCEL_ODR_3K2_LN 0x04
#define ICM456XX_ACCEL_ODR_1K6_LN 0x05
#define ICM456XX_ACCEL_ODR_800_LN 0x06
#define ICM456XX_ACCEL_ODR_400_LP_LN 0x07
#define ICM456XX_ACCEL_ODR_200_LP_LN 0x08
#define ICM456XX_ACCEL_ODR_100_LP_LN 0x09
#define ICM456XX_ACCEL_ODR_50_LP_LN 0x0A
#define ICM456XX_ACCEL_ODR_25_LP_LN 0x0B
#define ICM456XX_ACCEL_ODR_12_5_LP_LN 0x0C
#define ICM456XX_ACCEL_ODR_6_25_LP 0x0D
#define ICM456XX_ACCEL_ODR_3_125_LP 0x0E
#define ICM456XX_ACCEL_ODR_1_5625_LP 0x0F
// GYRO_CONFIG0 - 0x1C
#define ICM456XX_GYRO_FS_SEL_4000DPS (0x00 << 4)
#define ICM456XX_GYRO_FS_SEL_2000DPS (0x01 << 4)
#define ICM456XX_GYRO_FS_SEL_1000DPS (0x02 << 4)
#define ICM456XX_GYRO_FS_SEL_500DPS (0x03 << 4)
#define ICM456XX_GYRO_FS_SEL_250DPS (0x04 << 4)
#define ICM456XX_GYRO_FS_SEL_125DPS (0x05 << 4)
#define ICM456XX_GYRO_FS_SEL_62_5DPS (0x06 << 4)
#define ICM456XX_GYRO_FS_SEL_31_25DPS (0x07 << 4)
#define ICM456XX_GYRO_FS_SEL_15_625DPS (0x08 << 4)
// GYRO_CONFIG0 - 0x1C
#define ICM456XX_GYRO_ODR_6K4_LN 0x03
#define ICM456XX_GYRO_ODR_3K2_LN 0x04
#define ICM456XX_GYRO_ODR_1K6_LN 0x05
#define ICM456XX_GYRO_ODR_800_LN 0x06
#define ICM456XX_GYRO_ODR_400_LP_LN 0x07
#define ICM456XX_GYRO_ODR_200_LP_LN 0x08
#define ICM456XX_GYRO_ODR_100_LP_LN 0x09
#define ICM456XX_GYRO_ODR_50_LP_LN 0x0A
#define ICM456XX_GYRO_ODR_25_LP_LN 0x0B
#define ICM456XX_GYRO_ODR_12_5_LP_LN 0x0C
#define ICM456XX_GYRO_ODR_6_25_LP 0x0D
#define ICM456XX_GYRO_ODR_3_125_LP 0x0E
#define ICM456XX_GYRO_ODR_1_5625_LP 0x0F
// Accel IPREG_SYS2_REG_123 - 0x7B
#define ICM456XX_SRC_CTRL_AAF_ENABLE_BIT (1 << 0) // Anti-Alias Filter - AAF
#define ICM456XX_SRC_CTRL_INTERP_ENABLE_BIT (1 << 1) // Interpolator
// IPREG_SYS2_REG_123 - 0x7B
#define ICM456XX_ACCEL_SRC_CTRL_IREG_ADDR 0xA57B // To access register in IPREG_SYS2, add base address 0xA500 + offset
// IPREG_SYS1_REG_166 - 0xA6
#define ICM456XX_GYRO_SRC_CTRL_IREG_ADDR 0xA4A6 // To access register in IPREG_SYS1, add base address 0xA400 + offset
// HOST INDIRECT ACCESS REGISTER (IREG)
#define ICM456XX_REG_IREG_ADDR_15_8 0x7C
#define ICM456XX_REG_IREG_ADDR_7_0 0x7D
#define ICM456XX_REG_IREG_DATA 0x7E
// IPREG_SYS1_REG_172 - 0xAC
#define ICM456XX_GYRO_UI_LPF_CFG_IREG_ADDR 0xA4AC // To access register in IPREG_SYS1, add base address 0xA400 + offset
// LPF UI - 0xAC PREG_SYS1_REG_172 (bits 2:0)
#define ICM456XX_GYRO_UI_LPFBW_BYPASS 0x00
#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_4 0x01 // 1600 Hz ODR = 6400 Hz:
#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_8 0x02 // 800 Hz ODR = 6400 Hz:
#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_16 0x03 // 400 Hz ODR = 6400 Hz:
#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_32 0x04 // 200 Hz ODR = 6400 Hz
#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_64 0x05 // 100 Hz ODR = 6400 Hz
#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_128 0x06 // 50 Hz ODR = 6400 Hz
// IPREG_SYS2_REG_131 - 0x83
#define ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR 0xA583 // To access register in IPREG_SYS2, add base address 0xA500 + offset
// Accel UI path LPF - 0x83 IPREG_SYS2_REG_131 (bits 2:0)
#define ICM456XX_ACCEL_UI_LPFBW_BYPASS 0x00
#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_4 0x01 // 400 Hz ODR = 1600 Hz:
#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8 0x02 // 200 Hz ODR = 1600 Hz:
#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_16 0x03 // 100 Hz ODR = 1600 Hz:
#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_32 0x04 // 50 Hz ODR = 1600 Hz
#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_64 0x05 // 25 Hz ODR = 1600 Hz
#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_128 0x06 // 12.5 Hz ODR = 1600 Hz
#ifndef ICM456XX_CLOCK
// Default: 24 MHz max SPI frequency
#define ICM456XX_MAX_SPI_CLK_HZ 24000000
#else
#define ICM456XX_MAX_SPI_CLK_HZ ICM456XX_CLOCK
#endif
#define HZ_TO_US(hz) ((int32_t)((1000 * 1000) / (hz)))
#define ICM456XX_BIT_IREG_DONE (1 << 0)
#define ICM456XX_DATA_LENGTH 6 // 3 axes * 2 bytes per axis
#define ICM456XX_SPI_BUFFER_SIZE (1 + ICM456XX_DATA_LENGTH) // 1 byte register + 6 bytes data
static uint8_t getGyroLpfConfig(const gyroHardwareLpf_e hardwareLpf)
{
switch (hardwareLpf) {
case GYRO_HARDWARE_LPF_NORMAL:
return ICM456XX_GYRO_UI_LPFBW_ODR_DIV_32;
case GYRO_HARDWARE_LPF_OPTION_1:
return ICM456XX_GYRO_UI_LPFBW_ODR_DIV_16;
case GYRO_HARDWARE_LPF_OPTION_2:
return ICM456XX_GYRO_UI_LPFBW_ODR_DIV_8;
#ifdef USE_GYRO_DLPF_EXPERIMENTAL
case GYRO_HARDWARE_LPF_EXPERIMENTAL:
return ICM456XX_GYRO_UI_LPFBW_ODR_DIV_4;
#endif
default:
return ICM456XX_GYRO_UI_LPFBW_BYPASS;
}
}
/**
* @brief This function follows the IREG WRITE procedure (Section 14.1-14.4 of the datasheet)
* using indirect addressing via IREG_ADDR_15_8, IREG_ADDR_7_0, and IREG_DATA registers.
* After writing, an internal operation transfers the data to the target IREG address.
* Ensures compliance with the required minimum time gap and checks the IREG_DONE bit.
*
* @param dev Pointer to the SPI device structure.
* @param reg 16-bit internal IREG register address.
* @param value Value to be written to the register.
* @return true if the write was successful
*/
static bool icm456xx_write_ireg(const extDevice_t *dev, uint16_t reg, uint8_t value)
{
if (ARMING_FLAG(ARMED)) {
return false; // IREG write not allowed when armed
}
const uint8_t msb = (reg >> 8) & 0xFF;
const uint8_t lsb = reg & 0xFF;
spiWriteReg(dev, ICM456XX_REG_IREG_ADDR_15_8, msb);
spiWriteReg(dev, ICM456XX_REG_IREG_ADDR_7_0, lsb);
spiWriteReg(dev, ICM456XX_REG_IREG_DATA, value);
// Check IREG_DONE (bit 0 of REG_MISC2 = 0x7F)
for (int i = 0; i < 100; i++) {
const uint8_t misc2 = spiReadRegMsk(dev, ICM456XX_REG_MISC2);
if (misc2 & ICM456XX_BIT_IREG_DONE) {
return true;
}
delayMicroseconds(10);
}
return false; // timeout
}
static inline void icm456xx_enableAAFandInterpolator(const extDevice_t *dev, uint16_t reg, bool enableAAF, bool enableInterp)
{
const uint8_t value = (enableAAF ? ICM456XX_SRC_CTRL_AAF_ENABLE_BIT : 0)
| (enableInterp ? ICM456XX_SRC_CTRL_INTERP_ENABLE_BIT : 0);
icm456xx_write_ireg(dev, reg, value);
}
static bool icm456xx_configureLPF(const extDevice_t *dev, uint16_t reg, uint8_t lpfDiv)
{
if (lpfDiv > 0x07) {
return false;
}
return icm456xx_write_ireg(dev, reg, lpfDiv & 0x07);
}
static void icm456xx_enableSensors(const extDevice_t *dev, bool enable)
{
uint8_t value = enable
? (ICM456XX_GYRO_MODE_LN | ICM456XX_ACCEL_MODE_LN)
: (ICM456XX_GYRO_MODE_OFF | ICM456XX_ACCEL_MODE_OFF);
spiWriteReg(dev, ICM456XX_PWR_MGMT0, value);
}
void icm456xxAccInit(accDev_t *acc)
{
const extDevice_t *dev = &acc->gyro->dev;
spiWriteReg(dev, ICM456XX_REG_BANK_SEL, ICM456XX_BANK_0);
switch (acc->mpuDetectionResult.sensor) {
case ICM_45686_SPI:
acc->acc_1G = 1024; // 32g scale = 1024 LSB/g
acc->gyro->accSampleRateHz = 1600;
spiWriteReg(dev, ICM456XX_ACCEL_CONFIG0, ICM456XX_ACCEL_FS_SEL_32G | ICM456XX_ACCEL_ODR_1K6_LN);
break;
case ICM_45605_SPI:
default:
acc->acc_1G = 2048; // 16g scale = 2048 LSB/g
acc->gyro->accSampleRateHz = 1600;
spiWriteReg(dev, ICM456XX_ACCEL_CONFIG0, ICM456XX_ACCEL_FS_SEL_16G | ICM456XX_ACCEL_ODR_1K6_LN);
break;
}
// Enable Anti-Alias (AAF) Filter and Interpolator for Accel
icm456xx_enableAAFandInterpolator(dev, ICM456XX_ACCEL_SRC_CTRL_IREG_ADDR, true, true);
// Set the Accel UI LPF bandwidth cut-off
icm456xx_configureLPF(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8);
}
void icm456xxGyroInit(gyroDev_t *gyro)
{
const extDevice_t *dev = &gyro->dev;
spiSetClkDivisor(dev, spiCalculateDivider(ICM456XX_MAX_SPI_CLK_HZ));
mpuGyroInit(gyro);
spiWriteReg(dev, ICM456XX_REG_BANK_SEL, ICM456XX_BANK_0);
icm456xx_enableSensors(dev, true);
// Enable Anti-Alias (AAF) Filter and Interpolator for Gyro
icm456xx_enableAAFandInterpolator(dev, ICM456XX_GYRO_SRC_CTRL_IREG_ADDR, true, true);
// Set the Gyro UI LPF bandwidth cut-off
icm456xx_configureLPF(dev, ICM456XX_GYRO_UI_LPF_CFG_IREG_ADDR, getGyroLpfConfig(gyroConfig()->gyro_hardware_lpf));
switch (gyro->mpuDetectionResult.sensor) {
case ICM_45686_SPI:
case ICM_45605_SPI:
default:
gyro->scale = GYRO_SCALE_2000DPS;
gyro->gyroRateKHz = GYRO_RATE_6400_Hz;
gyro->gyroSampleRateHz = 6400;
spiWriteReg(dev, ICM456XX_GYRO_CONFIG0, ICM456XX_GYRO_FS_SEL_2000DPS | ICM456XX_GYRO_ODR_6K4_LN);
break;
}
gyro->gyroShortPeriod = clockMicrosToCycles(HZ_TO_US(gyro->gyroSampleRateHz));
spiWriteReg(dev, ICM456XX_INT1_CONFIG2, ICM456XX_INT1_MODE_PULSED | ICM456XX_INT1_DRIVE_CIRCUIT_PP |
ICM456XX_INT1_POLARITY_ACTIVE_HIGH);
spiWriteReg(dev, ICM456XX_INT1_CONFIG0, ICM456XX_INT1_STATUS_EN_DRDY);
}
uint8_t icm456xxSpiDetect(const extDevice_t *dev)
{
uint8_t icmDetected = MPU_NONE;
uint8_t attemptsRemaining = 20;
uint32_t waited_us = 0;
spiWriteReg(dev, ICM456XX_REG_BANK_SEL, ICM456XX_BANK_0);
// Soft reset
spiWriteReg(dev, ICM456XX_REG_MISC2, ICM456XX_SOFT_RESET);
// Wait for reset to complete (bit 1 of REG_MISC2 becomes 0)
while ((spiReadRegMsk(dev, ICM456XX_REG_MISC2) & ICM456XX_SOFT_RESET) != 0) {
delayMicroseconds(10);
waited_us += 10;
if (waited_us >= ICM456XX_RESET_TIMEOUT_US) {
return MPU_NONE;
}
}
do {
const uint8_t whoAmI = spiReadRegMsk(dev, ICM456XX_WHO_AM_REGISTER);
switch (whoAmI) {
case ICM45686_WHO_AM_I_CONST:
icmDetected = ICM_45686_SPI;
break;
case ICM45605_WHO_AM_I_CONST:
icmDetected = ICM_45605_SPI;
break;
default:
icmDetected = MPU_NONE;
break;
}
} while (icmDetected == MPU_NONE && attemptsRemaining--);
return icmDetected;
}
bool icm456xxAccReadSPI(accDev_t *acc)
{
switch (acc->gyro->gyroModeSPI) {
case GYRO_EXTI_INT:
case GYRO_EXTI_NO_INT:
{
#ifdef USE_DMA
if (spiUseDMA(&acc->gyro->dev)) {
acc->gyro->dev.txBuf[0] = ICM456XX_ACCEL_DATA_X1_UI | 0x80;
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, ICM456XX_SPI_BUFFER_SIZE, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
memset(&acc->gyro->dev.txBuf[1], 0xFF, 6);
segments[0].u.buffers.txData = acc->gyro->dev.txBuf;
segments[0].u.buffers.rxData = &acc->gyro->dev.rxBuf[1];
spiSequence(&acc->gyro->dev, &segments[0]);
// Wait for completion
spiWait(&acc->gyro->dev);
} else
#else
{
// Interrupts are present, but no DMA. Non-DMA read
uint8_t raw[ICM456XX_DATA_LENGTH];
const bool ack = spiReadRegMskBufRB(&acc->gyro->dev, ICM456XX_ACCEL_DATA_X1_UI, raw, ICM456XX_DATA_LENGTH);
if (!ack) {
return false;
}
acc->ADCRaw[X] = (int16_t)((raw[1] << 8) | raw[0]);
acc->ADCRaw[Y] = (int16_t)((raw[3] << 8) | raw[2]);
acc->ADCRaw[Z] = (int16_t)((raw[5] << 8) | raw[4]);
}
#endif
break;
}
case GYRO_EXTI_INT_DMA:
{
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
// up an old value.
// This data was read from the gyro, which is the same SPI device as the acc
acc->ADCRaw[X] = (int16_t)((acc->gyro->dev.rxBuf[2] << 8) | acc->gyro->dev.rxBuf[1]);
acc->ADCRaw[Y] = (int16_t)((acc->gyro->dev.rxBuf[4] << 8) | acc->gyro->dev.rxBuf[3]);
acc->ADCRaw[Z] = (int16_t)((acc->gyro->dev.rxBuf[6] << 8) | acc->gyro->dev.rxBuf[5]);
break;
}
default:
break;
}
return true;
}
bool icm456xxSpiAccDetect(accDev_t *acc)
{
switch (acc->mpuDetectionResult.sensor) {
case ICM_45686_SPI:
case ICM_45605_SPI:
acc->initFn = icm456xxAccInit;
acc->readFn = icm456xxAccReadSPI;
break;
default:
return false;
}
return true;
}
bool icm456xxGyroReadSPI(gyroDev_t *gyro)
{
switch (gyro->gyroModeSPI) {
case GYRO_EXTI_INIT:
{
// Initialise the tx buffer to all 0xff
memset(gyro->dev.txBuf, 0xff, ICM456XX_SPI_BUFFER_SIZE);
gyro->gyroDmaMaxDuration = 0; // INT gyroscope always calls that data is ready. We can read immediately
#ifdef USE_DMA
if (spiUseDMA(&gyro->dev)) {
gyro->dev.callbackArg = (uint32_t)gyro;
gyro->dev.txBuf[0] = ICM456XX_GYRO_DATA_X1_UI | 0x80;
gyro->segments[0].len = ICM456XX_SPI_BUFFER_SIZE;
gyro->segments[0].callback = mpuIntCallback;
gyro->segments[0].u.buffers.txData = gyro->dev.txBuf;
gyro->segments[0].u.buffers.rxData = gyro->dev.rxBuf;
gyro->segments[0].negateCS = true;
gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
} else
#endif
{
// Interrupts are present, but no DMA. Non-DMA read
uint8_t raw[ICM456XX_DATA_LENGTH];
const bool ack = spiReadRegMskBufRB(&gyro->dev, ICM456XX_GYRO_DATA_X1_UI, raw, ICM456XX_DATA_LENGTH);
if (!ack) {
return false;
}
gyro->gyroADCRaw[X] = (int16_t)((raw[1] << 8) | raw[0]);
gyro->gyroADCRaw[Y] = (int16_t)((raw[3] << 8) | raw[2]);
gyro->gyroADCRaw[Z] = (int16_t)((raw[5] << 8) | raw[4]);
gyro->gyroModeSPI = GYRO_EXTI_INT;
}
break;
}
case GYRO_EXTI_NO_INT:
{
gyro->dev.txBuf[0] = ICM456XX_GYRO_DATA_X1_UI | 0x80;
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, ICM456XX_SPI_BUFFER_SIZE, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
memset(&gyro->dev.txBuf[1], 0xFF, 6);
segments[0].u.buffers.txData = gyro->dev.txBuf;
segments[0].u.buffers.rxData = gyro->dev.rxBuf;
spiSequence(&gyro->dev, &segments[0]);
// Wait for completion
spiWait(&gyro->dev);
gyro->gyroADCRaw[X] = (int16_t)((gyro->dev.rxBuf[2] << 8) | gyro->dev.rxBuf[1]);
gyro->gyroADCRaw[Y] = (int16_t)((gyro->dev.rxBuf[4] << 8) | gyro->dev.rxBuf[3]);
gyro->gyroADCRaw[Z] = (int16_t)((gyro->dev.rxBuf[6] << 8) | gyro->dev.rxBuf[5]);
break;
}
case GYRO_EXTI_INT_DMA:
{
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
// up an old value.
gyro->gyroADCRaw[X] = (int16_t)((gyro->dev.rxBuf[2] << 8) | gyro->dev.rxBuf[1]);
gyro->gyroADCRaw[Y] = (int16_t)((gyro->dev.rxBuf[4] << 8) | gyro->dev.rxBuf[3]);
gyro->gyroADCRaw[Z] = (int16_t)((gyro->dev.rxBuf[6] << 8) | gyro->dev.rxBuf[5]);
break;
}
default:
break;
}
return true;
}
bool icm456xxSpiGyroDetect(gyroDev_t *gyro)
{
switch (gyro->mpuDetectionResult.sensor) {
case ICM_45686_SPI:
case ICM_45605_SPI:
gyro->initFn = icm456xxGyroInit;
gyro->readFn = icm456xxGyroReadSPI;
break;
default:
return false;
}
return true;
}
#endif // USE_ACCGYRO_ICM45686 || USE_ACCGYRO_ICM45605

32
src/main/drivers/accgyro/accgyro_spi_icm456xx.h Normal file
View file

@ -0,0 +1,32 @@
/*
* This file is part of Betaflight.
*
* Betaflight is free software. You can redistribute this software
* and/or modify this software under the terms of the GNU General
* Public License as published by the Free Software Foundation,
* either version 3 of the License, or (at your option) any later
* version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/bus.h"
void icm456xxAccInit(accDev_t *acc);
void icm456xxGyroInit(gyroDev_t *gyro);
uint8_t icm456xxSpiDetect(const extDevice_t *dev);
bool icm456xxSpiAccDetect(accDev_t *acc);
bool icm456xxSpiGyroDetect(gyroDev_t *gyro);

6
src/main/drivers/accgyro/gyro_sync.c
View file

@ -83,6 +83,12 @@ uint16_t gyroSetSampleRate(gyroDev_t *gyro)
accSampleRateHz = 833;
break;
#endif
case ICM_45686_SPI:
case ICM_45605_SPI:
gyro->gyroRateKHz = GYRO_RATE_6400_Hz;
gyroSampleRateHz = 6400;
accSampleRateHz = 1600;
break;
default:
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
gyroSampleRateHz = 8000;

15
src/main/drivers/dshot_command.c
View file

@ -149,7 +149,20 @@ static bool allMotorsAreIdle(void)
bool dshotStreamingCommandsAreEnabled(void)
{
return motorIsEnabled() && motorGetMotorEnableTimeMs() && millis() > motorGetMotorEnableTimeMs() + DSHOT_PROTOCOL_DETECTION_DELAY_MS;
static bool firstCommand = true;
bool goodMotorDetectDelay;
if (firstCommand) {
goodMotorDetectDelay = motorGetMotorEnableTimeMs() && (cmpTimeMs(millis(), motorGetMotorEnableTimeMs()) > DSHOT_PROTOCOL_DETECTION_DELAY_MS);
if (goodMotorDetectDelay) {
firstCommand = false;
}
} else {
goodMotorDetectDelay = true;
}
return motorIsEnabled() && goodMotorDetectDelay;
}
static bool dshotCommandsAreEnabled(dshotCommandType_e commandType)

38
src/main/drivers/io.c
View file

@ -26,10 +26,7 @@
#include "common/utils.h"
// io ports defs are stored in array by index now
struct ioPortDef_s {
rccPeriphTag_t rcc;
};
// io ports defs are stored by index in array ioRecs of ioRec_t
ioRec_t* IO_Rec(IO_t io)
{
@ -48,39 +45,6 @@ uint16_t IO_Pin(IO_t io)
return ioRec->pin;
}
#if defined(STM32F4) || defined(APM32F4)
int IO_EXTI_PortSourceGPIO(IO_t io)
{
return IO_GPIOPortIdx(io);
}
#endif
int IO_GPIO_PortSource(IO_t io)
{
return IO_GPIOPortIdx(io);
}
// zero based pin index
int IO_GPIOPinIdx(IO_t io)
{
if (!io) {
return -1;
}
return 31 - __builtin_clz(IO_Pin(io));
}
#if defined(STM32F4) || defined(APM32F4)
int IO_EXTI_PinSource(IO_t io)
{
return IO_GPIOPinIdx(io);
}
#endif
int IO_GPIO_PinSource(IO_t io)
{
return IO_GPIOPinIdx(io);
}
// claim IO pin, set owner and resources
void IOInit(IO_t io, resourceOwner_e owner, uint8_t index)
{

19
src/main/drivers/io_def.h
View file

@ -41,11 +41,24 @@
// get ioRec by index
#define DEFIO_REC_INDEXED(idx) (ioRecs + (idx))
// split ioTag bits between pin and port
// port is encoded as +1 to avoid collision with 0x0 (false as bool)
#ifndef DEFIO_PORT_PINS
// pins per port
#define DEFIO_PORT_PINS 16
#endif
STATIC_ASSERT((DEFIO_PORT_PINS & (DEFIO_PORT_PINS - 1)) == 0, "DEFIO_PORT_PINS must be power of 2");
#define DEFIO_PORT_BITSHIFT LOG2(DEFIO_PORT_PINS)
#define DEFIO_PIN_BITMASK ((1 << DEFIO_PORT_BITSHIFT ) - 1)
// ioTag_t accessor macros
#define DEFIO_TAG_MAKE(gpioid, pin) ((ioTag_t)((((gpioid) + 1) << 4) | (pin)))
#define DEFIO_TAG_MAKE(gpioid, pin) ((ioTag_t)((((gpioid) + 1) << DEFIO_PORT_BITSHIFT) | (pin)))
#define DEFIO_TAG_ISEMPTY(tag) (!(tag))
#define DEFIO_TAG_GPIOID(tag) (((tag) >> 4) - 1)
#define DEFIO_TAG_PIN(tag) ((tag) & 0x0f)
#define DEFIO_TAG_GPIOID(tag) (((tag) >> DEFIO_PORT_BITSHIFT) - 1)
#define DEFIO_TAG_PIN(tag) ((tag) & DEFIO_PIN_BITMASK)
// TARGET must define used pins
#include "target.h"

2
src/main/drivers/nvic.h
View file

@ -32,7 +32,7 @@
#define NVIC_PRIO_RX_INT_EXTI NVIC_BUILD_PRIORITY(3, 0x0f)
#define NVIC_PRIO_RX_BUSY_EXTI NVIC_BUILD_PRIORITY(3, 0x0f)
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0, 0) // This must be high priority as it drives the scheduler timing
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority?
#define NVIC_PRIO_SERIALUART_TXDMA NVIC_BUILD_PRIORITY(1, 1) // Highest of all SERIALUARTx_TXDMA

72
src/main/drivers/rangefinder/rangefinder_lidartf.c
View file

@ -37,6 +37,7 @@
#define TF_DEVTYPE_NONE 0
#define TF_DEVTYPE_MINI 1
#define TF_DEVTYPE_02 2
#define TF_DEVTYPE_NOVA 3
static uint8_t tfDevtype = TF_DEVTYPE_NONE;
@ -81,6 +82,25 @@ static uint8_t tfDevtype = TF_DEVTYPE_NONE;
//
#define TF_02_FRAME_SIG 4
//
// Benewake TFnova frame format
// Byte Off Description
// 1 - SYNC
// 2 - SYNC
// 3 0 Measured distance (LSB)
// 4 1 Measured distance (MSB)
// 5 2 Signal strength (LSB)
// 6 3 Signal strength (MSB)
// 7 4 Temp (Chip Temperature, degrees Celsius)
// 8 5 Confidence (Confidence level 0-100)
// 9 - Checksum (Unsigned 8-bit sum of bytes 0~7)
//
// Credibility
// 1. If Confidence level < 90, unreliable
// 2. If distance is 14m (1400cm), then OoR.
//
#define TF_NOVA_FRAME_CONFIDENCE 5
// Maximum ratings
#define TF_MINI_RANGE_MIN 40
@ -89,6 +109,9 @@ static uint8_t tfDevtype = TF_DEVTYPE_NONE;
#define TF_02_RANGE_MIN 40
#define TF_02_RANGE_MAX 2200
#define TF_NOVA_RANGE_MIN 10
#define TF_NOVA_RANGE_MAX 1400
#define TF_DETECTION_CONE_DECIDEGREES 900
static serialPort_t *tfSerialPort = NULL;
@ -104,14 +127,10 @@ static tfFrameState_e tfFrameState;
static uint8_t tfFrame[TF_FRAME_LENGTH];
static uint8_t tfReceivePosition;
// TFmini
// TFmini and TF02
// Command for 100Hz sampling (10msec interval)
// At 100Hz scheduling, skew will cause 10msec delay at the most.
static uint8_t tfCmdTFmini[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
// TF02
// Same as TFmini for now..
static uint8_t tfCmdTF02[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
static const uint8_t tfCmd[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
static int32_t lidarTFValue;
static uint16_t lidarTFerrors = 0;
@ -120,11 +139,12 @@ static void lidarTFSendCommand(void)
{
switch (tfDevtype) {
case TF_DEVTYPE_MINI:
serialWriteBuf(tfSerialPort, tfCmdTFmini, sizeof(tfCmdTFmini));
break;
case TF_DEVTYPE_02:
serialWriteBuf(tfSerialPort, tfCmdTF02, sizeof(tfCmdTF02));
serialWriteBuf(tfSerialPort, tfCmd, sizeof(tfCmd));
break;
default:
break;
}
}
@ -173,7 +193,7 @@ static void lidarTFUpdate(rangefinderDev_t *dev)
case TF_FRAME_STATE_WAIT_CKSUM:
{
uint8_t cksum = TF_FRAME_SYNC_BYTE + TF_FRAME_SYNC_BYTE;
for (int i = 0 ; i < TF_FRAME_LENGTH ; i++) {
for (int i = 0; i < TF_FRAME_LENGTH; i++) {
cksum += tfFrame[i];
}
@ -192,7 +212,7 @@ static void lidarTFUpdate(rangefinderDev_t *dev)
if (distance >= TF_MINI_RANGE_MIN && distance < TF_MINI_RANGE_MAX) {
lidarTFValue = distance;
if (tfFrame[TF_MINI_FRAME_INTEGRAL_TIME] == 7) {
// When integral time is long (7), measured distance tends to be longer by 12~13.
// When integral time is long (7), measured distance tends to be longer by 12~13.
lidarTFValue -= 13;
}
} else {
@ -207,6 +227,14 @@ static void lidarTFUpdate(rangefinderDev_t *dev)
lidarTFValue = -1;
}
break;
case TF_DEVTYPE_NOVA:
if (distance >= TF_NOVA_RANGE_MIN && distance <= TF_NOVA_RANGE_MAX && tfFrame[TF_NOVA_FRAME_CONFIDENCE] >= 90) {
lidarTFValue = distance;
} else {
lidarTFValue = -1;
}
break;
}
lastFrameReceivedMs = timeNowMs;
} else {
@ -256,7 +284,21 @@ static bool lidarTFDetect(rangefinderDev_t *dev, uint8_t devtype)
tfDevtype = devtype;
dev->delayMs = 10;
dev->maxRangeCm = (devtype == TF_DEVTYPE_MINI) ? TF_MINI_RANGE_MAX : TF_02_RANGE_MAX;
switch (devtype) {
case TF_DEVTYPE_MINI:
dev->maxRangeCm = TF_MINI_RANGE_MAX;
break;
case TF_DEVTYPE_02:
dev->maxRangeCm = TF_02_RANGE_MAX;
break;
case TF_DEVTYPE_NOVA:
dev->maxRangeCm = TF_NOVA_RANGE_MAX;
break;
default:
dev->maxRangeCm = 0;
break;
}
dev->detectionConeDeciDegrees = TF_DETECTION_CONE_DECIDEGREES;
dev->detectionConeExtendedDeciDegrees = TF_DETECTION_CONE_DECIDEGREES;
@ -276,4 +318,10 @@ bool lidarTF02Detect(rangefinderDev_t *dev)
{
return lidarTFDetect(dev, TF_DEVTYPE_02);
}
bool lidarTFNovaDetect(rangefinderDev_t *dev)
{
return lidarTFDetect(dev, TF_DEVTYPE_NOVA);
}
#endif

1
src/main/drivers/rangefinder/rangefinder_lidartf.h
View file

@ -26,3 +26,4 @@
bool lidarTFminiDetect(rangefinderDev_t *dev);
bool lidarTF02Detect(rangefinderDev_t *dev);
bool lidarTFNovaDetect(rangefinderDev_t *dev);

14
src/main/fc/core.c
View file

@ -558,18 +558,8 @@ void tryArm(void)
}
#endif
// choose crashflip outcome on arming
// disarm can arise in processRx() if the crashflip switch is reversed while in crashflip mode
// if we were unsuccessful, or cannot determin success, arming will be blocked and we can't get here
// hence we only get here with crashFlipModeActive if the switch was reversed and result successful
if (crashFlipModeActive) {
// flip was successful, continue into normal flight without need to disarm/rearm
// note: preceding disarm will have set motors to normal rotation direction
crashFlipModeActive = false;
} else {
// when arming and not in crashflip mode, block entry to crashflip if delayed by the dshot beeper,
// otherwise consider only the switch position
crashFlipModeActive = (tryingToArm == ARMING_DELAYED_CRASHFLIP) ? false : IS_RC_MODE_ACTIVE(BOXCRASHFLIP);
}
// consider only the switch position
crashFlipModeActive = IS_RC_MODE_ACTIVE(BOXCRASHFLIP);
setMotorSpinDirection(crashFlipModeActive ? DSHOT_CMD_SPIN_DIRECTION_REVERSED : DSHOT_CMD_SPIN_DIRECTION_NORMAL);
}

1
src/main/osd/osd.h
View file

@ -194,6 +194,7 @@ typedef enum {
OSD_CUSTOM_MSG1,
OSD_CUSTOM_MSG2,
OSD_CUSTOM_MSG3,
OSD_LIDAR_DIST,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;

29
src/main/osd/osd_elements.c
View file

@ -50,6 +50,8 @@
Add the mapping for the element ID to the background drawing function to the
osdElementBackgroundFunction array.
You should also add a corresponding entry to the file: cms_menu_osd.c
Accelerometer reqirement:
-------------------------
@ -170,6 +172,7 @@
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "sensors/rangefinder.h"
#ifdef USE_GPS_PLUS_CODES
// located in lib/main/google/olc
@ -320,7 +323,6 @@ int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius)
}
}
#endif
static void osdFormatAltitudeString(char * buff, int32_t altitudeCm, osdElementType_e variantType)
{
static const struct {
@ -682,6 +684,22 @@ char osdGetTemperatureSymbolForSelectedUnit(void)
// Element drawing functions
// *************************
#ifdef USE_RANGEFINDER
static void osdElementLidarDist(osdElementParms_t *element)
{
int16_t dist = rangefinderGetLatestAltitude();
if (dist > 0) {
tfp_sprintf(element->buff, "RF:%3d", dist);
} else {
tfp_sprintf(element->buff, "RF:---");
}
}
#endif
#ifdef USE_OSD_ADJUSTMENTS
static void osdElementAdjustmentRange(osdElementParms_t *element)
{
@ -1918,6 +1936,9 @@ static const uint8_t osdElementDisplayOrder[] = {
OSD_SYS_VTX_TEMP,
OSD_SYS_FAN_SPEED,
#endif
#ifdef USE_RANGEFINDER
OSD_LIDAR_DIST,
#endif
};
// Define the mapping between the OSD element id and the function to draw it
@ -2062,6 +2083,9 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
[OSD_SYS_VTX_TEMP] = osdElementSys,
[OSD_SYS_FAN_SPEED] = osdElementSys,
#endif
#ifdef USE_RANGEFINDER
[OSD_LIDAR_DIST] = osdElementLidarDist,
#endif
};
// Define the mapping between the OSD element id and the function to draw its background (static part)
@ -2508,7 +2532,7 @@ void osdUpdateAlarms(void)
#endif
#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
bool blink;
bool blink = false;
#if defined(USE_ESC_SENSOR)
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
@ -2518,7 +2542,6 @@ void osdUpdateAlarms(void)
#endif
#if defined(USE_DSHOT_TELEMETRY)
{
blink = false;
if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF) {
for (uint32_t k = 0; !blink && (k < getMotorCount()); k++) {
blink = (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0 &&

3
src/main/sensors/acceleration.h
View file

@ -51,6 +51,9 @@ typedef enum {
ACC_LSM6DSO,
ACC_LSM6DSV16X,
ACC_IIM42653,
ACC_ICM45605,
ACC_ICM45686,
ACC_ICM40609D,
ACC_VIRTUAL
} accelerationSensor_e;

31
src/main/sensors/acceleration_init.c
View file

@ -47,6 +47,8 @@
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_icm456xx.h"
#include "drivers/accgyro/accgyro_spi_icm40609.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_lsm6dsv16x.h"
@ -240,6 +242,26 @@ retry:
FALLTHROUGH;
#endif
#if defined(USE_ACCGYRO_ICM45686) || defined(USE_ACCGYRO_ICM45605)
case ACC_ICM45686:
case ACC_ICM45605:
if (icm456xxSpiAccDetect(dev)) {
switch (dev->mpuDetectionResult.sensor) {
case ICM_45686_SPI:
accHardware = ACC_ICM45686;
break;
case ICM_45605_SPI:
accHardware = ACC_ICM45605;
break;
default:
accHardware = ACC_NONE;
break;
}
break;
}
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_BMI160
case ACC_BMI160:
if (bmi160SpiAccDetect(dev)) {
@ -276,6 +298,15 @@ retry:
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_ICM40609D
case ACC_ICM40609D:
if (icm40609SpiAccDetect(dev)) {
accHardware = ACC_ICM40609D;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_VIRTUAL_ACC
case ACC_VIRTUAL:
if (virtualAccDetect(dev)) {

35
src/main/sensors/gyro_init.c
View file

@ -47,6 +47,8 @@
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_icm456xx.h"
#include "drivers/accgyro/accgyro_spi_icm40609.h"
#include "drivers/accgyro/accgyro_spi_l3gd20.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
@ -318,6 +320,8 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
case GYRO_ICM42688P:
case GYRO_IIM42653:
case GYRO_ICM42605:
case GYRO_ICM45686:
case GYRO_ICM45605:
gyroSensor->gyroDev.gyroHasOverflowProtection = true;
break;
@ -454,6 +458,27 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH;
#endif
#if defined(USE_ACCGYRO_ICM45686) || defined(USE_ACCGYRO_ICM45605)
case GYRO_ICM45686:
case GYRO_ICM45605:
if (icm456xxSpiGyroDetect(dev)) {
switch (dev->mpuDetectionResult.sensor) {
case ICM_45686_SPI:
gyroHardware = GYRO_ICM45686;
break;
case ICM_45605_SPI:
gyroHardware = GYRO_ICM45605;
break;
default:
gyroHardware = GYRO_NONE;
break;
}
break;
}
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_BMI160
case GYRO_BMI160:
if (bmi160SpiGyroDetect(dev)) {
@ -490,6 +515,16 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_ICM40609D
case GYRO_ICM40609D:
if (icm40609SpiGyroDetect(dev)) {
gyroHardware = GYRO_ICM40609D;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_VIRTUAL_GYRO
case GYRO_VIRTUAL:
if (virtualGyroDetect(dev)) {

9
src/main/sensors/rangefinder.c
View file

@ -127,6 +127,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
#endif
break;
case RANGEFINDER_TFNOVA:
#if defined(USE_RANGEFINDER_TF)
if (lidarTFNovaDetect(dev)) {
rangefinderHardware = RANGEFINDER_TFNOVA;
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS));
}
#endif
break;
#if defined(USE_RANGEFINDER_MT)
case RANGEFINDER_MTF01:
case RANGEFINDER_MTF02:

1
src/main/sensors/rangefinder.h
View file

@ -35,6 +35,7 @@ typedef enum {
RANGEFINDER_MTF02 = 5,
RANGEFINDER_MTF01P = 6,
RANGEFINDER_MTF02P = 7,
RANGEFINDER_TFNOVA = 8,
} rangefinderType_e;
typedef struct rangefinderConfig_s {

11
src/main/target/common_post.h
View file

@ -105,7 +105,10 @@
&& !defined(USE_ACC_SPI_ICM20649) \
&& !defined(USE_ACC_SPI_ICM20689) \
&& !defined(USE_ACC_SPI_ICM42605) \
&& !defined(USE_ACCGYRO_ICM40609D) \
&& !defined(USE_ACC_SPI_ICM42688P) \
&& !defined(USE_ACCGYRO_ICM45686) \
&& !defined(USE_ACCGYRO_ICM45605) \
&& !defined(USE_ACCGYRO_LSM6DSO) \
&& !defined(USE_ACCGYRO_LSM6DSV16X) \
&& !defined(USE_ACC_SPI_MPU6000) \
@ -126,6 +129,9 @@
&& !defined(USE_GYRO_SPI_ICM20689) \
&& !defined(USE_GYRO_SPI_ICM42605) \
&& !defined(USE_GYRO_SPI_ICM42688P) \
&& !defined(USE_ACCGYRO_ICM45686) \
&& !defined(USE_ACCGYRO_ICM45605) \
&& !defined(USE_ACCGYRO_ICM40609D) \
&& !defined(USE_ACCGYRO_LSM6DSO) \
&& !defined(USE_ACCGYRO_LSM6DSV16X) \
&& !defined(USE_GYRO_SPI_MPU6000) \
@ -472,8 +478,9 @@
// Generate USE_SPI_GYRO or USE_I2C_GYRO
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) \
|| defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653) \
|| defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSV16X) || defined(USE_ACCGYRO_LSM6DSO)
|| defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_ICM45686) \
|| defined(USE_ACCGYRO_ICM45605) || defined(USE_ACCGYRO_IIM42653) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) \
|| defined(USE_ACCGYRO_LSM6DSV16X) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_ACCGYRO_ICM40609D)
#ifndef USE_SPI_GYRO
#define USE_SPI_GYRO
#endif

3
src/main/target/common_pre.h
View file

@ -114,10 +114,13 @@
#define USE_ACCGYRO_BMI270
#define USE_GYRO_SPI_ICM42605
#define USE_GYRO_SPI_ICM42688P
#define USE_ACCGYRO_ICM45686
#define USE_ACCGYRO_ICM45605
#define USE_ACCGYRO_IIM42653
#define USE_ACC_SPI_ICM42605
#define USE_ACC_SPI_ICM42688P
#define USE_ACCGYRO_LSM6DSV16X
#define USE_ACCGYRO_ICM40609D
#if TARGET_FLASH_SIZE > 512
#define USE_ACC_MPU6050

287
src/platform/PICO/pico/platform.h
View file

@ -1,287 +0,0 @@
/*
* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
/** \file platform.h
* \defgroup pico_platform pico_platform
*
* \brief Macros and definitions (and functions when included by non assembly code) for the RP2 family device / architecture
* to provide a common abstraction over low level compiler / platform specifics
*
* This header may be included by assembly code
*/
#ifndef _PICO_PLATFORM_H
#define _PICO_PLATFORM_H
#ifndef _PICO_H
#error pico/platform.h should not be included directly; include pico.h instead
#endif
#include "pico/platform/compiler.h"
#include "pico/platform/sections.h"
#include "pico/platform/panic.h"
#include "hardware/regs/addressmap.h"
#include "hardware/regs/sio.h"
#ifdef __riscv
#include "hardware/regs/rvcsr.h"
#endif
// PICO_CONFIG: PICO_RP2350A, Whether the current board has an RP2350 in an A (30 GPIO) package, type=bool, default=Usually provided via board header, group=pico_platform
#if 0 // make tooling checks happy
#define PICO_RP2350A 0
#endif
// PICO_CONFIG: PICO_RP2350_A2_SUPPORTED, Whether to include any specific software support for RP2350 A2 revision, type=bool, default=1, advanced=true, group=pico_platform
#ifndef PICO_RP2350_A2_SUPPORTED
#define PICO_RP2350_A2_SUPPORTED 1
#endif
// PICO_CONFIG: PICO_STACK_SIZE, Minimum amount of stack space reserved in the linker script for each core. See also PICO_CORE1_STACK_SIZE, min=0x100, default=0x800, advanced=true, group=pico_platform
#ifndef PICO_STACK_SIZE
#define PICO_STACK_SIZE _u(0x800)
#endif
// PICO_CONFIG: PICO_HEAP_SIZE, Minimum amount of heap space reserved by the linker script, min=0x100, default=0x800, advanced=true, group=pico_platform
#ifndef PICO_HEAP_SIZE
#define PICO_HEAP_SIZE _u(0x800)
#endif
// PICO_CONFIG: PICO_NO_RAM_VECTOR_TABLE, Enable/disable the RAM vector table, type=bool, default=0, advanced=true, group=pico_platform
#ifndef PICO_NO_RAM_VECTOR_TABLE
#define PICO_NO_RAM_VECTOR_TABLE 0
#endif
#ifndef PICO_RAM_VECTOR_TABLE_SIZE
#define PICO_RAM_VECTOR_TABLE_SIZE (VTABLE_FIRST_IRQ + NUM_IRQS)
#endif
// PICO_CONFIG: PICO_USE_STACK_GUARDS, Enable/disable stack guards, type=bool, default=0, advanced=true, group=pico_platform
#ifndef PICO_USE_STACK_GUARDS
#define PICO_USE_STACK_GUARDS 0
#endif
// PICO_CONFIG: PICO_CLKDIV_ROUND_NEAREST, True if floating point clock divisors should be rounded to the nearest possible clock divisor by default rather than rounding down, type=bool, default=1, group=pico_platform
#ifndef PICO_CLKDIV_ROUND_NEAREST
#define PICO_CLKDIV_ROUND_NEAREST 1
#endif
#ifndef __ASSEMBLER__
/*! \brief No-op function for the body of tight loops
* \ingroup pico_platform
*
* No-op function intended to be called by any tight hardware polling loop. Using this ubiquitously
* makes it much easier to find tight loops, but also in the future \#ifdef-ed support for lockup
* debugging might be added
*/
static __force_inline void tight_loop_contents(void) {}
/*! \brief Helper method to busy-wait for at least the given number of cycles
* \ingroup pico_platform
*
* This method is useful for introducing very short delays.
*
* This method busy-waits in a tight loop for the given number of system clock cycles. The total wait time is only accurate to within 2 cycles,
* and this method uses a loop counter rather than a hardware timer, so the method will always take longer than expected if an
* interrupt is handled on the calling core during the busy-wait; you can of course disable interrupts to prevent this.
*
* You can use \ref clock_get_hz(clk_sys) to determine the number of clock cycles per second if you want to convert an actual
* time duration to a number of cycles.
*
* \param minimum_cycles the minimum number of system clock cycles to delay for
*/
static inline void busy_wait_at_least_cycles(uint32_t minimum_cycles) {
pico_default_asm_volatile (
#ifdef __riscv
// Note the range is halved on RISC-V due to signed comparison (no carry flag)
".option push\n"
".option norvc\n" // force 32 bit addi, so branch prediction guaranteed
".p2align 2\n"
"1: \n"
"addi %0, %0, -2 \n"
"bgez %0, 1b\n"
".option pop"
#else
"1: subs %0, #3\n"
"bcs 1b\n"
#endif
: "+r" (minimum_cycles) : : "cc", "memory"
);
}
// PICO_CONFIG: PICO_NO_FPGA_CHECK, Remove the FPGA platform check for small code size reduction, type=bool, default=1, advanced=true, group=pico_runtime
#ifndef PICO_NO_FPGA_CHECK
#define PICO_NO_FPGA_CHECK 1
#endif
// PICO_CONFIG: PICO_NO_SIM_CHECK, Remove the SIM platform check for small code size reduction, type=bool, default=1, advanced=true, group=pico_runtime
#ifndef PICO_NO_SIM_CHECK
#define PICO_NO_SIM_CHECK 1
#endif
#if PICO_NO_FPGA_CHECK
static inline bool running_on_fpga(void) {return false;}
#else
bool running_on_fpga(void);
#endif
#if PICO_NO_SIM_CHECK
static inline bool running_in_sim(void) {return false;}
#else
bool running_in_sim(void);
#endif
/*! \brief Execute a breakpoint instruction
* \ingroup pico_platform
*/
static __force_inline void __breakpoint(void) {
#ifdef __riscv
__asm ("ebreak");
#else
pico_default_asm_volatile ("bkpt #0" : : : "memory");
#endif
}
/*! \brief Get the current core number
* \ingroup pico_platform
*
* \return The core number the call was made from
*/
__force_inline static uint get_core_num(void) {
return (*(uint32_t *) (SIO_BASE + SIO_CPUID_OFFSET));
}
/*! \brief Get the current exception level on this core
* \ingroup pico_platform
*
* On Cortex-M this is the exception number defined in the architecture
* reference, which is equal to VTABLE_FIRST_IRQ + irq num if inside an
* interrupt handler. (VTABLE_FIRST_IRQ is defined in platform_defs.h).
*
* On Hazard3, this function returns VTABLE_FIRST_IRQ + irq num if inside of
* an external IRQ handler (or a fault from such a handler), and 0 otherwise,
* generally aligning with the Cortex-M values.
*
* \return the exception number if the CPU is handling an exception, or 0 otherwise
*/
static __force_inline uint __get_current_exception(void) {
#ifdef __riscv
uint32_t meicontext;
pico_default_asm_volatile (
"csrr %0, %1\n"
: "=r" (meicontext) : "i" (RVCSR_MEICONTEXT_OFFSET)
);
if (meicontext & RVCSR_MEICONTEXT_NOIRQ_BITS) {
return 0;
} else {
return VTABLE_FIRST_IRQ + (
(meicontext & RVCSR_MEICONTEXT_IRQ_BITS) >> RVCSR_MEICONTEXT_IRQ_LSB
);
}
#else
uint exception;
pico_default_asm_volatile (
"mrs %0, ipsr\n"
"uxtb %0, %0\n"
: "=l" (exception)
);
return exception;
#endif
}
/*! \brief Return true if executing in the NonSecure state (Arm-only)
* \ingroup pico_platform
*
* \return True if currently executing in the NonSecure state on an Arm processor
*/
__force_inline static bool pico_processor_state_is_nonsecure(void) {
#ifndef __riscv
// todo add a define to disable NS checking at all?
// IDAU-Exempt addresses return S=1 when tested in the Secure state,
// whereas executing a tt in the NonSecure state will always return S=0.
uint32_t tt;
pico_default_asm_volatile (
"movs %0, #0\n"
"tt %0, %0\n"
: "=r" (tt) : : "cc"
);
return !(tt & (1u << 22));
#else
// NonSecure is an Arm concept, there is nothing meaningful to return
// here. Note it's not possible in general to detect whether you are
// executing in U-mode as, for example, M-mode is classically
// virtualisable in U-mode.
return false;
#endif
}
#define host_safe_hw_ptr(x) ((uintptr_t)(x))
#define native_safe_hw_ptr(x) host_safe_hw_ptr(x)
/*! \brief Returns the RP2350 chip revision number
* \ingroup pico_platform
* @return the RP2350 chip revision number (1 for B0/B1, 2 for B2)
*/
uint8_t rp2350_chip_version(void);
/*! \brief Returns the RP2040 chip revision number for compatibility
* \ingroup pico_platform
* @return 2 RP2040 errata fixed in B2 are fixed in RP2350
*/
static inline uint8_t rp2040_chip_version(void) {
return 2;
}
/*! \brief Returns the RP2040 rom version number
* \ingroup pico_platform
* @return the RP2040 rom version number (1 for RP2040-B0, 2 for RP2040-B1, 3 for RP2040-B2)
*/
static inline uint8_t rp2040_rom_version(void) {
GCC_Pragma("GCC diagnostic push")
GCC_Pragma("GCC diagnostic ignored \"-Warray-bounds\"")
return *(uint8_t*)0x13;
GCC_Pragma("GCC diagnostic pop")
}
/*! \brief Multiply two integers using an assembly `MUL` instruction
* \ingroup pico_platform
*
* This multiplies a by b using multiply instruction using the ARM mul instruction regardless of values (the compiler
* might otherwise choose to perform shifts/adds), i.e. this is a 1 cycle operation.
*
* \param a the first operand
* \param b the second operand
* \return a * b
*/
__force_inline static int32_t __mul_instruction(int32_t a, int32_t b) {
#ifdef __riscv
__asm ("mul %0, %0, %1" : "+r" (a) : "r" (b) : );
#else
pico_default_asm ("muls %0, %1" : "+l" (a) : "l" (b) : "cc");
#endif
return a;
}
/*! \brief multiply two integer values using the fastest method possible
* \ingroup pico_platform
*
* Efficiently multiplies value a by possibly constant value b.
*
* If b is known to be constant and not zero or a power of 2, then a mul instruction is used rather than gcc's default
* which is often a slow combination of shifts and adds. If b is a power of 2 then a single shift is of course preferable
* and will be used
*
* \param a the first operand
* \param b the second operand
* \return a * b
*/
#define __fast_mul(a, b) __builtin_choose_expr(__builtin_constant_p(b) && !__builtin_constant_p(a), \
(__builtin_popcount(b) >= 2 ? __mul_instruction(a,b) : (a)*(b)), \
(a)*(b))
#endif // __ASSEMBLER__
#endif

19
src/platform/PICO/pico/version.h
View file

@ -1,19 +0,0 @@
/*
* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
// ---------------------------------------
// THIS FILE IS AUTOGENERATED; DO NOT EDIT
// ---------------------------------------
#ifndef _PICO_VERSION_H
#define _PICO_VERSION_H
#define PICO_SDK_VERSION_MAJOR 2
#define PICO_SDK_VERSION_MINOR 1
#define PICO_SDK_VERSION_REVISION 0
#define PICO_SDK_VERSION_STRING "2.1.0"
#endif

276
src/platform/PICO/usb/usb_cdc.c
View file

@ -1,276 +0,0 @@
/*
* This file is part of Betaflight.
*
* Betaflight is free software. You can redistribute this software
* and/or modify this software under the terms of the GNU General
* Public License as published by the Free Software Foundation,
* either version 3 of the License, or (at your option) any later
* version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#include "tusb_config.h"
#include "tusb.h"
#include "usb_cdc.h"
#include "pico/binary_info.h"
#include "pico/time.h"
#include "pico/mutex.h"
#include "pico/critical_section.h"
#include "hardware/irq.h"
#ifndef CDC_USB_TASK_INTERVAL_US
#define CDC_USB_TASK_INTERVAL_US 1000
#endif
#ifndef CDC_USB_WRITE_TIMEOUT_US
#define CDC_USB_WRITE_TIMEOUT_US 1000
#endif
#ifndef CDC_DEADLOCK_TIMEOUT_MS
#define CDC_DEADLOCK_TIMEOUT_MS 1000
#endif
#ifndef CDC_USB_BAUD_RATE
#define CDC_USD_BAUD_RATE 115200
#endif
static bool configured = false;
static mutex_t cdc_usb_mutex;
// if this crit_sec is initialized, we are not in periodic timer mode, and must make sure
// we don't either create multiple one shot timers, or miss creating one. this crit_sec
// is used to protect the one_shot_timer_pending flag
static critical_section_t one_shot_timer_crit_sec;
static volatile bool one_shot_timer_pending;
static uint8_t low_priority_irq_num;
static int64_t timer_task(alarm_id_t id, void *user_data)
{
UNUSED(id);
UNUSED(user_data);
int64_t repeat_time;
if (critical_section_is_initialized(&one_shot_timer_crit_sec)) {
critical_section_enter_blocking(&one_shot_timer_crit_sec);
one_shot_timer_pending = false;
critical_section_exit(&one_shot_timer_crit_sec);
repeat_time = 0; // don't repeat
} else {
repeat_time = CDC_USB_TASK_INTERVAL_US;
}
if (irq_is_enabled(low_priority_irq_num)) {
irq_set_pending(low_priority_irq_num);
return repeat_time;
} else {
return 0; // don't repeat
}
}
static void low_priority_worker_irq(void)
{
if (mutex_try_enter(&cdc_usb_mutex, NULL)) {
tud_task();
mutex_exit(&cdc_usb_mutex);
} else {
// if the mutex is already owned, then we are in non IRQ code in this file.
//
// it would seem simplest to just let that code call tud_task() at the end, however this
// code might run during the call to tud_task() and we might miss a necessary tud_task() call
//
// if we are using a periodic timer (crit_sec is not initialized in this case),
// then we are happy just to wait until the next tick, however when we are not using a periodic timer,
// we must kick off a one-shot timer to make sure the tud_task() DOES run (this method
// will be called again as a result, and will try the mutex_try_enter again, and if that fails
// create another one shot timer again, and so on).
if (critical_section_is_initialized(&one_shot_timer_crit_sec)) {
bool need_timer;
critical_section_enter_blocking(&one_shot_timer_crit_sec);
need_timer = !one_shot_timer_pending;
one_shot_timer_pending = true;
critical_section_exit(&one_shot_timer_crit_sec);
if (need_timer) {
add_alarm_in_us(CDC_USB_TASK_INTERVAL_US, timer_task, NULL, true);
}
}
}
}
static void usb_irq(void)
{
irq_set_pending(low_priority_irq_num);
}
int cdc_usb_write(const uint8_t *buf, unsigned length)
{
static uint64_t last_avail_time;
int written = 0;
if (!mutex_try_enter_block_until(&cdc_usb_mutex, make_timeout_time_ms(CDC_DEADLOCK_TIMEOUT_MS))) {
return -1;
}
if (cdc_usb_connected()) {
for (unsigned i = 0; i < length;) {
unsigned n = length - i;
uint32_t avail = tud_cdc_write_available();
if (n > avail) n = avail;
if (n) {
uint32_t n2 = tud_cdc_write(buf + i, n);
tud_task();
tud_cdc_write_flush();
i += n2;
written = i;
last_avail_time = time_us_64();
} else {
tud_task();
tud_cdc_write_flush();
if (!cdc_usb_connected() || (!tud_cdc_write_available() && time_us_64() > last_avail_time + CDC_USB_WRITE_TIMEOUT_US)) {
break;
}
}
}
} else {
// reset our timeout
last_avail_time = 0;
}
mutex_exit(&cdc_usb_mutex);
return written;
}
void cdc_usb_write_flush(void)
{
if (!mutex_try_enter_block_until(&cdc_usb_mutex, make_timeout_time_ms(CDC_DEADLOCK_TIMEOUT_MS))) {
return;
}
do {
tud_task();
} while (tud_cdc_write_flush());
mutex_exit(&cdc_usb_mutex);
}
int cdc_usb_read(uint8_t *buf, unsigned length)
{
// note we perform this check outside the lock, to try and prevent possible deadlock conditions
// with printf in IRQs (which we will escape through timeouts elsewhere, but that would be less graceful).
//
// these are just checks of state, so we can call them while not holding the lock.
// they may be wrong, but only if we are in the middle of a tud_task call, in which case at worst
// we will mistakenly think we have data available when we do not (we will check again), or
// tud_task will complete running and we will check the right values the next time.
//
int rc = PICO_ERROR_NO_DATA;
if (cdc_usb_connected() && tud_cdc_available()) {
if (!mutex_try_enter_block_until(&cdc_usb_mutex, make_timeout_time_ms(CDC_DEADLOCK_TIMEOUT_MS))) {
return PICO_ERROR_NO_DATA; // would deadlock otherwise
}
if (cdc_usb_connected() && tud_cdc_available()) {
uint32_t count = tud_cdc_read(buf, length);
rc = count ? (int)count : PICO_ERROR_NO_DATA;
} else {
// because our mutex use may starve out the background task, run tud_task here (we own the mutex)
tud_task();
}
mutex_exit(&cdc_usb_mutex);
}
return rc;
}
bool cdc_usb_init(void)
{
if (get_core_num() != alarm_pool_core_num(alarm_pool_get_default())) {
// included an assertion here rather than just returning false, as this is likely
// a coding bug, rather than anything else.
assert(false);
return false;
}
// initialize TinyUSB, as user hasn't explicitly linked it
tusb_init();
if (!mutex_is_initialized(&cdc_usb_mutex)) {
mutex_init(&cdc_usb_mutex);
}
bool rc = true;
low_priority_irq_num = (uint8_t)user_irq_claim_unused(true);
irq_set_exclusive_handler(low_priority_irq_num, low_priority_worker_irq);
irq_set_enabled(low_priority_irq_num, true);
if (irq_has_shared_handler(USBCTRL_IRQ)) {
critical_section_init_with_lock_num(&one_shot_timer_crit_sec, spin_lock_claim_unused(true));
// we can use a shared handler to notice when there may be work to do
irq_add_shared_handler(USBCTRL_IRQ, usb_irq, PICO_SHARED_IRQ_HANDLER_LOWEST_ORDER_PRIORITY);
} else {
// we use initialization state of the one_shot_timer_critsec as a flag
memset(&one_shot_timer_crit_sec, 0, sizeof(one_shot_timer_crit_sec));
rc = add_alarm_in_us(CDC_USB_TASK_INTERVAL_US, timer_task, NULL, true) >= 0;
}
configured = rc;
return rc;
}
bool cdc_usb_deinit(void)
{
if (get_core_num() != alarm_pool_core_num(alarm_pool_get_default())) {
// included an assertion here rather than just returning false, as this is likely
// a coding bug, rather than anything else.
assert(false);
return false;
}
assert(tud_inited()); // we expect the caller to have initialized when calling sdio_usb_init
if (irq_has_shared_handler(USBCTRL_IRQ)) {
spin_lock_unclaim(spin_lock_get_num(one_shot_timer_crit_sec.spin_lock));
critical_section_deinit(&one_shot_timer_crit_sec);
// we can use a shared handler to notice when there may be work to do
irq_remove_handler(USBCTRL_IRQ, usb_irq);
} else {
// timer is disabled by disabling the irq
}
irq_set_enabled(low_priority_irq_num, false);
user_irq_unclaim(low_priority_irq_num);
configured = false;
return true;
}
bool cdc_usb_configured(void)
{
return configured;
}
bool cdc_usb_connected(void)
{
return tud_cdc_connected();
}
bool cdc_usb_bytes_available(void)
{
return tud_cdc_available();
}
uint32_t cdc_usb_baud_rate(void)
{
return CDC_USD_BAUD_RATE;
}
uint32_t cdc_usb_tx_bytes_free(void)
{
return tud_cdc_write_available();
}

186
src/platform/PICO/usb/usb_descriptors.c
View file

@ -1,186 +0,0 @@
/*
* This file is part of Betaflight.
*
* Betaflight is free software. You can redistribute this software
* and/or modify this software under the terms of the GNU General
* Public License as published by the Free Software Foundation,
* either version 3 of the License, or (at your option) any later
* version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public
* License along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
/*
* This file is based on a file originally part of the
* MicroPython project, http://micropython.org/
*
* The MIT License (MIT)
*
* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
* Copyright (c) 2019 Damien P. George
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "tusb.h"
#include "pico/unique_id.h"
#include "common/utils.h"
#ifndef USBD_VID
// Raspberry Pi
#define USBD_VID (0x2E8A)
#endif
#ifndef USBD_PID
#if PICO_RP2040
// Raspberry Pi Pico SDK CDC for RP2040
#define USBD_PID (0x000a)
#else
// Raspberry Pi Pico SDK CDC
#define USBD_PID (0x0009)
#endif
#endif
#ifndef USBD_MANUFACTURER
#define USBD_MANUFACTURER "Betaflight Pico"
#endif
#ifndef USBD_PRODUCT
#define USBD_PRODUCT "Pico"
#endif
#define TUD_RPI_RESET_DESC_LEN 9
#define USBD_DESC_LEN (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN + TUD_RPI_RESET_DESC_LEN)
#define USBD_CONFIGURATION_DESCRIPTOR_ATTRIBUTE 0
#define USBD_MAX_POWER_MA 250
#define USBD_ITF_CDC 0 // needs 2 interfaces
#define USBD_ITF_MAX 2
#define USBD_CDC_EP_CMD 0x81
#define USBD_CDC_EP_OUT 0x02
#define USBD_CDC_EP_IN 0x82
#define USBD_CDC_CMD_MAX_SIZE 8
#define USBD_CDC_IN_OUT_MAX_SIZE 64
#define USBD_STR_0 0x00
#define USBD_STR_MANUF 0x01
#define USBD_STR_PRODUCT 0x02
#define USBD_STR_SERIAL 0x03
#define USBD_STR_CDC 0x04
#define USBD_STR_RPI_RESET 0x05
// Note: descriptors returned from callbacks must exist long enough for transfer to complete
static const tusb_desc_device_t usbd_desc_device = {
.bLength = sizeof(tusb_desc_device_t),
.bDescriptorType = TUSB_DESC_DEVICE,
.bcdUSB = 0x0200,
.bDeviceClass = TUSB_CLASS_MISC,
.bDeviceSubClass = MISC_SUBCLASS_COMMON,
.bDeviceProtocol = MISC_PROTOCOL_IAD,
.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
.idVendor = USBD_VID,
.idProduct = USBD_PID,
.bcdDevice = 0x0100,
.iManufacturer = USBD_STR_MANUF,
.iProduct = USBD_STR_PRODUCT,
.iSerialNumber = USBD_STR_SERIAL,
.bNumConfigurations = 1,
};
#define TUD_RPI_RESET_DESCRIPTOR(_itfnum, _stridx) \
/* Interface */\
9, TUSB_DESC_INTERFACE, _itfnum, 0, 0, TUSB_CLASS_VENDOR_SPECIFIC, RESET_INTERFACE_SUBCLASS, RESET_INTERFACE_PROTOCOL, _stridx,
static const uint8_t usbd_desc_cfg[USBD_DESC_LEN] = {
TUD_CONFIG_DESCRIPTOR(1, USBD_ITF_MAX, USBD_STR_0, USBD_DESC_LEN,
USBD_CONFIGURATION_DESCRIPTOR_ATTRIBUTE, USBD_MAX_POWER_MA),
TUD_CDC_DESCRIPTOR(USBD_ITF_CDC, USBD_STR_CDC, USBD_CDC_EP_CMD,
USBD_CDC_CMD_MAX_SIZE, USBD_CDC_EP_OUT, USBD_CDC_EP_IN, USBD_CDC_IN_OUT_MAX_SIZE),
};
static char usbd_serial_str[PICO_UNIQUE_BOARD_ID_SIZE_BYTES * 2 + 1];
static const char *const usbd_desc_str[] = {
[USBD_STR_MANUF] = USBD_MANUFACTURER,
[USBD_STR_PRODUCT] = USBD_PRODUCT,
[USBD_STR_SERIAL] = usbd_serial_str,
[USBD_STR_CDC] = "Board CDC",
};
const uint8_t *tud_descriptor_device_cb(void)
{
return (const uint8_t *)&usbd_desc_device;
}
const uint8_t *tud_descriptor_configuration_cb(uint8_t index)
{
UNUSED(index);
return usbd_desc_cfg;
}
const uint16_t *tud_descriptor_string_cb(uint8_t index, uint16_t langid)
{
UNUSED(langid);
#ifndef USBD_DESC_STR_MAX
#define USBD_DESC_STR_MAX (20)
#elif USBD_DESC_STR_MAX > 127
#error USBD_DESC_STR_MAX too high (max is 127).
#elif USBD_DESC_STR_MAX < 17
#error USBD_DESC_STR_MAX too low (min is 17).
#endif
static uint16_t desc_str[USBD_DESC_STR_MAX];
// Assign the SN using the unique flash id
if (!usbd_serial_str[0]) {
pico_get_unique_board_id_string(usbd_serial_str, sizeof(usbd_serial_str));
}
unsigned len;
if (index == 0) {
desc_str[1] = 0x0409; // supported language is English
len = 1;
} else {
if (index >= ARRAYLEN(usbd_desc_str)) {
return NULL;
}
const char *str = usbd_desc_str[index];
for (len = 0; len < USBD_DESC_STR_MAX - 1 && str[len]; ++len) {
desc_str[1 + len] = str[len];
}
}
// first byte is length (including header), second byte is string type
desc_str[0] = (uint16_t)((TUSB_DESC_STRING << 8) | (2 * len + 2));
return desc_str;
}

33
src/platform/common/stm32/io_impl.c
View file

@ -76,3 +76,36 @@ int IO_GPIOPortIdx(IO_t io)
}
return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10);
}
#if defined(STM32F4) || defined(APM32F4)
int IO_EXTI_PortSourceGPIO(IO_t io)
{
return IO_GPIOPortIdx(io);
}
#endif
int IO_GPIO_PortSource(IO_t io)
{
return IO_GPIOPortIdx(io);
}
// zero based pin index
int IO_GPIOPinIdx(IO_t io)
{
if (!io) {
return -1;
}
return 31 - __builtin_clz(IO_Pin(io));
}
#if defined(STM32F4) || defined(APM32F4)
int IO_EXTI_PinSource(IO_t io)
{
return IO_GPIOPinIdx(io);
}
#endif
int IO_GPIO_PinSource(IO_t io)
{
return IO_GPIOPinIdx(io);
}