dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

px4io cleanup LED and heater handling 

- most px4_io-v2 boards have a blue LED that breathes for status
 - the pixhawk 2.1 (hex) re-used this blue LED for as an IMU heater (active low), but kept the same board id (so we have to detect at runtime)
 - the new cubepilot boards (yellow, orange) inverted the polarity of this heater pin
 - untangle the mess slightly so that things we know statically (eg cubepilot cubeorange LEDs and heater polarity) are handled at build time.
This commit is contained in:
Daniel Agar 2022-04-13 18:43:59 -04:00 committed by GitHub
parent 510ad00024
commit 3f13c70cae
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 89 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
View File

Binary file not shown.

40
boards/cubepilot/io-v2/src/board_config.h
View File

@ -70,29 +70,19 @@
/* LEDS **********************************************************************/
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) // IO-LED_AMBER
#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) // IO-LED_SAFETY
#define GPIO_LED_GREEN (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) // IO-LED_POWER_BREATHING
#define GPIO_HEATER_OFF (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define LED_AMBER(on_true) stm32_gpiowrite(GPIO_LED_AMBER, !(on_true))
#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true))
#define LED_GREEN(on_true) stm32_gpiowrite(GPIO_LED_GREEN, (on_true))
#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
/* PixHawk 1:
* PC14 Floating
* PC15 Floating
*
* PixHawk 2:
* PC14 3.3v
* PC15 GND
*/
/* HEATER */
#define GPIO_HEATER_OUTPUT /* PB14 */ (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
#define HEATER_OUTPUT_EN(on_true) stm32_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
# define SENSE_PH1 0b10 /* Floating pulled as set */
# define SENSE_PH2 0b01 /* Driven as tied */
#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
@ -150,18 +140,6 @@
#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 PA8 */
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8)
/* LED definitions ******************************************************************/
/* PX4 has two LEDs that we will encode as: */
#define LED_STARTED 0 /* LED? */
#define LED_HEAPALLOCATE 1 /* LED? */
#define LED_IRQSENABLED 2 /* LED? + LED? */
#define LED_STACKCREATED 3 /* LED? */
#define LED_INIRQ 4 /* LED? + LED? */
#define LED_SIGNAL 5 /* LED? + LED? */
#define LED_ASSERTION 6 /* LED? + LED? + LED? */
#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */
#define BOARD_NUM_IO_TIMERS 3
#include <px4_platform_common/board_common.h>

37
boards/cubepilot/io-v2/src/init.c
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file px4iov2_init.c
* @file init.c
*
* PX4FMU-specific early startup code. This file implements the
* stm32_boardinitialize() function that is called during cpu startup.
@ -84,38 +84,14 @@
__EXPORT void stm32_boardinitialize(void)
{
/* configure GPIOs */
/* Set up for sensing HW */
stm32_configgpio(GPIO_SENSE_PC14_DN);
stm32_configgpio(GPIO_SENSE_PC15_UP);
stm32_configgpio(GPIO_HEATER_OUTPUT);
/* LEDS - default to off */
stm32_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED2);
stm32_configgpio(GPIO_LED3);
stm32_configgpio(GPIO_LED4);
/* PixHawk 1:
* PC14 Floating
* PC15 Floating
*
* PixHawk 2:
* PC14 3.3v
* PC15 GND
*/
uint8_t sense = stm32_gpioread(GPIO_SENSE_PC15_UP) << 1 | stm32_gpioread(GPIO_SENSE_PC14_DN);
if (sense == SENSE_PH2) {
stm32_configgpio(GPIO_HEATER_OFF);
}
stm32_configgpio(GPIO_PC14);
stm32_configgpio(GPIO_PC15);
stm32_configgpio(GPIO_LED_AMBER);
stm32_configgpio(GPIO_LED_SAFETY);
stm32_configgpio(GPIO_LED_GREEN);
stm32_configgpio(GPIO_BTN_SAFETY);
@ -163,4 +139,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_gpiowrite(GPIO_PWM8, true);
stm32_configgpio(GPIO_PWM8);
/* disable heater */
HEATER_OUTPUT_EN(false);
}

