diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors index 4291deaf29..d23597776c 100644 --- a/ROMFS/px4fmu_test/init.d/rc.sensors +++ b/ROMFS/px4fmu_test/init.d/rc.sensors @@ -12,9 +12,8 @@ if bst start -X then fi -if board_adc start -then -fi +board_adc start +system_power start if sdp3x_airspeed start -X then diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 589c546d25..8fc9cbe40a 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -45,6 +45,7 @@ px4_add_board( pwm_out rc_input roboclaw + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors index 30fca7a7ed..badb475379 100644 --- a/boards/airmind/mindpx-v2/init/rc.board_sensors +++ b/boards/airmind/mindpx-v2/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal I2C bus hmc5883 -T -I -R 12 start diff --git a/boards/airmind/mindpx-v2/src/board_config.h b/boards/airmind/mindpx-v2/src/board_config.h index 0bb915f402..5d1afdfb0c 100644 --- a/boards/airmind/mindpx-v2/src/board_config.h +++ b/boards/airmind/mindpx-v2/src/board_config.h @@ -72,39 +72,24 @@ #define ADC_RC_RSSI_CHANNEL 11 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 -/* Define Battery 1 Voltage Divider and A per V - */ - +/* Define Battery 1 Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (10.177939394f) #define BOARD_BATTERY1_A_PER_V (15.391030303f) -/* Power supply control and monitoring GPIOs */ -// #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) -// #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) -// #define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) -// #define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) -// #define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) -// #define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) - /* Tone alarm output */ #define TONE_ALARM_TIMER 14 /* timer 14 */ #define TONE_ALARM_CHANNEL 1 /* channel 1 */ #define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF9|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN7) -/* AUX PWMs - */ +/* AUX PWMs */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 #define DIRECT_INPUT_TIMER_CHANNELS 8 -/* USB OTG FS - * - * PA9 OTG_FS_VBUS VBUS sensing - */ +/* USB OTG FS */ #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) - /* High-resolution timer */ #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel */ @@ -143,46 +128,24 @@ #define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX) #define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) -#define BOARD_ADC_BRICK_VALID (1) -#define BOARD_ADC_SERVO_VALID (1) -#define BOARD_ADC_PERIPH_5V_OC (0) -#define BOARD_ADC_HIPOWER_5V_OC (0) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 -/* This board provides the board_on_reset interface */ - #define BOARD_HAS_ON_RESET 1 __BEGIN_DECLS -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - #ifndef __ASSEMBLY__ -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - /**************************************************************************************************** * Name: stm32_spiinitialize * * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * Called to configure SPI chip select GPIO pins for the board. * ****************************************************************************************************/ @@ -190,10 +153,8 @@ extern void stm32_spiinitialize(void); extern void stm32_usbinitialize(void); - #define board_peripheral_reset(ms) - #include #endif /* __ASSEMBLY__ */ diff --git a/boards/av/x-v1/src/board_config.h b/boards/av/x-v1/src/board_config.h index 68a2022d9b..736735b98f 100644 --- a/boards/av/x-v1/src/board_config.h +++ b/boards/av/x-v1/src/board_config.h @@ -49,11 +49,6 @@ #include -/* Configuration ************************************************************************************/ - -#define BOARD_HAS_NBAT_V 1 // Only one Vbat to ADC -#define BOARD_HAS_NBAT_I 0 // No Ibat ADC - /* * ADC channels * @@ -88,18 +83,14 @@ #define ADC_CHANNELS \ ((1 << ADC_BATTERY1_VOLTAGE_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V - */ - +/* Define Battery 1 Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (10.133333333f) #define BOARD_BATTERY1_A_PER_V (36.367515152f) /* HW has to large of R termination on ADC todo:change when HW value is chosen */ - #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) -/* PWM - */ +/* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 9 #define DIRECT_INPUT_TIMER_CHANNELS 9 @@ -110,7 +101,6 @@ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 3 */ /* RC Serial port */ - #define RC_SERIAL_PORT "/dev/ttyS4" /* Power switch controls ******************************************************/ @@ -118,27 +108,9 @@ #define SDIO_SLOTNO 0 /* Only one slot */ #define SDIO_MINOR 0 -/* SD card bringup does not work if performed on the IDLE thread because it - * will cause waiting. Use either: - * - * CONFIG_LIB_BOARDCTL=y, OR - * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y - */ - -#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && !defined(CONFIG_BOARD_INITTHREAD) -# warning SDIO initialization cannot be perfomed on the IDLE thread -#endif - -/* AV-X_V1 never powers off the Servo rail */ - -#define BOARD_ADC_SERVO_VALID (1) - #define ADC_BATTERY_VOLTAGE_CHANNEL 0 #define ADC_BATTERY_CURRENT_CHANNEL 1 // TODO: review -#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC)) -#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC)) - #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ diff --git a/boards/bitcraze/crazyflie/src/board_config.h b/boards/bitcraze/crazyflie/src/board_config.h index eb534441dd..78a34b4a16 100644 --- a/boards/bitcraze/crazyflie/src/board_config.h +++ b/boards/bitcraze/crazyflie/src/board_config.h @@ -109,13 +109,6 @@ */ #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) -/* - * ADC channels - * - * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver - */ -#define ADC_CHANNELS 0 - /* Tone alarm output : These are only applicable when the buzzer deck is attached */ #define TONE_ALARM_TIMER 5 /* timer 5 */ #define TONE_ALARM_CHANNEL 3 /* channel 3 */ @@ -123,9 +116,7 @@ #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN2) #define GPIO_TONE_ALARM_NEG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) -/* PWM -*/ - +/* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 4 /* This board overrides the defaults by providing diff --git a/boards/cuav/nora/default.cmake b/boards/cuav/nora/default.cmake index 188f93cf64..45603be750 100644 --- a/boards/cuav/nora/default.cmake +++ b/boards/cuav/nora/default.cmake @@ -50,6 +50,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/cuav/nora/init/rc.board_sensors b/boards/cuav/nora/init/rc.board_sensors index e12b50a5e5..2d8d2462a4 100644 --- a/boards/cuav/nora/init/rc.board_sensors +++ b/boards/cuav/nora/init/rc.board_sensors @@ -3,6 +3,7 @@ # board specific sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # SPI1 (internal) rm3100 -s start diff --git a/boards/cuav/nora/src/board_config.h b/boards/cuav/nora/src/board_config.h index 01baa4bf3d..69d3dbdee7 100644 --- a/boards/cuav/nora/src/board_config.h +++ b/boards/cuav/nora/src/board_config.h @@ -110,8 +110,6 @@ #define DIRECT_PWM_OUTPUT_CHANNELS 8 /* Power supply control and monitoring GPIOs */ -#define BOARD_NUMBER_BRICKS 2 - #define GPIO_nPOWER_IN_ADC /* PG1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN1) #define GPIO_nPOWER_IN_CAN /* PG2 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2) #define GPIO_nPOWER_IN_C /* PG0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0) diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index ad721b553a..5396c36ca0 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -53,6 +53,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/cuav/x7pro/init/rc.board_sensors b/boards/cuav/x7pro/init/rc.board_sensors index d7071ff79f..254d5d46c2 100644 --- a/boards/cuav/x7pro/init/rc.board_sensors +++ b/boards/cuav/x7pro/init/rc.board_sensors @@ -3,6 +3,7 @@ # CUAV X7Pro specific board sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # SPI1 (internal) #adis16470 -s start diff --git a/boards/cuav/x7pro/src/board_config.h b/boards/cuav/x7pro/src/board_config.h index 199d0b97fd..fcd84b79c7 100644 --- a/boards/cuav/x7pro/src/board_config.h +++ b/boards/cuav/x7pro/src/board_config.h @@ -110,8 +110,6 @@ #define DIRECT_PWM_OUTPUT_CHANNELS 8 /* Power supply control and monitoring GPIOs */ -#define BOARD_NUMBER_BRICKS 2 - #define GPIO_nPOWER_IN_ADC /* PG1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN1) #define GPIO_nPOWER_IN_CAN /* PG2 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2) #define GPIO_nPOWER_IN_C /* PG0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0) @@ -179,10 +177,7 @@ #define GPIO_LED_SAFETY FMU_LED_AMBER -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) #define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) diff --git a/boards/cubepilot/cubeorange/console.cmake b/boards/cubepilot/cubeorange/console.cmake index 4f52a483f0..a29306bc0e 100644 --- a/boards/cubepilot/cubeorange/console.cmake +++ b/boards/cubepilot/cubeorange/console.cmake @@ -52,6 +52,7 @@ px4_add_board( px4io roboclaw rpm + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake index 42fafe571f..aad57efc2e 100644 --- a/boards/cubepilot/cubeorange/default.cmake +++ b/boards/cubepilot/cubeorange/default.cmake @@ -52,6 +52,7 @@ px4_add_board( px4io roboclaw rpm + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/cubepilot/cubeorange/init/rc.board_sensors b/boards/cubepilot/cubeorange/init/rc.board_sensors index 8535383b4b..3638a664b2 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_sensors +++ b/boards/cubepilot/cubeorange/init/rc.board_sensors @@ -3,6 +3,7 @@ # Board specific sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # SPI4 ms5611 -s -b 4 start diff --git a/boards/cubepilot/cubeorange/src/board_config.h b/boards/cubepilot/cubeorange/src/board_config.h index 0f31ae8ee2..b95e591065 100644 --- a/boards/cubepilot/cubeorange/src/board_config.h +++ b/boards/cubepilot/cubeorange/src/board_config.h @@ -97,7 +97,6 @@ #define GPIO_PWM_VOLT_SEL /* PB4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* Power supply control and monitoring GPIOs */ -#define BOARD_NUMBER_BRICKS 2 #define GPIO_nVDD_BRICK1_VALID /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) // VDD_BRICK_VALID #define GPIO_nVDD_BRICK2_VALID /* PB7 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) // VDD_BACKUP_VALID #define GPIO_nVDD_USB_VALID /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) // VBUS_VALID @@ -124,10 +123,7 @@ #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) #define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) diff --git a/boards/cubepilot/cubeyellow/console.cmake b/boards/cubepilot/cubeyellow/console.cmake index 173b7b9edd..4a83230730 100644 --- a/boards/cubepilot/cubeyellow/console.cmake +++ b/boards/cubepilot/cubeyellow/console.cmake @@ -51,6 +51,7 @@ px4_add_board( px4io roboclaw rpm + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake index f06a255ac6..87f5bbeae8 100644 --- a/boards/cubepilot/cubeyellow/default.cmake +++ b/boards/cubepilot/cubeyellow/default.cmake @@ -51,6 +51,7 @@ px4_add_board( px4io roboclaw rpm + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/cubepilot/cubeyellow/init/rc.board_sensors b/boards/cubepilot/cubeyellow/init/rc.board_sensors index 8535383b4b..3638a664b2 100644 --- a/boards/cubepilot/cubeyellow/init/rc.board_sensors +++ b/boards/cubepilot/cubeyellow/init/rc.board_sensors @@ -3,6 +3,7 @@ # Board specific sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # SPI4 ms5611 -s -b 4 start diff --git a/boards/cubepilot/cubeyellow/src/board_config.h b/boards/cubepilot/cubeyellow/src/board_config.h index 388b4f232b..2a92733389 100644 --- a/boards/cubepilot/cubeyellow/src/board_config.h +++ b/boards/cubepilot/cubeyellow/src/board_config.h @@ -97,7 +97,6 @@ #define GPIO_PWM_VOLT_SEL /* PB4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* Power supply control and monitoring GPIOs */ -#define BOARD_NUMBER_BRICKS 2 #define GPIO_nVDD_BRICK1_VALID /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) // VDD_BRICK_VALID #define GPIO_nVDD_BRICK2_VALID /* PB7 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) // VDD_BACKUP_VALID #define GPIO_nVDD_USB_VALID /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) // VBUS_VALID @@ -124,10 +123,7 @@ #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) #define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index fea8499992..a56fb60919 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -30,6 +30,7 @@ px4_add_board( magnetometer/lsm9ds1_mag pwm_out_sim rc_input + system_power #telemetry # all available telemetry drivers MODULES airspeed_selector diff --git a/boards/emlid/navio2/src/board_config.h b/boards/emlid/navio2/src/board_config.h index 6443bb9c61..c71a0177f3 100644 --- a/boards/emlid/navio2/src/board_config.h +++ b/boards/emlid/navio2/src/board_config.h @@ -47,11 +47,9 @@ #define BOARD_MAX_LEDS 1 // Number of external LED's this board has - // I2C #define PX4_NUMBER_I2C_BUSES 1 - // ADC channels: // A0 - board voltage (shows 5V) // A1 - servo rail voltage diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 7cc7c16d38..b80c493d2e 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -51,6 +51,7 @@ px4_add_board( pwm_out px4io roboclaw + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/holybro/durandal-v1/init/rc.board_sensors b/boards/holybro/durandal-v1/init/rc.board_sensors index 99f3d37a74..091c00612d 100644 --- a/boards/holybro/durandal-v1/init/rc.board_sensors +++ b/boards/holybro/durandal-v1/init/rc.board_sensors @@ -3,6 +3,7 @@ # Holybro Durandal V1 specific board sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # Internal SPI bus ICM-20689 icm20689 -R 2 -s start diff --git a/boards/holybro/durandal-v1/src/board_config.h b/boards/holybro/durandal-v1/src/board_config.h index 815dad5930..03de4ab977 100644 --- a/boards/holybro/durandal-v1/src/board_config.h +++ b/boards/holybro/durandal-v1/src/board_config.h @@ -70,11 +70,6 @@ /* Configuration ************************************************************************************/ -# define BOARD_HAS_LTC44XX_VALIDS 2 // No LTC or N Bricks -# define BOARD_HAS_USB_VALID 1 // LTC Has No USB valid -# define BOARD_HAS_NBAT_V 2 // Only one Vbat to ADC -# define BOARD_HAS_NBAT_I 2 // No Ibat ADC - /* Holybro Durandal V1 GPIOs ************************************************************************/ /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ @@ -95,11 +90,9 @@ */ /* ADC defines to be used in sensors.cpp to read from a particular channel */ - #define ADC1_CH(n) (n) /* Define GPIO pins used as ADC N.B. Channel numbers must match below */ - #define PX4_ADC_GPIO \ /* PA0 */ GPIO_ADC1_INP16, \ /* PA1 */ GPIO_ADC1_INP17, \ @@ -213,7 +206,6 @@ #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ -#define BOARD_NUMBER_BRICKS 2 #define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ #define GPIO_nVDD_5V_PERIPH_EN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) @@ -314,17 +306,10 @@ # warning SDIO initialization cannot be perfomed on the IDLE thread #endif -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) -/* Board never powers off the Servo rail */ - -#define BOARD_ADC_SERVO_VALID (1) - #define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) #define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) diff --git a/boards/holybro/kakutef7/src/board_config.h b/boards/holybro/kakutef7/src/board_config.h index 2822eddb43..b1381cf38f 100644 --- a/boards/holybro/kakutef7/src/board_config.h +++ b/boards/holybro/kakutef7/src/board_config.h @@ -49,12 +49,6 @@ #include -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ - -/* GPIOs ***********************************************************************************/ - /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ #define GPIO_nLED_BLUE /* PA2 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN2) @@ -64,7 +58,6 @@ #define FLASH_BASED_PARAMS - /* * ADC channels * @@ -92,13 +85,11 @@ (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ (1 << ADC_RSSI_IN_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V - */ +/* Define Battery 1 Voltage Divider and A per V. */ #define BOARD_BATTERY1_V_DIV (10.9f) #define BOARD_BATTERY1_A_PER_V (17.f) -/* PWM - */ +/* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 #define DIRECT_INPUT_TIMER_CHANNELS 6 @@ -106,10 +97,7 @@ #define GPIO_TONE_ALARM_IDLE /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) #define GPIO_TONE_ALARM_GPIO /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN15) -/* USB OTG FS - * - * PA8 OTG_FS_VBUS VBUS sensing - */ +/* USB OTG FS PA8 OTG_FS_VBUS VBUS sensing */ #define GPIO_OTGFS_VBUS /* PA8 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN8) /* High-resolution timer */ @@ -117,8 +105,7 @@ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ /* RC Serial port */ - -#define RC_SERIAL_PORT "/dev/ttyS4" +#define RC_SERIAL_PORT "/dev/ttyS4" #define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) @@ -142,20 +129,8 @@ __BEGIN_DECLS -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - #ifndef __ASSEMBLY__ -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - /**************************************************************************************************** * Name: stm32_spiinitialize * diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 2654472a25..9c219573b3 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -55,6 +55,7 @@ px4_add_board( roboclaw rpm safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/holybro/pix32v5/init/rc.board_sensors b/boards/holybro/pix32v5/init/rc.board_sensors index 26d3e136a0..b21d57554c 100644 --- a/boards/holybro/pix32v5/init/rc.board_sensors +++ b/boards/holybro/pix32v5/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal SPI bus ICM-20602 icm20602 -s -R 2 start diff --git a/boards/holybro/pix32v5/src/board_config.h b/boards/holybro/pix32v5/src/board_config.h index 98e5e6f9b1..440b48880f 100644 --- a/boards/holybro/pix32v5/src/board_config.h +++ b/boards/holybro/pix32v5/src/board_config.h @@ -68,26 +68,8 @@ #define PX4IO_SERIAL_CLOCK STM32_PCLK1_FREQUENCY #define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ -/* Configuration ************************************************************************************/ - -#define BOARD_HAS_LTC4417 - -#if defined(BOARD_HAS_LTC4417) -# define BOARD_HAS_LTC44XX_VALIDS 2 // No LTC or N Bricks -# define BOARD_HAS_USB_VALID 1 // LTC Has No USB valid -# define BOARD_HAS_NBAT_V 2 // Only one Vbat to ADC -# define BOARD_HAS_NBAT_I 2 // No Ibat ADC -#else -# define BOARD_HAS_LTC44XX_VALIDS 0 // No LTC or N Bricks -# define BOARD_HAS_USB_VALID 0 // LTC Has No USB valid -# define BOARD_HAS_NBAT_V 1 // Only one Vbat to ADC -# define BOARD_HAS_NBAT_I 0 // No Ibat ADC -#endif - -/* PX4FMU GPIOs ***********************************************************************************/ /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ - #define GPIO_nLED_RED /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) #define GPIO_nLED_GREEN /* PC6 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6) #define GPIO_nLED_BLUE /* PC7 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) @@ -137,7 +119,6 @@ #define ADC_HW_REV_SENSE_CHANNEL /* PC3 */ ADC1_CH(13) #define ADC1_SPARE_1_CHANNEL /* PC4 */ ADC1_CH(14) -#if BOARD_HAS_NBAT_V == 2 && BOARD_HAS_NBAT_I == 2 #define ADC_CHANNELS \ ((1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \ (1 << ADC_BATTERY1_CURRENT_CHANNEL) | \ @@ -150,41 +131,15 @@ (1 << ADC_HW_VER_SENSE_CHANNEL) | \ (1 << ADC_HW_REV_SENSE_CHANNEL) | \ (1 << ADC1_SPARE_1_CHANNEL)) -#elif BOARD_HAS_NBAT_V == 1 && BOARD_HAS_NBAT_I == 1 -#define ADC_CHANNELS \ - ((1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \ - (1 << ADC_BATTERY1_CURRENT_CHANNEL) | \ - (1 << ADC1_SPARE_2_CHANNEL) | \ - (1 << ADC_RSSI_IN_CHANNEL) | \ - (1 << ADC_SCALED_V5_CHANNEL) | \ - (1 << ADC_SCALED_VDD_3V3_SENSORS_CHANNEL) | \ - (1 << ADC_HW_VER_SENSE_CHANNEL) | \ - (1 << ADC_HW_REV_SENSE_CHANNEL) | \ - (1 << ADC1_SPARE_1_CHANNEL)) -#elif BOARD_HAS_NBAT_V == 1 && BOARD_HAS_NBAT_I == 0 -#define ADC_CHANNELS \ - ((1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \ - (1 << ADC1_SPARE_2_CHANNEL) | \ - (1 << ADC_RSSI_IN_CHANNEL) | \ - (1 << ADC_SCALED_V5_CHANNEL) | \ - (1 << ADC_SCALED_VDD_3V3_SENSORS_CHANNEL) | \ - (1 << ADC_HW_VER_SENSE_CHANNEL) | \ - (1 << ADC_HW_REV_SENSE_CHANNEL) | \ - (1 << ADC1_SPARE_1_CHANNEL)) -#endif - -/* Define Battery 1 Voltage Divider and A per V - */ +/* Define Battery 1 Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ #define BOARD_BATTERY1_A_PER_V (36.367515152f) /* HW has to large of R termination on ADC todo:change when HW value is chosen */ - #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* HW Version and Revision drive signals Default to 1 to detect */ - #define BOARD_HAS_HW_VERSIONING #define GPIO_HW_REV_DRIVE /* PH14 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN14) @@ -256,7 +211,6 @@ #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ -#define BOARD_NUMBER_BRICKS 2 #define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ #define GPIO_nVDD_5V_PERIPH_EN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) @@ -380,43 +334,11 @@ # warning SDIO initialization cannot be perfomed on the IDLE thread #endif -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) - -#if BOARD_HAS_USB_VALID == 1 -# define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) -#else -# define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED -#endif - -/* FMUv5 never powers odd the Servo rail */ - -#define BOARD_ADC_SERVO_VALID (1) - -#if !defined(BOARD_HAS_LTC44XX_VALIDS) || BOARD_HAS_LTC44XX_VALIDS == 0 -# define BOARD_ADC_BRICK1_VALID (1) -# define BOARD_ADC_BRICK2_VALID (0) -#elif BOARD_HAS_LTC44XX_VALIDS == 1 -# define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) -# define BOARD_ADC_BRICK2_VALID (0) -#elif BOARD_HAS_LTC44XX_VALIDS == 2 -# define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) -# define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) -#elif BOARD_HAS_LTC44XX_VALIDS == 3 -# define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) -# define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) -# define BOARD_ADC_BRICK3_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK3_VALID)) -#elif BOARD_HAS_LTC44XX_VALIDS == 4 -# define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) -# define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) -# define BOARD_ADC_BRICK3_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK3_VALID)) -# define BOARD_ADC_BRICK4_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK4_VALID)) -#else -# error Unsupported BOARD_HAS_LTC44XX_VALIDS value -#endif +#define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) +#define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +#define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC)) diff --git a/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp b/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp index 7c184d2fa2..f8b722d8f5 100644 --- a/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp +++ b/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp @@ -123,7 +123,7 @@ void AEROFC_ADC::RunImpl() adc_report.raw_data[i] = (buffer[i * 2] | (buffer[i * 2 + 1] << 8)); } - for (; i < PX4_MAX_ADC_CHANNELS; ++i) { // set unused channel id to -1 + for (; i < adc_report_s::MAX_ADC_CHANNELS; ++i) { // set unused channel id to -1 adc_report.channel_id[i] = -1; } diff --git a/boards/intel/aerofc-v1/src/board_config.h b/boards/intel/aerofc-v1/src/board_config.h index ea265027ce..2f7858c84e 100644 --- a/boards/intel/aerofc-v1/src/board_config.h +++ b/boards/intel/aerofc-v1/src/board_config.h @@ -42,20 +42,10 @@ #pragma once -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - #include #include #include -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -/* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ #define GPIO_LED0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9) #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10) @@ -89,34 +79,23 @@ #define ADC_BATTERY_VOLTAGE_CHANNEL 1 #define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) -/* - * Define Battery 1 Voltage Divider, using default for A/V - */ +/* Define Battery 1 Voltage Divider, using default for A/V */ #define BOARD_BATTERY1_V_DIV (9.0f) #define DIRECT_PWM_OUTPUT_CHANNELS 1 #define BOARD_HAS_PWM 0 -/* - * USB OTG FS - */ - -/* - * RC Serial port - */ +/* RC Serial port */ #define RC_SERIAL_PORT "/dev/ttyS2" /* No HW invert support */ -/* - * High-resolution timer - */ + +/* High-resolution timer */ #define HRT_TIMER 3 /* use timer3 for the HRT */ #define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */ #define FLASH_BASED_PARAMS #define FLASH_BASED_DATAMAN -/* - * ESCs do not respond - */ +/* ESCs do not respond */ #define BOARD_TAP_ESC_MODE 1 #define MEMORY_CONSTRAINED_SYSTEM @@ -125,42 +104,12 @@ __BEGIN_DECLS -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - #ifndef __ASSEMBLY__ -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - -/**************************************************************************************************** - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ****************************************************************************************************/ - extern void stm32_spiinitialize(void); #define board_peripheral_reset(ms) -/************************************************************************************ - * Name: board_sdio_initialize - * - * Description: - * Called to configure SDIO. - * - ************************************************************************************/ - -extern int board_sdio_initialize(void); - #include #endif /* __ASSEMBLY__ */ diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index bc08640995..ebaa7c03f7 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -45,6 +45,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index 8261c9b5c2..09adc9a9bc 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Start Digital power monitors voxlpm -X -b 3 -k -T VBATT start diff --git a/boards/modalai/fc-v1/src/board_config.h b/boards/modalai/fc-v1/src/board_config.h index 6db8e25798..3eaddf9594 100644 --- a/boards/modalai/fc-v1/src/board_config.h +++ b/boards/modalai/fc-v1/src/board_config.h @@ -223,18 +223,6 @@ # warning SDIO initialization cannot be perfomed on the IDLE thread #endif -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ -#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) - -/* ModalAI FC-v1 never powers off the Servo rail */ - -#define BOARD_ADC_SERVO_VALID (1) -#define BOARD_ADC_BRICK_VALID (1) - - #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 25209d9672..f228969040 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -49,6 +49,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_sensors b/boards/mro/ctrl-zero-f7/init/rc.board_sensors index 88205f4ec8..2909fb7c9c 100644 --- a/boards/mro/ctrl-zero-f7/init/rc.board_sensors +++ b/boards/mro/ctrl-zero-f7/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal ICM-20602 icm20602 -s -b 1 -R 8 start diff --git a/boards/mro/ctrl-zero-f7/src/board_config.h b/boards/mro/ctrl-zero-f7/src/board_config.h index a6f5e70f5b..06f3a4dac2 100644 --- a/boards/mro/ctrl-zero-f7/src/board_config.h +++ b/boards/mro/ctrl-zero-f7/src/board_config.h @@ -113,7 +113,6 @@ #define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ -#define BOARD_NUMBER_BRICKS 1 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) @@ -168,13 +167,8 @@ #define SDIO_SLOTNO 0 /* Only one slot */ #define SDIO_MINOR 0 -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) -#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED -#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ #define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS diff --git a/boards/mro/pixracerpro/default.cmake b/boards/mro/pixracerpro/default.cmake index af008a3dfe..04a6638865 100644 --- a/boards/mro/pixracerpro/default.cmake +++ b/boards/mro/pixracerpro/default.cmake @@ -49,6 +49,7 @@ px4_add_board( rc_input roboclaw rpm + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/mro/pixracerpro/init/rc.board_sensors b/boards/mro/pixracerpro/init/rc.board_sensors index bb25512841..cb7349bdc3 100644 --- a/boards/mro/pixracerpro/init/rc.board_sensors +++ b/boards/mro/pixracerpro/init/rc.board_sensors @@ -3,6 +3,7 @@ # Board specific sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # Internal ICM-20602 icm20602 -s -b 1 -R 8 start diff --git a/boards/mro/pixracerpro/src/board_config.h b/boards/mro/pixracerpro/src/board_config.h index 28d730206f..8233edfdd0 100644 --- a/boards/mro/pixracerpro/src/board_config.h +++ b/boards/mro/pixracerpro/src/board_config.h @@ -90,7 +90,6 @@ #define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ -#define BOARD_NUMBER_BRICKS 1 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) @@ -142,8 +141,6 @@ * provides the true logic GPIO BOARD_ADC_xxxx macros. */ #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) -#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED -#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ #define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index 38649c6f90..86d9644d6c 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -49,6 +49,7 @@ px4_add_board( px4io roboclaw rpm + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/mro/x21-777/init/rc.board_sensors b/boards/mro/x21-777/init/rc.board_sensors index 402b84792a..434b522ae6 100644 --- a/boards/mro/x21-777/init/rc.board_sensors +++ b/boards/mro/x21-777/init/rc.board_sensors @@ -3,6 +3,7 @@ # Board specific sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # SPI1 ms5611 -s -b 1 start diff --git a/boards/mro/x21-777/src/board_config.h b/boards/mro/x21-777/src/board_config.h index 62a44f7597..767aa24da7 100644 --- a/boards/mro/x21-777/src/board_config.h +++ b/boards/mro/x21-777/src/board_config.h @@ -108,15 +108,10 @@ #define PWMIN_TIMER_CHANNEL 2 #define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 -/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC - * system_power interface, and therefore provides the true logic - * GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) -#define BOARD_ADC_SERVO_VALID (1) #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) -#define BOARD_ADC_HIPOWER_5V_OC (0) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index 2a9afdd7d7..6d8479038d 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -46,6 +46,7 @@ px4_add_board( pwm_out px4io roboclaw + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/mro/x21/init/rc.board_sensors b/boards/mro/x21/init/rc.board_sensors index 22ccc1b52d..a194277c75 100644 --- a/boards/mro/x21/init/rc.board_sensors +++ b/boards/mro/x21/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal SPI bus ICM-20608-G icm20608g -s -R 8 start diff --git a/boards/mro/x21/src/board_config.h b/boards/mro/x21/src/board_config.h index 605ec66c8b..f3c8b10bd8 100644 --- a/boards/mro/x21/src/board_config.h +++ b/boards/mro/x21/src/board_config.h @@ -119,15 +119,10 @@ #define PWMIN_TIMER_CHANNEL 2 #define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 -/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC - * system_power interface, and therefore provides the true logic - * GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) -#define BOARD_ADC_SERVO_VALID (1) #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) -#define BOARD_ADC_HIPOWER_5V_OC (0) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS diff --git a/boards/nxp/fmuk66-e/default.cmake b/boards/nxp/fmuk66-e/default.cmake index 4fe3ccec4e..5e7a9d3854 100644 --- a/boards/nxp/fmuk66-e/default.cmake +++ b/boards/nxp/fmuk66-e/default.cmake @@ -46,6 +46,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers #test_ppm # NOT Portable YET diff --git a/boards/nxp/fmuk66-e/init/rc.board_sensors b/boards/nxp/fmuk66-e/init/rc.board_sensors index 492c942f68..018a11593a 100644 --- a/boards/nxp/fmuk66-e/init/rc.board_sensors +++ b/boards/nxp/fmuk66-e/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal Mag I2C bus roll 180, yaw 90 bmm150 -I -R 10 start diff --git a/boards/nxp/fmuk66-e/socketcan.cmake b/boards/nxp/fmuk66-e/socketcan.cmake index c8394ee3c6..f47a03c506 100644 --- a/boards/nxp/fmuk66-e/socketcan.cmake +++ b/boards/nxp/fmuk66-e/socketcan.cmake @@ -45,6 +45,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers #test_ppm # NOT Portable YET diff --git a/boards/nxp/fmuk66-e/src/board_config.h b/boards/nxp/fmuk66-e/src/board_config.h index f3beb66dd6..11fa0228ab 100644 --- a/boards/nxp/fmuk66-e/src/board_config.h +++ b/boards/nxp/fmuk66-e/src/board_config.h @@ -275,11 +275,6 @@ __END_DECLS #define GPIO_ULTRASOUND_TRIGGER /* PTD0 */ (GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO | PIN_PORTD | PIN0) #define GPIO_ULTRASOUND_ECHO /* PTA10 */ (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTA | PIN10) -/* Power supply control and monitoring GPIOs */ -// None - -#define GPIO_PERIPH_3V3_EN 0 - /* Tone alarm output PTA11 - TMP 2_CH1 is On +P12-4, -P12-5 * It is driving a NPN @@ -318,17 +313,7 @@ __END_DECLS #define SPEKTRUM_POWER(on_true) VDD_3V3_SPEKTRUM_POWER_EN((on_true)) -/* - * By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ - #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_USB_VBUS_VALID)) -#define BOARD_ADC_BRICK_VALID (1) -#define BOARD_ADC_SERVO_VALID (1) -#define BOARD_ADC_PERIPH_5V_OC (0) -#define BOARD_ADC_HIPOWER_5V_OC (0) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index ab4f7597b6..08179e715b 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -46,6 +46,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers #test_ppm # NOT Portable YET diff --git a/boards/nxp/fmuk66-v3/init/rc.board_defaults b/boards/nxp/fmuk66-v3/init/rc.board_defaults index 1345753002..ecfb478517 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_defaults +++ b/boards/nxp/fmuk66-v3/init/rc.board_defaults @@ -11,4 +11,3 @@ fi rgbled_pwm start safety_button start - diff --git a/boards/nxp/fmuk66-v3/init/rc.board_sensors b/boards/nxp/fmuk66-v3/init/rc.board_sensors index c6ca931a93..65ac0b023e 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_sensors +++ b/boards/nxp/fmuk66-v3/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal Mag I2C bus roll 180, yaw 90 bmm150 -I -R 10 start diff --git a/boards/nxp/fmuk66-v3/rtps.cmake b/boards/nxp/fmuk66-v3/rtps.cmake index c296678279..5f25e0a216 100644 --- a/boards/nxp/fmuk66-v3/rtps.cmake +++ b/boards/nxp/fmuk66-v3/rtps.cmake @@ -45,6 +45,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers #test_ppm # NOT Portable YET diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake index abd0660527..c5b225085b 100644 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ b/boards/nxp/fmuk66-v3/socketcan.cmake @@ -45,6 +45,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers #test_ppm # NOT Portable YET diff --git a/boards/nxp/fmuk66-v3/src/board_config.h b/boards/nxp/fmuk66-v3/src/board_config.h index 51586205fe..8b4890bcaa 100644 --- a/boards/nxp/fmuk66-v3/src/board_config.h +++ b/boards/nxp/fmuk66-v3/src/board_config.h @@ -39,9 +39,6 @@ #pragma once -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ #include #include #include @@ -55,7 +52,6 @@ __BEGIN_DECLS __END_DECLS -/* FMUK66 GPIOs ***********************************************************************************/ /* LEDs */ /* An RGB LED is connected through GPIO as shown below: * TBD (no makring on schematic) @@ -96,9 +92,7 @@ __END_DECLS #define HRT_TIMER 1 /* TPM1 timer for the HRT */ #define HRT_TIMER_CHANNEL 0 /* Use capture/compare channel 0 */ -/* PPM IN - */ - +/* PPM IN */ #define HRT_PPM_CHANNEL 1 /* Use TPM1 capture/compare channel 1 */ #define GPIO_PPM_IN PIN_TPM1_CH1_1 /* PTC3 USART1 RX and PTA9 and PIN_TPM1_CH1 AKA FrSky_IN_RC_IN */ @@ -123,7 +117,6 @@ __END_DECLS * as an input Therefore we drive are UARTx_RX (normaly an input) as an * output */ - #define GPIO_PPM_IN_AS_OUT (GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE | PIN_PORTC | PIN3) #define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) @@ -131,7 +124,6 @@ __END_DECLS #define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) /* RC input */ - #define RC_SERIAL_PORT "/dev/ttyS2" /* UART1 */ #define GPIO_RSSI_IN PIN_ADC1_SE13 @@ -140,7 +132,6 @@ __END_DECLS * Uninitialized to Reset Disabled and Inhibited * All pins driven low to not back feed when power is off */ - #define nGPIO_ETHERNET_P_EN (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTB | PIN3) #define GPIO_ENET_RST (GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO | PIN_PORTA | PIN28) #define GPIO_ENET_EN (GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO | PIN_PORTA | PIN29) @@ -200,7 +191,6 @@ __END_DECLS #define GPIO_A_RST (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE | PIN_PORTA | PIN25) /* Sensor interrupts */ - #define GPIO_EXTI_GYRO_INT1 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTE | PIN7) #define GPIO_EXTI_GYRO_INT2 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTE | PIN6) #define GPIO_EXTI_ACCEL_MAG_INT1 (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTE | PIN9) @@ -218,12 +208,10 @@ __END_DECLS * Only ADC1 is used * Bits 31:0 are ADC1 channels 31:0 */ - #define ADC1_CH(c) (((c) & 0x1f)) /* Define ADC number Channel number */ #define ADC1_GPIO(n) PIN_ADC1_SE##n /* ADC defines to be used in sensors.cpp to read from a particular channel */ - #define ADC_USB_VBUS_VALID ADC1_CH(0) /* USB_VBUS_VALID 29 - ADC1_DP0 */ #define ADC_BATTERY_VOLTAGE_CHANNEL ADC1_CH(10) /* BAT_VSENS 85 PTB4 ADC1_SE10 */ #define ADC_BATTERY_CURRENT_CHANNEL ADC1_CH(11) /* BAT_ISENS 86 PTB5 ADC1_SE11 */ @@ -234,7 +222,6 @@ __END_DECLS #define ADC_AD3 ADC1_CH(23) /* AD3 39 - ADC1_SE23 */ /* Mask use to initialize the ADC driver */ - #define ADC_CHANNELS ((1 << ADC_USB_VBUS_VALID) | \ (1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ @@ -253,15 +240,9 @@ __END_DECLS /* PTB6 ADC1_SE12 */ ADC1_GPIO(12), \ /* PTB7 ADC1_SE13 */ ADC1_GPIO(13) - - -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - - -/* User GPIOs - * - */ +/* Define Battery 1 Voltage Divider and A per V. */ +#define BOARD_BATTERY1_V_DIV (10.177939394f) +#define BOARD_BATTERY1_A_PER_V (15.391030303f) /* Timer I/O PWM and capture * @@ -279,11 +260,6 @@ __END_DECLS #define GPIO_ULTRASOUND_TRIGGER /* PTD0 */ (GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO | PIN_PORTD | PIN0) #define GPIO_ULTRASOUND_ECHO /* PTA10 */ (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTA | PIN10) -/* Power supply control and monitoring GPIOs */ -// None - -#define GPIO_PERIPH_3V3_EN 0 - /* Tone alarm output PTA11 - TMP 2_CH1 is On +P12-4, -P12-5 * It is driving a NPN @@ -322,17 +298,7 @@ __END_DECLS #define SPEKTRUM_POWER(on_true) VDD_3V3_SPEKTRUM_POWER_EN((on_true)) -/* - * By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ - #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_USB_VBUS_VALID)) -#define BOARD_ADC_BRICK_VALID (1) -#define BOARD_ADC_SERVO_VALID (1) -#define BOARD_ADC_PERIPH_5V_OC (0) -#define BOARD_ADC_HIPOWER_5V_OC (0) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS @@ -421,18 +387,10 @@ __END_DECLS #define BOARD_NUM_IO_TIMERS 3 -/************************************************************************************ - * Public data - ************************************************************************************/ - __BEGIN_DECLS #ifndef __ASSEMBLY__ -/************************************************************************************ - * Public Functions - ************************************************************************************/ - /************************************************************************************ * Name: fmuk66_spidev_initialize * diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index fa4f14052f..aeb41df971 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -45,6 +45,7 @@ px4_add_board( rc_input roboclaw safety_button + #system_power # REVIEW tap_esc telemetry # all available telemetry drivers tone_alarm diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_sensors b/boards/nxp/fmurt1062-v1/init/rc.board_sensors index 8d2b0d7808..c7fb252197 100644 --- a/boards/nxp/fmurt1062-v1/init/rc.board_sensors +++ b/boards/nxp/fmurt1062-v1/init/rc.board_sensors @@ -16,6 +16,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal SPI bus ICM-20602 icm20602 -R 2 -s start diff --git a/boards/nxp/fmurt1062-v1/src/board_config.h b/boards/nxp/fmurt1062-v1/src/board_config.h index aa2dccac41..194dfa6123 100644 --- a/boards/nxp/fmurt1062-v1/src/board_config.h +++ b/boards/nxp/fmurt1062-v1/src/board_config.h @@ -265,7 +265,6 @@ #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ -#define BOARD_NUMBER_BRICKS 2 #define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ #define OC_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ) @@ -372,18 +371,10 @@ # warning SDIO initialization cannot be perfomed on the IDLE thread #endif -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ #define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) #define BOARD_ADC_USB_CONNECTED (board_read_VBUS_state() == 0) -/* FMUv5 never powers odd the Servo rail */ - -#define BOARD_ADC_SERVO_VALID (1) - #define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) #define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) diff --git a/boards/omnibus/f4sd/src/board_config.h b/boards/omnibus/f4sd/src/board_config.h index 72e7051e77..9758cc54fd 100644 --- a/boards/omnibus/f4sd/src/board_config.h +++ b/boards/omnibus/f4sd/src/board_config.h @@ -47,12 +47,6 @@ #include #include -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -/* omnibusf4sd GPIOs ***********************************************************************************/ /* LEDs */ // power - green // LED1 - PB5 - blue @@ -63,9 +57,8 @@ #define FLASH_BASED_PARAMS -/* - * ADC channels - * +/** + * ADC channels: * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver */ #define ADC_CHANNELS (1 << 0) | (1 << 11) | (1 << 12) @@ -74,8 +67,7 @@ #define ADC_BATTERY_CURRENT_CHANNEL 11 #define ADC_RC_RSSI_CHANNEL 0 -/* Define Battery 1 Voltage Divider and A per V - */ +/* Define Battery 1 Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (11.12f) #define BOARD_BATTERY1_A_PER_V (31.f) @@ -108,16 +100,10 @@ //#define GPIO_GPIO4_OUTPUT _MK_GPIO_OUTPUT(GPIO_TIM5_CH2OUT) //#define GPIO_GPIO5_OUTPUT _MK_GPIO_OUTPUT(GPIO_TIM1_CH1OUT) -/* USB OTG FS - * - * PA9 OTG_FS_VBUS VBUS sensing - */ +/* USB OTG FS PA9 OTG_FS_VBUS VBUS sensing */ #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN5) -/* PWM - * - * Alternatively CH3/CH4 could be assigned to UART6_TX/RX - */ +/* PWM Alternatively CH3/CH4 could be assigned to UART6_TX/RX */ #define DIRECT_PWM_OUTPUT_CHANNELS 4 #define DIRECT_INPUT_TIMER_CHANNELS 4 @@ -160,20 +146,8 @@ __BEGIN_DECLS -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - #ifndef __ASSEMBLY__ -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - /**************************************************************************************************** * Name: stm32_spiinitialize * @@ -184,15 +158,6 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); - -/**************************************************************************************************** - * Name: stm32_usbinitialize - * - * Description: - * Called to configure USB IO. - * - ****************************************************************************************************/ - extern void stm32_usbinitialize(void); extern void board_peripheral_reset(int ms); diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 3c2a081646..521fcf6612 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -61,6 +61,7 @@ px4_add_board( pwm_out px4io #roboclaw + system_power #tap_esc #telemetry # all available telemetry drivers #test_ppm diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index 022d996539..36f1671c6e 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -7,6 +7,7 @@ rgbled start -I rgbled_ncp5623c start -I board_adc start +system_power start # Internal I2C bus hmc5883 -T -I -R 4 start diff --git a/boards/px4/fmu-v2/src/board_config.h b/boards/px4/fmu-v2/src/board_config.h index 0ef74dad9b..f7636ca4e8 100644 --- a/boards/px4/fmu-v2/src/board_config.h +++ b/boards/px4/fmu-v2/src/board_config.h @@ -108,9 +108,7 @@ #define ADC_5V_RAIL_SENSE 4 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 -/* Define Battery 1 Voltage Divider and A per V - */ - +/* Define Battery 1 Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (10.177939394f) #define BOARD_BATTERY1_A_PER_V (15.391030303f) @@ -148,10 +146,7 @@ #define PWMIN_TIMER_CHANNEL 2 #define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) #define BOARD_ADC_SERVO_VALID (!px4_arch_gpioread(GPIO_VDD_SERVO_VALID)) diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 633255a76e..7dcb21b061 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -56,6 +56,7 @@ px4_add_board( pwm_out px4io #roboclaw + system_power #tap_esc #telemetry # all available telemetry drivers #test_ppm diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index a895867b37..2513ef636a 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -57,6 +57,7 @@ px4_add_board( pwm_out px4io roboclaw + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index dc9fe7e5c3..e5a3f3eac2 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -7,6 +7,7 @@ rgbled start -I rgbled_ncp5623c start -I board_adc start +system_power start # Internal I2C bus hmc5883 -T -I -R 4 start diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index a8937a0ac0..1574107322 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -55,6 +55,7 @@ px4_add_board( pwm_out px4io roboclaw + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v3/src/board_config.h b/boards/px4/fmu-v3/src/board_config.h index 0ef74dad9b..f7636ca4e8 100644 --- a/boards/px4/fmu-v3/src/board_config.h +++ b/boards/px4/fmu-v3/src/board_config.h @@ -108,9 +108,7 @@ #define ADC_5V_RAIL_SENSE 4 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 -/* Define Battery 1 Voltage Divider and A per V - */ - +/* Define Battery 1 Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (10.177939394f) #define BOARD_BATTERY1_A_PER_V (15.391030303f) @@ -148,10 +146,7 @@ #define PWMIN_TIMER_CHANNEL 2 #define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) #define BOARD_ADC_SERVO_VALID (!px4_arch_gpioread(GPIO_VDD_SERVO_VALID)) diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 4a925a6839..88690dc59f 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -51,6 +51,7 @@ px4_add_board( pwm_out px4io roboclaw + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 81da152d20..67e82ae94b 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -53,6 +53,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors index b6f2662f17..7cec77f65f 100644 --- a/boards/px4/fmu-v4/init/rc.board_sensors +++ b/boards/px4/fmu-v4/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # We know there are sketchy boards out there # as chinese companies produce Pixracers without diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index d8b51abb17..376f98ff2c 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -49,6 +49,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index a822097199..5539fdc5a2 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -39,20 +39,10 @@ #pragma once -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - #include #include #include -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -/* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) @@ -98,18 +88,11 @@ #define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) -/** - * PWM: - * - * Six PWM outputs are configured. - */ +/* PWM: Six PWM outputs are configured */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 #define DIRECT_INPUT_TIMER_CHANNELS 6 -/** - * USB OTG FS: - * PA9 OTG_FS_VBUS VBUS sensing. - */ +/* USB OTG FS: PA9 OTG_FS_VBUS VBUS sensing. */ #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ @@ -120,7 +103,6 @@ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) /* RC Serial port */ - #define RC_SERIAL_PORT "/dev/ttyS4" /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2. */ @@ -149,7 +131,6 @@ #define GPIO_HEATER_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) /* Power switch controls */ - #define SPEKTRUM_POWER(_on_true) px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (!_on_true)) /** @@ -166,17 +147,9 @@ #define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ #define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) -/** - * By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) #define BOARD_ADC_USB_VALID (px4_arch_gpioread(GPIO_VDD_USB_VALID)) -#define BOARD_ADC_SERVO_VALID (1) -#define BOARD_ADC_PERIPH_5V_OC (0) -#define BOARD_ADC_HIPOWER_5V_OC (0) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS @@ -189,25 +162,13 @@ __BEGIN_DECLS -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - #ifndef __ASSEMBLY__ -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - /**************************************************************************************************** * Name: stm32_spiinitialize * * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * Called to configure SPI chip select GPIO pins for the board. * ****************************************************************************************************/ diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 67af305537..cb5a59109d 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -49,6 +49,7 @@ px4_add_board( rc_input #roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 9b1824e29c..0c29bf55b5 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -50,6 +50,7 @@ px4_add_board( pwm_out px4io roboclaw + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v4pro/init/rc.board_sensors b/boards/px4/fmu-v4pro/init/rc.board_sensors index c687c96a4c..c6f2cccd16 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_sensors +++ b/boards/px4/fmu-v4pro/init/rc.board_sensors @@ -7,6 +7,7 @@ rgbled start -I rgbled_ncp5623c start -I board_adc start +system_power start # Internal SPI bus ICM-20602 or ICM-20608-G if ! icm20602 -s -R 8 start diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 7a87c46b58..6454b9cd5d 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -48,6 +48,7 @@ px4_add_board( pwm_out px4io roboclaw + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v4pro/src/board_config.h b/boards/px4/fmu-v4pro/src/board_config.h index c759fb714f..9b4a7fdd74 100644 --- a/boards/px4/fmu-v4pro/src/board_config.h +++ b/boards/px4/fmu-v4pro/src/board_config.h @@ -104,7 +104,7 @@ #define ADC_BATTERY1_VOLTAGE_CHANNEL 2 // PA2 #define ADC_BATTERY1_CURRENT_CHANNEL 3 // PA3 -#define ADC_5V_RAIL_SENSE 4 // PA4 +#define ADC_5V_RAIL_SENSE 4 // PA4 #define ADC_BATTERY2_VOLTAGE_CHANNEL 11 // PC1 #define ADC_BATTERY2_CURRENT_CHANNEL 13 // PC3 @@ -131,8 +131,7 @@ /* Power supply control and monitoring GPIOs */ #define GPIO_nVDD_BRICK1_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_nVDD_BRICK2_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN5) -#define BOARD_NUMBER_BRICKS 2 -#define GPIO_nVDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) +#define GPIO_nVDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) #define GPIO_VDD_3V3_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN10) #define GPIO_VDD_5V_HIPOWER_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN4) @@ -145,8 +144,7 @@ #define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) -/* PWM - */ +/* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 #define DIRECT_INPUT_TIMER_CHANNELS 6 @@ -184,15 +182,11 @@ * the connector interface and Spektrum requires VDD 3v3 to be controllable */ -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) #define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) #define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) -#define BOARD_ADC_SERVO_VALID (1) #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_OC)) diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 49fe32447b..8e5725cb78 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -53,6 +53,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index 9edd9ca693..c8d43cb159 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -56,6 +56,7 @@ px4_add_board( #roboclaw rpm safety_button + system_power #tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 8c9f8519be..bfa83e1f82 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -56,6 +56,7 @@ px4_add_board( roboclaw rpm safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index f7b35b887e..d98e7285c1 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -41,6 +41,7 @@ px4_add_board( px4io rc_input safety_button + system_power telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors index 712e3b6d7e..0440c97d0f 100644 --- a/boards/px4/fmu-v5/init/rc.board_sensors +++ b/boards/px4/fmu-v5/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal SPI bus ICM-20602 icm20602 -s -R 2 -q start diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index 7c566f9b54..dedeea8c3a 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -54,6 +54,7 @@ px4_add_board( roboclaw rpm safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 607e1a74c6..6f2de8ad97 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -46,6 +46,7 @@ px4_add_board( rc_input roboclaw safety_button + system_power tap_esc telemetry # all available telemetry drivers tone_alarm diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index 46a2d2599c..eb9471c3e7 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -49,6 +49,7 @@ px4_add_board( rc_input #roboclaw safety_button + system_power #tap_esc #telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index beb4a7ef7b..8ec30b1651 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -54,6 +54,7 @@ px4_add_board( roboclaw rpm safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h index 6cfdf8fa60..569c301274 100644 --- a/boards/px4/fmu-v5/src/board_config.h +++ b/boards/px4/fmu-v5/src/board_config.h @@ -173,18 +173,14 @@ (1 << ADC1_SPARE_1_CHANNEL)) #endif -/* Define Battery 1 Voltage Divider and A per V - */ - +/* Define Battery 1 Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ #define BOARD_BATTERY1_A_PER_V (36.367515152f) /* HW has to large of R termination on ADC todo:change when HW value is chosen */ - #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* HW Version and Revision drive signals Default to 1 to detect */ - #define BOARD_HAS_HW_VERSIONING #define GPIO_HW_REV_DRIVE /* PH14 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN14) @@ -194,20 +190,15 @@ #define HW_INFO_INIT {'V','5','x', 'x',0} #define HW_INFO_INIT_VER 2 #define HW_INFO_INIT_REV 3 -/* CAN Silence - * - * Silent mode control \ ESC Mux select - */ +/* CAN Silence Silent mode control \ ESC Mux select */ #define GPIO_CAN1_SILENT_S0 /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) #define GPIO_CAN2_SILENT_S1 /* PH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN3) #define GPIO_CAN3_SILENT_S2 /* PH4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN4) #define UAVCAN_NUM_IFACES_RUNTIME 1 -/* HEATER - * PWM in future - */ +/* HEATER PWM in future */ #define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) /* PWM Capture @@ -256,7 +247,6 @@ #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ -#define BOARD_NUMBER_BRICKS 2 #define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ #define GPIO_nVDD_5V_PERIPH_EN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) @@ -304,7 +294,6 @@ #define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2 /* RC Serial port */ - #define RC_SERIAL_PORT "/dev/ttyS4" #define RC_SERIAL_SINGLEWIRE @@ -371,40 +360,16 @@ #define SDIO_SLOTNO 0 /* Only one slot */ #define SDIO_MINOR 0 -/* SD card bringup does not work if performed on the IDLE thread because it - * will cause waiting. Use either: - * - * CONFIG_LIB_BOARDCTL=y, OR - * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y - */ - -#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \ - !defined(CONFIG_BOARD_INITTHREAD) -# warning SDIO initialization cannot be perfomed on the IDLE thread -#endif - -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) -#if BOARD_HAS_USB_VALID == 1 +#if defined(GPIO_nVDD_USB_VALID) # define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) -#else -# define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED #endif -/* FMUv5 never powers odd the Servo rail */ - -#define BOARD_ADC_SERVO_VALID (1) - #if !defined(BOARD_HAS_LTC44XX_VALIDS) || BOARD_HAS_LTC44XX_VALIDS == 0 # define BOARD_ADC_BRICK1_VALID (1) -# define BOARD_ADC_BRICK2_VALID (0) #elif BOARD_HAS_LTC44XX_VALIDS == 1 # define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) -# define BOARD_ADC_BRICK2_VALID (0) #elif BOARD_HAS_LTC44XX_VALIDS == 2 # define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) # define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index b7a4635baf..8eeda554fa 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -56,6 +56,7 @@ px4_add_board( #roboclaw #rpm safety_button + system_power #tap_esc telemetry # all available telemetry drivers #test_ppm diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index 50a8c31665..94a77d0df6 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -53,6 +53,7 @@ px4_add_board( roboclaw rpm safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 985a63bd73..1d0d99e7a7 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -55,6 +55,7 @@ px4_add_board( roboclaw rpm safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 8449a80dc9..d58910e594 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -3,6 +3,7 @@ # PX4 FMUv5X specific board sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # Start Digital power monitors ina226 -X -b 1 -t 1 -k start diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 64bcd8e4fb..1b207861cb 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -71,9 +71,6 @@ /* Configuration ************************************************************************************/ #define BOARD_HAS_LTC44XX_VALIDS 2 // N Bricks -#define BOARD_HAS_USB_VALID 1 // LTC Has USB valid -#define BOARD_HAS_NBAT_V 2d // 2 Digital Voltage -#define BOARD_HAS_NBAT_I 2d // 2 Digital Current /* PX4FMU GPIOs ***********************************************************************************/ @@ -230,8 +227,6 @@ #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ -#define BOARD_NUMBER_BRICKS 2 -#define BOARD_NUMBER_DIGITAL_BRICKS 2 #define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ #define GPIO_VDD_5V_PERIPH_nEN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) @@ -343,35 +338,13 @@ #define SDIO_SLOTNO 0 /* Only one slot */ #define SDIO_MINOR 0 -/* SD card bringup does not work if performed on the IDLE thread because it - * will cause waiting. Use either: - * - * CONFIG_LIB_BOARDCTL=y, OR - * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y - */ - -#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \ - !defined(CONFIG_BOARD_INITTHREAD) -# warning SDIO initialization cannot be perfomed on the IDLE thread -#endif - -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) -/* FMUv5X never powers off the Servo rail */ - -#define BOARD_ADC_SERVO_VALID (1) - #if !defined(BOARD_HAS_LTC44XX_VALIDS) || BOARD_HAS_LTC44XX_VALIDS == 0 # define BOARD_ADC_BRICK1_VALID (1) -# define BOARD_ADC_BRICK2_VALID (0) #elif BOARD_HAS_LTC44XX_VALIDS == 1 # define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) -# define BOARD_ADC_BRICK2_VALID (0) #elif BOARD_HAS_LTC44XX_VALIDS == 2 # define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) # define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 2f0068b40e..3623f2928e 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -54,6 +54,7 @@ px4_add_board( roboclaw rpm safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index d763b03898..683dc73226 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -3,6 +3,7 @@ # PX4 FMUv6X specific board sensors init #------------------------------------------------------------------------------ board_adc start +system_power start # Start Digital power monitors ina226 -X -b 1 -t 1 -k start diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index 33dda6df67..e683da2be1 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -74,9 +74,6 @@ /* Configuration ************************************************************************************/ #define BOARD_HAS_LTC44XX_VALIDS 2 // N Bricks -#define BOARD_HAS_USB_VALID 1 // LTC Has USB valid -#define BOARD_HAS_NBAT_V 2d // 2 Digital Voltage -#define BOARD_HAS_NBAT_I 2d // 2 Digital Current /* PX4FMU GPIOs ***********************************************************************************/ @@ -268,8 +265,6 @@ #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ -#define BOARD_NUMBER_BRICKS 2 -#define BOARD_NUMBER_DIGITAL_BRICKS 2 #define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ #define GPIO_VDD_5V_PERIPH_nEN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) @@ -390,17 +385,10 @@ # warning SDIO initialization cannot be perfomed on the IDLE thread #endif -/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) -/* FMUv6X never powers off the Servo rail */ - -#define BOARD_ADC_SERVO_VALID (1) - #if !defined(BOARD_HAS_LTC44XX_VALIDS) || BOARD_HAS_LTC44XX_VALIDS == 0 # define BOARD_ADC_BRICK1_VALID (1) # define BOARD_ADC_BRICK2_VALID (0) diff --git a/boards/px4/fmu-v6x/stackcheck.cmake b/boards/px4/fmu-v6x/stackcheck.cmake index 2f0068b40e..3623f2928e 100644 --- a/boards/px4/fmu-v6x/stackcheck.cmake +++ b/boards/px4/fmu-v6x/stackcheck.cmake @@ -54,6 +54,7 @@ px4_add_board( roboclaw rpm safety_button + system_power tap_esc telemetry # all available telemetry drivers test_ppm diff --git a/boards/px4/sitl/src/board_config.h b/boards/px4/sitl/src/board_config.h index 09294c5232..d0772a135f 100644 --- a/boards/px4/sitl/src/board_config.h +++ b/boards/px4/sitl/src/board_config.h @@ -50,7 +50,6 @@ #define PX4_NUMBER_I2C_BUSES 1 -#define BOARD_NUMBER_BRICKS 0 #define BOARD_HAS_CONTROL_STATUS_LEDS 1 #define BOARD_OVERLOAD_LED LED_RED #define BOARD_ARMED_LED LED_BLUE diff --git a/boards/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index dfa30b3047..fe79f6dfb4 100644 --- a/boards/spracing/h7extreme/default.cmake +++ b/boards/spracing/h7extreme/default.cmake @@ -49,6 +49,7 @@ px4_add_board( #roboclaw #tap_esc rc_input + system_power telemetry # all available telemetry drivers #test_ppm tone_alarm diff --git a/boards/spracing/h7extreme/src/board_config.h b/boards/spracing/h7extreme/src/board_config.h index 1fd2a0c2b6..1c9f07e018 100644 --- a/boards/spracing/h7extreme/src/board_config.h +++ b/boards/spracing/h7extreme/src/board_config.h @@ -50,17 +50,10 @@ #include -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ - #define FLASH_BASED_PARAMS #define BOARD_USE_EXTERNAL_FLASH //Configuration and firmware are in external flash -#define BOARD_HAS_USB_VALID 1 // LTC Has No USB valid - /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ - #define GPIO_nLED_RED /* PE3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define BOARD_HAS_CONTROL_STATUS_LEDS 1 @@ -80,7 +73,6 @@ /* PC1 */ GPIO_ADC123_INP11, \ /* PC0 */ GPIO_ADC123_INP10 - /* Define Channel numbers must match above GPIO pin IN(n)*/ #define ADC_RSSI_IN_CHANNEL /* PC4 */ 4 #define ADC_BATTERY_VOLTAGE_CHANNEL /* PC1 */ 11 @@ -88,13 +80,11 @@ #define ADC_CHANNELS (1 << 4) | (1 << 10) | (1 << 11) -/* Define Battery 1 Voltage Divider and A per V - */ +/* Define Battery 1 Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (10.9f) #define BOARD_BATTERY1_A_PER_V (17.f) -/* PWM - */ +/* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 #define DIRECT_INPUT_TIMER_CHANNELS 8 @@ -102,19 +92,14 @@ #define GPIO_TONE_ALARM_IDLE /* PE5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN5) #define GPIO_TONE_ALARM_GPIO /* PE5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN5) -/* USB OTG FS - * - * PA8 OTG_FS_VBUS VBUS sensing - */ +/* USB OTG FS PA8 OTG_FS_VBUS VBUS sensing */ #define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ #define HRT_TIMER 2 /* use timer2 for the HRT */ #define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ - /* RC Serial port */ - #define RC_SERIAL_PORT "/dev/ttyS0" #define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) @@ -131,8 +116,8 @@ #define BOARD_HAS_ON_RESET 1 #define PX4_GPIO_INIT_LIST { \ - PX4_ADC_GPIO, \ - GPIO_TONE_ALARM_IDLE, \ + PX4_ADC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ GPIO_RSSI_IN, \ } @@ -144,20 +129,8 @@ __BEGIN_DECLS -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - #ifndef __ASSEMBLY__ -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - /**************************************************************************** * Name: stm32_sdio_initialize * diff --git a/boards/uvify/core/init/rc.board_sensors b/boards/uvify/core/init/rc.board_sensors index eb3e3a410d..1986950e53 100644 --- a/boards/uvify/core/init/rc.board_sensors +++ b/boards/uvify/core/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ board_adc start +system_power start # Internal SPI if ! ms5611 -T 5607 -s start diff --git a/boards/uvify/core/src/board_config.h b/boards/uvify/core/src/board_config.h index de1b4730c5..57c004bb9f 100644 --- a/boards/uvify/core/src/board_config.h +++ b/boards/uvify/core/src/board_config.h @@ -164,17 +164,10 @@ #define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ #define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) -/** - * By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) - * this board support the ADC system_power interface, and therefore - * provides the true logic GPIO BOARD_ADC_xxxx macros. - */ + #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) #define BOARD_ADC_USB_VALID (px4_arch_gpioread(GPIO_VDD_USB_VALID)) -#define BOARD_ADC_SERVO_VALID (1) -#define BOARD_ADC_PERIPH_5V_OC (0) -#define BOARD_ADC_HIPOWER_5V_OC (0) #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS diff --git a/msg/adc_report.msg b/msg/adc_report.msg index 1ae72b6d6c..6fb021f619 100644 --- a/msg/adc_report.msg +++ b/msg/adc_report.msg @@ -4,3 +4,5 @@ int16[12] channel_id # ADC channel IDs, negative for non-existent, TODO: should int32[12] raw_data # ADC channel raw value, accept negative value, valid if channel ID is positive uint32 resolution # ADC channel resolution float32 v_ref # ADC channel voltage reference, use to calculate LSB voltage(lsb=scale/resolution) + +uint8 MAX_ADC_CHANNELS = 12 diff --git a/msg/system_power.msg b/msg/system_power.msg index 21b35ba1dc..2a34474c5c 100644 --- a/msg/system_power.msg +++ b/msg/system_power.msg @@ -1,21 +1,30 @@ -uint64 timestamp # time since system start (microseconds) -float32 voltage5v_v # peripheral 5V rail voltage -float32[4] sensors3v3 # Sensors 3V3 rail voltage -uint8 sensors3v3_valid # Sensors 3V3 rail voltage was read (bitfield). -uint8 usb_connected # USB is connected when 1 -uint8 brick_valid # brick bits power is good when bit 1 -uint8 usb_valid # USB is valid when 1 -uint8 servo_valid # servo power is good when 1 -uint8 periph_5v_oc # peripheral overcurrent when 1 -uint8 hipower_5v_oc # high power peripheral overcurrent when 1 -uint8 comp_5v_valid # 5V to companion valid -uint8 can1_gps1_5v_valid # 5V for CAN1/GPS1 valid +uint64 timestamp # time since system start (microseconds) -uint8 BRICK1_VALID_SHIFTS=0 -uint8 BRICK1_VALID_MASK=1 -uint8 BRICK2_VALID_SHIFTS=1 -uint8 BRICK2_VALID_MASK=2 -uint8 BRICK3_VALID_SHIFTS=2 -uint8 BRICK3_VALID_MASK=4 -uint8 BRICK4_VALID_SHIFTS=3 -uint8 BRICK4_VALID_MASK=8 +bool voltage5v_available # peripheral 5V rail voltage status available +float32 voltage5v_v # peripheral 5V rail voltage + +bool[4] sensors3v3_available # sensor 3.3V rail voltage status available +float32[4] sensors3v3 # Sensors 3V3 rail voltage + +bool usb_connected # USB is connected when true +bool usb_power_valid # On USB power when true + +bool servo_power_available # servo power status available +bool servo_power_valid # servo power is good when true + +bool peripheral_5v_available # 5V peripheral status available +bool peripheral_5v_overcurrent # 5V peripheral overcurrent when true + +bool hipower_5v_available # 5V hi power peripheral status available +bool hipower_5v_overcurrent # 5V hi power peripheral overcurrent when true + +bool comp_5v_available # 5V to companion status available +bool comp_5v_valid # 5V to companion valid + +bool can1_gps1_5v_available # 5V for CAN1/GPS1 status available +bool can1_gps1_5v_valid # 5V for CAN1/GPS1 valid + +uint8 bricks_supported +bool[4] brick_valid # brick power is good when true +float32[4] brick_voltage_adc # brick voltage ADC +float32[4] brick_current_adc # brick current ADC diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 2cbd524766..38dfc7fa2d 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -129,37 +129,41 @@ # define BOARD_VALID_UV (3.7f) #endif -/* Legacy default */ - -#if !defined(BOARD_NUMBER_BRICKS) -# define BOARD_NUMBER_BRICKS 1 -# if !defined(BOARD_ADC_BRICK_VALID) -# define BOARD_ADC_BRICK_VALID (1) -# endif -#endif - -#if BOARD_NUMBER_BRICKS == 0 -/* allow SITL to disable all bricks */ -#elif BOARD_NUMBER_BRICKS == 1 -# define BOARD_BATT_V_LIST {ADC_BATTERY_VOLTAGE_CHANNEL} -# define BOARD_BATT_I_LIST {ADC_BATTERY_CURRENT_CHANNEL} -# define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK_VALID} -#elif BOARD_NUMBER_BRICKS == 2 -# if !defined(BOARD_NUMBER_DIGITAL_BRICKS) -# define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL} -# define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL} -# endif -# define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK1_VALID, BOARD_ADC_BRICK2_VALID} -#elif BOARD_NUMBER_BRICKS == 3 -# define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL, ADC_BATTERY3_VOLTAGE_CHANNEL} -# define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL, ADC_BATTERY3_CURRENT_CHANNEL} -# define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK1_VALID, BOARD_ADC_BRICK2_VALID, BOARD_ADC_BRICK3_VALID} -#elif BOARD_NUMBER_BRICKS == 4 +#if defined(ADC_BATTERY4_VOLTAGE_CHANNEL) && defined(ADC_BATTERY4_CURRENT_CHANNEL) +# define BOARD_NUMBER_BRICKS 4 # define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL, ADC_BATTERY3_VOLTAGE_CHANNEL, ADC_BATTERY4_VOLTAGE_CHANNEL} # define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL, ADC_BATTERY3_CURRENT_CHANNEL, ADC_BATTERY4_CURRENT_CHANNEL} +# if defined(BOARD_ADC_BRICK4_VALID) # define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK1_VALID, BOARD_ADC_BRICK2_VALID, BOARD_ADC_BRICK3_VALID, BOARD_ADC_BRICK4_VALID} -#else -# error Unsuported BOARD_NUMBER_BRICKS number. +# endif // BOARD_ADC_BRICK4_VALID +#elif defined(ADC_BATTERY3_VOLTAGE_CHANNEL) && defined(ADC_BATTERY3_CURRENT_CHANNEL) +# define BOARD_NUMBER_BRICKS 3 +# define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL, ADC_BATTERY3_VOLTAGE_CHANNEL} +# define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL, ADC_BATTERY3_CURRENT_CHANNEL} +# if defined(BOARD_ADC_BRICK3_VALID) +# define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK1_VALID, BOARD_ADC_BRICK2_VALID, BOARD_ADC_BRICK3_VALID} +# endif // BOARD_ADC_BRICK3_VALID +#elif defined(ADC_BATTERY2_VOLTAGE_CHANNEL) && defined(ADC_BATTERY2_CURRENT_CHANNEL) +# define BOARD_NUMBER_BRICKS 2 +# define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL} +# define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL} +# if defined(BOARD_ADC_BRICK2_VALID) +# define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK1_VALID, BOARD_ADC_BRICK2_VALID} +# endif // BOARD_ADC_BRICK2_VALID +#elif defined(ADC_BATTERY1_VOLTAGE_CHANNEL) && defined(ADC_BATTERY1_CURRENT_CHANNEL) +# define BOARD_NUMBER_BRICKS 1 +# define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL} +# define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL} +# if defined(BOARD_ADC_BRICK1_VALID) +# define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK1_VALID} +# endif // BOARD_ADC_BRICK1_VALID +#elif defined(ADC_BATTERY_VOLTAGE_CHANNEL) && defined(ADC_BATTERY_CURRENT_CHANNEL) +# define BOARD_NUMBER_BRICKS 1 +# define BOARD_BATT_V_LIST {ADC_BATTERY_VOLTAGE_CHANNEL} +# define BOARD_BATT_I_LIST {ADC_BATTERY_CURRENT_CHANNEL} +# if defined(BOARD_ADC_BRICK_VALID) +# define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK_VALID} +# endif // BOARD_ADC_BRICK_VALID #endif /* Choose the source for ADC_SCALED_V5_SENSE */ diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 55bdb73284..35ce4a476a 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -35,6 +35,7 @@ ms5611 -X start navio_rgbled start board_adc start +system_power start battery_status start gps start -d /dev/spidev0.0 -i spi -p ubx diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 7f2f5c228e..ea529727b1 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -35,6 +35,7 @@ ms5611 -X start navio_rgbled start board_adc start +system_power start battery_status start gps start -d /dev/spidev0.0 -i spi -p ubx diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index d31dc08ffe..8202808283 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -35,6 +35,7 @@ ms5611 -X start navio_rgbled start board_adc start +system_power start battery_status start gps start -d /dev/spidev0.0 -i spi -p ubx diff --git a/src/drivers/adc/ads1115/ads1115_main.cpp b/src/drivers/adc/ads1115/ads1115_main.cpp index 9d09f09e95..950aa83909 100644 --- a/src/drivers/adc/ads1115/ads1115_main.cpp +++ b/src/drivers/adc/ads1115/ads1115_main.cpp @@ -51,7 +51,7 @@ ADS1115::ADS1115(I2CSPIBusOption bus_option, int bus, int addr, int bus_frequenc _adc_report.resolution = 32768; _adc_report.v_ref = 6.144f; - for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { + for (unsigned i = 0; i < adc_report_s::MAX_ADC_CHANNELS; ++i) { _adc_report.channel_id[i] = -1; } @@ -209,4 +209,4 @@ extern "C" int ads1115_main(int argc, char *argv[]) ThisDriver::print_usage(); return -1; -} \ No newline at end of file +} diff --git a/src/drivers/adc/board_adc/ADC.cpp b/src/drivers/adc/board_adc/ADC.cpp index 7552dd8a20..b1a56c01d2 100644 --- a/src/drivers/adc/board_adc/ADC.cpp +++ b/src/drivers/adc/board_adc/ADC.cpp @@ -35,15 +35,20 @@ #include "ADC.hpp" -#ifdef CONFIG_DEV_GPIO -#include -#endif - ADC::ADC(uint32_t base_address, uint32_t channels) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), _base_address(base_address) { + for (unsigned i = 0; i < adc_report_s::MAX_ADC_CHANNELS; i++) { + // set all channels to -1 initially + _am_channel[i] = -1; + } + + for (unsigned i = 0; i < sizeof(adc_report_s::channel_id) / sizeof(adc_report_s::channel_id[0]); i++) { + // set all channels to -1 initially + _adc_report.channel_id[i] = -1; + } + /* always enable the temperature sensor */ channels |= px4_arch_adc_temp_sensor_mask(); @@ -54,22 +59,13 @@ ADC::ADC(uint32_t base_address, uint32_t channels) : } } - if (_channel_count > PX4_MAX_ADC_CHANNELS) { - PX4_ERR("PX4_MAX_ADC_CHANNELS is too small (%d, %d)", (unsigned)PX4_MAX_ADC_CHANNELS, _channel_count); - } - - _samples = new px4_adc_msg_t[_channel_count]; - /* prefill the channel numbers in the sample array */ - if (_samples != nullptr) { - unsigned index = 0; + unsigned index = 0; - for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) { - if (channels & (1 << i)) { - _samples[index].am_channel = i; - _samples[index].am_data = 0; - index++; - } + for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) { + if (channels & (1 << i)) { + _am_channel[index] = i; + index++; } } } @@ -78,13 +74,8 @@ ADC::~ADC() { ScheduleClear(); - if (_samples != nullptr) { - delete _samples; - } - - perf_free(_sample_perf); + perf_free(_cycle_perf); px4_arch_adc_uninit(_base_address); - close_gpio_devices(); } int ADC::init() @@ -104,199 +95,41 @@ int ADC::init() void ADC::Run() { - if (_first_run) { - open_gpio_devices(); - _first_run = false; + if (should_exit()) { + exit_and_cleanup(); + return; } - hrt_abstime now = hrt_absolute_time(); + perf_begin(_cycle_perf); - /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) { - _samples[i].am_data = sample(_samples[i].am_channel); + bool updated = false; + + if (hrt_elapsed_time(&_adc_report.timestamp) > 1_s) { + // force update + updated = true; } - update_adc_report(now); - update_system_power(now); -} + for (unsigned i = 0; i < math::min(_channel_count, adc_report_s::MAX_ADC_CHANNELS); i++) { + if (_am_channel[i] != -1) { + uint32_t result = px4_arch_adc_sample(_base_address, _am_channel[i]); + _adc_report.channel_id[i] = _am_channel[i]; -void ADC::open_gpio_devices() -{ -#ifdef BOARD_GPIO_VDD_5V_COMP_VALID - _5v_comp_valid_fd = open(BOARD_GPIO_VDD_5V_COMP_VALID, O_RDONLY); -#endif -#ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID - _5v_can1_gps1_valid_fd = open(BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID, O_RDONLY); -#endif -} - -void ADC::close_gpio_devices() -{ -#ifdef BOARD_GPIO_VDD_5V_COMP_VALID - close(_5v_comp_valid_fd); -#endif -#ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID - close(_5v_can1_gps1_valid_fd); -#endif -} - -void ADC::update_adc_report(hrt_abstime now) -{ - adc_report_s adc = {}; - adc.timestamp = now; - adc.device_id = BUILTIN_ADC_DEVID; - - unsigned max_num = _channel_count; - - if (max_num > (sizeof(adc.channel_id) / sizeof(adc.channel_id[0]))) { - max_num = (sizeof(adc.channel_id) / sizeof(adc.channel_id[0])); - } - - unsigned i; - - for (i = 0; i < max_num; i++) { - adc.channel_id[i] = _samples[i].am_channel; - adc.raw_data[i] = _samples[i].am_data; - } - - for (; i < PX4_MAX_ADC_CHANNELS; ++i) { // set unused channel id to -1 - adc.channel_id[i] = -1; - } - - adc.v_ref = px4_arch_adc_reference_v(); - adc.resolution = px4_arch_adc_dn_fullcount(); - - _to_adc_report.publish(adc); -} - -uint8_t ADC::read_gpio_value(int fd) -{ -#ifdef CONFIG_DEV_GPIO - - if (fd == -1) { - return 0xff; - } - - bool value; - - if (ioctl(fd, GPIOC_READ, (long)&value) != 0) { - return 0xff; - } - - return value; -#else - return 0xff; -#endif /* CONFIG_DEV_GPIO */ -} - -void ADC::update_system_power(hrt_abstime now) -{ -#if defined (BOARD_ADC_USB_CONNECTED) - system_power_s system_power {}; - - /* Assume HW provides only ADC_SCALED_V5_SENSE */ - int cnt = 1; - /* HW provides both ADC_SCALED_V5_SENSE and ADC_SCALED_V3V3_SENSORS_SENSE */ -# if defined(ADC_SCALED_V5_SENSE) && defined(ADC_SCALED_V3V3_SENSORS_SENSE) - cnt += ADC_SCALED_V3V3_SENSORS_COUNT; -# endif - - for (unsigned i = 0; i < _channel_count; i++) { -# if defined(ADC_SCALED_V5_SENSE) - - if (_samples[i].am_channel == ADC_SCALED_V5_SENSE) { - // it is 2:1 scaled - system_power.voltage5v_v = _samples[i].am_data * (ADC_V5_V_FULL_SCALE / px4_arch_adc_dn_fullcount()); - cnt--; - - } else -# endif -# if defined(ADC_SCALED_V3V3_SENSORS_SENSE) - { - const int sensors_channels[ADC_SCALED_V3V3_SENSORS_COUNT] = ADC_SCALED_V3V3_SENSORS_SENSE; - static_assert(sizeof(system_power.sensors3v3) / sizeof(system_power.sensors3v3[0]) >= ADC_SCALED_V3V3_SENSORS_COUNT, - "array too small"); - - for (int j = 0; j < ADC_SCALED_V3V3_SENSORS_COUNT; ++j) { - if (_samples[i].am_channel == sensors_channels[j]) { - // it is 2:1 scaled - system_power.sensors3v3[j] = _samples[i].am_data * (ADC_3V3_SCALE * (3.3f / px4_arch_adc_dn_fullcount())); - system_power.sensors3v3_valid |= 1 << j; - cnt--; - } + if ((result != UINT32_MAX) && ((int32_t)result != _adc_report.raw_data[i])) { + _adc_report.raw_data[i] = result; + updated = true; } } - -# endif - - if (cnt == 0) { - break; - } } - /* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED, - * It must provide the true logic GPIO BOARD_ADC_xxxx macros. - */ - // these are not ADC related, but it is convenient to - // publish these to the same topic - - system_power.usb_connected = BOARD_ADC_USB_CONNECTED; - /* If provided used the Valid signal from HW*/ -#if defined(BOARD_ADC_USB_VALID) - system_power.usb_valid = BOARD_ADC_USB_VALID; -#else - /* If not provided then use connected */ - system_power.usb_valid = system_power.usb_connected; -#endif - -#if defined(BOARD_BRICK_VALID_LIST) - /* The valid signals (HW dependent) are associated with each brick */ - bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST; - system_power.brick_valid = 0; - - for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { - system_power.brick_valid |= valid_chan[b] ? 1 << b : 0; + if (updated) { + _adc_report.device_id = BUILTIN_ADC_DEVID; + _adc_report.v_ref = px4_arch_adc_reference_v(); + _adc_report.resolution = px4_arch_adc_dn_fullcount(); + _adc_report.timestamp = hrt_absolute_time(); + _adc_report_pub.publish(_adc_report); } -#endif - -#if defined(BOARD_ADC_SERVO_VALID) - system_power.servo_valid = BOARD_ADC_SERVO_VALID; -#endif - -#if defined(BOARD_ADC_PERIPH_5V_OC) - // OC pins are active low - system_power.periph_5v_oc = BOARD_ADC_PERIPH_5V_OC; -#endif - -#if defined(BOARD_ADC_HIPOWER_5V_OC) - system_power.hipower_5v_oc = BOARD_ADC_HIPOWER_5V_OC; -#endif - -#ifdef BOARD_GPIO_VDD_5V_COMP_VALID - system_power.comp_5v_valid = read_gpio_value(_5v_comp_valid_fd); -#endif -#ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID - system_power.can1_gps1_5v_valid = read_gpio_value(_5v_can1_gps1_valid_fd); -#endif - - system_power.timestamp = hrt_absolute_time(); - _to_system_power.publish(system_power); - -#endif // BOARD_ADC_USB_CONNECTED -} - -uint32_t ADC::sample(unsigned channel) -{ - perf_begin(_sample_perf); - uint32_t result = px4_arch_adc_sample(_base_address, channel); - - if (result == UINT32_MAX) { - PX4_ERR("sample timeout"); - } - - perf_end(_sample_perf); - return result; + perf_end(_cycle_perf); } int ADC::test() @@ -312,7 +145,7 @@ int ADC::test() PX4_INFO_RAW("Voltage Reference: %f\n", (double)adc.v_ref); for (unsigned l = 0; l < 20; ++l) { - for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { + for (unsigned i = 0; i < adc_report_s::MAX_ADC_CHANNELS; ++i) { if (adc.channel_id[i] >= 0) { PX4_INFO_RAW("% 2d:% 6d", adc.channel_id[i], adc.raw_data[i]); } diff --git a/src/drivers/adc/board_adc/ADC.hpp b/src/drivers/adc/board_adc/ADC.hpp index b53fe3d47e..d37827f7ad 100644 --- a/src/drivers/adc/board_adc/ADC.hpp +++ b/src/drivers/adc/board_adc/ADC.hpp @@ -50,7 +50,6 @@ #include #include #include -#include using namespace time_literals; @@ -84,36 +83,16 @@ private: void Run() override; - /** - * Sample a single channel and return the measured value. - * - * @param channel The channel to sample. - * @return The sampled value, or UINT32_MAX if sampling failed. - */ - uint32_t sample(unsigned channel); - - void update_adc_report(hrt_abstime now); - void update_system_power(hrt_abstime now); - - void open_gpio_devices(); - void close_gpio_devices(); - uint8_t read_gpio_value(int fd); - static const hrt_abstime kINTERVAL{10_ms}; /**< 100Hz base rate */ - perf_counter_t _sample_perf; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - unsigned _channel_count{0}; const uint32_t _base_address; - px4_adc_msg_t *_samples{nullptr}; /**< sample buffer */ + uint8_t _channel_count{0}; - uORB::Publication _to_adc_report{ORB_ID(adc_report)}; - uORB::Publication _to_system_power{ORB_ID(system_power)}; -#ifdef BOARD_GPIO_VDD_5V_COMP_VALID - int _5v_comp_valid_fd {-1}; -#endif -#ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID - int _5v_can1_gps1_valid_fd {-1}; -#endif - bool _first_run {true}; + int8_t _am_channel[adc_report_s::MAX_ADC_CHANNELS] {}; + + uORB::Publication _adc_report_pub{ORB_ID(adc_report)}; + + adc_report_s _adc_report{}; }; diff --git a/src/drivers/drv_adc.h b/src/drivers/drv_adc.h index 15d372621a..8b5a4517cb 100644 --- a/src/drivers/drv_adc.h +++ b/src/drivers/drv_adc.h @@ -45,22 +45,8 @@ #include #include -/* Define the PX4 low level format ADC and the maximum - * number of channels that can be returned by a lowlevel - * ADC driver. Drivers may return less than PX4_MAX_ADC_CHANNELS - * but no more than PX4_MAX_ADC_CHANNELS. - * - */ -#define PX4_MAX_ADC_CHANNELS arraySize(adc_report_s::channel_id) -typedef struct __attribute__((packed)) px4_adc_msg_t { - uint8_t am_channel; /* The 8-bit ADC Channel */ - int32_t am_data; /* ADC convert result (4 bytes) */ -} px4_adc_msg_t; - - #define BUILTIN_ADC_DEVID 0xffffffff // TODO: integrate into existing ID management - __BEGIN_DECLS /** diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 9ed3ef830c..3b63b37f05 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -327,7 +327,7 @@ void RCInput::Run() adc_report_s adc; if (_adc_sub.update(&adc)) { - for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { + for (unsigned i = 0; i < adc_report_s::MAX_ADC_CHANNELS; ++i) { if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) { float adc_volt = adc.raw_data[i] * adc.v_ref / diff --git a/src/drivers/system_power/CMakeLists.txt b/src/drivers/system_power/CMakeLists.txt new file mode 100644 index 0000000000..ee1dc968e7 --- /dev/null +++ b/src/drivers/system_power/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2019-2020 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__system_power + MAIN system_power + SRCS + SystemPower.cpp + SystemPower.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/system_power/SystemPower.cpp b/src/drivers/system_power/SystemPower.cpp new file mode 100644 index 0000000000..711b67f42a --- /dev/null +++ b/src/drivers/system_power/SystemPower.cpp @@ -0,0 +1,417 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include "SystemPower.hpp" + +#if defined(CONFIG_DEV_GPIO) +# include +#endif // CONFIG_DEV_GPIO + +SystemPower::SystemPower() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +SystemPower::~SystemPower() +{ + DataReadyInterruptDisable(); + ScheduleClear(); + + perf_free(_cycle_perf); + + CloseGpioDevices(); +} + +int SystemPower::init() +{ + DataReadyInterruptConfigure(); + ScheduleDelayed(1_s); + return PX4_OK; +} + +void SystemPower::OpenGpioDevices() +{ +#if defined(BOARD_GPIO_VDD_5V_COMP_VALID) + _5v_comp_valid_fd = open(BOARD_GPIO_VDD_5V_COMP_VALID, O_RDONLY); +#endif // BOARD_GPIO_VDD_5V_COMP_VALID + +#if defined(BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID) + _5v_can1_gps1_valid_fd = open(BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID, O_RDONLY); +#endif // BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID +} + +void SystemPower::CloseGpioDevices() +{ +#if defined(BOARD_GPIO_VDD_5V_COMP_VALID) + close(_5v_comp_valid_fd); +#endif // BOARD_GPIO_VDD_5V_COMP_VALID + +#if defined(BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID) + close(_5v_can1_gps1_valid_fd); +#endif // BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID +} + +uint8_t SystemPower::ReadGpioValue(int fd) +{ +#if defined(CONFIG_DEV_GPIO) + + if (fd == -1) { + return 0xff; + } + + bool value; + + if (ioctl(fd, GPIOC_READ, (long)&value) != 0) { + return 0xff; + } + + return value; +#else + return 0xff; +#endif // CONFIG_DEV_GPIO +} + +int SystemPower::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void SystemPower::DataReady() +{ + ScheduleNow(); +} + +bool SystemPower::DataReadyInterruptConfigure() +{ + // Setup data ready on rising and falling edge +#if defined(GPIO_OTGFS_VBUS) + px4_arch_gpiosetevent(GPIO_OTGFS_VBUS, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_OTGFS_VBUS + +#if defined(GPIO_nVDD_USB_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_USB_VALID, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_nVDD_USB_VALID + +#if defined(GPIO_VDD_USB_VALID) + px4_arch_gpiosetevent(GPIO_VDD_USB_VALID, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_VDD_USB_VALID + +#if defined(GPIO_VDD_SERVO_VALID) + px4_arch_gpiosetevent(GPIO_VDD_SERVO_VALID, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_VDD_SERVO_VALID + +#if defined(GPIO_nVDD_5V_PERIPH_OC) + px4_arch_gpiosetevent(GPIO_nVDD_5V_PERIPH_OC, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_nVDD_5V_PERIPH_OC + +#if defined(GPIO_VDD_5V_HIPOWER_OC) + px4_arch_gpiosetevent(GPIO_VDD_5V_HIPOWER_OC, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_VDD_5V_HIPOWER_OC + +#if defined(GPIO_nVDD_BRICK1_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_BRICK1_VALID, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_nVDD_BRICK1_VALID + +#if defined(GPIO_nVDD_BRICK2_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_BRICK2_VALID, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_nVDD_BRICK2_VALID + +#if defined(GPIO_nVDD_BRICK3_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_BRICK3_VALID, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_nVDD_BRICK3_VALID + +#if defined(GPIO_nVDD_BRICK4_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_BRICK4_VALID, true, true, true, &DataReadyInterruptCallback, this); +#endif // GPIO_nVDD_BRICK4_VALID + + return true; +} + +bool SystemPower::DataReadyInterruptDisable() +{ +#if defined(GPIO_OTGFS_VBUS) + px4_arch_gpiosetevent(GPIO_OTGFS_VBUS, false, false, false, nullptr, nullptr); +#endif // GPIO_OTGFS_VBUS + +#if defined(GPIO_nVDD_USB_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_USB_VALID, false, false, false, nullptr, nullptr); +#endif // GPIO_nVDD_USB_VALID + +#if defined(GPIO_VDD_USB_VALID) + px4_arch_gpiosetevent(GPIO_VDD_USB_VALID, false, false, false, nullptr, nullptr); +#endif // GPIO_VDD_USB_VALID + +#if defined(GPIO_VDD_SERVO_VALID) + px4_arch_gpiosetevent(GPIO_VDD_SERVO_VALID, false, false, false, nullptr, nullptr); +#endif // GPIO_VDD_SERVO_VALID + +#if defined(GPIO_nVDD_5V_PERIPH_OC) + px4_arch_gpiosetevent(GPIO_nVDD_5V_PERIPH_OC, false, false, false, nullptr, nullptr); +#endif // GPIO_nVDD_5V_PERIPH_OC + +#if defined(GPIO_VDD_5V_HIPOWER_OC) + px4_arch_gpiosetevent(GPIO_VDD_5V_HIPOWER_OC, false, false, false, nullptr, nullptr); +#endif // GPIO_VDD_5V_HIPOWER_OC + +#if defined(GPIO_nVDD_BRICK1_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_BRICK1_VALID, false, false, false, nullptr, nullptr); +#endif // GPIO_nVDD_BRICK1_VALID + +#if defined(GPIO_nVDD_BRICK2_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_BRICK2_VALID, false, false, false, nullptr, nullptr); +#endif // GPIO_nVDD_BRICK2_VALID + +#if defined(GPIO_nVDD_BRICK3_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_BRICK3_VALID, false, false, false, nullptr, nullptr); +#endif // GPIO_nVDD_BRICK3_VALID + +#if defined(GPIO_nVDD_BRICK4_VALID) + px4_arch_gpiosetevent(GPIO_nVDD_BRICK4_VALID, false, false, false, nullptr, nullptr); +#endif // GPIO_nVDD_BRICK4_VALID + + return true; +} + +void SystemPower::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + if (_first_run) { + OpenGpioDevices(); + _first_run = false; + } + + perf_begin(_cycle_perf); + ScheduleDelayed(500_ms); + + bool updated = false; + + system_power_s system_power{}; + + for (auto &v : system_power.sensors3v3) { + v = NAN; + } + + for (auto &v : system_power.brick_voltage_adc) { + v = NAN; + } + + for (auto &v : system_power.brick_current_adc) { + v = NAN; + } + +#if defined(ADC_SCALED_V5_SENSE) || defined(ADC_SCALED_V3V3_SENSORS_SENSE) || defined(BOARD_NUMBER_BRICKS) + adc_report_s adc_report; + + if (_adc_report_sub.copy(&adc_report)) { + if (hrt_elapsed_time(&adc_report.timestamp) < 1_s) { + for (unsigned adc_index = 0; adc_index < (sizeof(adc_report.channel_id) / sizeof(adc_report.channel_id[0])); + adc_index++) { +# if defined(ADC_SCALED_V5_SENSE) + + if (adc_report.channel_id[adc_index] == ADC_SCALED_V5_SENSE) { + system_power.voltage5v_v = adc_report.raw_data[adc_index] * (ADC_V5_V_FULL_SCALE / adc_report.resolution); + system_power.voltage5v_available = true; + updated = true; + } + +# endif // ADC_SCALED_V5_SENSE + +# if defined(ADC_SCALED_V3V3_SENSORS_SENSE) + const int sensors_channels[ADC_SCALED_V3V3_SENSORS_COUNT] = ADC_SCALED_V3V3_SENSORS_SENSE; + static_assert(sizeof(system_power.sensors3v3) / sizeof(system_power.sensors3v3[0]) >= ADC_SCALED_V3V3_SENSORS_COUNT, + "array too small"); + + for (int sensor_index = 0; sensor_index < ADC_SCALED_V3V3_SENSORS_COUNT; sensor_index++) { + if (adc_report.channel_id[adc_index] == sensors_channels[sensor_index]) { + // it is 2:1 scaled + system_power.sensors3v3[sensor_index] = adc_report.raw_data[adc_index] * (ADC_3V3_SCALE * + (3.3f / adc_report.resolution)); + system_power.sensors3v3_available[sensor_index] = true; + updated = true; + } + } + +# endif // ADC_SCALED_V3V3_SENSORS_SENSE + + +# if defined(BOARD_NUMBER_BRICKS) + const float scale = adc_report.v_ref / adc_report.resolution; + + for (int brick_index = 0; brick_index < BOARD_NUMBER_BRICKS; brick_index++) { + // brick voltage ADC reading + static constexpr int batt_v_list[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST; + + if (adc_report.channel_id[adc_index] == batt_v_list[brick_index]) { + system_power.brick_voltage_adc[brick_index] = adc_report.raw_data[adc_index] * scale; + } + + // brick current ADC reading + static constexpr int batt_i_list[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST; + + if (adc_report.channel_id[adc_index] == batt_i_list[brick_index]) { + system_power.brick_current_adc[brick_index] = adc_report.raw_data[adc_index] * scale; + } + } + +#endif // BOARD_NUMBER_BRICKS + } + } + } + +#endif // ADC_SCALED_V5_SENSE || ADC_SCALED_V3V3_SENSORS_SENSE || BOARD_NUMBER_BRICKS + + +#if defined(BOARD_ADC_USB_CONNECTED) + system_power.usb_connected = BOARD_ADC_USB_CONNECTED; + updated = true; +#endif // BOARD_ADC_USB_CONNECTED + +#if defined(BOARD_ADC_USB_VALID) + // If provided used the Valid signal from HW + system_power.usb_power_valid = BOARD_ADC_USB_VALID; + updated = true; +#else + // If not provided then use connected + system_power.usb_power_valid = system_power.usb_connected; +#endif + +#if defined(BOARD_BRICK_VALID_LIST) + system_power.bricks_supported = BOARD_NUMBER_BRICKS; + + // The valid signals (HW dependent) are associated with each brick + bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST; + + for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { + system_power.brick_valid[b] = valid_chan[b]; + updated = true; + } + +#endif // BOARD_BRICK_VALID_LIST + +#if defined(BOARD_ADC_SERVO_VALID) + system_power.servo_power_available = true; + system_power.servo_power_valid = BOARD_ADC_SERVO_VALID; + updated = true; +#endif // BOARD_ADC_SERVO_VALID + +#if defined(BOARD_ADC_PERIPH_5V_OC) + system_power.peripheral_5v_available = true; + system_power.peripheral_5v_overcurrent = BOARD_ADC_PERIPH_5V_OC; + updated = true; +#endif // BOARD_ADC_PERIPH_5V_OC + +#if defined(BOARD_ADC_HIPOWER_5V_OC) + system_power.hipower_5v_available = true; + system_power.hipower_5v_overcurrent = BOARD_ADC_HIPOWER_5V_OC; + updated = true; +#endif // BOARD_ADC_HIPOWER_5V_OC + +#if defined(BOARD_GPIO_VDD_5V_COMP_VALID) + system_power.comp_5v_availabled = true; + system_power.comp_5v_valid = ReadGpioValue(_5v_comp_valid_fd); + updated = true; +#endif // BOARD_GPIO_VDD_5V_COMP_VALID + +#if defined(BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID) + system_power.can1_gps1_5v_available = true; + system_power.can1_gps1_5v_valid = ReadGpioValue(_5v_can1_gps1_valid_fd); + updated = true; +#endif // BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID + + if (updated) { + system_power.timestamp = hrt_absolute_time(); + _system_power_pub.publish(system_power); + } + + perf_end(_cycle_perf); +} + +int SystemPower::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SystemPower::task_spawn(int argc, char *argv[]) +{ + SystemPower *instance = new SystemPower(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SystemPower::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +ADC driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("system_power", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int system_power_main(int argc, char *argv[]) +{ + return SystemPower::main(argc, argv); +} diff --git a/src/drivers/system_power/SystemPower.hpp b/src/drivers/system_power/SystemPower.hpp new file mode 100644 index 0000000000..b27f9d046d --- /dev/null +++ b/src/drivers/system_power/SystemPower.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file SystemPower.hpp + * + * Driver for an ADC. + * + */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SystemPower : public ModuleBase, public px4::ScheduledWorkItem +{ +public: + SystemPower(); + ~SystemPower() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + int init(); + +private: + void Run() override; + + void OpenGpioDevices(); + void CloseGpioDevices(); + uint8_t ReadGpioValue(int fd); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + uORB::Publication _system_power_pub{ORB_ID(system_power)}; + uORB::Subscription _adc_report_sub{ORB_ID(adc_report)}; + +#if defined(BOARD_GPIO_VDD_5V_COMP_VALID) + int _5v_comp_valid_fd {-1}; +#endif // BOARD_GPIO_VDD_5V_COMP_VALID +#if defined(BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID) + int _5v_can1_gps1_valid_fd {-1}; +#endif // BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID + + bool _first_run{true}; +}; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 9012aad797..488063cd46 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -51,6 +51,8 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) ModuleParams(parent), _index(index < 1 || index > 9 ? 1 : index) { + _battery_status_pub.advertised(); + const float expected_filter_dt = static_cast(sample_interval_us) / 1_s; _voltage_filter_v.setParameters(expected_filter_dt, 1.f); _current_filter_a.setParameters(expected_filter_dt, .5f); @@ -164,16 +166,11 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, } if (source == _params.source) { - publish(); + _battery_status.timestamp = hrt_absolute_time(); + _battery_status_pub.publish(_battery_status); } } -void Battery::publish() -{ - _battery_status.timestamp = hrt_absolute_time(); - _battery_status_pub.publish(_battery_status); -} - void Battery::sumDischarged(const hrt_abstime ×tamp, float current_a) { // Not a valid measurement diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index bed43eaf4f..2c4bcd6c8a 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -155,11 +155,6 @@ protected: bool _first_parameter_update{true}; void updateParams() override; - /** - * Publishes the uORB battery_status message with the most recently-updated data. - */ - void publish(); - /** * This function helps migrating and syncing from/to deprecated parameters. BAT_* BAT1_* * @tparam T Type of the parameter (int or float) diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index dc649217b4..d6b671dcd7 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -35,20 +35,6 @@ #include #include "analog_battery.h" -// Defaults to use if the parameters are not set -#if BOARD_NUMBER_BRICKS > 0 -#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST) -static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST; -static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST; -#else -static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0}; -static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0}; -#endif -#else -static constexpr int DEFAULT_V_CHANNEL[1] = {0}; -static constexpr int DEFAULT_I_CHANNEL[1] = {0}; -#endif - AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us) : Battery(index, parent, sample_interval_us) { @@ -80,23 +66,12 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float voltage_v = voltage_raw * _analog_params.v_div; float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; - bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && - (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); + bool connected = (voltage_v > BOARD_ADC_OPEN_CIRCUIT_V);// && (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV); - - Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected, - source, priority, throttle_normalized); -} - -bool AnalogBattery::is_valid() -{ -#ifdef BOARD_BRICK_VALID_LIST - bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST; - return valid[_index - 1]; -#else - // TODO: Maybe return false instead? - return true; -#endif + // only publish if connected + if (connected) { + Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected, source, priority, throttle_normalized); + } } int AnalogBattery::get_voltage_channel() @@ -105,7 +80,12 @@ int AnalogBattery::get_voltage_channel() return _analog_params.v_channel; } else { - return DEFAULT_V_CHANNEL[_index - 1]; +#if defined(BOARD_BATT_V_LIST) + static constexpr int batt_v_list[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST; + return batt_v_list[_index - 1]; +#else + return 0; +#endif // BOARD_BATT_V_LIST } } @@ -115,7 +95,12 @@ int AnalogBattery::get_current_channel() return _analog_params.i_channel; } else { - return DEFAULT_I_CHANNEL[_index - 1]; +#if defined(BOARD_BATT_I_LIST) + static constexpr int batt_i_list[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST; + return batt_i_list[_index - 1]; +#else + return 0; +#endif // BOARD_BATT_I_LIST } } diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index ce42b103b5..a22f08241b 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -54,12 +54,6 @@ public: void updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw, int source, int priority, float throttle_normalized); - /** - * Whether the ADC channel for the voltage of this battery is valid. - * Corresponds to BOARD_BRICK_VALID_LIST - */ - bool is_valid(); - /** * Which ADC channel is used for voltage reading of this battery */ diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index a509a3fea0..ccfeb90beb 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -71,15 +71,6 @@ using namespace time_literals; /** * The channel definitions (e.g., ADC_BATTERY_VOLTAGE_CHANNEL, ADC_BATTERY_CURRENT_CHANNEL, and ADC_AIRSPEED_VOLTAGE_CHANNEL) are defined in board_config.h */ - -#ifndef BOARD_NUMBER_BRICKS -#error "battery_status module requires power bricks" -#endif - -#if BOARD_NUMBER_BRICKS == 0 -#error "battery_status module requires power bricks" -#endif - class BatteryStatus : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -187,13 +178,13 @@ BatteryStatus::adc_poll() if (_adc_report_sub.update(&adc_report)) { /* Read add channels we got */ - for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { + for (unsigned i = 0; i < adc_report_s::MAX_ADC_CHANNELS; ++i) { for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { /* Once we have subscriptions, Do this once for the lowest (highest priority * supply on power controller) that is valid. */ - if (selected_source < 0 && _analogBatteries[b]->is_valid()) { + if (selected_source < 0) { /* Indicate the lowest brick (highest priority supply on power controller) * that is valid as the one that is the selected source for the * VDD_5V_IN diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index 3ad148a8c0..387b1b5ab9 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -41,18 +41,6 @@ using namespace time_literals; -unsigned int countSetBits(unsigned int n) -{ - unsigned int count = 0; - - while (n) { - count += n & 1; - n >>= 1; - } - - return count; -} - bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm) { @@ -73,29 +61,37 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta // Check avionics rail voltages (if USB isn't connected) if (!system_power.usb_connected) { - float avionics_power_rail_voltage = system_power.voltage5v_v; + if (system_power.voltage5v_available) { + float avionics_power_rail_voltage = system_power.voltage5v_v; - if (avionics_power_rail_voltage < 4.5f) { - success = false; + if (avionics_power_rail_voltage < 4.5f) { + success = false; - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", - (double)avionics_power_rail_voltage); - } + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", + (double)avionics_power_rail_voltage); + } - } else if (avionics_power_rail_voltage < 4.8f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); - } + } else if (avionics_power_rail_voltage < 4.8f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); + } - } else if (avionics_power_rail_voltage > 5.4f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage); + } else if (avionics_power_rail_voltage > 5.4f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage); + } } } - const int power_module_count = countSetBits(system_power.brick_valid); + int power_module_count = 0; + + for (const auto &n : system_power.brick_valid) { + if (n) { + power_module_count++; + } + }; if (power_module_count < required_power_module_count) { success = false; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c7716717b6..3f14d771ad 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1656,9 +1656,17 @@ Commander::run() _system_power_sub.copy(&system_power); if (hrt_elapsed_time(&system_power.timestamp) < 1_s) { - if (system_power.servo_valid && - !system_power.brick_valid && - !system_power.usb_connected) { + bool brick_valid = false; + + for (auto &b : system_power.brick_valid) { + if (b) { + brick_valid = true; + break; + } + } + + if (system_power.servo_power_available && system_power.servo_power_valid && + !brick_valid && !system_power.usb_connected) { /* flying only on servo rail, this is unsafe */ _status_flags.condition_power_input_valid = false; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 332e454749..83c2bccef0 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -80,7 +80,7 @@ void LoggedTopics::add_default_topics() add_topic("sensor_preflight_mag", 500); add_topic("sensor_selection"); add_topic("sensors_status_imu", 200); - add_topic("system_power", 500); + add_topic("system_power"); add_topic("tecs_status", 200); add_topic("test_motor", 500); add_topic("trajectory_setpoint", 200); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8cf3faa1f8..5379101d65 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -435,7 +435,7 @@ void Sensors::adc_poll() if (_adc_report_sub.update(&adc)) { /* Read add channels we got */ - for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) { + for (unsigned i = 0; i < adc_report_s::MAX_ADC_CHANNELS; i++) { if (adc.channel_id[i] == -1) { continue; // skip non-exist channels } diff --git a/src/systemcmds/tests/test_adc.cpp b/src/systemcmds/tests/test_adc.cpp index 1f6489350f..8bef8a7950 100644 --- a/src/systemcmds/tests/test_adc.cpp +++ b/src/systemcmds/tests/test_adc.cpp @@ -56,7 +56,7 @@ int test_adc(int argc, char *argv[]) PX4_INFO_RAW("Resolution: %d\n", adc.resolution); PX4_INFO_RAW("Voltage Reference: %f\n", adc.v_ref); - for (int i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { + for (int i = 0; i < adc_report_s::MAX_ADC_CHANNELS; ++i) { PX4_INFO_RAW("%d: %d ", adc.channel_id[i], adc.raw_data[i]); } diff --git a/src/systemcmds/tests/test_jig_voltages.cpp b/src/systemcmds/tests/test_jig_voltages.cpp index e4462c9483..1a3ba7f2cf 100644 --- a/src/systemcmds/tests/test_jig_voltages.cpp +++ b/src/systemcmds/tests/test_jig_voltages.cpp @@ -58,7 +58,7 @@ int test_jig_voltages(int argc, char *argv[]) unsigned channels = 0; - for (int i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { + for (int i = 0; i < adc_report_s::MAX_ADC_CHANNELS; ++i) { PX4_INFO_RAW("%d: %d ", adc.channel_id[i], adc.raw_data[i]); if (adc.channel_id[i] != -1) {