fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 14:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Changes to px4fmu-v2 for upstream Nuttx and hardfault logging

Browse Source
This commit is contained in:
David Sidrane
2016-12-21 08:34:21 +01:00
committed by Lorenz Meier
parent 82cb9353d1
commit d9575964a4
16 changed files with 1431 additions and 603 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/px4fmu-v2/board_config.h
+32 -29
View File
@@ -47,11 +47,6 @@
#include <nuttx/compiler.h>
#include <stdint.h>
#include <stm32.h>
#include <arch/board/board.h>
#define UDID_START 0x1FFF7A10
/****************************************************************************************************
* Definitions
****************************************************************************************************/
@@ -241,9 +236,9 @@
#define BOARD_NAME "PX4FMU_V2"
/* 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.
/* 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))
@@ -295,30 +290,38 @@ __BEGIN_DECLS
****************************************************************************************************/
extern void stm32_spiinitialize(void);
extern void board_spi_reset(int ms);
/************************************************************************************
* Name: stm32_spi_bus_initialize
*
* Description:
* Called to configure SPI Buses.
*
************************************************************************************/
extern int stm32_spi_bus_initialize(void);
/****************************************************************************************************
* Name: board_spi_reset board_peripheral_reset
*
* Description:
* Called to reset SPI and the perferal bus
*
****************************************************************************************************/
void board_spi_reset(int ms);
extern void board_peripheral_reset(int ms);
/****************************************************************************************************
* Name: stm32_usbinitialize
*
* Description:
* Called to configure USB IO.
*
****************************************************************************************************/
extern void stm32_usbinitialize(void);
extern void board_peripheral_reset(int ms);
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization for NSH.
*
* CONFIG_NSH_ARCHINIT=y :
* Called from the NSH library
*
* CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
* CONFIG_NSH_ARCHINIT=n :
* Called from board_initialize().
*
****************************************************************************/
#ifdef CONFIG_NSH_LIBRARY
int nsh_archinitialize(void);
#endif
#include "../common/board_common.h"
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+371 -47
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@@ -32,13 +32,13 @@
****************************************************************************/
/**
* @file px4fmu_init.c
* @file px4fmu2_init.c
*
* PX4FMU-specific early startup code. This file implements the
* nsh_archinitialize() function that is called early by nsh during startup.
* PX4FMUv2-specific early startup code. This file implements the
* board_app_initialize() function that is called early by nsh during startup.
*
* Code here is run before the rcS script is invoked; it should start required
* subsystems and perform board-specific initialisation.
* subsystems and perform board-specific initialization.
*/
/****************************************************************************
@@ -49,12 +49,14 @@
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/arch.h>
#include <nuttx/spi.h>
#include <nuttx/i2c.h>
#include <nuttx/board.h>
#include <nuttx/spi/spi.h>
#include <nuttx/i2c/i2c_master.h>
#include <nuttx/sdio.h>
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
@@ -68,10 +70,15 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_led.h>
#include <systemlib/px4_macros.h>
#include <systemlib/cpuload.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/hardfault_log.h>
#include <systemlib/systemlib.h>
/****************************************************************************
* Pre-Processor Definitions
****************************************************************************/
@@ -82,13 +89,13 @@
#ifdef CONFIG_CPP_HAVE_VARARGS
# ifdef CONFIG_DEBUG
# define message(...) lowsyslog(__VA_ARGS__)
# define message(...) syslog(__VA_ARGS__)
# else
# define message(...) printf(__VA_ARGS__)
# endif
#else
# ifdef CONFIG_DEBUG
# define message lowsyslog
# define message syslog
# else
# define message printf
# endif
@@ -122,8 +129,8 @@ __END_DECLS
__EXPORT void board_peripheral_reset(int ms)
{
/* set the peripheral rails off */
px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1);
stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1);
/* wait for the peripheral rail to reach GND */
usleep(ms * 1000);
@@ -132,7 +139,7 @@ __EXPORT void board_peripheral_reset(int ms)
/* re-enable power */
/* switch the peripheral rail back on */
px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
}
/************************************************************************************
@@ -148,18 +155,60 @@ __EXPORT void board_peripheral_reset(int ms)
__EXPORT void
stm32_boardinitialize(void)
{
/* configure ADC pins */
stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
/* configure power supply control/sense pins */
stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
stm32_configgpio(GPIO_VDD_BRICK_VALID);
stm32_configgpio(GPIO_VDD_SERVO_VALID);
stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
/* configure the GPIO pins to outputs and keep them low */
stm32_configgpio(GPIO_GPIO0_OUTPUT);
stm32_configgpio(GPIO_GPIO1_OUTPUT);
stm32_configgpio(GPIO_GPIO2_OUTPUT);
stm32_configgpio(GPIO_GPIO3_OUTPUT);
stm32_configgpio(GPIO_GPIO4_OUTPUT);
stm32_configgpio(GPIO_GPIO5_OUTPUT);
/* configure SPI interfaces */
stm32_spiinitialize();
/* configure LEDs */
up_ledinit();
board_autoled_initialize();
}
/****************************************************************************
* Name: nsh_archinitialize
* Name: board_app_initialize
*
* Description:
* Perform architecture specific initialization
* Perform application specific initialization. This function is never
* called directly from application code, but only indirectly via the
* (non-standard) boardctl() interface using the command BOARDIOC_INIT.
*
* Input Parameters:
* arg - The boardctl() argument is passed to the board_app_initialize()
* implementation without modification. The argument has no
* meaning to NuttX; the meaning of the argument is a contract
* between the board-specific initalization logic and the the
* matching application logic. The value cold be such things as a
* mode enumeration value, a set of DIP switch switch settings, a
* pointer to configuration data read from a file or serial FLASH,
* or whatever you would like to do with it. Every implementation
* should accept zero/NULL as a default configuration.
*
* Returned Value:
* Zero (OK) is returned on success; a negated errno value is returned on
* any failure to indicate the nature of the failure.
*
****************************************************************************/
@@ -168,37 +217,21 @@ static struct spi_dev_s *spi2;
static struct spi_dev_s *spi4;
static struct sdio_dev_s *sdio;
#include <math.h>
__EXPORT int nsh_archinitialize(void)
__EXPORT int board_app_initialize(uintptr_t arg)
{
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
/* configure ADC pins */
px4_arch_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
px4_arch_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
px4_arch_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
// px4_arch_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */
// px4_arch_configgpio(GPIO_ADC1_IN11); /* unused */
// px4_arch_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */
px4_arch_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
px4_arch_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
px4_arch_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
/* run C++ ctors before we go any further */
/* configure power supply control/sense pins */
px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
px4_arch_configgpio(GPIO_VDD_BRICK_VALID);
px4_arch_configgpio(GPIO_VDD_SERVO_VALID);
px4_arch_configgpio(GPIO_VDD_5V_HIPOWER_OC);
px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC);
up_cxxinitialize();
/* configure the GPIO pins to outputs and keep them low */
px4_arch_configgpio(GPIO_GPIO0_OUTPUT);
px4_arch_configgpio(GPIO_GPIO1_OUTPUT);
px4_arch_configgpio(GPIO_GPIO2_OUTPUT);
px4_arch_configgpio(GPIO_GPIO3_OUTPUT);
px4_arch_configgpio(GPIO_GPIO4_OUTPUT);
px4_arch_configgpio(GPIO_GPIO5_OUTPUT);
# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE)
# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.
# endif
#else
# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.
#endif
/* configure the high-resolution time/callout interface */
hrt_init();
@@ -231,17 +264,151 @@ __EXPORT int nsh_archinitialize(void)
(hrt_callout)stm32_serial_dma_poll,
NULL);
#if defined(CONFIG_STM32_BBSRAM)
if (hardfault_check_status) {
/* NB. the use of the console requires the hrt running
* to poll the DMA
*/
/* Using Battery Backed Up SRAM */
int filesizes[CONFIG_STM32_BBSRAM_FILES + 1] = BSRAM_FILE_SIZES;
stm32_bbsraminitialize(BBSRAM_PATH, filesizes);
#if defined(CONFIG_STM32_SAVE_CRASHDUMP)
/* Panic Logging in Battery Backed Up Files */
/*
* In an ideal world, if a fault happens in flight the
* system save it to BBSRAM will then reboot. Upon
* rebooting, the system will log the fault to disk, recover
* the flight state and continue to fly. But if there is
* a fault on the bench or in the air that prohibit the recovery
* or committing the log to disk, the things are too broken to
* fly. So the question is:
*
* Did we have a hard fault and not make it far enough
* through the boot sequence to commit the fault data to
* the SD card?
*/
/* Do we have an uncommitted hard fault in BBSRAM?
* - this will be reset after a successful commit to SD
*/
int hadCrash = hardfault_check_status("boot");
if (hadCrash == OK) {
message("[boot] There is a hard fault logged. Hold down the SPACE BAR," \
" while booting to halt the system!\n");
/* Yes. So add one to the boot count - this will be reset after a successful
* commit to SD
*/
int reboots = hardfault_increment_reboot("boot", false);
/* Also end the misery for a user that holds for a key down on the console */
int bytesWaiting;
ioctl(fileno(stdin), FIONREAD, (unsigned long)((uintptr_t) &bytesWaiting));
if (reboots > 2 || bytesWaiting != 0) {
/* Since we can not commit the fault dump to disk. Display it
* to the console.
*/
hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false);
message("[boot] There were %d reboots with Hard fault that were not committed to disk - System halted %s\n",
reboots,
(bytesWaiting == 0 ? "" : " Due to Key Press\n"));
/* For those of you with a debugger set a break point on up_assert and
* then set dbgContinue = 1 and go.
*/
/* Clear any key press that got us here */
static volatile bool dbgContinue = false;
int c = '>';
while (!dbgContinue) {
switch (c) {
case EOF:
case '\n':
case '\r':
case ' ':
continue;
default:
putchar(c);
putchar('\n');
switch (c) {
case 'D':
case 'd':
hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false);
break;
case 'C':
case 'c':
hardfault_rearm("boot");
hardfault_increment_reboot("boot", true);
break;
case 'B':
case 'b':
dbgContinue = true;
break;
default:
break;
} // Inner Switch
message("\nEnter B - Continue booting\n" \
"Enter C - Clear the fault log\n" \
"Enter D - Dump fault log\n\n?>");
fflush(stdout);
if (!dbgContinue) {
c = getchar();
}
break;
} // outer switch
} // for
} // inner if
} // outer if
} // hardfault_check_status
#endif // CONFIG_STM32_SAVE_CRASHDUMP
#endif // CONFIG_STM32_BBSRAM
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
/* Configure SPI-based devices */
spi1 = px4_spibus_initialize(1);
spi1 = stm32_spibus_initialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\n");
up_ledon(LED_AMBER);
board_autoled_on(LED_AMBER);
return -ENODEV;
}
@@ -257,11 +424,11 @@ __EXPORT int nsh_archinitialize(void)
/* Get the SPI port for the FRAM */
spi2 = px4_spibus_initialize(2);
spi2 = stm32_spibus_initialize(2);
if (!spi2) {
message("[boot] FAILED to initialize SPI port 2\n");
up_ledon(LED_AMBER);
board_autoled_on(LED_AMBER);
return -ENODEV;
}
@@ -274,7 +441,7 @@ __EXPORT int nsh_archinitialize(void)
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_FLASH, false);
spi4 = px4_spibus_initialize(4);
spi4 = stm32_spibus_initialize(4);
/* Default SPI4 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi4, 10000000);
@@ -309,3 +476,160 @@ __EXPORT int nsh_archinitialize(void)
return OK;
}
static void copy_reverse(stack_word_t *dest, stack_word_t *src, int size)
{
while (size--) {
*dest++ = *src--;
}
}
__EXPORT void board_crashdump(uintptr_t currentsp, FAR void *tcb, FAR const uint8_t *filename, int lineno)
{
if (hardfault_check_status) {
/* We need a chunk of ram to save the complete context in.
* Since we are going to reboot we will use &_sdata
* which is the lowest memory and the amount we will save
* _should be_ below any resources we need herein.
* Unfortunately this is hard to test. See dead below
*/
fullcontext_s *pdump = (fullcontext_s *)&_sdata;
(void)enter_critical_section();
struct tcb_s *rtcb = (struct tcb_s *)tcb;
/* Zero out everything */
memset(pdump, 0, sizeof(fullcontext_s));
/* Save Info */
pdump->info.lineno = lineno;
if (filename) {
int offset = 0;
unsigned int len = strlen((char *)filename) + 1;
if (len > sizeof(pdump->info.filename)) {
offset = len - sizeof(pdump->info.filename) ;
}
strncpy(pdump->info.filename, (char *)&filename[offset], sizeof(pdump->info.filename));
}
/* Save the value of the pointer for current_regs as debugging info.
* It should be NULL in case of an ASSERT and will aid in cross
* checking the validity of system memory at the time of the
* fault.
*/
pdump->info.current_regs = (uintptr_t) CURRENT_REGS;
/* Save Context */
#if CONFIG_TASK_NAME_SIZE > 0
strncpy(pdump->info.name, rtcb->name, CONFIG_TASK_NAME_SIZE);
#endif
pdump->info.pid = rtcb->pid;
/* If current_regs is not NULL then we are in an interrupt context
* and the user context is in current_regs else we are running in
* the users context
*/
if (CURRENT_REGS) {
pdump->info.stacks.interrupt.sp = currentsp;
pdump->info.flags |= (eRegsPresent | eUserStackPresent | eIntStackPresent);
memcpy(pdump->info.regs, (void *)CURRENT_REGS, sizeof(pdump->info.regs));
pdump->info.stacks.user.sp = pdump->info.regs[REG_R13];
} else {
/* users context */
pdump->info.flags |= eUserStackPresent;
pdump->info.stacks.user.sp = currentsp;
}
if (pdump->info.pid == 0) {
pdump->info.stacks.user.top = g_idle_topstack - 4;
pdump->info.stacks.user.size = CONFIG_IDLETHREAD_STACKSIZE;
} else {
pdump->info.stacks.user.top = (uint32_t) rtcb->adj_stack_ptr;
pdump->info.stacks.user.size = (uint32_t) rtcb->adj_stack_size;;
}
#if CONFIG_ARCH_INTERRUPTSTACK > 3
/* Get the limits on the interrupt stack memory */
pdump->info.stacks.interrupt.top = (uint32_t)&g_intstackbase;
pdump->info.stacks.interrupt.size = (CONFIG_ARCH_INTERRUPTSTACK & ~3);
/* If In interrupt Context save the interrupt stack data centered
* about the interrupt stack pointer
*/
if ((pdump->info.flags & eIntStackPresent) != 0) {
stack_word_t *ps = (stack_word_t *) pdump->info.stacks.interrupt.sp;
copy_reverse(pdump->istack, &ps[arraySize(pdump->istack) / 2], arraySize(pdump->istack));
}
/* Is it Invalid? */
if (!(pdump->info.stacks.interrupt.sp <= pdump->info.stacks.interrupt.top &&
pdump->info.stacks.interrupt.sp > pdump->info.stacks.interrupt.top - pdump->info.stacks.interrupt.size)) {
pdump->info.flags |= eInvalidIntStackPrt;
}
#endif
/* If In interrupt context or User save the user stack data centered
* about the user stack pointer
*/
if ((pdump->info.flags & eUserStackPresent) != 0) {
stack_word_t *ps = (stack_word_t *) pdump->info.stacks.user.sp;
copy_reverse(pdump->ustack, &ps[arraySize(pdump->ustack) / 2], arraySize(pdump->ustack));
}
/* Is it Invalid? */
if (!(pdump->info.stacks.user.sp <= pdump->info.stacks.user.top &&
pdump->info.stacks.user.sp > pdump->info.stacks.user.top - pdump->info.stacks.user.size)) {
pdump->info.flags |= eInvalidUserStackPtr;
}
int rv = stm32_bbsram_savepanic(HARDFAULT_FILENO, (uint8_t *)pdump, sizeof(fullcontext_s));
/* Test if memory got wiped because of using _sdata */
if (rv == -ENXIO) {
char *dead = "Memory wiped - dump not saved!";
while (*dead) {
up_lowputc(*dead++);
}
} else if (rv == -ENOSPC) {
/* hard fault again */
up_lowputc('!');
}
#if defined(CONFIG_BOARD_RESET_ON_CRASH)
px4_systemreset(false);
#endif
} // hardfault_check_status
}
src/drivers/boards/px4fmu-v2/px4fmu2_led.c
+6 -6
View File
@@ -64,14 +64,14 @@ __EXPORT void led_init()
{
/* Configure LED1 GPIO for output */
px4_arch_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED1);
}
__EXPORT void led_on(int led)
{
if (led == 1) {
/* Pull down to switch on */
px4_arch_gpiowrite(GPIO_LED1, false);
stm32_gpiowrite(GPIO_LED1, false);
}
}
@@ -79,18 +79,18 @@ __EXPORT void led_off(int led)
{
if (led == 1) {
/* Pull up to switch off */
px4_arch_gpiowrite(GPIO_LED1, true);
stm32_gpiowrite(GPIO_LED1, true);
}
}
__EXPORT void led_toggle(int led)
{
if (led == 1) {
if (px4_arch_gpioread(GPIO_LED1)) {
px4_arch_gpiowrite(GPIO_LED1, false);
if (stm32_gpioread(GPIO_LED1)) {
stm32_gpiowrite(GPIO_LED1, false);
} else {
px4_arch_gpiowrite(GPIO_LED1, true);
stm32_gpiowrite(GPIO_LED1, true);
}
}
}
src/drivers/boards/px4fmu-v2/px4fmu_can.c
+3 -18
View File
@@ -46,7 +46,7 @@
#include <errno.h>
#include <debug.h>
#include <nuttx/can.h>
#include <nuttx/drivers/can.h>
#include <arch/board/board.h>
#include "chip.h"
@@ -74,21 +74,6 @@
# define CAN_PORT 2
#endif
/* Debug ***************************************************************************/
/* Non-standard debug that may be enabled just for testing CAN */
#ifdef CONFIG_DEBUG_CAN
# define candbg dbg
# define canvdbg vdbg
# define canlldbg lldbg
# define canllvdbg llvdbg
#else
# define candbg(x...)
# define canvdbg(x...)
# define canlldbg(x...)
# define canllvdbg(x...)
#endif
/************************************************************************************
* Private Functions
************************************************************************************/
@@ -121,7 +106,7 @@ int can_devinit(void)
can = stm32_caninitialize(CAN_PORT);
if (can == NULL) {
candbg("ERROR: Failed to get CAN interface\n");
canerr("ERROR: Failed to get CAN interface\n");
return -ENODEV;
}
@@ -130,7 +115,7 @@ int can_devinit(void)
ret = can_register("/dev/can0", can);
if (ret < 0) {
candbg("ERROR: can_register failed: %d\n", ret);
canerr("ERROR: can_register failed: %d\n", ret);
return ret;
}
src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+113 -112
View File
@@ -47,7 +47,8 @@
#include <stdbool.h>
#include <debug.h>
#include <unistd.h>
#include <nuttx/spi.h>
#include <nuttx/spi/spi.h>
#include <arch/board/board.h>
#include <up_arch.h>
@@ -71,42 +72,42 @@
__EXPORT void stm32_spiinitialize(void)
{
#ifdef CONFIG_STM32_SPI1
px4_arch_configgpio(GPIO_SPI_CS_GYRO);
px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG);
px4_arch_configgpio(GPIO_SPI_CS_BARO);
px4_arch_configgpio(GPIO_SPI_CS_HMC);
px4_arch_configgpio(GPIO_SPI_CS_MPU);
stm32_configgpio(GPIO_SPI_CS_GYRO);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
stm32_configgpio(GPIO_SPI_CS_BARO);
stm32_configgpio(GPIO_SPI_CS_HMC);
stm32_configgpio(GPIO_SPI_CS_MPU);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY);
px4_arch_configgpio(GPIO_EXTI_MAG_DRDY);
px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY);
px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
stm32_configgpio(GPIO_EXTI_MAG_DRDY);
stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
stm32_configgpio(GPIO_EXTI_MPU_DRDY);
#endif
#ifdef CONFIG_STM32_SPI2
px4_arch_configgpio(GPIO_SPI_CS_FRAM);
px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1);
stm32_configgpio(GPIO_SPI_CS_FRAM);
stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
#endif
#ifdef CONFIG_STM32_SPI4
px4_arch_configgpio(GPIO_SPI_CS_EXT0);
px4_arch_configgpio(GPIO_SPI_CS_EXT1);
px4_arch_configgpio(GPIO_SPI_CS_EXT2);
px4_arch_configgpio(GPIO_SPI_CS_EXT3);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
stm32_configgpio(GPIO_SPI_CS_EXT0);
stm32_configgpio(GPIO_SPI_CS_EXT1);
stm32_configgpio(GPIO_SPI_CS_EXT2);
stm32_configgpio(GPIO_SPI_CS_EXT3);
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
#endif
}
@@ -117,56 +118,56 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
switch (devid) {
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_ACCEL_MAG:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_BARO:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_HMC:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_LIS:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_LIS, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_LIS, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
break;
default:
@@ -184,7 +185,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
/* there can only be one device on this bus, so always select it */
px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
}
__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
@@ -201,34 +202,34 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
switch (devid) {
case PX4_SPIDEV_EXT0:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
break;
case PX4_SPIDEV_EXT1:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
break;
case PX4_SPIDEV_EXT2:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
break;
case PX4_SPIDEV_EXT3:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
break;
default:
@@ -245,37 +246,37 @@ __EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
__EXPORT void board_spi_reset(int ms)
{
/* disable SPI bus */
px4_arch_configgpio(GPIO_SPI_CS_GYRO_OFF);
px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF);
px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF);
stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
stm32_configgpio(GPIO_SPI_CS_MPU_OFF);
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
px4_arch_configgpio(GPIO_SPI1_SCK_OFF);
px4_arch_configgpio(GPIO_SPI1_MISO_OFF);
px4_arch_configgpio(GPIO_SPI1_MOSI_OFF);
stm32_configgpio(GPIO_SPI1_SCK_OFF);
stm32_configgpio(GPIO_SPI1_MISO_OFF);
stm32_configgpio(GPIO_SPI1_MOSI_OFF);
px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
px4_arch_configgpio(GPIO_GYRO_DRDY_OFF);
px4_arch_configgpio(GPIO_MAG_DRDY_OFF);
px4_arch_configgpio(GPIO_ACCEL_DRDY_OFF);
px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
stm32_configgpio(GPIO_GYRO_DRDY_OFF);
stm32_configgpio(GPIO_MAG_DRDY_OFF);
stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
px4_arch_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
px4_arch_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
px4_arch_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
/* set the sensor rail off */
px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
/* wait for the sensor rail to reach GND */
usleep(ms * 1000);
@@ -284,36 +285,36 @@ __EXPORT void board_spi_reset(int ms)
/* re-enable power */
/* switch the sensor rail back on */
px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
/* wait a bit before starting SPI, different times didn't influence results */
usleep(100);
/* reconfigure the SPI pins */
#ifdef CONFIG_STM32_SPI1
px4_arch_configgpio(GPIO_SPI_CS_GYRO);
px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG);
px4_arch_configgpio(GPIO_SPI_CS_BARO);
px4_arch_configgpio(GPIO_SPI_CS_MPU);
stm32_configgpio(GPIO_SPI_CS_GYRO);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
stm32_configgpio(GPIO_SPI_CS_BARO);
stm32_configgpio(GPIO_SPI_CS_MPU);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
px4_arch_configgpio(GPIO_SPI1_SCK);
px4_arch_configgpio(GPIO_SPI1_MISO);
px4_arch_configgpio(GPIO_SPI1_MOSI);
stm32_configgpio(GPIO_SPI1_SCK);
stm32_configgpio(GPIO_SPI1_MISO);
stm32_configgpio(GPIO_SPI1_MOSI);
// // XXX bring up the EXTI pins again
// px4_arch_configgpio(GPIO_GYRO_DRDY);
// px4_arch_configgpio(GPIO_MAG_DRDY);
// px4_arch_configgpio(GPIO_ACCEL_DRDY);
// px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
// stm32_configgpio(GPIO_GYRO_DRDY);
// stm32_configgpio(GPIO_MAG_DRDY);
// stm32_configgpio(GPIO_ACCEL_DRDY);
// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
#endif
}
src/drivers/boards/px4fmu-v2/px4fmu_usb.c
+4 -4
View File
@@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void)
/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
#ifdef CONFIG_STM32_OTGFS
px4_arch_configgpio(GPIO_OTGFS_VBUS);
stm32_configgpio(GPIO_OTGFS_VBUS);
/* XXX We only support device mode
px4_arch_configgpio(GPIO_OTGFS_PWRON);
px4_arch_configgpio(GPIO_OTGFS_OVER);
stm32_configgpio(GPIO_OTGFS_PWRON);
stm32_configgpio(GPIO_OTGFS_OVER);
*/
#endif
}
@@ -103,6 +103,6 @@ __EXPORT void stm32_usbinitialize(void)
__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
{
//ulldbg("resume: %d\n", resume);
uinfo("resume: %d\n", resume);
}