BIN
boards/holybro/durandal-v1/extras/px4_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/holybro/pix32v5/extras/px4_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/mro/x21-777/extras/px4_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/px4/fmu-v2/extras/px4_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/px4/fmu-v3/extras/px4_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/px4/fmu-v5/extras/px4_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/px4/fmu-v5x/extras/px4_io-v2_default.bin
View File

Binary file not shown.

BIN
boards/px4/fmu-v6x/extras/px4_io-v2_default.bin
View File

Binary file not shown.

36
boards/px4/io-v2/src/board_config.h
View File

@ -70,12 +70,10 @@
/* LEDS **********************************************************************/
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
#define GPIO_HEATER_OFF (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED_BLUE (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) // IO-LED_BLUE or IMU_TEMPERATURE_CONTROL on Pixhawk 2.1
#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) // IO-LED_AMBER
#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) // IO-LED_SAFETY
#define GPIO_LED_GREEN (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) // IO-LED_POWER_BREATHING
#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
@ -88,12 +86,24 @@
* PC14 3.3v
* PC15 GND
*/
#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
# define SENSE_PH1 0b10 /* Floating pulled as set */
# define SENSE_PH2 0b01 /* Driven as tied */
#define SENSE_PIXHAWK2() (((stm32_gpioread(GPIO_SENSE_PC15_UP) << 1 | stm32_gpioread(GPIO_SENSE_PC14_DN)) == SENSE_PH2) ? 1 : 0)
#define LED_BLUE(on_true) (SENSE_PIXHAWK2() ? (void)0 : stm32_gpiowrite(GPIO_LED_BLUE, !(on_true)))
#define LED_AMBER(on_true) stm32_gpiowrite(GPIO_LED_AMBER, !(on_true))
#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true))
#define LED_GREEN(on_true) stm32_gpiowrite(GPIO_LED_GREEN, (on_true))
/* HEATER */
#define GPIO_HEATER_OUTPUT /* PB14 */ (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define HEATER_OUTPUT_EN(on_true) (SENSE_PIXHAWK2() ? stm32_gpiowrite(GPIO_HEATER_OUTPUT, !(on_true)) : (void)0)
#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
/* Safety switch button *******************************************************/
@ -150,18 +160,6 @@
#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 PA8 */
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8)
/* LED definitions ******************************************************************/
/* PX4 has two LEDs that we will encode as: */
#define LED_STARTED 0 /* LED? */
#define LED_HEAPALLOCATE 1 /* LED? */
#define LED_IRQSENABLED 2 /* LED? + LED? */
#define LED_STACKCREATED 3 /* LED? */
#define LED_INIRQ 4 /* LED? + LED? */
#define LED_SIGNAL 5 /* LED? + LED? */
#define LED_ASSERTION 6 /* LED? + LED? + LED? */
#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */
#define BOARD_NUM_IO_TIMERS 3
#include <px4_platform_common/board_common.h>

39
boards/px4/io-v2/src/init.c
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file px4iov2_init.c
* @file init.c
*
* PX4FMU-specific early startup code. This file implements the
* stm32_boardinitialize() function that is called during cpu startup.
@ -84,39 +84,33 @@
__EXPORT void stm32_boardinitialize(void)
{
/* configure GPIOs */
/* Set up for sensing HW */
stm32_configgpio(GPIO_SENSE_PC14_DN);
stm32_configgpio(GPIO_SENSE_PC15_UP);
/* LEDS - default to off */
stm32_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED2);
stm32_configgpio(GPIO_LED3);
stm32_configgpio(GPIO_LED4);
/* some boards such as Pixhawk 2.1 made
the unfortunate choice to combine the blue led channel with
the IMU heater. We need a software hack to fix the hardware hack
by allowing to disable the LED / heater.
*/
if (SENSE_PIXHAWK2()) {
stm32_configgpio(GPIO_HEATER_OUTPUT);
/* PixHawk 1:
* PC14 Floating
* PC15 Floating
*
* PixHawk 2:
* PC14 3.3v
* PC15 GND
*/
uint8_t sense = stm32_gpioread(GPIO_SENSE_PC15_UP) << 1 | stm32_gpioread(GPIO_SENSE_PC14_DN);
if (sense == SENSE_PH2) {
stm32_configgpio(GPIO_HEATER_OFF);
} else {
stm32_configgpio(GPIO_LED_BLUE);
}
stm32_configgpio(GPIO_PC14);
stm32_configgpio(GPIO_PC15);
/* LEDS - default to off */
stm32_configgpio(GPIO_LED_AMBER);
stm32_configgpio(GPIO_LED_SAFETY);
stm32_configgpio(GPIO_LED_GREEN);
stm32_configgpio(GPIO_BTN_SAFETY);
/* spektrum power enable is active high - enable it by default */
@ -163,4 +157,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_gpiowrite(GPIO_PWM8, true);
stm32_configgpio(GPIO_PWM8);
/* disable heater */
HEATER_OUTPUT_EN(false);
}

