forked from Archive/PX4-Autopilot
px4_fmuv6xrt::Support base version selection
This commit is contained in:
parent
9849abcb63
commit
ab969c71fc
|
@ -296,7 +296,11 @@
|
|||
#define GPIO_HW_REV_SENSE /* GPIO_AD_22 GPIO9 Pin 21 */ ADC_GPIO(4, 21)
|
||||
#define GPIO_HW_VER_SENSE /* GPIO_AD_23 GPIO9 Pin 22 */ ADC_GPIO(5, 22)
|
||||
#define HW_INFO_INIT_PREFIX "V6XRT"
|
||||
|
||||
#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0 on HB Base and T1 Base
|
||||
// Base/FMUM
|
||||
#define V6XRT_00 HW_VER_REV(0x0,0x0) // First Release
|
||||
#define V6XRT_30 HW_VER_REV(0x3,0x0) // T1 Base w/o PX4IO
|
||||
#define V6XRT_50 HW_VER_REV(0x5,0x0) // HB Mini Rev 0
|
||||
|
||||
#define BOARD_I2C_LATEINIT 1 /* See Note about SE550 Eanable */
|
||||
|
|
|
@ -73,7 +73,8 @@ static const px4_hw_mft_item_t device_unsupported = {0, 0, 0};
|
|||
// List of components on a specific board configuration
|
||||
// The index of those components is given by the enum (px4_hw_mft_item_id_t)
|
||||
// declared in board_common.h
|
||||
static const px4_hw_mft_item_t hw_mft_list_V00[] = {
|
||||
|
||||
static const px4_hw_mft_item_t hw_mft_list_V00[] = { // HB Base
|
||||
{
|
||||
// PX4_MFT_PX4IO
|
||||
.present = 1,
|
||||
|
@ -100,7 +101,34 @@ static const px4_hw_mft_item_t hw_mft_list_V00[] = {
|
|||
},
|
||||
};
|
||||
|
||||
static const px4_hw_mft_item_t hw_mft_list_V50[] = {
|
||||
static const px4_hw_mft_item_t hw_mft_list_V30[] = { // NXP T1 Base
|
||||
{
|
||||
// PX4_MFT_PX4IO
|
||||
.present = 0,
|
||||
.mandatory = 0,
|
||||
.connection = px4_hw_con_unknown,
|
||||
},
|
||||
{
|
||||
// PX4_MFT_USB
|
||||
.present = 1,
|
||||
.mandatory = 1,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
{
|
||||
// PX4_MFT_CAN2
|
||||
.present = 1,
|
||||
.mandatory = 1,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
{
|
||||
// PX4_MFT_CAN3
|
||||
.present = 1,
|
||||
.mandatory = 1,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
};
|
||||
|
||||
static const px4_hw_mft_item_t hw_mft_list_V50[] = { // HB Mini
|
||||
{
|
||||
// PX4_MFT_PX4IO
|
||||
.present = 1,
|
||||
|
@ -119,10 +147,18 @@ static const px4_hw_mft_item_t hw_mft_list_V50[] = {
|
|||
.mandatory = 0,
|
||||
.connection = px4_hw_con_unknown,
|
||||
},
|
||||
{
|
||||
// PX4_MFT_CAN3
|
||||
.present = 0,
|
||||
.mandatory = 0,
|
||||
.connection = px4_hw_con_unknown,
|
||||
},
|
||||
};
|
||||
|
||||
|
||||
static px4_hw_mft_list_entry_t mft_lists[] = {
|
||||
{V6XRT_00, hw_mft_list_V00, arraySize(hw_mft_list_V00)},
|
||||
{V6XRT_00, hw_mft_list_V00, arraySize(hw_mft_list_V00)}, // HB Base
|
||||
{V6XRT_30, hw_mft_list_V30, arraySize(hw_mft_list_V30)}, // NXP T1 Base
|
||||
{V6XRT_50, hw_mft_list_V50, arraySize(hw_mft_list_V50)}, // HB Mini
|
||||
};
|
||||
|
||||
|
|
|
@ -41,47 +41,65 @@
|
|||
#include <drivers/drv_sensor.h>
|
||||
#include <nuttx/spi/spi.h>
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_log.h>
|
||||
constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
|
||||
initSPIHWVersion(V6XRT_00, {
|
||||
initSPIBus(SPI::Bus::LPSPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */
|
||||
}, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
initSPIBus(SPI::Bus::LPSPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */
|
||||
}, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22
|
||||
|
||||
#include <nuttx/spi/spi.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include <px4_platform/gpio.h>
|
||||
initSPIBus(SPI::Bus::LPSPI3, {
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */
|
||||
}, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14
|
||||
|
||||
#include <arm_internal.h>
|
||||
#include <chip.h>
|
||||
#include "imxrt_lpspi.h"
|
||||
#include "imxrt_gpio.h"
|
||||
#include "board_config.h"
|
||||
#include <systemlib/err.h>
|
||||
initSPIBusExternal(SPI::Bus::LPSPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/
|
||||
}),
|
||||
}),
|
||||
|
||||
#ifdef CONFIG_IMXRT_LPSPI
|
||||
initSPIHWVersion(V6XRT_30, {
|
||||
initSPIBus(SPI::Bus::LPSPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */
|
||||
}, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01
|
||||
|
||||
initSPIBus(SPI::Bus::LPSPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */
|
||||
}, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22
|
||||
|
||||
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||
initSPIBus(SPI::Bus::LPSPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */
|
||||
}, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01
|
||||
initSPIBus(SPI::Bus::LPSPI3, {
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */
|
||||
}, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14
|
||||
|
||||
initSPIBus(SPI::Bus::LPSPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */
|
||||
}, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22
|
||||
initSPIBusExternal(SPI::Bus::LPSPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/
|
||||
}),
|
||||
}),
|
||||
initSPIHWVersion(V6XRT_50, {
|
||||
initSPIBus(SPI::Bus::LPSPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */
|
||||
}, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01
|
||||
|
||||
initSPIBus(SPI::Bus::LPSPI3, {
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */
|
||||
}, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14
|
||||
initSPIBus(SPI::Bus::LPSPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */
|
||||
}, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22
|
||||
|
||||
initSPIBusExternal(SPI::Bus::LPSPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/
|
||||
initSPIBus(SPI::Bus::LPSPI3, {
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */
|
||||
}, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14
|
||||
|
||||
initSPIBusExternal(SPI::Bus::LPSPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/
|
||||
}),
|
||||
}),
|
||||
};
|
||||
|
||||
#endif
|
||||
static constexpr bool unused = validateSPIConfig(px4_spi_buses);
|
||||
static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);
|
||||
|
|
Loading…
Reference in New Issue