Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

px4_fmuv-6x Sensor set 3 #19314

Merged
merged 1 commit into from
Mar 23, 2022
Merged

px4_fmuv-6x Sensor set 3 #19314

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions boards/px4/fmu-v6x/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

FYI the icm42670p was a community contribution with some differences we need to fix. For example it still has the sensor side filter (AKA DLPF) enabled at 34 Hz.

PX4-Autopilot/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp

Lines 178 to 179 in 497d396

{ Register::BANK_0::GYRO_CONFIG1, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_34Hz, Bit1 },
{ Register::BANK_0::ACCEL_CONFIG1, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_34Hz, Bit1 },

I'll do a thorough review with updates once I have hardware in hand.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure, I will send the hardware over once available.

CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_COMMON_LIGHT=y
CONFIG_COMMON_MAGNETOMETER=y
Expand All @@ -39,6 +40,7 @@ CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
Expand All @@ -47,6 +49,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
Expand All @@ -61,17 +64,15 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_BL_UPDATE=y
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
CONFIG_SYSTEMCMDS_BL_UPDATE=y
CONFIG_SYSTEMCMDS_DMESG=y
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
Expand Down
12 changes: 9 additions & 3 deletions boards/px4/fmu-v6x/init/rc.board_sensors
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,14 @@ bmi088 -G -R 4 -s start
# Internal SPI bus ICM42688p
icm42688p -R 6 -s start

# Internal SPI bus ICM-20649 (hard-mounted)
icm20649 -R 14 -s start
if ver hwtypecmp V6X03 V6X13
then
# Internal SPI bus ICM-42670-P (hard-mounted)
icm42670p -R 14 -s start
else
# Internal SPI bus ICM-20649 (hard-mounted)
icm20649 -R 14 -s start
fi

# Internal magnetometer on I2c
bmm150 -I start
Expand All @@ -44,7 +50,7 @@ ist8310 -X -b 1 -R 10 start
# Possible internal Baro
bmp388 -I -a 0x77 start

if ver hwtypecmp V6X00
if ver hwtypecmp V6X00 V6X10
then
bmp388 -I start
else
Expand Down
9 changes: 9 additions & 0 deletions boards/px4/fmu-v6x/src/board_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -215,6 +215,15 @@
#define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */
#define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */

#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 and Rev 3 Sensor sets
// Base/FMUM
#define V6X00 HW_VER_REV(0x0,0x0) // FMUV6X, Rev 0
#define V6X01 HW_VER_REV(0x0,0x1) // FMUV6X, BMI388 I2C2 Rev 1
#define V6X03 HW_VER_REV(0x0,0x3) // FMUV6X, Sensor Set Rev 3
#define V6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0
#define V6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3


/* HEATER
* PWM in future
*/
Expand Down
14 changes: 5 additions & 9 deletions boards/px4/fmu-v6x/src/init.c
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
Expand Down Expand Up @@ -173,12 +173,6 @@ stm32_boardinitialize(void)
const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
px4_gpio_init(gpio, arraySize(gpio));

/* configure SPI interfaces (we can do this here as long as we only have a single SPI hw config version -
* otherwise we need to move this after board_determine_hw_info()) */
_Static_assert(BOARD_NUM_SPI_CFG_HW_VERSIONS == 1, "Need to move the SPI initialization for multi-version support");

stm32_spiinitialize();

/* configure USB interfaces */

stm32_usbinitialize();
Expand Down Expand Up @@ -212,14 +206,12 @@ stm32_boardinitialize(void)
*
****************************************************************************/


__EXPORT int board_app_initialize(uintptr_t arg)
{
/* Power on Interfaces */
VDD_3V3_SD_CARD_EN(true);
VDD_5V_PERIPH_EN(true);
VDD_5V_HIPOWER_EN(true);
board_spi_reset(10, 0xffff);
VDD_3V3_SENSORS4_EN(true);
VDD_3V3_SPEKTRUM_POWER_EN(true);

Expand All @@ -236,6 +228,10 @@ __EXPORT int board_app_initialize(uintptr_t arg)
syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n");
}

stm32_spiinitialize();

board_spi_reset(10, 0xffff);

/* configure the DMA allocator */

if (board_dma_alloc_init() < 0) {
Expand Down
21 changes: 19 additions & 2 deletions boards/px4/fmu-v6x/src/manifest.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,10 +81,27 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
},
};

static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
{
.present = 0,
.mandatory = 0,
.connection = px4_hw_con_unknown,
},
{
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
};


static px4_hw_mft_list_entry_t mft_lists[] = {
// ver_rev
{0x0000, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
{0x0001, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2
{V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
{V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO
{V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2
{V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3
{V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3
};

/************************************************************************************
Expand Down
70 changes: 48 additions & 22 deletions boards/px4/fmu-v6x/src/spi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2020 PX4 Development Team. All rights reserved.
* Copyright (C) 2020, 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
Expand Down Expand Up @@ -35,28 +35,54 @@
#include <drivers/drv_sensor.h>
#include <nuttx/spi/spi.h>

constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
initSPIHWVersion(HW_VER_REV(0, 0), {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
}),
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),

initSPIHWVersion(HW_VER_REV(0, 3), {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
}),
}),
};

static constexpr bool unused = validateSPIConfig(px4_spi_buses);
static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);