Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-20 09:49:20 +08:00
Code Issues Actions 11 Packages Projects Releases Wiki Activity

Merged master into driver_framework

Browse Source
This commit is contained in:
Lorenz Meier
2015-12-01 12:34:02 +01:00
parent 1e99b04d9c 93bb1ab80f
commit f0a4979da6
109 changed files with 5346 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/px4fmu-v1/px4fmu_init.c
-14
View File
@@ -145,20 +145,6 @@ static struct spi_dev_s *spi3;
#include <math.h>
#if 0
#ifdef __cplusplus
__EXPORT int matherr(struct __exception *e)
{
return 1;
}
#else
__EXPORT int matherr(struct exception *e)
{
return 1;
}
#endif
#endif
__EXPORT int nsh_archinitialize(void)
{
int result;
src/drivers/boards/px4fmu-v2/CMakeLists.txt
+1 -1
View File
@@ -44,4 +44,4 @@ px4_add_module(
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
-13
View File
@@ -212,19 +212,6 @@ static struct sdio_dev_s *sdio;
#include <math.h>
/* TODO XXX commented this out to get cmake build working */
/*#ifdef __cplusplus*/
/*__EXPORT int matherr(struct __exception *e)*/
/*{*/
/*return 1;*/
/*}*/
/*#else*/
/*__EXPORT int matherr(struct exception *e)*/
/*{*/
/*return 1;*/
/*}*/
/*#endif*/
__EXPORT int nsh_archinitialize(void)
{
src/drivers/boards/px4fmu-v4/CMakeLists.txt
+47
View File
@@ -0,0 +1,47 @@
############################################################################
#
# Copyright (c) 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
# 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__boards__px4fmu-v4
COMPILE_FLAGS
-Os
SRCS
px4fmu_can.c
px4fmu_init.c
px4fmu_pwm_servo.c
px4fmu_spi.c
px4fmu_usb.c
px4fmu_led.c
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/boards/px4fmu-v4/board_config.h
+282
View File
@@ -0,0 +1,282 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 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 board_config.h
*
* PX4FMUv2 internal definitions
*/
#pragma once
/****************************************************************************************************
* Included Files
****************************************************************************************************/
#include <px4_config.h>
#include <nuttx/compiler.h>
#include <stdint.h>
__BEGIN_DECLS
/* these headers are not C++ safe */
#include <stm32.h>
#include <arch/board/board.h>
#define UDID_START 0x1FFF7A10
/****************************************************************************************************
* Definitions
****************************************************************************************************/
/* Configuration ************************************************************************************/
//{GPIO_RSSI_IN, 0, 0}, - pio Analog used as PWM
//{0, GPIO_LED_SAFETY, 0}, pio replacement
//{GPIO_SAFETY_SWITCH_IN, 0, 0}, pio replacement
//{0, GPIO_PERIPH_3V3_EN, 0}, Owned by the 8266 driver
//{0, GPIO_SBUS_INV, 0}, https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c
//{GPIO_8266_GPIO0, 0, 0}, Owned by the 8266 driver
//{0, GPIO_SPEKTRUM_POWER, 0}, Owned Spektum driver input to auto pilot
//{0, GPIO_8266_PD, 0}, Owned by the 8266 driver
//{0, GPIO_8266_RST, 0}, Owned by the 8266 driver
/* 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_PIN10)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3)
#define GPIO_LED_RED GPIO_LED1
#define GPIO_LED_GREEN GPIO_LED2
#define GPIO_LED_BLUE GPIO_LED3
/* Define the Chip Selects */
#define GPIO_SPI_CS_MPU9250 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
#define GPIO_SPI_CS_HMC5983 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15)
#define GPIO_SPI_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_ICM_20608_G (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
/* Define the Ready interrupts */
#define GPIO_DRDY_MPU9250 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
#define GPIO_DRDY_HMC5983 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12)
#define GPIO_DRDY_ICM_20608_G (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14)
/*
* Define the ability to shut off off the sensor signals
* by changing the signals to inputs
*/
#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz))
#define GPIO_SPI_CS_OFF_MPU9250 _PIN_OFF(GPIO_SPI_CS_MPU9250)
#define GPIO_SPI_CS_OFF_HMC5983 _PIN_OFF(GPIO_SPI_CS_HMC5983)
#define GPIO_SPI_CS_OFF_MS5611 _PIN_OFF(GPIO_SPI_CS_MS5611)
#define GPIO_SPI_CS_OFF_ICM_20608_G _PIN_OFF(GPIO_SPI_CS_ICM_20608_G)
#define GPIO_DRDY_OFF_MPU9250 _PIN_OFF(GPIO_DRDY_MPU9250)
#define GPIO_DRDY_OFF_HMC5983 _PIN_OFF(GPIO_DRDY_HMC5983)
#define GPIO_DRDY_OFF_ICM_20608_G _PIN_OFF(GPIO_DRDY_ICM_20608_G)
/* SPI1 off */
#define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK)
#define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO)
#define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI)
#define PX4_SPI_BUS_SENSORS 1
#define PX4_SPI_BUS_RAMTRON 2
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
#define PX4_SPIDEV_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2
#define PX4_SPIDEV_BARO 3
#define PX4_SPIDEV_MPU 4
#define PX4_SPIDEV_HMC 5
#define PX4_SPIDEV_ICM 6
/* I2C busses */
#define PX4_I2C_BUS_EXPANSION 1
#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION
/* Devices on the external bus.
*
* Note that these are unshifted addresses.
*/
#define PX4_I2C_OBDEV_LED 0x55
#define PX4_I2C_OBDEV_HMC5883 0x1e
/*
* 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 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)
// ADC defines to be used in sensors.cpp to read from a particular channel
#define ADC_BATTERY_VOLTAGE_CHANNEL 2
#define ADC_BATTERY_CURRENT_CHANNEL 3
#define ADC_5V_RAIL_SENSE 4
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
/* User GPIOs
*
* GPIO0-5 are the PWM servo outputs.
*/
#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14)
/* Power supply control and monitoring GPIOs */
#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
/* Tone alarm output */
#define TONE_ALARM_TIMER 2 /* timer 2 */
#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_PIN15)
#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
/* PWM
*
* Six PWM outputs are configured.
*
* Pins:
*
* CH1 : PE14 : TIM1_CH4
* CH2 : PE13 : TIM1_CH3
* CH3 : PE11 : TIM1_CH2
* CH4 : PE9 : TIM1_CH1
* CH5 : PD13 : TIM4_CH2
* CH6 : PD14 : TIM4_CH3
*/
#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2
#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2
#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2
#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2
#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2
#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2
/* 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 */
#define HRT_TIMER 3 /* use timer8 for the HRT */
#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */
#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */
#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
#define SBUS_SERIAL_PORT "/dev/ttyS4"
/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
#define PWMIN_TIMER 4
#define PWMIN_TIMER_CHANNEL 2
#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2
#define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
#define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
#define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2)
#define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
#define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5)
#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
/****************************************************************************************************
* 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);
extern void stm32_usbinitialize(void);
/****************************************************************************
* 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
#endif /* __ASSEMBLY__ */
__END_DECLS
src/drivers/boards/px4fmu-v4/px4fmu_can.c
+144
View File
@@ -0,0 +1,144 @@
/****************************************************************************
*
* Copyright (C) 2012 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 px4fmu_can.c
*
* Board-specific CAN functions.
*/
/************************************************************************************
* Included Files
************************************************************************************/
#include <px4_config.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/can.h>
#include <arch/board/board.h>
#include "chip.h"
#include "up_arch.h"
#include "stm32.h"
#include "stm32_can.h"
#include "board_config.h"
#ifdef CONFIG_CAN
/************************************************************************************
* Pre-processor Definitions
************************************************************************************/
/* Configuration ********************************************************************/
#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
# undef CONFIG_STM32_CAN2
#endif
#ifdef CONFIG_STM32_CAN1
# define CAN_PORT 1
#else
# 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
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: can_devinit
*
* Description:
* All STM32 architectures must provide the following interface to work with
* examples/can.
*
************************************************************************************/
int can_devinit(void)
{
static bool initialized = false;
struct can_dev_s *can;
int ret;
/* Check if we have already initialized */
if (!initialized) {
/* Call stm32_caninitialize() to get an instance of the CAN interface */
can = stm32_caninitialize(CAN_PORT);
if (can == NULL) {
candbg("ERROR: Failed to get CAN interface\n");
return -ENODEV;
}
/* Register the CAN driver at "/dev/can0" */
ret = can_register("/dev/can0", can);
if (ret < 0) {
candbg("ERROR: can_register failed: %d\n", ret);
return ret;
}
/* Now we are initialized */
initialized = true;
}
return OK;
}
#endif
src/drivers/boards/px4fmu-v4/px4fmu_init.c
+335
View File
@@ -0,0 +1,335 @@
/****************************************************************************
*
* Copyright (C) 2012 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 px4fmu_init.c
*
* PX4FMU-specific early startup code. This file implements the
* nsh_archinitialize() 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.
*/
/****************************************************************************
* Included Files
****************************************************************************/
#include <px4_config.h>
#include <stdbool.h>
#include <stdio.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/arch.h>
#include <nuttx/spi.h>
#include <nuttx/i2c.h>
#include <nuttx/sdio.h>
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
#include <nuttx/gran.h>
#include <stm32.h>
#include "board_config.h"
#include <stm32_uart.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_led.h>
#include <systemlib/cpuload.h>
#include <systemlib/perf_counter.h>
/****************************************************************************
* Pre-Processor Definitions
****************************************************************************/
/* Configuration ************************************************************/
/* Debug ********************************************************************/
#ifdef CONFIG_CPP_HAVE_VARARGS
# ifdef CONFIG_DEBUG
# define message(...) lowsyslog(__VA_ARGS__)
# else
# define message(...) printf(__VA_ARGS__)
# endif
#else
# ifdef CONFIG_DEBUG
# define message lowsyslog
# else
# define message printf
# endif
#endif
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
#if defined(CONFIG_FAT_DMAMEMORY)
# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
# error microSD DMA support requires CONFIG_GRAN
# endif
static GRAN_HANDLE dma_allocator;
/*
* The DMA heap size constrains the total number of things that can be
* ready to do DMA at a time.
*
* For example, FAT DMA depends on one sector-sized buffer per filesystem plus
* one sector-sized buffer per file.
*
* We use a fundamental alignment / granule size of 64B; this is sufficient
* to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
*/
static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
static perf_counter_t g_dma_perf;
static void
dma_alloc_init(void)
{
dma_allocator = gran_initialize(g_dma_heap,
sizeof(g_dma_heap),
7, /* 128B granule - must be > alignment (XXX bug?) */
6); /* 64B alignment */
if (dma_allocator == NULL) {
message("[boot] DMA allocator setup FAILED");
} else {
g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
}
}
/****************************************************************************
* Public Functions
****************************************************************************/
/*
* DMA-aware allocator stubs for the FAT filesystem.
*/
__EXPORT void *fat_dma_alloc(size_t size);
__EXPORT void fat_dma_free(FAR void *memory, size_t size);
void *
fat_dma_alloc(size_t size)
{
perf_count(g_dma_perf);
return gran_alloc(dma_allocator, size);
}
void
fat_dma_free(FAR void *memory, size_t size)
{
gran_free(dma_allocator, memory, size);
}
#else
# define dma_alloc_init()
#endif
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the intitialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
__EXPORT void
stm32_boardinitialize(void)
{
/* configure SPI interfaces */
stm32_spiinitialize();
/* configure LEDs */
up_ledinit();
}
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization
*
****************************************************************************/
static struct spi_dev_s *spi1;
static struct spi_dev_s *spi2;
static struct sdio_dev_s *sdio;
#include <math.h>
__EXPORT int nsh_archinitialize(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_PERIPH_3V3_EN);
stm32_configgpio(GPIO_VDD_BRICK_VALID);
stm32_configgpio(GPIO_GPIO5_OUTPUT);
stm32_configgpio(GPIO_SBUS_INV);
stm32_configgpio(GPIO_8266_GPIO0);
stm32_configgpio(GPIO_SPEKTRUM_POWER);
stm32_configgpio(GPIO_8266_PD);
stm32_configgpio(GPIO_8266_RST);
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure the DMA allocator */
dma_alloc_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
/* initial LED state */
drv_led_start();
led_off(LED_RED);
led_off(LED_GREEN);
led_off(LED_BLUE);
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\n");
up_ledon(LED_RED);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_HMC, false);
SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
/* Get the SPI port for the FRAM */
spi2 = up_spiinitialize(2);
if (!spi2) {
message("[boot] FAILED to initialize SPI port 2\n");
up_ledon(LED_RED);
return -ENODEV;
}
/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
* and de-assert the known chip selects. */
// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_FLASH, false);
#ifdef CONFIG_MMCSD
/* First, get an instance of the SDIO interface */
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!sdio) {
message("[boot] Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}
/* Now bind the SDIO interface to the MMC/SD driver */
int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
if (ret != OK) {
message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}
/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
sdio_mediachange(sdio, true);
#endif
return OK;
}
src/drivers/boards/px4fmu-v4/px4fmu_led.c
+106
View File
@@ -0,0 +1,106 @@
/****************************************************************************
*
* Copyright (c) 2013 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 px4fmu2_led.c
*
* PX4FMU LED backend.
*/
#include <px4_config.h>
#include <stdbool.h>
#include "stm32.h"
#include "board_config.h"
#include <arch/board/board.h>
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
__END_DECLS
static uint32_t g_ledmap[] = {
GPIO_LED_BLUE, // Indexed by LED_BLUE
GPIO_LED_RED, // Indexed by LED_RED, LED_AMBER
GPIO_LED_SAFETY, // Indexed by LED_SAFETY
GPIO_LED_GREEN, // Indexed by LED_GREEN
};
__EXPORT void led_init(void)
{
/* Configure LED GPIOs for output */
for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
stm32_configgpio(g_ledmap[l]);
}
}
static void phy_set_led(int led, bool state)
{
/* Pull Down to switch on */
stm32_gpiowrite(g_ledmap[led], !state);
}
static bool phy_get_led(int led)
{
return !stm32_gpioread(g_ledmap[led]);
}
__EXPORT void led_on(int led)
{
phy_set_led(led, true);
}
__EXPORT void led_off(int led)
{
phy_set_led(led, false);
}
__EXPORT void led_toggle(int led)
{
phy_set_led(led, !phy_get_led(led));
}
src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c
+105
View File
@@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (C) 2012 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 px4fmu_pwm_servo.c
*
* Configuration data for the stm32 pwm_servo driver.
*
* Note that these arrays must always be fully-sized.
*/
#include <stdint.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
#include <drivers/stm32/drv_pwm_servo.h>
#include <drivers/drv_pwm_output.h>
#include "board_config.h"
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
{
.base = STM32_TIM1_BASE,
.clock_register = STM32_RCC_APB2ENR,
.clock_bit = RCC_APB2ENR_TIM1EN,
.clock_freq = STM32_APB2_TIM1_CLKIN
},
{
.base = STM32_TIM4_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM4EN,
.clock_freq = STM32_APB1_TIM4_CLKIN
}
};
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
{
.gpio = GPIO_TIM1_CH4OUT,
.timer_index = 0,
.timer_channel = 4,
.default_value = 1000,
},
{
.gpio = GPIO_TIM1_CH3OUT,
.timer_index = 0,
.timer_channel = 3,
.default_value = 1000,
},
{
.gpio = GPIO_TIM1_CH2OUT,
.timer_index = 0,
.timer_channel = 2,
.default_value = 1000,
},
{
.gpio = GPIO_TIM1_CH1OUT,
.timer_index = 0,
.timer_channel = 1,
.default_value = 1000,
},
{
.gpio = GPIO_TIM4_CH2OUT,
.timer_index = 1,
.timer_channel = 2,
.default_value = 1000,
},
{
.gpio = GPIO_TIM4_CH3OUT,
.timer_index = 1,
.timer_channel = 3,
.default_value = 1000,
}
};
src/drivers/boards/px4fmu-v4/px4fmu_spi.c
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* Copyright (C) 2012 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 px4fmu_spi.c
*
* Board-specific SPI functions.
*/
/************************************************************************************
* Included Files
************************************************************************************/
#include <px4_config.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <nuttx/spi.h>
#include <arch/board/board.h>
#include <up_arch.h>
#include <chip.h>
#include <stm32.h>
#include "board_config.h"
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: stm32_spiinitialize
*
* Description:
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
*
************************************************************************************/
__EXPORT void stm32_spiinitialize(void)
{
#ifdef CONFIG_STM32_SPI1
stm32_configgpio(GPIO_SPI_CS_MPU9250);
stm32_configgpio(GPIO_SPI_CS_HMC5983);
stm32_configgpio(GPIO_SPI_CS_MS5611);
stm32_configgpio(GPIO_SPI_CS_ICM_20608_G);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
stm32_configgpio(GPIO_DRDY_MPU9250);
stm32_configgpio(GPIO_DRDY_HMC5983);
stm32_configgpio(GPIO_DRDY_ICM_20608_G);
#endif
#ifdef CONFIG_STM32_SPI2
stm32_configgpio(GPIO_SPI_CS_FRAM);
stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
#endif
}
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
/* SPI select is active low, so write !selected to select the device */
switch (devid) {
case PX4_SPIDEV_ICM:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected);
break;
case PX4_SPIDEV_ACCEL_MAG:
/* Making sure the other peripherals are not selected */
break;
case PX4_SPIDEV_BARO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected);
stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
break;
case PX4_SPIDEV_HMC:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected);
stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
break;
default:
break;
}
}
__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
{
return SPI_STATUS_PRESENT;
}
#ifdef CONFIG_STM32_SPI2
__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 */
stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
}
__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
{
/* FRAM is always present */
return SPI_STATUS_PRESENT;
}
#endif
src/drivers/boards/px4fmu-v4/px4fmu_usb.c
+108
View File
@@ -0,0 +1,108 @@
/****************************************************************************
*
* Copyright (C) 2012 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 px4fmu_usb.c
*
* Board-specific USB functions.
*/
/************************************************************************************
* Included Files
************************************************************************************/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <nuttx/usb/usbdev.h>
#include <nuttx/usb/usbdev_trace.h>
#include <up_arch.h>
#include <stm32.h>
#include "board_config.h"
/************************************************************************************
* Definitions
************************************************************************************/
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: stm32_usbinitialize
*
* Description:
* Called to setup USB-related GPIO pins for the PX4FMU board.
*
************************************************************************************/
__EXPORT void stm32_usbinitialize(void)
{
/* The OTG FS has an internal soft pull-up */
/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
#ifdef CONFIG_STM32_OTGFS
stm32_configgpio(GPIO_OTGFS_VBUS);
/* XXX We only support device mode
stm32_configgpio(GPIO_OTGFS_PWRON);
stm32_configgpio(GPIO_OTGFS_OVER);
*/
#endif
}
/************************************************************************************
* Name: stm32_usbsuspend
*
* Description:
* Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
* used. This function is called whenever the USB enters or leaves suspend mode.
* This is an opportunity for the board logic to shutdown clocks, power, etc.
* while the USB is suspended.
*
************************************************************************************/
__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
{
ulldbg("resume: %d\n", resume);
}
src/drivers/boards/px4io-v1/board_config.h
+2
View File
@@ -65,6 +65,8 @@
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10)
/* Safety switch button *************************************************************/
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
src/drivers/boards/px4io-v2/board_config.h
+2
View File
@@ -79,6 +79,8 @@
#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_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10)
/* Safety switch button *******************************************************/
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
src/drivers/drv_gpio.h
+24 -1
View File
@@ -94,6 +94,29 @@
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
/*
* PX4FMUv3 GPIO numbers.
*
* There are no alternate functions on this board.
*/
# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */
# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */
# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */
# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */
# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */
# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */
# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */
# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */
/**
* Device paths for things that support the GPIO ioctl protocol.
*/
# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
#endif
#ifdef CONFIG_ARCH_BOARD_AEROCORE
/*
* AeroCore GPIO numbers and configuration.
@@ -121,7 +144,7 @@
#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \
!defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \
!defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \
!defined(CONFIG_ARCH_BOARD_SITL)
!defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL)
# error No CONFIG_ARCH_BOARD_xxxx set
#endif
/*
src/drivers/drv_led.h
+2
View File
@@ -53,6 +53,8 @@
#define LED_RED 1 /* some boards have red rather than amber */
#define LED_BLUE 0
#define LED_SAFETY 2
#define LED_GREEN 3
#define LED_ON _PX4_IOC(_LED_BASE, 0)
#define LED_OFF _PX4_IOC(_LED_BASE, 1)
src/drivers/gps/ubx.cpp
+1
View File
@@ -874,6 +874,7 @@ UBX::payload_rx_done(void)
_gps_position->alt = _buf.payload_rx_nav_posllh.hMSL;
_gps_position->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m
_gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m
_gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height;
_gps_position->timestamp_position = hrt_absolute_time();
src/drivers/ll40ls/ll40ls.cpp
+14 -10
View File
@@ -97,7 +97,7 @@ LidarLite *get_dev(const bool use_i2c, const int bus)
LidarLite *g_dev = nullptr;
if (use_i2c) {
g_dev = static_cast<LidarLite *>(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
g_dev = static_cast<LidarLite *>(bus == PX4_I2C_BUS_EXPANSION ? g_dev_ext : g_dev_int);
if (g_dev == nullptr) {
errx(1, "i2c driver not running");
@@ -231,11 +231,15 @@ void start(const bool use_i2c, const int bus)
fail:
#ifdef PX4_I2C_BUS_ONBOARD
if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
delete g_dev_int;
g_dev_int = nullptr;
}
#endif
if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
delete g_dev_ext;
g_dev_ext = nullptr;
@@ -255,17 +259,17 @@ fail:
void stop(const bool use_i2c, const int bus)
{
if (use_i2c) {
if (bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr) {
delete g_dev_int;
g_dev_int = nullptr;
}
} else {
if (bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev_ext != nullptr) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
} else {
if (g_dev_int != nullptr) {
delete g_dev_int;
g_dev_int = nullptr;
}
}
} else {
@@ -293,7 +297,7 @@ test(const bool use_i2c, const int bus)
const char *path;
if (use_i2c) {
path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT);
} else {
path = LL40LS_DEVICE_PATH_PWM;
@@ -366,7 +370,7 @@ reset(const bool use_i2c, const int bus)
const char *path;
if (use_i2c) {
path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT);
} else {
path = LL40LS_DEVICE_PATH_PWM;
src/drivers/meas_airspeed/meas_airspeed.cpp
+2 -2
View File
@@ -335,7 +335,7 @@ MEASAirspeed::cycle()
void
MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
if (_t_system_power == -1) {
_t_system_power = orb_subscribe(ORB_ID(system_power));
@@ -389,7 +389,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
}
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
}
/**
src/drivers/mpu6000/mpu6000.cpp
+76 -15
View File
@@ -149,6 +149,21 @@
#define BIT_INT_STATUS_DATA 0x01
#define MPU_WHOAMI_6000 0x68
#define ICM_WHOAMI_20608 0xaf
// ICM2608 specific registers
/* this is an undocumented register which
if set incorrectly results in getting a 2.7m/s/s offset
on the Y axis of the accelerometer
*/
#define MPUREG_ICM_UNDOC1 0x11
#define MPUREG_ICM_UNDOC1_VALUE 0xc9
// Product ID Description for ICM2608
// There is none
#define ICM20608_REV_00 0
// Product ID Description for MPU6000
// high 4 bits low 4 bits
@@ -208,7 +223,8 @@ class MPU6000_gyro;
class MPU6000 : public device::SPI
{
public:
MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation,
int device_type);
virtual ~MPU6000();
virtual int init();
@@ -242,6 +258,7 @@ protected:
virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
int _device_type;
MPU6000_gyro *_gyro;
uint8_t _product; /** product code */
@@ -292,7 +309,7 @@ private:
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define MPU6000_NUM_CHECKED_REGISTERS 9
#define MPU6000_NUM_CHECKED_REGISTERS 10
static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
@@ -325,6 +342,15 @@ private:
*/
int reset();
/**
* is_icm_device
*/
bool is_icm_device() { return _device_type == 20608;}
/**
* is_mpu_device
*/
bool is_mpu_device() { return _device_type == 6000;}
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -468,7 +494,8 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
MPUREG_GYRO_CONFIG,
MPUREG_ACCEL_CONFIG,
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG
MPUREG_INT_PIN_CFG,
MPUREG_ICM_UNDOC1
};
@@ -506,8 +533,10 @@ private:
/** driver 'main' command */
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) :
MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation,
int device_type) :
SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_device_type(device_type),
_gyro(new MPU6000_gyro(this, path_gyro)),
_product(0),
_call{},
@@ -768,6 +797,10 @@ int MPU6000::reset()
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
usleep(1000);
if (is_icm_device()) {
write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE);
}
// Oscillator set
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
@@ -778,16 +811,15 @@ int MPU6000::reset()
int
MPU6000::probe()
{
uint8_t whoami;
whoami = read_reg(MPUREG_WHOAMI);
uint8_t whoami = read_reg(MPUREG_WHOAMI);
uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
if (whoami != MPU_WHOAMI_6000) {
if (whoami != expected) {
DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami);
return -EIO;
}
/* look for a product ID we recognise */
/* look for a product ID we recognize */
_product = read_reg(MPUREG_PRODUCT_ID);
// verify product revision
@@ -804,6 +836,7 @@ MPU6000::probe()
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
case ICM20608_REV_00:
DEVICE_DEBUG("ID 0x%02x", _product);
_checked_values[0] = _product;
return OK;
@@ -1483,6 +1516,13 @@ MPU6000::read_reg(unsigned reg, uint32_t speed)
{
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
// There is no MPUREG_PRODUCT_ID on the icm device
// so lets make dummy it up and allow the rest of the
// code to run as is
if (reg == MPUREG_PRODUCT_ID && is_icm_device()) {
return ICM20608_REV_00;
}
// general register transfer at low clock speed
set_frequency(speed);
@@ -1641,6 +1681,11 @@ MPU6000::check_registers(void)
*/
uint8_t v;
// the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented)
if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) {
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) {
/*
@@ -2051,7 +2096,7 @@ namespace mpu6000
MPU6000 *g_dev_int; // on internal bus
MPU6000 *g_dev_ext; // on external bus
void start(bool, enum Rotation, int range);
void start(bool, enum Rotation, int range, int device_type);
void stop(bool);
void test(bool);
void reset(bool);
@@ -2068,7 +2113,7 @@ void usage();
* or failed to detect the sensor.
*/
void
start(bool external_bus, enum Rotation rotation, int range)
start(bool external_bus, enum Rotation rotation, int range, int device_type)
{
int fd;
MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
@@ -2084,13 +2129,23 @@ start(bool external_bus, enum Rotation rotation, int range)
/* create the driver */
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
# if defined(PX4_SPIDEV_EXT_ICM)
spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM);
# else
spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU;
# endif
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, cs, rotation, device_type);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
#if defined(PX4_SPIDEV_ICM)
spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM);
#else
spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_MPU;
#endif
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, cs, rotation, device_type);
}
if (*g_dev_ptr == nullptr) {
@@ -2334,6 +2389,7 @@ usage()
warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -M 6000|20608 (default 6000)");
warnx(" -R rotation");
warnx(" -a accel range (in g)");
}
@@ -2344,17 +2400,22 @@ int
mpu6000_main(int argc, char *argv[])
{
bool external_bus = false;
int device_type = 6000;
int ch;
enum Rotation rotation = ROTATION_NONE;
int accel_range = 8;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'T':
device_type = atoi(optarg);
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
@@ -2376,7 +2437,7 @@ mpu6000_main(int argc, char *argv[])
*/
if (!strcmp(verb, "start")) {
mpu6000::start(external_bus, rotation, accel_range);
mpu6000::start(external_bus, rotation, accel_range, device_type);
}
if (!strcmp(verb, "stop")) {
src/drivers/px4flow/px4flow.cpp
+2
View File
@@ -694,7 +694,9 @@ start()
#ifdef PX4_I2C_BUS_ESC
PX4_I2C_BUS_ESC,
#endif
#ifdef PX4_I2C_BUS_ONBOARD
PX4_I2C_BUS_ONBOARD,
#endif
-1
};
src/drivers/px4fmu/fmu.cpp
+185 -11
View File
@@ -71,6 +71,11 @@
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
#include <lib/rc/sbus.h>
#include <lib/rc/dsm.h>
#include <lib/rc/st24.h>
#include <lib/rc/sumd.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
@@ -89,7 +94,7 @@
* This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
*/
#define CONTROL_INPUT_DROP_LIMIT_MS 20
#define CONTROL_INPUT_DROP_LIMIT_US 1800
#define NAN_VALUE (0.0f/0.0f)
class PX4FMU : public device::CDev
@@ -121,7 +126,7 @@ private:
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
static const unsigned _max_actuators = 4;
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
static const unsigned _max_actuators = 6;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
@@ -141,6 +146,8 @@ private:
orb_advert_t _outputs_pub;
unsigned _num_outputs;
int _class_instance;
int _sbus_fd;
int _dsm_fd;
volatile bool _initialized;
bool _servo_armed;
@@ -232,6 +239,17 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_VDD_5V_HIPOWER_OC, 0, 0},
{GPIO_VDD_5V_PERIPH_OC, 0, 0},
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0},
{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
{0, GPIO_VDD_3V3_SENSORS_EN, 0},
{GPIO_VDD_BRICK_VALID, 0, 0},
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
/* AeroCore breaks out User GPIOs on J11 */
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
@@ -274,6 +292,8 @@ PX4FMU::PX4FMU() :
_outputs_pub(nullptr),
_num_outputs(0),
_class_instance(0),
_sbus_fd(-1),
_dsm_fd(-1),
_initialized(false),
_servo_armed(false),
_pwm_on(false),
@@ -307,6 +327,11 @@ PX4FMU::PX4FMU() :
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
#endif
#ifdef GPIO_SBUS_INV
// this board has a GPIO to control SBUS inversion
stm32_configgpio(GPIO_SBUS_INV);
#endif
/* only enable this during development */
_debug_enabled = false;
}
@@ -569,7 +594,7 @@ PX4FMU::update_pwm_rev_mask()
void
PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues)
{
actuator_outputs_s outputs;
actuator_outputs_s outputs = {};
outputs.noutputs = numvalues;
outputs.timestamp = hrt_absolute_time();
@@ -620,6 +645,15 @@ PX4FMU::cycle()
update_pwm_rev_mask();
#ifdef SBUS_SERIAL_PORT
_sbus_fd = sbus_init(SBUS_SERIAL_PORT, true);
#endif
#ifdef DSM_SERIAL_PORT
// XXX rather than opening it we need to cycle between protocols until one is locked in
//_dsm_fd = dsm_init(DSM_SERIAL_PORT);
#endif
_initialized = true;
}
@@ -776,10 +810,47 @@ PX4FMU::cycle()
update_pwm_rev_mask();
}
bool rc_updated = false;
#ifdef SBUS_SERIAL_PORT
bool sbus_failsafe, sbus_frame_drop;
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
uint16_t raw_rc_count;
bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
input_rc_s::RC_INPUT_MAX_CHANNELS);
if (sbus_updated) {
// we have a new PPM frame. Publish it.
_rc_in.channel_count = raw_rc_count;
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
_rc_in.values[i] = raw_rc_values[i];
}
_rc_in.timestamp_publication = hrt_absolute_time();
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
_rc_in.rc_ppm_frame_length = 0;
_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0;
_rc_in.rc_failsafe = false;
_rc_in.rc_lost = false;
_rc_in.rc_lost_frame_count = 0;
_rc_in.rc_total_frame_count = 0;
rc_updated = true;
}
#endif
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if (ppm_last_valid_decode != _rc_in.timestamp_last_signal) {
if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) &&
ppm_decoded_channels > 3) {
// we have a new PPM frame. Publish it.
_rc_in.channel_count = ppm_decoded_channels;
@@ -801,6 +872,12 @@ PX4FMU::cycle()
_rc_in.rc_lost_frame_count = 0;
_rc_in.rc_total_frame_count = 0;
rc_updated = true;
}
#endif
if (rc_updated) {
/* lazily advertise on first publication */
if (_to_input_rc == nullptr) {
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in);
@@ -810,8 +887,7 @@ PX4FMU::cycle()
}
}
#endif
work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000));
work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US));
}
void PX4FMU::work_stop()
@@ -1266,7 +1342,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_4PWM);
break;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
case 6:
set_mode(MODE_6PWM);
@@ -1479,6 +1555,79 @@ PX4FMU::sensor_reset(int ms)
// stm32_configgpio(GPIO_ACCEL_DRDY);
// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
#endif
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
if (ms < 1) {
ms = 1;
}
/* disable SPI bus */
stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250);
stm32_configgpio(GPIO_SPI_CS_OFF_HMC5983);
stm32_configgpio(GPIO_SPI_CS_OFF_MS5611);
stm32_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G);
stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0);
stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0);
stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0);
stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0);
stm32_configgpio(GPIO_SPI1_SCK_OFF);
stm32_configgpio(GPIO_SPI1_MISO_OFF);
stm32_configgpio(GPIO_SPI1_MOSI_OFF);
stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
stm32_configgpio(GPIO_DRDY_OFF_MPU9250);
stm32_configgpio(GPIO_DRDY_OFF_HMC5983);
stm32_configgpio(GPIO_DRDY_OFF_ICM_20608_G);
stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0);
stm32_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0);
stm32_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0);
/* set the sensor rail off */
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);
warnx("reset done, %d ms", ms);
/* re-enable power */
/* switch the sensor rail back on */
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
stm32_configgpio(GPIO_SPI_CS_MPU9250);
stm32_configgpio(GPIO_SPI_CS_HMC5983);
stm32_configgpio(GPIO_SPI_CS_MS5611);
stm32_configgpio(GPIO_SPI_CS_ICM_20608_G);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 1);
stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 1);
stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 1);
stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 1);
// // XXX bring up the EXTI pins again
// stm32_configgpio(GPIO_GYRO_DRDY);
// stm32_configgpio(GPIO_MAG_DRDY);
// stm32_configgpio(GPIO_ACCEL_DRDY);
// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
#endif
#endif
}
@@ -1505,6 +1654,31 @@ PX4FMU::peripheral_reset(int ms)
/* switch the peripheral rail back on */
stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
if (ms < 1) {
ms = 10;
}
/* set the peripheral rails off */
stm32_configgpio(GPIO_PERIPH_3V3_EN);
stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0);
bool last = stm32_gpioread(GPIO_SPEKTRUM_POWER);
/* Keep Spektum on to discharge rail*/
stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1);
/* wait for the peripheral rail to reach GND */
usleep(ms * 1000);
warnx("reset done, %d ms", ms);
/* re-enable power */
/* switch the peripheral rail back on */
stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last);
stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1);
#endif
}
void
@@ -1696,7 +1870,7 @@ fmu_new_mode(PortMode new_mode)
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_4PWM;
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
servo_mode = PX4FMU::MODE_6PWM;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
@@ -1704,7 +1878,7 @@ fmu_new_mode(PortMode new_mode)
#endif
break;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
case PORT_PWM4:
/* select 4-pin PWM mode */
@@ -2012,7 +2186,7 @@ fmu_main(int argc, char *argv[])
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
} else if (!strcmp(verb, "mode_pwm4")) {
new_mode = PORT_PWM4;
@@ -2101,7 +2275,7 @@ fmu_main(int argc, char *argv[])
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr,
" mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE)
fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c <bus> <hz>\n");
#endif
exit(1);
src/drivers/stm32/adc/adc.cpp
+34
View File
@@ -348,6 +348,40 @@ ADC::update_system_power(void)
}
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
system_power_s system_power;
system_power.timestamp = hrt_absolute_time();
system_power.voltage5V_v = 0;
for (unsigned i = 0; i < _channel_count; i++) {
if (_samples[i].am_channel == 4) {
// it is 2:1 scaled
system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
}
}
// these are not ADC related, but it is convenient to
// publish these to the same topic
system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
// note that the valid pins are active High
system_power.brick_valid = stm32_gpioread(GPIO_VDD_BRICK_VALID);
system_power.servo_valid = 1;
// OC pins are not supported
system_power.periph_5V_OC = 0;
system_power.hipower_5V_OC = 0;
/* lazily publish */
if (_to_system_power != nullptr) {
orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
} else {
_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
}
#endif // CONFIG_ARCH_BOARD_PX4FMU_V4
}
uint16_t
src/drivers/stm32/drv_hrt.c
+2 -1
View File
@@ -221,6 +221,7 @@
/*
* Specific registers and bits used by HRT sub-functions
*/
/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
#if HRT_TIMER_CHANNEL == 1
# define rCCR_HRT rCCR1 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */
@@ -294,7 +295,7 @@ static void hrt_call_invoke(void);
# define GTIM_CCER_CC4NP 0
# define PPM_EDGE_FLIP
# endif
/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
# if HRT_PPM_CHANNEL == 1
# define rCCR_PPM rCCR1 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */
src/firmware/nuttx/CMakeLists.txt
+7 -3
View File
@@ -41,13 +41,17 @@ add_custom_target(check_weak
if(NOT ${BOARD} STREQUAL "sim")
if (config_io_board)
set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
endif()
px4_nuttx_add_romfs(OUT romfs
ROOT ROMFS/px4fmu_common
EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin
EXTRAS ${extras}
)
add_dependencies(romfs fw_io)
if (config_io_board)
add_dependencies(romfs fw_io)
endif()
set(fw_file
${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4)
src/lib/rc/CMakeLists.txt
+2
View File
@@ -37,6 +37,8 @@ px4_add_module(
SRCS
st24.c
sumd.c
sbus.c
dsm.c
DEPENDS
platforms__common
)
src/modules/px4iofirmware/dsm.c → src/lib/rc/dsm.c
+29 -22
View File
@@ -46,9 +46,16 @@
#include <unistd.h>
#include <termios.h>
#include "dsm.h"
#include <drivers/drv_hrt.h>
#include "px4io.h"
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#include <px4io.h>
#endif
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#include <px4io.h>
#endif
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
@@ -61,6 +68,9 @@ static unsigned dsm_partial_frame_count; /**< Count of bytes received for curren
static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
static bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values);
/**
* Attempt to decode a single channel raw channel datum
*
@@ -185,18 +195,18 @@ dsm_guess_format(bool reset)
if ((votes11 == 1) && (votes10 == 0)) {
dsm_channel_shift = 11;
debug("DSM: 11-bit format");
//debug("DSM: 11-bit format");
return;
}
if ((votes10 == 1) && (votes11 == 0)) {
dsm_channel_shift = 10;
debug("DSM: 10-bit format");
//debug("DSM: 10-bit format");
return;
}
/* call ourselves to reset our state ... we have to try again */
debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
//debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
dsm_guess_format(true);
}
@@ -237,11 +247,11 @@ dsm_init(const char *device)
/* reset the format detector */
dsm_guess_format(true);
debug("DSM: ready");
//debug("DSM: ready");
} else {
debug("DSM: open failed");
//debug("DSM: open failed");
}
@@ -257,11 +267,8 @@ dsm_init(const char *device)
void
dsm_bind(uint16_t cmd, int pulses)
{
#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)
#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
#if !defined(GPIO_USART1_RX_SPEKTRUM)
#else
const uint32_t usart1RxAsOutp =
GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
if (dsm_fd < 0) {
return;
@@ -272,9 +279,9 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_power_down:
/*power down DSM satellite*/
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
POWER_RELAY1(0);
#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
POWER_SPEKTRUM(0);
#endif
break;
@@ -282,9 +289,9 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_power_up:
/*power up DSM satellite*/
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
POWER_RELAY1(1);
#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
POWER_SPEKTRUM(1);
#endif
dsm_guess_format(true);
@@ -293,7 +300,7 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_set_rx_out:
/*Set UART RX pin to active output mode*/
stm32_configgpio(usart1RxAsOutp);
stm32_configgpio(GPIO_USART1_RX_SPEKTRUM);
break;
case dsm_bind_send_pulses:
@@ -301,9 +308,9 @@ dsm_bind(uint16_t cmd, int pulses)
/*Pulse RX pin a number of times*/
for (int i = 0; i < pulses; i++) {
up_udelay(120);
stm32_gpiowrite(usart1RxAsOutp, false);
stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, false);
up_udelay(120);
stm32_gpiowrite(usart1RxAsOutp, true);
stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, true);
}
break;
@@ -327,8 +334,8 @@ dsm_bind(uint16_t cmd, int pulses)
* @param[out] num_values pointer to number of raw channel values returned
* @return true=DSM frame successfully decoded, false=no update
*/
static bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values)
{
/*
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
@@ -374,7 +381,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
}
/* ignore channels out of range */
if (channel >= PX4IO_RC_INPUT_CHANNELS) {
if (channel >= max_values) {
continue;
}
@@ -473,7 +480,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
* @return true=decoded raw channel values updated, false=no update
*/
bool
dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values)
{
ssize_t ret;
hrt_abstime now;
@@ -523,5 +530,5 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by
* decode it.
*/
dsm_partial_frame_count = 0;
return dsm_decode(now, values, num_values);
return dsm_decode(now, values, num_values, max_values);
}
src/lib/rc/dsm.h
+52
View File
@@ -0,0 +1,52 @@
/****************************************************************************
*
* 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
* 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 dsm.h
*
* RC protocol definition for Spektrum RC
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include <stdint.h>
__BEGIN_DECLS
__EXPORT int dsm_init(const char *device);
__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values);
__EXPORT void dsm_bind(uint16_t cmd, int pulses);
__END_DECLS
src/modules/px4iofirmware/sbus.c → src/lib/rc/sbus.c
+17 -24
View File
@@ -43,15 +43,9 @@
#include <unistd.h>
#include <termios.h>
#include <systemlib/ppm_decode.h>
#include "sbus.h"
#include <drivers/drv_hrt.h>
#define DEBUG
#include "px4io.h"
#include "protocol.h"
#include "debug.h"
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE 23
@@ -77,8 +71,6 @@
#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
static hrt_abstime last_txframe_time = 0;
@@ -93,11 +85,9 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_
bool *sbus_frame_drop, uint16_t max_channels);
int
sbus_init(const char *device)
sbus_init(const char *device, bool singlewire)
{
if (sbus_fd < 0) {
sbus_fd = open(device, O_RDWR | O_NONBLOCK);
}
int sbus_fd = open(device, O_RDWR | O_NONBLOCK);
if (sbus_fd >= 0) {
struct termios t;
@@ -108,21 +98,24 @@ sbus_init(const char *device)
t.c_cflag |= (CSTOPB | PARENB);
tcsetattr(sbus_fd, TCSANOW, &t);
if (singlewire) {
/* only defined in configs capable of IOCTL */
#ifdef SBUS_SERIAL_PORT
ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#endif
}
/* initialise the decoder */
partial_frame_count = 0;
last_rx_time = hrt_absolute_time();
debug("S.Bus: ready");
} else {
debug("S.Bus: open failed");
}
return sbus_fd;
}
void
sbus1_output(uint16_t *values, uint16_t num_values)
sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values)
{
uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
uint8_t offset = 0;
@@ -161,14 +154,15 @@ sbus1_output(uint16_t *values, uint16_t num_values)
}
}
void
sbus2_output(uint16_t *values, uint16_t num_values)
sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values)
{
char b = 'B';
write(sbus_fd, &b, 1);
}
bool
sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
@@ -180,8 +174,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
* frame transmission time is ~2ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 3ms passes between calls,
* If an interval of more than 4ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
@@ -190,7 +183,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) {
if ((now - last_rx_time) > 4000) {
if (partial_frame_count > 0) {
sbus_frame_drops++;
partial_frame_count = 0;
@@ -331,7 +324,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
}
/* decode switch channels if data fields are wide enough */
if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
if (max_values > 17 && chancount > 15) {
chancount = 18;
/* channel 17 (index 16) */
src/lib/rc/sbus.h
+74
View File
@@ -0,0 +1,74 @@
/****************************************************************************
*
* 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
* 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 sbus.h
*
* RC protocol definition for S.BUS
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
__BEGIN_DECLS
__EXPORT int sbus_init(const char *device, bool singlewire);
/**
* Parse serial bytes on the S.BUS bus
*
* The S.bus protocol doesn't provide reliable framing,
* so we detect frame boundaries by the inter-frame delay.
*
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
* frame transmission time is ~2ms.
*
* If an interval of more than 4ms (7 ms frame spacing plus margin)
* passes between calls, the first byte we read will be the first
* byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
bool *sbus_frame_drop,
uint16_t max_channels);
__EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values);
__EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values);
__END_DECLS
src/lib/rc/sumd.h
+2 -2
View File
@@ -101,8 +101,8 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value);
__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
uint16_t *channels, uint16_t max_chan_count);
*/
int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
uint16_t *channels, uint16_t max_chan_count);
__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
uint16_t *channels, uint16_t max_chan_count);
__END_DECLS
src/lib/version/version.h
+4
View File
@@ -51,6 +51,10 @@
#define HW_ARCH "PX4FMU_V2"
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
#define HW_ARCH "PX4FMU_V4"
#endif
#ifdef CONFIG_ARCH_BOARD_AEROCORE
#define HW_ARCH "AEROCORE"
#endif
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+26 -3
View File
@@ -59,6 +59,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -122,6 +123,7 @@ private:
int _params_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
int _airspeed_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
@@ -161,6 +163,8 @@ private:
att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
airspeed_s _airspeed = {};
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
@@ -202,6 +206,8 @@ private:
AttitudeEstimatorQ::AttitudeEstimatorQ() :
_vel_prev(0, 0, 0),
_pos_acc(0, 0, 0),
_voter_gyro(3),
_voter_accel(3),
_voter_mag(3),
@@ -258,7 +264,7 @@ int AttitudeEstimatorQ::start()
_control_task = px4_task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2100,
2400,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
@@ -299,6 +305,8 @@ void AttitudeEstimatorQ::task_main()
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -334,6 +342,10 @@ void AttitudeEstimatorQ::task_main()
// Update sensors
sensor_combined_s sensors;
int best_gyro = 0;
int best_accel = 0;
int best_mag = 0;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
// Feed validator with recent sensor data
@@ -370,8 +382,6 @@ void AttitudeEstimatorQ::task_main()
}
}
int best_gyro, best_accel, best_mag;
// Get best measurement values
hrt_abstime curr_time = hrt_absolute_time();
_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
@@ -488,6 +498,14 @@ void AttitudeEstimatorQ::task_main()
_mocap_hdg = Rmoc.transposed() * v;
}
// Update airspeed
bool airspeed_updated = false;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
// Check for timeouts on data
if (_ext_hdg_mode == 1) {
_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
@@ -601,6 +619,9 @@ void AttitudeEstimatorQ::task_main()
ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
/* Airspeed - take airspeed measurement directly here as no wind is estimated */
ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
@@ -723,6 +744,8 @@ bool AttitudeEstimatorQ::update(float dt)
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
}
_q.normalize();
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
src/modules/commander/commander.cpp
+2 -2
View File
@@ -1645,7 +1645,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle really is of type vtol*/
/* Make sure that this is only adjusted if vehicle really is of type vtol */
if (is_vtol(&status)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
status.in_transition_mode = vtol_status.vtol_in_trans_mode;
@@ -2365,7 +2365,7 @@ int commander_thread_main(int argc, char *argv[])
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+3 -1
View File
@@ -856,7 +856,9 @@ void AttitudePositionEstimatorEKF::publishControlState()
/* Accelerations in Body Frame */
_ctrl_state.x_acc = _ekf->accel.x;
_ctrl_state.y_acc = _ekf->accel.y;
_ctrl_state.z_acc = _ekf->accel.z;
_ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13];
_ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal();
/* Velocity in Body Frame */
Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
src/modules/ekf_att_pos_estimator/estimator_22states.h
+2
View File
@@ -382,6 +382,8 @@ public:
void get_covariance(float c[28]);
float getAccNavMagHorizontal() { return _accNavMagHorizontal; }
protected:
/**
src/modules/fw_att_control/fw_att_control_main.cpp
+11 -9
View File
@@ -59,6 +59,7 @@
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
@@ -142,6 +143,7 @@ private:
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
@@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_rates_sp_id(0),
_actuators_id(0),
_attitude_setpoint_id(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll()
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
@@ -1015,15 +1020,12 @@ FixedwingAttitudeControl::task_main()
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
}
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+18 -4
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -166,6 +167,8 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
@@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
_tecs_status_pub(nullptr),
_nav_capabilities_pub(nullptr),
/* publication ID */
_attitude_setpoint_id(0),
/* states */
_ctrl_state(),
_att_sp(),
@@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
}
@@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main()
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
/* XXX check if radius makes sense here */
src/modules/gpio_led/gpio_led.c
+9 -3
View File
@@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[])
"\t\tr2\tPX4IO RELAY2"
);
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
"\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
);
@@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[])
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
char *pin_name = "PX4FMU GPIO_EXT1";
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
char pin_name[] = "AUX OUT 1";
#endif
@@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[])
}
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
unsigned int n = strtoul(argv[3], NULL, 10);
if (n >= 1 && n <= 6) {
@@ -207,6 +207,8 @@ void gpio_led_start(FAR void *arg)
char *gpio_dev;
#if defined(PX4IO_DEVICE_PATH)
if (priv->use_io) {
gpio_dev = PX4IO_DEVICE_PATH;
@@ -214,6 +216,10 @@ void gpio_led_start(FAR void *arg)
gpio_dev = PX4FMU_DEVICE_PATH;
}
#else
gpio_dev = PX4FMU_DEVICE_PATH;
#endif
/* open GPIO device */
priv->gpio_fd = open(gpio_dev, 0);
src/modules/land_detector/CMakeLists.txt
+1
View File
@@ -41,6 +41,7 @@ px4_add_module(
LandDetector.cpp
MulticopterLandDetector.cpp
FixedwingLandDetector.cpp
VtolLandDetector.cpp
DEPENDS
platforms__common
)
src/modules/land_detector/FixedwingLandDetector.cpp
+32 -24
View File
@@ -36,6 +36,7 @@
* Land detection algorithm for fixedwings
*
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include "FixedwingLandDetector.h"
@@ -48,42 +49,44 @@
FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_paramHandle(),
_params(),
_vehicleLocalPositionSub(-1),
_vehicleLocalPosition( {}),
_airspeedSub(-1),
_vehicleStatusSub(-1),
_armingSub(-1),
_airspeed{},
_vehicleStatus{},
_arming{},
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_landDetectTrigger(0)
_controlStateSub(-1),
_vehicleStatusSub(-1),
_armingSub(-1),
_airspeedSub(-1),
_controlState{},
_vehicleStatus{},
_arming{},
_airspeed{},
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_accel_horz_lp(0.0f),
_landDetectTrigger(0)
{
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
_paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX");
}
void FixedwingLandDetector::initialize()
{
// Subscribe to local position and airspeed data
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
_controlStateSub = orb_subscribe(ORB_ID(control_state));
_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
updateParameterCache(true);
}
void FixedwingLandDetector::updateSubscriptions()
{
orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
}
bool FixedwingLandDetector::update()
@@ -99,29 +102,33 @@ bool FixedwingLandDetector::update()
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel *
_controlState.x_vel + _controlState.y_vel * _controlState.y_vel);
if (PX4_ISFINITE(val)) {
_velocity_xy_filtered = val;
}
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel);
if (PX4_ISFINITE(val)) {
_velocity_z_filtered = val;
}
}
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
// a leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses
_accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f;
}
// crude land detector for fixedwing
if (_velocity_xy_filtered < _params.maxVelocity
&& _velocity_z_filtered < _params.maxClimbRate
&& _airspeed_filtered < _params.maxAirSpeed) {
&& _airspeed_filtered < _params.maxAirSpeed
&& _accel_horz_lp < _params.maxIntVelocity) {
// these conditions need to be stable for a period of time before we trust them
if (now > _landDetectTrigger) {
@@ -151,5 +158,6 @@ void FixedwingLandDetector::updateParameterCache(const bool force)
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
}
}
src/modules/land_detector/FixedwingLandDetector.h
+9 -6
View File
@@ -42,11 +42,11 @@
#define __FIXED_WING_LAND_DETECTOR_H__
#include "LandDetector.h"
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/airspeed.h>
#include <systemlib/param/param.h>
class FixedwingLandDetector : public LandDetector
@@ -83,28 +83,31 @@ private:
param_t maxVelocity;
param_t maxClimbRate;
param_t maxAirSpeed;
param_t maxIntVelocity;
} _paramHandle;
struct {
float maxVelocity;
float maxClimbRate;
float maxAirSpeed;
float maxIntVelocity;
} _params;
private:
int _vehicleLocalPositionSub; /**< notification of local position */
struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */
int _airspeedSub;
int _controlStateSub; /**< notification of local position */
int _vehicleStatusSub;
int _armingSub;
struct airspeed_s _airspeed;
int _airspeedSub;
struct control_state_s _controlState; /**< the result from local position subscription */
struct vehicle_status_s _vehicleStatus;
struct actuator_armed_s _arming;
struct airspeed_s _airspeed;
int _parameterSub;
float _velocity_xy_filtered;
float _velocity_z_filtered;
float _airspeed_filtered;
float _accel_horz_lp;
uint64_t _landDetectTrigger;
};
src/modules/land_detector/LandDetector.cpp
+4 -3
View File
@@ -57,14 +57,14 @@ _work{} {
LandDetector::~LandDetector()
{
work_cancel(LPWORK, &_work);
work_cancel(HPWORK, &_work);
_taskShouldExit = true;
}
int LandDetector::start()
{
/* schedule a cycle to start things */
work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
return 0;
}
@@ -107,10 +107,11 @@ void LandDetector::cycle()
// publish the land detected broadcast
orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
//warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF");
}
if (!_taskShouldExit) {
work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
}
}
src/modules/land_detector/MulticopterLandDetector.cpp
+7
View File
@@ -95,6 +95,13 @@ bool MulticopterLandDetector::update()
// first poll for new data from our subscriptions
updateSubscriptions();
updateParameterCache(false);
return get_landed_state();
}
bool MulticopterLandDetector::get_landed_state()
{
// only trigger flight conditions if we are armed
if (!_arming.armed) {
_arming_time = 0;
src/modules/land_detector/MulticopterLandDetector.h
+11 -6
View File
@@ -60,24 +60,29 @@ protected:
/**
* @brief polls all subscriptions and pulls any data that has changed
**/
void updateSubscriptions();
virtual void updateSubscriptions();
/**
* @brief Runs one iteration of the land detection algorithm
**/
bool update() override;
virtual bool update() override;
/**
* @brief Initializes the land detection algorithm
**/
void initialize() override;
virtual void initialize() override;
private:
/**
* @brief download and update local parameter cache
**/
void updateParameterCache(const bool force);
virtual void updateParameterCache(const bool force);
/**
* @brief get multicopter landed state
**/
bool get_landed_state();
private:
/**
* @brief Handles for interesting parameters
src/modules/land_detector/VtolLandDetector.cpp
+116
View File
@@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 2013-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
* 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 VtolLandDetector.cpp
* Land detection algorithm for vtol
*
* @author Roman Bapst <bapstroma@gmail.com>
*/
#include "VtolLandDetector.h"
#include <drivers/drv_hrt.h>
VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
_paramHandle(),
_params(),
_airspeedSub(-1),
_parameterSub(-1),
_airspeed{},
_was_in_air(false),
_airspeed_filtered(0)
{
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
}
void VtolLandDetector::initialize()
{
MulticopterLandDetector::initialize();
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
// download parameters
updateParameterCache(true);
}
void VtolLandDetector::updateSubscriptions()
{
MulticopterLandDetector::updateSubscriptions();
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
}
bool VtolLandDetector::update()
{
updateSubscriptions();
updateParameterCache(false);
// this is returned from the mutlicopter land detector
bool landed = get_landed_state();
// for vtol we additionally consider airspeed
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
} else {
// if airspeed does not update, set it to zero and rely on multicopter land detector
_airspeed_filtered = 0.0f;
}
// only consider airspeed if we have been in air before to avoid false
// detections in the case of wind on the ground
if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) {
landed = false;
}
_was_in_air = !landed;
return landed;
}
void VtolLandDetector::updateParameterCache(const bool force)
{
MulticopterLandDetector::updateParameterCache(force);
bool updated;
parameter_update_s paramUpdate;
orb_check(_parameterSub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
}
if (updated || force) {
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
}
}
src/modules/land_detector/VtolLandDetector.h
+94
View File
@@ -0,0 +1,94 @@
/****************************************************************************
*
* Copyright (c) 2013-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
* 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 VtolLandDetector.h
* Land detection algorithm for vtol
*
* @author Roman Bapst <bapstr@gmail.com>
*/
#ifndef __VTOL_LAND_DETECTOR_H__
#define __VTOL_LAND_DETECTOR_H__
#include "MulticopterLandDetector.h"
#include <uORB/topics/airspeed.h>
class VtolLandDetector : public MulticopterLandDetector
{
public:
VtolLandDetector();
private:
/**
* @brief polls all subscriptions and pulls any data that has changed
**/
void updateSubscriptions() override;
/**
* @brief Runs one iteration of the land detection algorithm
**/
bool update() override;
/**
* @brief Initializes the land detection algorithm
**/
void initialize() override;
/**
* @brief download and update local parameter cache
**/
void updateParameterCache(const bool force) override;
/**
* @brief Handles for interesting parameters
**/
struct {
param_t maxAirSpeed;
} _paramHandle;
struct {
float maxAirSpeed;
} _params;
int _airspeedSub;
int _parameterSub;
struct airspeed_s _airspeed;
bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */
float _airspeed_filtered; /**< low pass filtered airspeed */
};
#endif
src/modules/land_detector/land_detector_main.cpp
+4
View File
@@ -53,6 +53,7 @@
#include "FixedwingLandDetector.h"
#include "MulticopterLandDetector.h"
#include "VtolLandDetector.h"
//Function prototypes
static int land_detector_start(const char *mode);
@@ -113,6 +114,9 @@ static int land_detector_start(const char *mode)
} else if (!strcmp(mode, "multicopter")) {
land_detector_task = new MulticopterLandDetector();
} else if (!strcmp(mode, "vtol")) {
land_detector_task = new VtolLandDetector();
} else {
warnx("[mode] must be either 'fixedwing' or 'multicopter'");
return -1;
src/modules/land_detector/land_detector_params.c
+13
View File
@@ -109,6 +109,19 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f);
*/
PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f);
/**
* Fixedwing max short-term velocity
*
* Maximum velocity integral in flight direction allowed in the landed state (m/s)
*
* @min 2
* @max 10
* @unit m/s
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f);
/**
* Airspeed max
*
src/modules/mavlink/mavlink_main.cpp
+1 -1
View File
@@ -1750,7 +1750,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VFR_HUD", 20.0f);
configure_stream("ATTITUDE", 100.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("RC_CHANNELS", 5.0f);
configure_stream("RC_CHANNELS", 10.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
src/modules/mavlink/mavlink_receiver.cpp
+1 -2
View File
@@ -1324,8 +1324,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
struct airspeed_s airspeed = {};
float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
// XXX need to fix this
float tas = ias;
float tas = calc_true_airspeed_from_indicated(ias, imu.abs_pressure * 100, imu.temperature);
airspeed.timestamp = timestamp;
airspeed.indicated_airspeed_m_s = ias;

Some files were not shown because too many files have changed in this diff Show More