67
src/modules/px4iofirmware/px4io.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -173,18 +173,20 @@ update_mem_usage(void)
static void
heartbeat_blink(void)
{
#if defined(LED_BLUE)
static bool heartbeat = false;
LED_BLUE(heartbeat = !heartbeat);
#endif /* LED_BLUE */
}
static void
ring_blink(void)
{
#ifdef GPIO_LED4
#if defined(LED_GREEN)
if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
LED_RING(1);
LED_GREEN(true);
return;
}
@ -205,7 +207,7 @@ ring_blink(void)
// remove the ! in the line below
// to return to the proper breathe
// animation / pattern (currently inverted)
LED_RING(!on);
LED_GREEN(!on);
brightness_counter++;
if (on) {
@ -301,11 +303,13 @@ user_start(int argc, char *argv[])
/* default all the LEDs to off while we start */
LED_AMBER(false);
#if defined(LED_BLUE)
LED_BLUE(false);
#endif /* LED_BLUE */
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
#if defined(LED_GREEN)
LED_GREEN(false);
#endif /* LED_GREEN */
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
@ -377,34 +381,35 @@ user_start(int argc, char *argv[])
perf_end(controls_perf);
#endif
/* some boards such as Pixhawk 2.1 made
the unfortunate choice to combine the blue led channel with
the IMU heater. We need a software hack to fix the hardware hack
by allowing to disable the LED / heater.
*/
if (r_page_setup[PX4IO_P_SETUP_THERMAL] == PX4IO_THERMAL_IGNORE) {
/*
blink blue LED at 4Hz in normal operation. When in
override blink 4x faster so the user can clearly see
that override is happening. This helps when
pre-flight testing the override system
*/
uint32_t heartbeat_period_us = 250 * 1000UL;
if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
/*
blink blue LED at 4Hz in normal operation. When in
override blink 4x faster so the user can clearly see
that override is happening. This helps when
pre-flight testing the override system
*/
uint32_t heartbeat_period_us = 250 * 1000UL;
} else if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) {
/* switch resistive heater off */
LED_BLUE(false);
} else {
/* switch resistive heater hard on */
LED_BLUE(true);
if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
#if defined(HEATER_OUTPUT_EN)
if (r_page_setup[PX4IO_P_SETUP_THERMAL] != PX4IO_THERMAL_IGNORE) {
if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) {
/* switch resistive heater off */
HEATER_OUTPUT_EN(false);
} else {
/* switch resistive heater hard on */
HEATER_OUTPUT_EN(true);
}
}
#endif /* HEATER_OUTPUT_EN */
update_mem_usage();
ring_blink();

16
src/modules/px4iofirmware/px4io.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -123,20 +123,6 @@ extern struct sys_state_s system_state;
extern bool update_mc_thrust_param;
extern bool update_trims;
/*
* GPIO handling.
*/
/* HEX Cube Orange and Cube Yellow uses an inverted signal to control the IMU heater */
#ifdef CONFIG_ARCH_BOARD_CUBEPILOT_IO_V2
#define LED_BLUE(_s) px4_arch_gpiowrite(GPIO_LED1, (_s))
#else
#define LED_BLUE(_s) px4_arch_gpiowrite(GPIO_LED1, !(_s))
#endif
#define LED_AMBER(_s) px4_arch_gpiowrite(GPIO_LED2, !(_s))
#define LED_SAFETY(_s) px4_arch_gpiowrite(GPIO_LED3, !(_s))
#define LED_RING(_s) px4_arch_gpiowrite(GPIO_LED4, (_s))
# define ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT))