From 973068ebfea4823607125b4048400fc417bc176e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 12 May 2021 11:33:47 -0700 Subject: [PATCH] holybro_can-gps-v1:Rev to RC9 HW holybro_can-gps-v1:Fix LEDs holybro_can-gps-v1:Only CAN1 holybro_can-gps-v1:ncp5623c at 0x39 --- boards/holybro/can-gps-v1/debug.cmake | 2 +- boards/holybro/can-gps-v1/default.cmake | 4 +- .../holybro/can-gps-v1/init/rc.board_defaults | 2 +- .../holybro/can-gps-v1/init/rc.board_sensors | 9 +- .../can-gps-v1/nuttx-config/include/board.h | 26 ++-- .../can-gps-v1/nuttx-config/nsh/defconfig | 8 +- boards/holybro/can-gps-v1/src/board_config.h | 82 ++++++----- boards/holybro/can-gps-v1/src/boot.c | 23 ++- boards/holybro/can-gps-v1/src/i2c.cpp | 1 + boards/holybro/can-gps-v1/src/init.c | 25 +--- boards/holybro/can-gps-v1/src/led.c | 132 ++++-------------- boards/holybro/can-gps-v1/src/led.h | 1 - boards/holybro/can-gps-v1/src/spi.cpp | 3 +- 13 files changed, 105 insertions(+), 213 deletions(-) diff --git a/boards/holybro/can-gps-v1/debug.cmake b/boards/holybro/can-gps-v1/debug.cmake index 7e1ced9e31..3c72c1065f 100644 --- a/boards/holybro/can-gps-v1/debug.cmake +++ b/boards/holybro/can-gps-v1/debug.cmake @@ -16,7 +16,7 @@ px4_add_board( barometer/bmp388 bootloaders gps - imu/bosch/bmi088 + imu/invensense/icm20649 lights/rgbled_ncp5623c magnetometer/bosch/bmm150 uavcannode diff --git a/boards/holybro/can-gps-v1/default.cmake b/boards/holybro/can-gps-v1/default.cmake index 2ec355f30e..dc810b18c0 100644 --- a/boards/holybro/can-gps-v1/default.cmake +++ b/boards/holybro/can-gps-v1/default.cmake @@ -10,13 +10,13 @@ px4_add_board( CONSTRAINED_MEMORY CONSTRAINED_FLASH ROMFSROOT cannode - UAVCAN_INTERFACES 2 + UAVCAN_INTERFACES 1 DRIVERS adc/board_adc barometer/bmp388 bootloaders gps - imu/bosch/bmi088 + imu/invensense/icm20649 lights/rgbled_ncp5623c magnetometer/bosch/bmm150 uavcannode diff --git a/boards/holybro/can-gps-v1/init/rc.board_defaults b/boards/holybro/can-gps-v1/init/rc.board_defaults index 3882c0a242..c888bbc686 100644 --- a/boards/holybro/can-gps-v1/init/rc.board_defaults +++ b/boards/holybro/can-gps-v1/init/rc.board_defaults @@ -3,4 +3,4 @@ # board specific defaults #------------------------------------------------------------------------------ -rgbled_ncp5623c -I -b 1 -a 0x39 start +rgbled_ncp5623c -I -b 2 -a 0x39 start diff --git a/boards/holybro/can-gps-v1/init/rc.board_sensors b/boards/holybro/can-gps-v1/init/rc.board_sensors index fbc9fa43a4..9adcd321dd 100644 --- a/boards/holybro/can-gps-v1/init/rc.board_sensors +++ b/boards/holybro/can-gps-v1/init/rc.board_sensors @@ -5,13 +5,12 @@ gps start -d /dev/ttyS0 -g 38400 -p ubx -# TODO:Check for Correct Rotations -# Internal SPI BMI088 -bmi088 -A -R 0 -s start -bmi088 -G -R 0 -s start + +# Internal SPI ICM20649 +icm20649 -s -b 1 -R 8 start # Internal Baro bmp388 -I -b 1 -a 0x77 start # Internal magnetometer on I2c -bmm150 -I -b 1 start +bmm150 -I -b 1 -R 2 start diff --git a/boards/holybro/can-gps-v1/nuttx-config/include/board.h b/boards/holybro/can-gps-v1/nuttx-config/include/board.h index d2361d3b5b..c905886c75 100644 --- a/boards/holybro/can-gps-v1/nuttx-config/include/board.h +++ b/boards/holybro/can-gps-v1/nuttx-config/include/board.h @@ -128,34 +128,34 @@ /* UARTs */ -#define GPIO_USART1_TX /* PA15 */ GPIO_USART1_TX_3 -#define GPIO_USART1_RX /* PA10 */ GPIO_USART1_RX_1 +#define GPIO_USART1_TX /* PA9 */ GPIO_USART1_TX_1 +#define GPIO_USART1_RX /* PA10 */ GPIO_USART1_RX_1 -#define GPIO_USART2_TX /* PA2 */ GPIO_USART2_TX_1 -#define GPIO_USART2_RX /* PA3 */ GPIO_USART2_RX_1 +#define GPIO_USART2_TX /* PA2 */ GPIO_USART2_TX_1 +#define GPIO_USART2_RX /* PA3 */ GPIO_USART2_RX_1 /* CAN */ -#define GPIO_CAN1_TX /* PB9 */ GPIO_CAN1_TX_2 -#define GPIO_CAN1_RX /* PB8 */ GPIO_CAN1_RX_2 +#define GPIO_CAN1_TX /* PA12 */ GPIO_CAN1_TX_1 +#define GPIO_CAN1_RX /* PA11 */ GPIO_CAN1_RX_1 -#define GPIO_CAN2_TX /* PB13 */ GPIO_CAN2_TX_1 -#define GPIO_CAN2_RX /* PB12 */ GPIO_CAN2_RX_2 +#define GPIO_CAN2_TX /* PB13 */ GPIO_CAN2_TX_1 +#define GPIO_CAN2_RX /* PB12 */ GPIO_CAN2_RX_2 /* I2C */ -#define GPIO_MCU_I2C1_SCL -#define GPIO_MCU_I2C1_SDA +#define GPIO_I2C1_SCL /* PB6 */ GPIO_I2C1_SCL_1 +#define GPIO_I2C1_SDA /* PB7 */ GPIO_I2C1_SDA_1 -#define GPIO_I2C1_SCL /* PB6 */ GPIO_I2C1_SCL_1 -#define GPIO_I2C1_SDA /* PB7 */ GPIO_I2C1_SDA_1 +#define GPIO_I2C2_SCL /* PB10 */ GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA /* PB3 */ GPIO_I2C2_SDA_3 /* SPI */ #define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) #define GPIO_SPI1_MISO /* PA6 */ GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI /* PA7 */ GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_MOSI /* PB5 */ GPIO_SPI1_MOSI_2 #define GPIO_SPI1_SCK /* PA5 */ ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) #endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig index 31f71e8355..92b5e13a9f 100644 --- a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig +++ b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig @@ -35,11 +35,6 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x004f -CONFIG_CDCACM_PRODUCTSTR="PX4 HolyBro CAN GPS" -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="HolyBro" CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y @@ -136,8 +131,8 @@ CONFIG_STM32_DMA2=y CONFIG_STM32_FLASH_PREFETCH=y CONFIG_STM32_FLOWCONTROL_BROKEN=y CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y CONFIG_STM32_JTAG_SW_ENABLE=y -CONFIG_STM32_OTGFS=y CONFIG_STM32_PWR=y CONFIG_STM32_RTC=y CONFIG_STM32_RTC_HSECLOCK=y @@ -167,7 +162,6 @@ CONFIG_USART2_RXBUFSIZE=600 CONFIG_USART2_RXDMA=y CONFIG_USART2_SERIAL_CONSOLE=y CONFIG_USART2_TXBUFSIZE=2500 -CONFIG_USBDEV=y CONFIG_USEC_PER_TICK=1000 CONFIG_USERMAIN_STACKSIZE=4096 CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/holybro/can-gps-v1/src/board_config.h b/boards/holybro/can-gps-v1/src/board_config.h index 78339042c7..1c5f8ef02d 100644 --- a/boards/holybro/can-gps-v1/src/board_config.h +++ b/boards/holybro/can-gps-v1/src/board_config.h @@ -50,17 +50,6 @@ // todo:NCP5623 datasheet says 0x38 driver says 0x39 - needs testing /* GPIO *************************************************************************** */ - -/* LEDs are driven with push open drain to support Anode to 3.3V */ - -#define GPIO_MCU_NLED_RED /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) -#define GPIO_MCU_NLED_BLUE /* PB2 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN2) -#define GPIO_MCU_NLED_GREEN /* PB10 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) - -#define BOARD_HAS_CONTROL_STATUS_LEDS 1 -#define BOARD_OVERLOAD_LED LED_RED -#define BOARD_ARMED_STATE_LED LED_BLUE - /* * ADC channels * @@ -80,18 +69,18 @@ /* Define Channel numbers must match above GPIO pin IN(n)*/ -#define ADC_HW_VER_SENSE_CHANNEL /* PA0 */ ADC1_CH(0) -#define ADC_HW_REV_SENSE_CHANNEL /* PA1 */ ADC1_CH(1) +#define ADC_HW_REV_SENSE_CHANNEL /* PA0 */ ADC1_CH(0) +#define ADC_HW_VER_SENSE_CHANNEL /* PA1 */ ADC1_CH(1) #define ADC_CHANNELS \ - ((1 << ADC_HW_VER_SENSE_CHANNEL) | \ - (1 << ADC_HW_REV_SENSE_CHANNEL)) + ((1 << ADC_HW_REV_SENSE_CHANNEL) | \ + (1 << ADC_HW_VER_SENSE_CHANNEL)) /* HW Version and Revision drive signals Default to 1 to detect */ #define BOARD_HAS_HW_VERSIONING -#define GPIO_HW_VER_REV_DRIVE /* PB5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) +#define GPIO_HW_VER_REV_DRIVE /* PB1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) #define GPIO_HW_REV_SENSE /* PA0 */ ADC1_GPIO(0) #define GPIO_HW_VER_SENSE /* PA1 */ ADC1_GPIO(1) #define HW_INFO_INIT {'C','A','N','G','P','S','x', 'x',0} @@ -104,43 +93,52 @@ * * Silent mode control */ -#define GPIO_MCU_CAN1_SILENT /* PB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_MCU_CAN1_SILENT /* PB2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN2) #define GPIO_MCU_CAN2_SILENT /* PA4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN4) -// Spi pinning is in spi.ccp +// SPI pinning for ICM20649 is in spi.ccp -// GPIO_MCU_SPI1_DRDY /* PA8 */ -// GPIO_MCU_SPI1_NCS_GYRO /* PB14 */ -// GPIO_MCU_SPI1_NCS_ACC /* PB15 */ +// GPIO_MCU_SPI1_DRDY /* PB0 */ +// GPIO_MCU_SPI1_NCS_ACC /* PA8 */ #define GPIO_SENSOR_3V3_EN /* PC15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) -#define GPIO_I2C1_SCL_RESET /* PB6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6) -#define GPIO_I2C1_SDA_RESET /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7) -#define GPIO_USB_SWITCH_DET /* PC14 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN14) -#define GPIO_NOPT_WAIT_FOR_GETNODEINFO /* PC13 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN13) +#define GPIO_MCU_I2C1_SCL_RESET /* PB6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6) +#define GPIO_MCU_I2C1_SDA_RESET /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7) + +#define GPIO_MCU_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_MCU_I2C2_SDA_RESET /* PB3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) + + +#define GPIO_NOPT_WAIT_FOR_GETNODEINFO /* PC14 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN14) + +#define GPIO_TIM1_CH1N /* PA7 */ GPIO_TIM1_CH1N_1 /* NLED_RED */ +#define GPIO_TIM1_CH2N /* PB14 */ GPIO_TIM1_CH2N_2 /* NLED_BLUE */ +#define GPIO_TIM1_CH3N /* PB15 */ GPIO_TIM1_CH3N_2 /* NLED_GREEN */ -#define GPIO_GPS_PPS_IN /* PB4 */ GPIO_TIM3_CH1_2 +#define GPIO_TIM2_CH1 /* PA15 */ GPIO_TIM2_CH1_3 /* GPS_PPS_IN */ // todo:needs Driver +#define GPIO_GPS_PPS_IN /* PA15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN15) /* High-resolution timer */ -#define HRT_TIMER 3 /* use timer 3 for the HRT */ -#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ +#define HRT_TIMER 3 /* use timer 3 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ #define PX4_GPIO_INIT_LIST { \ - PX4_ADC_GPIO, \ - GPIO_HW_VER_REV_DRIVE, \ - GPIO_SENSOR_3V3_EN, \ - GPIO_I2C1_SCL_RESET, \ - GPIO_I2C1_SDA_RESET, \ - GPIO_CAN1_TX, \ - GPIO_CAN1_RX, \ - GPIO_CAN2_TX, \ - GPIO_CAN2_RX, \ - GPIO_MCU_CAN1_SILENT, \ - GPIO_MCU_CAN2_SILENT, \ - GPIO_USB_SWITCH_DET, \ - GPIO_NOPT_WAIT_FOR_GETNODEINFO, \ - GPIO_GPS_PPS_IN /* todo:needs driver */ \ + PX4_ADC_GPIO, \ + GPIO_HW_VER_REV_DRIVE, \ + GPIO_SENSOR_3V3_EN, \ + GPIO_MCU_I2C1_SCL_RESET, \ + GPIO_MCU_I2C1_SDA_RESET, \ + GPIO_MCU_I2C2_SCL_RESET, \ + GPIO_MCU_I2C2_SDA_RESET, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_MCU_CAN1_SILENT, \ + GPIO_MCU_CAN2_SILENT, \ + GPIO_NOPT_WAIT_FOR_GETNODEINFO, \ + GPIO_GPS_PPS_IN \ } __BEGIN_DECLS diff --git a/boards/holybro/can-gps-v1/src/boot.c b/boards/holybro/can-gps-v1/src/boot.c index 4dc967f8f3..cd86a1613b 100644 --- a/boards/holybro/can-gps-v1/src/boot.c +++ b/boards/holybro/can-gps-v1/src/boot.c @@ -69,7 +69,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_CAN2_TX); stm32_configgpio(GPIO_MCU_CAN1_SILENT); stm32_configgpio(GPIO_MCU_CAN2_SILENT); - led_init(); // todo:Remove with creation of proper LED driver putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST | RCC_APB1RSTR_CAN2RST, STM32_RCC_APB1RSTR); putreg32(getreg32(STM32_RCC_APB1RSTR) & ~(RCC_APB1RSTR_CAN1RST | RCC_APB1RSTR_CAN2RST), STM32_RCC_APB1RSTR); @@ -168,17 +167,17 @@ typedef begin_packed_struct struct led_t { static const led_t i2l[] = { led(0, off, 0, 0, 0, 0), - led(1, reset, 128, 128, 128, 30), - led(2, autobaud_start, 0, 128, 0, 1), - led(3, autobaud_end, 0, 128, 0, 2), - led(4, allocation_start, 0, 0, 64, 2), - led(5, allocation_end, 0, 128, 64, 3), - led(6, fw_update_start, 32, 128, 64, 3), - led(7, fw_update_erase_fail, 32, 128, 32, 3), - led(8, fw_update_invalid_response, 64, 0, 0, 1), - led(9, fw_update_timeout, 64, 0, 0, 2), - led(a, fw_update_invalid_crc, 64, 0, 0, 4), - led(b, jump_to_app, 0, 128, 0, 10), + led(1, reset, 255, 255, 255, 30), + led(2, autobaud_start, 0, 255, 0, 1), + led(3, autobaud_end, 0, 255, 0, 2), + led(4, allocation_start, 0, 0, 128, 2), + led(5, allocation_end, 0, 255, 128, 3), + led(6, fw_update_start, 64, 255, 128, 3), + led(7, fw_update_erase_fail, 64, 255, 64, 3), + led(8, fw_update_invalid_response, 128, 0, 0, 1), + led(9, fw_update_timeout, 128, 0, 0, 2), + led(a, fw_update_invalid_crc, 128, 0, 0, 4), + led(b, jump_to_app, 0, 255, 0, 10), }; diff --git a/boards/holybro/can-gps-v1/src/i2c.cpp b/boards/holybro/can-gps-v1/src/i2c.cpp index d59c97bcc2..8b4552bc7a 100644 --- a/boards/holybro/can-gps-v1/src/i2c.cpp +++ b/boards/holybro/can-gps-v1/src/i2c.cpp @@ -35,4 +35,5 @@ constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusInternal(1), + initI2CBusInternal(2), }; diff --git a/boards/holybro/can-gps-v1/src/init.c b/boards/holybro/can-gps-v1/src/init.c index a22d839cd4..451233c068 100644 --- a/boards/holybro/can-gps-v1/src/init.c +++ b/boards/holybro/can-gps-v1/src/init.c @@ -65,7 +65,8 @@ #include #include -#include + +#include "led.h" #include @@ -78,20 +79,6 @@ # include #endif -/* - * Ideally we'd be able to get these from arm_internal.h, - * but since we want to be able to disable the NuttX use - * of leds for system indication at will and there is no - * separate switch, we need to build independent of the - * CONFIG_ARCH_LEDS configuration switch. - */ -__BEGIN_DECLS -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -__END_DECLS - - /************************************************************************************ * Name: stm32_boardinitialize * @@ -196,12 +183,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) (hrt_callout)stm32_serial_dma_poll, NULL); #endif - - /* initial LED state */ - drv_led_start(); - led_off(LED_RED); - led_on(LED_GREEN); // Indicate Power. - led_off(LED_BLUE); - + rgb_led(0, 255, 0, 0); return OK; } diff --git a/boards/holybro/can-gps-v1/src/led.c b/boards/holybro/can-gps-v1/src/led.c index f87ec58e37..99c67d9d05 100644 --- a/boards/holybro/can-gps-v1/src/led.c +++ b/boards/holybro/can-gps-v1/src/led.c @@ -48,107 +48,17 @@ #include #include -// TODO:This needs a complete rewrite We need to change the HW -// Swap PB0/PB2 and use timer for all LEDS +#include "led.h" #define TMR_BASE STM32_TIM1_BASE #define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN #define TMR_REG(o) (TMR_BASE+(o)) -static uint8_t _off[] = {0, 0, 0}; -static uint8_t _rgb[] = {0, 0, 0}; - -/* - * Ideally we'd be able to get these from arm_internal.h, - * but since we want to be able to disable the NuttX use - * of leds for system indication at will and there is no - * separate switch, we need to build independent of the - * CONFIG_ARCH_LEDS configuration switch. - */ -__BEGIN_DECLS -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -extern void led_toggle(int led); -__END_DECLS - -#define LED_RED 1 -#define LED_BLUE 0 -#define LED_GREEN 3 - -#define xlat(p) (p) -static uint32_t g_ledmap[] = { - GPIO_MCU_NLED_BLUE, // Indexed by LED_BLUE - GPIO_MCU_NLED_RED, // Indexed by LED_RED, LED_AMBER - 0, // Indexed by LED_SAFETY (defaulted to an input) - GPIO_MCU_NLED_GREEN // Indexed by LED_GREEN -}; - -__EXPORT void led_init(void) -{ - for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { - if (g_ledmap[l] != 0) { - stm32_configgpio(g_ledmap[l]); - } - } -} - -static void phy_set_led(int led, bool state) -{ - /* Drive Low to switch on */ - - if (g_ledmap[led] != 0) { - stm32_gpiowrite(g_ledmap[led], !state); - } -} - -static bool phy_get_led(int led) -{ - /* If Low it is on */ - if (g_ledmap[led] != 0) { - return !stm32_gpioread(g_ledmap[led]); - } - - return false; -} - -__EXPORT void led_on(int led) -{ - phy_set_led(xlat(led), true); -} - -__EXPORT void led_off(int led) -{ - phy_set_led(xlat(led), false); -} - -__EXPORT void led_toggle(int led) -{ - phy_set_led(xlat(led), !phy_get_led(xlat(led))); -} - - -static void setled(uint8_t *rgb) -{ - phy_set_led(LED_RED, _rgb[0]); - phy_set_led(LED_GREEN, _rgb[1]); - phy_set_led(LED_BLUE, _rgb[2]); -} - - -static int timerInterrupt(int irq, void *context, void *arg) -{ - putreg16(~getreg16(TMR_REG(STM32_GTIM_SR_OFFSET)), TMR_REG(STM32_GTIM_SR_OFFSET)); - - static int d2 = 1; - setled((d2++ & 1) ? _rgb : _off); - return 0; -} - void rgb_led(int r, int g, int b, int freqs) { + long fosc = TMR_FREQUENCY; - long prescale = 1536; + long prescale = 9600; long p1s = fosc / prescale; long p0p5s = p1s / 2; uint16_t val; @@ -156,38 +66,49 @@ void rgb_led(int r, int g, int b, int freqs) if (!once) { once = 1; - setled(_off); /* Enable Clock to Block */ modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); /* Reload */ + val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET)); val |= ATIM_EGR_UG; putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET)); /* Set Prescaler STM32_TIM_SETCLOCK */ + putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET)); /* Enable STM32_TIM_SETMODE*/ + putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET)); - putreg32(p0p5s + 1, TMR_REG(STM32_BTIM_ARR_OFFSET)); + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE | + (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET)); + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET)); + putreg16(ATIM_CCER_CC3NE | ATIM_CCER_CC3NP | ATIM_CCER_CC2NE | ATIM_CCER_CC2NP | ATIM_CCER_CC1NE | ATIM_CCER_CC1NP, + TMR_REG(STM32_GTIM_CCER_OFFSET)); - irq_attach(STM32_IRQ_TIM1CC, timerInterrupt, NULL); - up_enable_irq(STM32_IRQ_TIM1CC); - putreg16(GTIM_DIER_CC1IE, TMR_REG(STM32_GTIM_DIER_OFFSET)); + stm32_configgpio(GPIO_TIM1_CH1N); + stm32_configgpio(GPIO_TIM1_CH2N); + stm32_configgpio(GPIO_TIM1_CH3N); + + /* master output enable = on */ + + putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET))); } - long p = freqs == 0 ? p1s + 1 : p0p5s / freqs; - putreg32(p + 1, TMR_REG(STM32_BTIM_ARR_OFFSET)); - putreg32(p, TMR_REG(STM32_GTIM_CCR1_OFFSET)); - _rgb[0] = g; - _rgb[1] = r; - _rgb[2] = b; - setled(_rgb); + long p = freqs == 0 ? p1s : p1s / freqs; + putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET)); + + p = freqs == 0 ? p1s + 1 : p0p5s / freqs; + + putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET)); + putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET)); + putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET)); val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET)); @@ -199,4 +120,5 @@ void rgb_led(int r, int g, int b, int freqs) } putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); + } diff --git a/boards/holybro/can-gps-v1/src/led.h b/boards/holybro/can-gps-v1/src/led.h index cf12884e56..b68e4aa70d 100644 --- a/boards/holybro/can-gps-v1/src/led.h +++ b/boards/holybro/can-gps-v1/src/led.h @@ -34,5 +34,4 @@ __BEGIN_DECLS void rgb_led(int r, int g, int b, int freqs); -__EXPORT void led_init(void); __END_DECLS diff --git a/boards/holybro/can-gps-v1/src/spi.cpp b/boards/holybro/can-gps-v1/src/spi.cpp index 754ea6d0b5..b6cc6258d9 100644 --- a/boards/holybro/can-gps-v1/src/spi.cpp +++ b/boards/holybro/can-gps-v1/src/spi.cpp @@ -37,8 +37,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortB, GPIO::Pin14}, SPI::DRDY{GPIO::PortA, GPIO::Pin8}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortB, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin8}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), }, {GPIO::PortC, GPIO::Pin15}), };