From 635eab0df3ad4cead3816787d8ea0d5794f64064 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 16:20:19 +1100 Subject: [PATCH 1/4] hwdef: added baro in HolybroG4_GPS new units have a ICP201XX baro disable baro by default to avoid issues with airflow and GPS modules installed on the outside of the aircraft --- libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat index 895e3703bc86b..c2d5beca9a014 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat @@ -95,6 +95,10 @@ COMPASS IST8310 I2C:0:0x0E false ROTATION_NONE # compass RM3100 on can-F9P-v2 COMPASS RM3100 I2C:0:0x20 false ROTATION_NONE +# baro, disabled by default +BARO ICP201XX I2C:0:0x64 +define AP_PERIPH_BARO_ENABLE_DEFAULT 0 + define HAL_USE_ADC FALSE define STM32_ADC_USE_ADC1 FALSE define HAL_DISABLE_ADC_DRIVER TRUE @@ -133,6 +137,7 @@ define GPS_MOVING_BASELINE 1 # GPS+MAG+LEDs define HAL_PERIPH_ENABLE_GPS define HAL_PERIPH_ENABLE_MAG +define HAL_PERIPH_ENABLE_BARO define HAL_PERIPH_ENABLE_NOTIFY define HAL_PERIPH_ENABLE_RC_OUT From d2f945bb3e19fc153120164745a4d946afd6a6f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 17:34:38 +1100 Subject: [PATCH 2/4] AP_Periph: fixed IMU in periph wait_for_sample() can't handle very low sample rates without a separate delay() by a ms value --- Tools/AP_Periph/imu.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/imu.cpp b/Tools/AP_Periph/imu.cpp index c0c5aaf40128d..06e961e44b135 100644 --- a/Tools/AP_Periph/imu.cpp +++ b/Tools/AP_Periph/imu.cpp @@ -11,8 +11,15 @@ extern const AP_HAL::HAL &hal; void AP_Periph_FW::can_imu_update(void) { while (true) { - // sleep for a bit to avoid flooding the CPU - hal.scheduler->delay_microseconds(100); + // we need to delay by a ms value as hal->schedule->delay_microseconds_boost + // used in wait_for_sample() takes uint16_t + const uint32_t delay_ms = 1000U / g.imu_sample_rate; + hal.scheduler->delay(delay_ms); + + if (delay_ms == 0) { + // sleep for a bit to avoid flooding the CPU + hal.scheduler->delay_microseconds(100); + } imu.update(); @@ -38,6 +45,11 @@ void AP_Periph_FW::can_imu_update(void) pkt.accelerometer_latest[1] = tmp.y; pkt.accelerometer_latest[2] = tmp.z; + tmp = imu.get_gyro(); + pkt.rate_gyro_latest[0] = tmp.x; + pkt.rate_gyro_latest[1] = tmp.y; + pkt.rate_gyro_latest[2] = tmp.z; + uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE]; uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE, From cc06a6ed9f847ada44eb16ab6d2b44078c9a9717 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 17:34:55 +1100 Subject: [PATCH 3/4] waf: enable IMU in simulated AP_Periph --- Tools/ardupilotwaf/boards.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 0e692be45a6fe..017c779b283e0 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -949,6 +949,7 @@ def configure_env(self, cfg, env): HAL_PERIPH_ENABLE_AIRSPEED = 1, HAL_PERIPH_ENABLE_MAG = 1, HAL_PERIPH_ENABLE_BARO = 1, + HAL_PERIPH_ENABLE_IMU = 1, HAL_PERIPH_ENABLE_RANGEFINDER = 1, HAL_PERIPH_ENABLE_BATTERY = 1, HAL_PERIPH_ENABLE_EFI = 1, @@ -963,6 +964,7 @@ def configure_env(self, cfg, env): HAL_WITH_ESC_TELEM = 1, AP_EXTENDED_ESC_TELEM_ENABLED = 1, AP_TERRAIN_AVAILABLE = 1, + HAL_GYROFFT_ENABLED = 0, ) class sitl_periph_gps(sitl_periph): From 5b88da6a21c1a5b53699f093f0328e0f16f8e4e9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 17:37:14 +1100 Subject: [PATCH 4/4] hwdef: enable IMU in HolybroG4_GPS IMU rate zero by default --- .../AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat index c2d5beca9a014..9dd7533f6c2a1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat @@ -74,9 +74,9 @@ define HAL_I2C_INTERNAL_MASK 3 I2C_ORDER I2C1 I2C2 # one SPI bus (for IMU, unused) -#PA5 SPI1_SCK SPI1 -#PA6 SPI1_MISO SPI1 -#PA7 SPI1_MOSI SPI1 +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 # SPI CS PC4 GYR_CS CS @@ -134,10 +134,17 @@ DMA_NOSHARE USART3* # add support for moving baseline yaw define GPS_MOVING_BASELINE 1 +SPIDEV icm42688 SPI1 DEVID1 ICM_CS MODE0 24*MHZ 24*MHZ + +IMU Invensensev3 SPI:icm42688 ROTATION_YAW_180 + +define HAL_PERIPH_ENABLE_IMU + # GPS+MAG+LEDs define HAL_PERIPH_ENABLE_GPS define HAL_PERIPH_ENABLE_MAG define HAL_PERIPH_ENABLE_BARO +define HAL_PERIPH_ENABLE_IMU define HAL_PERIPH_ENABLE_NOTIFY define HAL_PERIPH_ENABLE_RC_OUT