diff options
Diffstat (limited to 'src/drivers')
53 files changed, 4016 insertions, 495 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 524151c90..41942aacd 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -82,10 +82,12 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), _sensor_ok(false), + _last_published_sensor_ok(true), /* initialize differently to force publication */ _measure_ticks(0), _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), + _subsys_pub(-1), _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), @@ -149,8 +151,7 @@ Airspeed::init() } ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + out: return ret; } @@ -344,22 +345,6 @@ Airspeed::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_DIFFPRESSURE - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } } void @@ -369,11 +354,34 @@ Airspeed::stop() } void +Airspeed::update_status() +{ + if (_sensor_ok != _last_published_sensor_ok) { + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + _sensor_ok, + SUBSYSTEM_TYPE_DIFFPRESSURE + }; + + if (_subsys_pub > 0) { + orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { + _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); + } + + _last_published_sensor_ok = _sensor_ok; + } +} + +void Airspeed::cycle_trampoline(void *arg) { Airspeed *dev = (Airspeed *)arg; dev->cycle(); + dev->update_status(); } void diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 186602eda..0b8e949c9 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -118,14 +118,21 @@ protected: virtual int measure() = 0; virtual int collect() = 0; + /** + * Update the subsystem status + */ + void update_status(); + work_s _work; float _max_differential_pressure_pa; bool _sensor_ok; + bool _last_published_sensor_ok; int _measure_ticks; bool _collect_phase; float _diff_pres_offset; orb_advert_t _airspeed_pub; + orb_advert_t _subsys_pub; int _class_instance; diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 2361f4dd1..5c502f682 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -48,11 +48,14 @@ * The recognized number off cells, will be blinked 5 times in purple color. * 2 Cells = 2 blinks * ... - * 5 Cells = 5 blinks + * 6 Cells = 6 blinks * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts. * - * System disarmed: - * The BlinkM should lit solid red. + * System disarmed and safe: + * The BlinkM should light solid cyan. + * + * System safety off but not armed: + * The BlinkM should light flashing orange * * System armed: * One message is made of 4 Blinks and a pause in the same length as the 4 blinks. @@ -67,10 +70,10 @@ * (X = on, _=off) * * The first 3 blinks indicates the status of the GPS-Signal (red): - * 0-4 satellites = X-X-X-X-_-_-_-_-_-_- - * 5 satellites = X-X-_-X-_-_-_-_-_-_- - * 6 satellites = X-_-_-X-_-_-_-_-_-_- - * >=7 satellites = _-_-_-X-_-_-_-_-_-_- + * 0-4 satellites = X-X-X-X-X-_-_-_-_-_- + * 5 satellites = X-X-_-X-X-_-_-_-_-_- + * 6 satellites = X-_-_-X-X-_-_-_-_-_- + * >=7 satellites = _-_-_-X-X-_-_-_-_-_- * If no GPS is found the first 3 blinks are white * * The fourth Blink indicates the Flightmode: @@ -119,6 +122,7 @@ #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/safety.h> static const float MAX_CELL_VOLTAGE = 4.3f; static const int LED_ONTIME = 120; @@ -166,10 +170,12 @@ private: enum ledColors { LED_OFF, LED_RED, + LED_ORANGE, LED_YELLOW, LED_PURPLE, LED_GREEN, LED_BLUE, + LED_CYAN, LED_WHITE, LED_AMBER }; @@ -188,6 +194,26 @@ private: bool systemstate_run; + int vehicle_status_sub_fd; + int vehicle_control_mode_sub_fd; + int vehicle_gps_position_sub_fd; + int actuator_armed_sub_fd; + int safety_sub_fd; + + int num_of_cells; + int detected_cells_runcount; + + int t_led_color[8]; + int t_led_blink; + int led_thread_runcount; + int led_interval; + + bool topic_initialized; + bool detected_cells_blinked; + bool led_thread_ready; + + int num_of_used_sats; + void setLEDColor(int ledcolor); static void led_trampoline(void *arg); void led(); @@ -259,7 +285,22 @@ BlinkM::BlinkM(int bus, int blinkm) : led_color_7(LED_OFF), led_color_8(LED_OFF), led_blink(LED_NOBLINK), - systemstate_run(false) + systemstate_run(false), + vehicle_status_sub_fd(-1), + vehicle_control_mode_sub_fd(-1), + vehicle_gps_position_sub_fd(-1), + actuator_armed_sub_fd(-1), + safety_sub_fd(-1), + num_of_cells(0), + detected_cells_runcount(0), + t_led_color({0}), + t_led_blink(0), + led_thread_runcount(0), + led_interval(1000), + topic_initialized(false), + detected_cells_blinked(false), + led_thread_ready(true), + num_of_used_sats(0) { memset(&_work, 0, sizeof(_work)); } @@ -376,42 +417,22 @@ void BlinkM::led() { - static int vehicle_status_sub_fd; - static int vehicle_control_mode_sub_fd; - static int vehicle_gps_position_sub_fd; - static int actuator_armed_sub_fd; - - static int num_of_cells = 0; - static int detected_cells_runcount = 0; - - static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0}; - static int t_led_blink = 0; - static int led_thread_runcount=0; - static int led_interval = 1000; - - static int no_data_vehicle_status = 0; - static int no_data_vehicle_control_mode = 0; - static int no_data_actuator_armed = 0; - static int no_data_vehicle_gps_position = 0; - - static bool topic_initialized = false; - static bool detected_cells_blinked = false; - static bool led_thread_ready = true; - - int num_of_used_sats = 0; - if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(vehicle_status_sub_fd, 1000); + orb_set_interval(vehicle_status_sub_fd, 250); vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(vehicle_control_mode_sub_fd, 1000); + orb_set_interval(vehicle_control_mode_sub_fd, 250); actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(actuator_armed_sub_fd, 1000); + orb_set_interval(actuator_armed_sub_fd, 250); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(vehicle_gps_position_sub_fd, 1000); + orb_set_interval(vehicle_gps_position_sub_fd, 250); + + /* Subscribe to safety topic */ + safety_sub_fd = orb_subscribe(ORB_ID(safety)); + orb_set_interval(safety_sub_fd, 250); topic_initialized = true; } @@ -433,7 +454,9 @@ BlinkM::led() if(num_of_cells > 4) { t_led_color[4] = LED_PURPLE; } - t_led_color[5] = LED_OFF; + if(num_of_cells > 5) { + t_led_color[5] = LED_PURPLE; + } t_led_color[6] = LED_OFF; t_led_color[7] = LED_OFF; t_led_blink = LED_BLINK; @@ -467,17 +490,25 @@ BlinkM::led() struct vehicle_control_mode_s vehicle_control_mode; struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; + struct safety_s safety; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); + memset(&safety, 0, sizeof(safety)); bool new_data_vehicle_status; bool new_data_vehicle_control_mode; bool new_data_actuator_armed; bool new_data_vehicle_gps_position; + bool new_data_safety; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); + int no_data_vehicle_status = 0; + int no_data_vehicle_control_mode = 0; + int no_data_actuator_armed = 0; + int no_data_vehicle_gps_position = 0; + if (new_data_vehicle_status) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); no_data_vehicle_status = 0; @@ -520,7 +551,12 @@ BlinkM::led() no_data_vehicle_gps_position = 3; } + /* update safety topic */ + orb_check(safety_sub_fd, &new_data_safety); + if (new_data_safety) { + orb_copy(ORB_ID(safety), safety_sub_fd, &safety); + } /* get number of used satellites in navigation */ num_of_used_sats = 0; @@ -541,19 +577,7 @@ BlinkM::led() printf("<blinkm> cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* LED Pattern for battery low warning */ - led_color_1 = LED_YELLOW; - led_color_2 = LED_YELLOW; - led_color_3 = LED_YELLOW; - led_color_4 = LED_YELLOW; - led_color_5 = LED_YELLOW; - led_color_6 = LED_YELLOW; - led_color_7 = LED_YELLOW; - led_color_8 = LED_YELLOW; - led_blink = LED_BLINK; - - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -565,21 +589,56 @@ BlinkM::led() led_color_8 = LED_RED; led_blink = LED_BLINK; + } else if(vehicle_status_raw.rc_signal_lost) { + /* LED Pattern for FAILSAFE */ + led_color_1 = LED_BLUE; + led_color_2 = LED_BLUE; + led_color_3 = LED_BLUE; + led_color_4 = LED_BLUE; + led_color_5 = LED_BLUE; + led_color_6 = LED_BLUE; + led_color_7 = LED_BLUE; + led_color_8 = LED_BLUE; + led_blink = LED_BLINK; + + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + /* LED Pattern for battery low warning */ + led_color_1 = LED_YELLOW; + led_color_2 = LED_YELLOW; + led_color_3 = LED_YELLOW; + led_color_4 = LED_YELLOW; + led_color_5 = LED_YELLOW; + led_color_6 = LED_YELLOW; + led_color_7 = LED_YELLOW; + led_color_8 = LED_YELLOW; + led_blink = LED_BLINK; + } else { /* no battery warnings here */ if(actuator_armed.armed == false) { /* system not armed */ - led_color_1 = LED_RED; - led_color_2 = LED_RED; - led_color_3 = LED_RED; - led_color_4 = LED_RED; - led_color_5 = LED_RED; - led_color_6 = LED_RED; - led_color_7 = LED_RED; - led_color_8 = LED_RED; - led_blink = LED_NOBLINK; - + if(safety.safety_off){ + led_color_1 = LED_ORANGE; + led_color_2 = LED_ORANGE; + led_color_3 = LED_ORANGE; + led_color_4 = LED_ORANGE; + led_color_5 = LED_ORANGE; + led_color_6 = LED_ORANGE; + led_color_7 = LED_ORANGE; + led_color_8 = LED_ORANGE; + led_blink = LED_BLINK; + }else{ + led_color_1 = LED_CYAN; + led_color_2 = LED_CYAN; + led_color_3 = LED_CYAN; + led_color_4 = LED_CYAN; + led_color_5 = LED_CYAN; + led_color_6 = LED_CYAN; + led_color_7 = LED_CYAN; + led_color_8 = LED_CYAN; + led_blink = LED_NOBLINK; + } } else { /* armed system - initial led pattern */ led_color_1 = LED_RED; @@ -593,23 +652,22 @@ BlinkM::led() led_blink = LED_BLINK; if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { - - //XXX please check - if (vehicle_control_mode.flag_control_position_enabled) + /* indicate main control state */ + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; - else if (vehicle_control_mode.flag_control_velocity_enabled) + else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_control_mode.flag_control_attitude_enabled) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; - else if (vehicle_control_mode.flag_control_manual_enabled) - led_color_4 = LED_AMBER; + else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) + led_color_4 = LED_WHITE; else led_color_4 = LED_OFF; - + led_color_5 = led_color_4; } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat�s */ + /* handling used satus */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; @@ -690,8 +748,11 @@ void BlinkM::setLEDColor(int ledcolor) { case LED_RED: // red set_rgb(255,0,0); break; + case LED_ORANGE: // orange + set_rgb(255,150,0); + break; case LED_YELLOW: // yellow - set_rgb(255,70,0); + set_rgb(200,200,0); break; case LED_PURPLE: // purple set_rgb(255,0,255); @@ -702,11 +763,14 @@ void BlinkM::setLEDColor(int ledcolor) { case LED_BLUE: // blue set_rgb(0,0,255); break; + case LED_CYAN: // cyan + set_rgb(0,128,128); + break; case LED_WHITE: // white set_rgb(255,255,255); break; case LED_AMBER: // amber - set_rgb(255,20,0); + set_rgb(255,65,0); break; } } diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c new file mode 100644 index 000000000..4e3ba2d7e --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * 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 aerocore_init.c + * + * AeroCore-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 <nuttx/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/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 + +/**************************************************************************** + * 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 *spi3; +static struct spi_dev_s *spi4; + +#include <math.h> + +#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) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + stm32_configgpio(GPIO_ADC1_IN11); /* J1 breakout */ + stm32_configgpio(GPIO_ADC1_IN12); /* J1 breakout */ + stm32_configgpio(GPIO_ADC1_IN13); /* J1 breakout */ + + /* 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_AMBER); + + /* Configure Sensors on SPI bus #3 */ + spi3 = up_spiinitialize(3); + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* Default: 1MHz, 8 bits, Mode 3 */ + SPI_SETFREQUENCY(spi3, 10000000); + SPI_SETBITS(spi3, 8); + SPI_SETMODE(spi3, SPIDEV_MODE3); + SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi3, PX4_SPIDEV_BARO, false); + up_udelay(20); + message("[boot] Initialized SPI port 3 (SENSORS)\n"); + + /* Configure FRAM on SPI bus #4 */ + spi4 = up_spiinitialize(4); + if (!spi4) { + message("[boot] FAILED to initialize SPI port 4\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* Default: ~10MHz, 8 bits, Mode 3 */ + SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE0); + SPI_SELECT(spi4, SPIDEV_FLASH, false); + message("[boot] Initialized SPI port 4 (FRAM)\n"); + + return OK; +} diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c new file mode 100644 index 000000000..e40d1730c --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 aerocore_led.c + * + * AeroCore LED backend. + */ + +#include <nuttx/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(); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +__EXPORT void led_init() +{ + stm32_configgpio(GPIO_LED0); + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + switch (led) { + case 0: + stm32_gpiowrite(GPIO_LED0, true); + break; + + case 1: + stm32_gpiowrite(GPIO_LED1, true); + break; + + default: + warnx("LED ID not recognized\n"); + } +} + +__EXPORT void led_off(int led) +{ + switch (led) { + case 0: + stm32_gpiowrite(GPIO_LED0, false); + break; + + case 1: + stm32_gpiowrite(GPIO_LED1, false); + break; + + default: + warnx("LED ID not recognized\n"); + } +} + +__EXPORT void led_toggle(int led) +{ + switch (led) { + case 0: + if (stm32_gpioread(GPIO_LED0)) + stm32_gpiowrite(GPIO_LED0, false); + else + stm32_gpiowrite(GPIO_LED0, true); + break; + + case 1: + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + break; + + default: + warnx("LED ID not recognized\n"); + } +} diff --git a/src/drivers/boards/aerocore/aerocore_pwm_servo.c b/src/drivers/boards/aerocore/aerocore_pwm_servo.c new file mode 100644 index 000000000..251eaff7b --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_pwm_servo.c @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 aerocore_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_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1500, + } +}; diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c new file mode 100644 index 000000000..e329bd9d1 --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * 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 aerocore_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/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 weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI1_NSS); + stm32_gpiowrite(GPIO_SPI1_NSS, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI2_NSS); + stm32_gpiowrite(GPIO_SPI2_NSS, 1); +#endif + +#ifdef CONFIG_STM32_SPI3 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); +#endif + +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI4_NSS); + stm32_gpiowrite(GPIO_SPI4_NSS, 1); +#endif +} + +#ifdef CONFIG_STM32_SPI1 +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there is only one device broken-out so select it */ + stm32_gpiowrite(GPIO_SPI1_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there is only one device broken-out so select it */ + stm32_gpiowrite(GPIO_SPI2_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI3 +__EXPORT void stm32_spi3select(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_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + + +#ifdef CONFIG_STM32_SPI4 +__EXPORT void stm32_spi4select(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_SPI4_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h new file mode 100644 index 000000000..70142a314 --- /dev/null +++ b/src/drivers/boards/aerocore/board_config.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * 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 + * + * AeroCore internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include <nuttx/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 + ****************************************************************************************************/ + +/* LEDs */ +#define GPIO_LED0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10) + +/* Gyro */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0) +#define SENSOR_BOARD_ROTATION_DEFAULT 3 /* SENSOR_BOARD_ROTATION_270_DEG */ + +/* Accel & Mag */ +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2) + +/* GPS */ +#define GPIO_GPS_NRESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPS_TIMEPULSE (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4) +#define GPS_DEFAULT_UART_PORT "/dev/ttyS0" + +/* SPI3--Sensors */ +#define PX4_SPI_BUS_SENSORS 3 +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +/* Nominal chip selects for devices on SPI bus #3 */ +#define PX4_SPIDEV_ACCEL_MAG 0 +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_BARO 2 + +/* User GPIOs broken out on J11 */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO8_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO9_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO10_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_GPIO11_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO8_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO9_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) +#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) + +/* PWM + * + * Eight PWM outputs are configured. + * + * Pins: + * + * CH1 : PA8 : TIM1_CH1 + * CH2 : PA9 : TIM1_CH2 + * CH3 : PA10 : TIM1_CH3 + * CH4 : PA11 : TIM1_CH4 + * CH5 : PC6 : TIM3_CH1 + * CH6 : PC7 : TIM3_CH2 + * CH7 : PC8 : TIM3_CH3 + * CH8 : PC9 : TIM3_CH4 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_1 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_1 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_1 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_1 +#define GPIO_TIM3_CH1OUT GPIO_TIM3_CH1OUT_3 +#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3 +#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2 +#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2 + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer 8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/* Tone Alarm (no onboard speaker )*/ +#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_PIN0) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0) + + +/**************************************************************************************************** + * 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); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk new file mode 100644 index 000000000..b53fe0a29 --- /dev/null +++ b/src/drivers/boards/aerocore/module.mk @@ -0,0 +1,8 @@ +# +# Board-specific startup code for the AeroCore +# + +SRCS = aerocore_init.c \ + aerocore_pwm_servo.c \ + aerocore_spi.c \ + aerocore_led.c diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 02c26b5c0..58273f2d2 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -85,6 +85,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) +#define PX4_SPI_BUS_SENSORS 1 + /* * Use these in place of the spi_dev_e enumeration to * select a specific SPI device on SPI1 diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 7cfca7656..c2de1bfba 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -107,6 +107,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define PX4_SPI_BUS_SENSORS 1 + /* 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 diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index b157b3f18..6e2ebb1f7 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname) if (ret == OK) break; } else { char name[32]; - snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, &fops, 0666, (void *)this); if (ret == OK) break; } diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index f60964c2b..5acd0d343 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,6 +94,14 @@ #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +/* + * AeroCore GPIO numbers and configuration. + * + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +#endif + #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 /* no GPIO driver on the PX4IOv1 board */ #endif @@ -146,4 +154,4 @@ #define GPIO_SENSOR_RAIL_RESET GPIOC(13) -#endif /* _DRV_GPIO_H */
\ No newline at end of file +#endif /* _DRV_GPIO_H */ diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 06e3535b3..e14f4e00d 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -43,10 +43,14 @@ #include <stdint.h> #include <sys/ioctl.h> +#include "board_config.h" + #include "drv_sensor.h" #include "drv_orb_dev.h" +#ifndef GPS_DEFAULT_UART_PORT #define GPS_DEFAULT_UART_PORT "/dev/ttyS3" +#endif #define GPS_DEVICE_PATH "/dev/gps" diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index d130d68b3..8bfc90c64 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -41,6 +41,7 @@ #include <sys/types.h> #include <stdbool.h> +#include <inttypes.h> #include <time.h> #include <queue.h> diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 7a93e513e..972573f9f 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -199,6 +199,9 @@ ORB_DECLARE(output_pwm); /** get the lockdown override flag to enable outputs in HIL */ #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) +/** force safety switch off (to disable use of safety switch) */ +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) + /* * * diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h new file mode 100644 index 000000000..bef02d54e --- /dev/null +++ b/src/drivers/drv_px4flow.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 Rangefinder driver interface. + */ + +#ifndef _DRV_PX4FLOW_H +#define _DRV_PX4FLOW_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define PX4FLOW_DEVICE_PATH "/dev/px4flow" + +/** + * Optical flow in NED body frame in SI units. + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct px4flow_report { + + uint64_t timestamp; /**< in microseconds since system start */ + + int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ + int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ + float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ + float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ + float ground_distance_m; /**< Altitude / distance to ground in meters */ + uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + +}; + +/* + * ObjDev tag for px4flow data. + */ +ORB_DECLARE(optical_flow); + +/* + * ioctl() definitions + * + * px4flow drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _PX4FLOWIOCBASE (0x7700) +#define __PX4FLOWIOC(_n) (_IOC(_PX4FLOWIOCBASE, _n)) + + +#endif /* _DRV_PX4FLOW_H */ diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index cf91f7030..e45939b37 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,6 +46,10 @@ #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" +enum RANGE_FINDER_TYPE { + RANGE_FINDER_TYPE_LASER = 0, +}; + /** * range finder report structure. Reads from the device must be in multiples of this * structure. @@ -53,8 +57,11 @@ struct range_finder_report { uint64_t timestamp; uint64_t error_count; - float distance; /** in meters */ - uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ + unsigned type; /**< type, following RANGE_FINDER_TYPE enum */ + float distance; /**< in meters */ + float minimum_distance; /**< minimum distance the sensor can measure */ + float maximum_distance; /**< maximum distance the sensor can measure */ + uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ }; /* diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index cb5fd53a5..b7981e9c4 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -147,6 +147,8 @@ enum { TONE_BATTERY_WARNING_SLOW_TUNE, TONE_BATTERY_WARNING_FAST_TUNE, TONE_GPS_WARNING_TUNE, + TONE_ARMING_FAILURE_TUNE, + TONE_PARACHUTE_RELEASE_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b6e80ce1d..146a06e7c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -132,7 +132,6 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); } return ret; @@ -177,11 +176,14 @@ ETSAirspeed::collect() _max_differential_pressure_pa = diff_pres_pa; } - // XXX we may want to smooth out the readings to remove noise. differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.differential_pressure_pa = (float)diff_pres_pa; + + // XXX we may want to smooth out the readings to remove noise. + report.differential_pressure_filtered_pa = (float)diff_pres_pa; + report.temperature = -1000.0f; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -205,14 +207,18 @@ ETSAirspeed::collect() void ETSAirspeed::cycle() { + int ret; + /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + ret = collect(); + if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ start(); + _sensor_ok = false; return; } @@ -236,8 +242,12 @@ ETSAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) - log("measure error"); + ret = measure(); + if (OK != ret) { + debug("measure error"); + } + + _sensor_ok = (ret == OK); /* next phase is collection */ _collect_phase = true; @@ -308,7 +318,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no ETS airspeed sensor connected"); } /** diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index cfcf91e3f..18de3f4da 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -53,6 +53,8 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_status.h> +#include <drivers/drv_hrt.h> + /* FrSky sensor hub data IDs */ #define FRSKY_ID_GPS_ALT_BP 0x01 #define FRSKY_ID_TEMP1 0x02 @@ -225,7 +227,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.global_valid) { + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 7b08ca69e..6e0839043 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -222,7 +222,7 @@ int frsky_telemetry_main(int argc, char *argv[]) frsky_task = task_spawn_cmd("frsky_telemetry", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 2000, frsky_telemetry_thread_main, (const char **)argv); diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk index 1632c74f7..9a49589ee 100644 --- a/src/drivers/frsky_telemetry/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = frsky_telemetry SRCS = frsky_data.c \ frsky_telemetry.c + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a736cbdf6..6195cd6ea 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -56,6 +56,7 @@ #include <arch/board/board.h> #include <drivers/drv_hrt.h> #include <drivers/device/i2c.h> +#include <systemlib/systemlib.h> #include <systemlib/perf_counter.h> #include <systemlib/scheduling_priorities.h> #include <systemlib/err.h> @@ -63,6 +64,8 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_gps_position.h> +#include <board_config.h> + #include "ubx.h" #include "mtk.h" @@ -76,12 +79,6 @@ #endif static const int ERROR = -1; -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - - - class GPS : public device::CDev { public: @@ -209,7 +206,8 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_spawn_cmd("gps", SCHED_DEFAULT, + SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -232,6 +230,11 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCRESET: cmd_reset(); break; + + default: + /* give it to parent if no one wants it */ + ret = CDev::ioctl(filp, cmd, arg); + break; } unlock(); @@ -271,14 +274,14 @@ GPS::task_main() _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; + _report.alt = (int32_t)1200e3f; _report.timestamp_variance = hrt_absolute_time(); _report.s_variance_m_s = 10.0f; _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; + _report.eph_m = 0.9f; + _report.epv_m = 1.8f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; @@ -416,7 +419,14 @@ GPS::task_main() void GPS::cmd_reset() { - //XXX add reset? +#ifdef GPIO_GPS_NRESET + warnx("Toggling GPS reset pin"); + stm32_configgpio(GPIO_GPS_NRESET); + stm32_gpiowrite(GPIO_GPS_NRESET, 0); + usleep(100); + stm32_gpiowrite(GPIO_GPS_NRESET, 1); + warnx("Toggled GPS reset pin"); +#endif } void diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index 82c67d40a..eb382c4b2 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -41,3 +41,5 @@ SRCS = gps.cpp \ gps_helper.cpp \ mtk.cpp \ ubx.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index c90ecbe28..680f00d97 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -249,12 +249,18 @@ MTK::handle_message(gps_mtk_packet_t &packet) warnx("mtk: unknown revision"); _gps_position->lat = 0; _gps_position->lon = 0; + + // Indicate this data is not usable and bail out + _gps_position->eph_m = 1000.0f; + _gps_position->epv_m = 1000.0f; + _gps_position->fix_type = 0; + return; } _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; - _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit - _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->eph_m = packet.hdop / 100.0f; // from cm to m + _gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_visible = packet.satellites; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 0a047f38f..55cc077fb 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -122,7 +122,7 @@ private: actuator_controls_s _controls; static void task_main_trampoline(int argc, char *argv[]); - void task_main() __attribute__((noreturn)); + void task_main(); static int control_callback(uintptr_t handle, uint8_t control_group, diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4c85c0cda..fddba806e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -158,6 +158,7 @@ private: int _class_instance; orb_advert_t _mag_topic; + orb_advert_t _subsystem_pub; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -169,6 +170,8 @@ private: int _bus; /**< the bus the device is connected to */ + struct mag_report _last_report; /**< used for info() */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -322,7 +325,9 @@ HMC5883::HMC5883(int bus) : _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), + _collect_phase(false), _mag_topic(-1), + _subsystem_pub(-1), _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), @@ -713,7 +718,7 @@ HMC5883::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + debug("collection error"); /* restart the measurement state machine */ start(); return; @@ -740,7 +745,7 @@ HMC5883::cycle() /* measurement phase */ if (OK != measure()) - log("measure error"); + debug("measure error"); /* next phase is collection */ _collect_phase = true; @@ -870,6 +875,8 @@ HMC5883::collect() } } + _last_report = new_report; + /* post a report to the ring */ if (_reports->force(&new_report)) { perf_count(_buffer_overflows); @@ -1042,31 +1049,28 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - /* set back to normal mode */ - /* Set to 1.1 Gauss */ - if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { - warnx("failed to set 1.1 Ga range"); - goto out; - } - - if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("failed to disable sensor calibration mode"); - goto out; - } - /* set scaling in device */ mscale_previous.x_scale = scaling[0]; mscale_previous.y_scale = scaling[1]; mscale_previous.z_scale = scaling[2]; + ret = OK; + +out: + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { warn("WARNING: failed to set new scale / offsets for mag"); - goto out; } - ret = OK; + /* set back to normal mode */ + /* Set to 1.1 Gauss */ + if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { + warnx("failed to set 1.1 Ga range"); + } -out: + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { + warnx("failed to disable sensor calibration mode"); + } if (ret == OK) { if (!check_scale()) { @@ -1136,13 +1140,12 @@ int HMC5883::check_calibration() true, _calibrated, SUBSYSTEM_TYPE_MAG}; - static orb_advert_t pub = -1; if (!(_pub_blocked)) { - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); + if (_subsystem_pub > 0) { + orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); + _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); } } } @@ -1221,6 +1224,7 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 90a744015..1e779e8dc 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -51,6 +51,8 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_gps_position.h> +#include <drivers/drv_hrt.h> + /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 @@ -62,7 +64,6 @@ static int _airspeed_sub = -1; static int _esc_sub = -1; static orb_advert_t _esc_pub; -struct esc_status_s _esc; static bool _home_position_set = false; static double _home_lat = 0.0d; @@ -82,8 +83,6 @@ init_sub_messages(void) void init_pub_messages(void) { - memset(&_esc, 0, sizeof(_esc)); - _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); } void @@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer) size_t size = sizeof(msg); memset(&msg, 0, size); memcpy(&msg, buffer, size); + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + + // Publish it. + esc.timestamp = hrt_absolute_time(); + esc.esc_count = 1; + esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; + + esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; + esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + esc.esc[0].esc_temperature = msg.temperature1 - 20; + esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); + esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); /* announce the esc if needed, just publish else */ if (_esc_pub > 0) { - orb_publish(ORB_ID(esc_status), _esc_pub, &_esc); + orb_publish(ORB_ID(esc_status), _esc_pub, &esc); } else { - _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); + _esc_pub = orb_advertise(ORB_ID(esc_status), &esc); } - - // Publish it. - _esc.esc_count = 1; - _esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; - - _esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; - _esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; - _esc.esc[0].esc_temperature = msg.temperature1 - 20; - _esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); - _esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); } void diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 90c3db9ae..37e72388b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -34,6 +34,9 @@ /** * @file l3gd20.cpp * Driver for the ST L3GD20 MEMS gyro connected via SPI. + * + * Note: With the exception of the self-test feature, the ST L3G4200D is + * also supported by this driver. */ #include <nuttx/config.h> @@ -89,9 +92,11 @@ static const int ERROR = -1; #define ADDR_WHO_AM_I 0x0F #define WHO_I_AM_H 0xD7 #define WHO_I_AM 0xD4 +#define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */ #define ADDR_CTRL_REG1 0x20 #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ + /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) @@ -166,9 +171,14 @@ static const int ERROR = -1; #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) #define L3GD20_DEFAULT_RATE 760 +#define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 +#ifndef SENSOR_BOARD_ROTATION_DEFAULT +#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG +#endif + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI @@ -216,6 +226,9 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + /* true if an L3G4200D is detected */ + bool _is_l3g4200d; + /** * Start automatic measurement. */ @@ -324,14 +337,15 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _class_instance(-1), _current_rate(0), - _orientation(SENSOR_BOARD_ROTATION_270_DEG), + _orientation(SENSOR_BOARD_ROTATION_DEFAULT), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), - _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) + _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _is_l3g4200d(false) { // enable debug() calls _debug_enabled = true; @@ -413,14 +427,7 @@ L3GD20::probe() /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - _orientation = SENSOR_BOARD_ROTATION_270_DEG; - #elif CONFIG_ARCH_BOARD_PX4FMU_V2 - _orientation = SENSOR_BOARD_ROTATION_270_DEG; - #else - #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 - #endif - + _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; } @@ -430,6 +437,13 @@ L3GD20::probe() success = true; } + /* Detect the L3G4200D used on AeroCore */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) { + _is_l3g4200d = true; + _orientation = SENSOR_BOARD_ROTATION_DEFAULT; + success = true; + } + if (success) return OK; @@ -502,6 +516,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: + if (_is_l3g4200d) { + return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE); + } return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ @@ -683,23 +700,26 @@ L3GD20::set_samplerate(unsigned frequency) uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; if (frequency == 0) - frequency = 760; + frequency = _is_l3g4200d ? 800 : 760; - /* use limits good for H or non-H models */ + /* + * Use limits good for H or non-H models. Rates are slightly different + * for L3G4200D part but register settings are the same. + */ if (frequency <= 100) { - _current_rate = 95; + _current_rate = _is_l3g4200d ? 100 : 95; bits |= RATE_95HZ_LP_25HZ; } else if (frequency <= 200) { - _current_rate = 190; + _current_rate = _is_l3g4200d ? 200 : 190; bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { - _current_rate = 380; + _current_rate = _is_l3g4200d ? 400 : 380; bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { - _current_rate = 760; + _current_rate = _is_l3g4200d ? 800 : 760; bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; @@ -772,7 +792,7 @@ L3GD20::reset() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_samplerate(0); // 760Hz + set_samplerate(0); // 760Hz or 800Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); @@ -971,7 +991,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4dee7649b..4ca8b5e42 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1793,7 +1793,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index c910e2890..5adb8cf69 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -37,7 +37,7 @@ * * Driver for the Maxbotix sonar range finders connected via I2C. */ - + #include <nuttx/config.h> #include <drivers/device/i2c.h> @@ -84,7 +84,7 @@ /* Device limits */ #define MB12XX_MIN_DISTANCE (0.20f) #define MB12XX_MAX_DISTANCE (7.65f) - + #define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ /* oddly, ERROR is not defined for c++ */ @@ -102,17 +102,17 @@ class MB12XX : public device::I2C public: MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); virtual ~MB12XX(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); @@ -124,13 +124,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + orb_advert_t _range_finder_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + /** * Test whether the device supported by the driver is present at a * specific address. @@ -139,7 +139,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -147,12 +147,12 @@ private: * to make it more aggressive about resetting the bus in case of errors. */ void start(); - + /** * Stop the automatic measurement state machine. */ void stop(); - + /** * Set the min and max distance thresholds if you want the end points of the sensors * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE @@ -162,7 +162,7 @@ private: void set_maximum_distance(float max); float get_minimum_distance(); float get_maximum_distance(); - + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -177,8 +177,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -200,8 +200,8 @@ MB12XX::MB12XX(int bus, int address) : _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) { // enable debug() calls - _debug_enabled = true; - + _debug_enabled = false; + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -212,8 +212,9 @@ MB12XX::~MB12XX() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } } int @@ -222,22 +223,25 @@ MB12XX::init() int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* get a publish handle on the range finder topic */ struct range_finder_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); - if (_range_finder_topic < 0) + if (_range_finder_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -256,13 +260,13 @@ void MB12XX::set_minimum_distance(float min) { _min_distance = min; -} +} void MB12XX::set_maximum_distance(float max) { _max_distance = max; -} +} float MB12XX::get_minimum_distance() @@ -284,20 +288,20 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -307,13 +311,14 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -322,15 +327,17 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -338,45 +345,49 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; - - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); - return -ENOMEM; + + return OK; } - irqrestore(flags); - - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - - case RANGEFINDERIOCSETMINIUMDISTANCE: - { - set_minimum_distance(*(float *)arg); - return 0; - } - break; - case RANGEFINDERIOCSETMAXIUMDISTANCE: - { - set_maximum_distance(*(float *)arg); - return 0; - } - break; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -391,8 +402,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -453,14 +465,14 @@ MB12XX::measure() uint8_t cmd = MB12XX_TAKE_RANGE_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); return ret; } + ret = OK; - + return ret; } @@ -468,32 +480,31 @@ int MB12XX::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[2] = {0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 2); - - if (ret < 0) - { + + if (ret < 0) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - + uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); report.distance = si_units; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - + /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); @@ -519,17 +530,19 @@ MB12XX::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, true, true, - SUBSYSTEM_TYPE_RANGEFINDER}; + SUBSYSTEM_TYPE_RANGEFINDER + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -583,8 +596,9 @@ MB12XX::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -635,33 +649,37 @@ start() { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new MB12XX(MB12XX_BUS); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -674,15 +692,14 @@ fail: */ void stop() { - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + + } else { errx(1, "driver not running"); } + exit(0); } @@ -700,22 +717,25 @@ test() int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("measurement: %0.2f m", (double)report.distance); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -726,20 +746,27 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("measurement: %0.3f", (double)report.distance); warnx("time: %lld", report.timestamp); } + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + errx(0, "PASS"); } @@ -751,14 +778,17 @@ reset() { int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -769,8 +799,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -786,32 +817,37 @@ mb12xx_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { mb12xx::start(); - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - mb12xx::stop(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + mb12xx::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { mb12xx::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { mb12xx::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { mb12xx::info(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index d43e3aef9..5d1f58b85 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include <arch/board/board.h> #include <mavlink/mavlink_log.h> -#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/Publication.hpp> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_hrt.h> @@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu float prev_revolution = md25.getRevolutions1(); // debug publication - control::UOrbPublication<debug_key_value_s> debug_msg(NULL, + uORB::Publication<debug_key_value_s> debug_msg(NULL, ORB_ID(debug_key_value)); // sine wave for motor 1 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 1661f67f9..962c6b881 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include <poll.h> #include <stdio.h> -#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/Subscription.hpp> #include <uORB/topics/actuator_controls.h> #include <drivers/device/i2c.h> @@ -270,7 +270,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription<actuator_controls_s> _actuators; + uORB::Subscription<actuator_controls_s> _actuators; // local copy of data from i2c device uint8_t _version; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 06d89abf7..c0f3c28e0 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -79,12 +79,15 @@ #include <systemlib/param/param.h> #include <systemlib/perf_counter.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> + #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> #include <uORB/topics/subsystem_info.h> +#include <uORB/topics/system_power.h> #include <drivers/airspeed/airspeed.h> @@ -99,12 +102,14 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ +#define MEAS_RATE 100.0f +#define MEAS_DRIVER_FILTER_FREQ 3.0f #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); protected: @@ -116,6 +121,15 @@ protected: virtual int measure(); virtual int collect(); + math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa, float &temperature); + + int _t_system_power; + struct system_power_s system_power; }; /* @@ -123,10 +137,12 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) +MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1) { - + memset(&system_power, 0, sizeof(system_power)); } int @@ -161,23 +177,25 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } - uint8_t status = val[0] & 0xC0; + uint8_t status = (val[0] & 0xC0) >> 6; - if (status == 2) { - log("err: stale data"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } else if (status == 3) { - log("err: fault"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; + switch (status) { + case 0: + break; + + case 1: + /* fallthrough */ + case 2: + /* fallthrough */ + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; @@ -186,28 +204,67 @@ MEASAirspeed::collect() dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; - float temperature = ((200 * dT_raw) / 2047) - 50; + float temperature = ((200.0f * dT_raw) / 2047) - 50; - /* calculate differential pressure. As its centered around 8000 - * and can go positive or negative, enforce absolute value - */ + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; - if (diff_press_pa < 0.0f) - diff_press_pa = 0.0f; + const float PSI_to_Pa = 6894.757f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet + + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; + + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa_raw, temperature); + float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + + /* + note that we return both the absolute value with offset + applied and a raw value without the offset applied. This + makes it possible for higher level code to detect if the + user has the tubes connected backwards, and also makes it + possible to correctly use offsets calculated by a higher + level airspeed driver. + + With the above calculation the MS4525 sensor will produce a + positive number when the top port is used as a dynamic port + and bottom port is used as the static port + + Also note that the _diff_pres_offset is applied before the + fabsf() not afterwards. It needs to be done this way to + prevent a bias at low speeds, but this also means that when + setting a offset you must set it based on the raw value, not + the offset value + */ + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_press_pa; + _max_differential_pressure_pa = diff_press_pa; } report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + + /* the dynamics of the filter can make it overshoot into the negative range */ + if (report.differential_pressure_filtered_pa < 0.0f) { + report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + } + + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -231,13 +288,17 @@ MEASAirspeed::collect() void MEASAirspeed::cycle() { + int ret; + /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + ret = collect(); + if (OK != ret) { /* restart the measurement state machine */ start(); + _sensor_ok = false; return; } @@ -261,8 +322,12 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) - log("measure error"); + ret = measure(); + if (OK != ret) { + debug("measure error"); + } + + _sensor_ok = (ret == OK); /* next phase is collection */ _collect_phase = true; @@ -276,6 +341,62 @@ MEASAirspeed::cycle() } /** + correct for 5V rail voltage if the system_power ORB topic is + available + + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void +MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + if (_t_system_power == -1) { + // not available + return; + } + bool updated = false; + orb_check(_t_system_power, &updated); + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 65.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + temperature -= voltage_diff * temp_slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + +/** * Local functions in support of the shell command. */ namespace meas_airspeed @@ -303,15 +424,17 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver, try the MS4525DO first */ g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { @@ -319,22 +442,26 @@ start(int i2c_bus) g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* both versions failed if the init for the MS5525DSO fails, give up */ - if (OK != g_dev->Airspeed::init()) + if (OK != g_dev->Airspeed::init()) { goto fail; + } } /* set the poll rate to default, starts automatic data collection */ fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); @@ -345,7 +472,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no MS4525 airspeed sensor connected"); } /** @@ -379,21 +506,24 @@ test() int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -404,23 +534,26 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { errx(1, "failed to set default rate"); + } errx(0, "PASS"); } @@ -433,14 +566,17 @@ reset() { int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -451,8 +587,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -491,32 +628,37 @@ meas_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { meas_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { meas_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { meas_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { meas_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { meas_airspeed::info(); + } meas_airspeed_usage(); exit(0); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 705e98eea..5954c40da 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -92,8 +92,20 @@ #define MOTOR_SPINUP_COUNTER 30 #define ESC_UORB_PUBLISH_DELAY 500000 - - +struct MotorData_t { + unsigned int Version; // the version of the BL (0 = old) + unsigned int SetPoint; // written by attitude controller + unsigned int SetPointLowerBits; // for higher Resolution of new BLs + float SetPoint_PX4; // Values from PX4 + unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present + unsigned int ReadMode; // select data to read + unsigned short RawPwmValue; // length of PWM pulse + // the following bytes must be exactly in that order! + unsigned int Current; // in 0.1 A steps, read back from BL + unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit + unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in + unsigned int RoundCount; +}; class MK : public device::I2C { @@ -154,8 +166,10 @@ private: actuator_controls_s _controls; + MotorData_t Motor[MAX_MOTORS]; + static void task_main_trampoline(int argc, char *argv[]); - void task_main() __attribute__((noreturn)); + void task_main(); static int control_callback(uintptr_t handle, uint8_t control_group, @@ -195,24 +209,6 @@ const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0}; int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0}; -struct MotorData_t { - unsigned int Version; // the version of the BL (0 = old) - unsigned int SetPoint; // written by attitude controller - unsigned int SetPointLowerBits; // for higher Resolution of new BLs - float SetPoint_PX4; // Values from PX4 - unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present - unsigned int ReadMode; // select data to read - unsigned short RawPwmValue; // length of PWM pulse - // the following bytes must be exactly in that order! - unsigned int Current; // in 0.1 A steps, read back from BL - unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit - unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in - unsigned int RoundCount; -}; - -MotorData_t Motor[MAX_MOTORS]; - - namespace { @@ -1015,7 +1011,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): /* copy the current output value from the channel */ - *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue; + *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_GET(0)].RawPwmValue; break; diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0ef056273..3fe1b0abc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -526,6 +526,7 @@ void MS5611::cycle() { int ret; + unsigned dummy; /* collection phase? */ if (_collect_phase) { @@ -542,6 +543,8 @@ MS5611::cycle() } else { //log("collection error %d", ret); } + /* issue a reset command to the sensor */ + _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ start_cycle(); return; @@ -573,6 +576,8 @@ MS5611::cycle() ret = measure(); if (ret != OK) { //log("measure error %d", ret); + /* issue a reset command to the sensor */ + _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ start_cycle(); return; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 26216e840..8759d16a1 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -117,7 +117,7 @@ private: device::Device * MS5611_spi_interface(ms5611::prom_u &prom_buf) { - return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk new file mode 100644 index 000000000..d3062e457 --- /dev/null +++ b/src/drivers/px4flow/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the PX4FLOW driver. +# + +MODULE_COMMAND = px4flow + +SRCS = px4flow.cpp diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp new file mode 100644 index 000000000..f214b5964 --- /dev/null +++ b/src/drivers/px4flow/px4flow.cpp @@ -0,0 +1,806 @@ +/**************************************************************************** + * + * 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 px4flow.cpp + * @author Dominik Honegger + * + * Driver for the PX4FLOW module connected via I2C. + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_px4flow.h> +#include <drivers/device/ringbuffer.h> + +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> +//#include <uORB/topics/optical_flow.h> + +#include <board_config.h> + +/* Configuration Constants */ +#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION +#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 +//range 0x42 - 0x49 + +/* PX4FLOW Registers addresses */ +#define PX4FLOW_REG 0x00 /* Measure Register */ + +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +//struct i2c_frame +//{ +// uint16_t frame_count; +// int16_t pixel_flow_x_sum; +// int16_t pixel_flow_y_sum; +// int16_t flow_comp_m_x; +// int16_t flow_comp_m_y; +// int16_t qual; +// int16_t gyro_x_rate; +// int16_t gyro_y_rate; +// int16_t gyro_z_rate; +// uint8_t gyro_range; +// uint8_t sonar_timestamp; +// int16_t ground_distance; +//}; +// +//struct i2c_frame f; + +class PX4FLOW : public device::I2C +{ +public: + PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); + virtual ~PX4FLOW(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _px4flow_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); + +PX4FLOW::PX4FLOW(int bus, int address) : + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _px4flow_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), + _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +PX4FLOW::~PX4FLOW() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete _reports; +} + +int +PX4FLOW::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(px4flow_report)); + + if (_reports == nullptr) + goto out; + + /* get a publish handle on the px4flow topic */ + struct px4flow_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); + + if (_px4flow_topic < 0) + debug("failed to create px4flow object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +PX4FLOW::probe() +{ + return measure(); +} + +int +PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct px4flow_report); + struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(PX4FLOW_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +PX4FLOW::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = PX4FLOW_REG; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + printf("i2c::transfer flow returned %d"); + return ret; + } + ret = OK; + + return ret; +} + +int +PX4FLOW::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 22); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + +// f.frame_count = val[1] << 8 | val[0]; +// f.pixel_flow_x_sum= val[3] << 8 | val[2]; +// f.pixel_flow_y_sum= val[5] << 8 | val[4]; +// f.flow_comp_m_x= val[7] << 8 | val[6]; +// f.flow_comp_m_y= val[9] << 8 | val[8]; +// f.qual= val[11] << 8 | val[10]; +// f.gyro_x_rate= val[13] << 8 | val[12]; +// f.gyro_y_rate= val[15] << 8 | val[14]; +// f.gyro_z_rate= val[17] << 8 | val[16]; +// f.gyro_range= val[18]; +// f.sonar_timestamp= val[19]; +// f.ground_distance= val[21] << 8 | val[20]; + + int16_t flowcx = val[7] << 8 | val[6]; + int16_t flowcy = val[9] << 8 | val[8]; + int16_t gdist = val[21] << 8 | val[20]; + + struct px4flow_report report; + report.flow_comp_x_m = float(flowcx)/1000.0f; + report.flow_comp_y_m = float(flowcy)/1000.0f; + report.flow_raw_x= val[3] << 8 | val[2]; + report.flow_raw_y= val[5] << 8 | val[4]; + report.ground_distance_m =float(gdist)/1000.0f; + report.quality= val[10]; + report.sensor_id = 0; + report.timestamp = hrt_absolute_time(); + + + /* publish it */ + orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + + /* post a report to the ring */ + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +PX4FLOW::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_OPTICALFLOW}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +PX4FLOW::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +PX4FLOW::cycle_trampoline(void *arg) +{ + PX4FLOW *dev = (PX4FLOW *)arg; + + dev->cycle(); +} + +void +PX4FLOW::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&PX4FLOW::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&PX4FLOW::cycle_trampoline, + this, + USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); +} + +void +PX4FLOW::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace px4flow +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +PX4FLOW *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new PX4FLOW(PX4FLOW_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct px4flow_report report; + ssize_t sz; + int ret; + + int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + // err(1, "immediate read failed"); + + warnx("single read"); + warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); + warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); + warnx("time: %lld", report.timestamp); + + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); + warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); + warnx("time: %lld", report.timestamp); + + + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +px4flow_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + px4flow::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + px4flow::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + px4flow::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + px4flow::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + px4flow::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a70ff6c5c..fd69cf795 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -92,6 +92,7 @@ public: MODE_2PWM, MODE_4PWM, MODE_6PWM, + MODE_8PWM, }; PX4FMU(); virtual ~PX4FMU(); @@ -113,6 +114,9 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) static const unsigned _max_actuators = 6; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + static const unsigned _max_actuators = 8; +#endif Mode _mode; unsigned _pwm_default_rate; @@ -120,19 +124,25 @@ private: uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; int _task; - int _t_actuators; - int _t_actuator_armed; - orb_advert_t _t_outputs; + int _armed_sub; + orb_advert_t _outputs_pub; + actuator_armed_s _armed; unsigned _num_outputs; bool _primary_pwm_device; volatile bool _task_should_exit; - bool _armed; + bool _servo_armed; bool _pwm_on; MixerGroup *_mixers; - actuator_controls_s _controls; + uint32_t _groups_required; + uint32_t _groups_subscribed; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; pwm_limit_t _pwm_limit; uint16_t _failsafe_pwm[_max_actuators]; @@ -143,13 +153,13 @@ private: unsigned _num_disarmed_set; static void task_main_trampoline(int argc, char *argv[]); - void task_main() __attribute__((noreturn)); + void task_main(); static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - + void subscribe(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -197,6 +207,20 @@ 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_AEROCORE) + /* AeroCore breaks out User GPIOs on J11 */ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, + {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, + {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, + {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, + {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -216,15 +240,18 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _t_actuators(-1), - _t_actuator_armed(-1), - _t_outputs(0), + _control_subs({-1}), + _poll_fds_num(0), + _armed_sub(-1), + _outputs_pub(-1), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), - _armed(false), + _servo_armed(false), _pwm_on(false), _mixers(nullptr), + _groups_required(0), + _groups_subscribed(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -235,6 +262,14 @@ PX4FMU::PX4FMU() : _max_pwm[i] = PWM_DEFAULT_MAX; } + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); + _debug_enabled = true; } @@ -365,6 +400,20 @@ PX4FMU::set_mode(Mode mode) break; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: // AeroCore PWMs as 8 PWM outs + debug("MODE_8PWM"); + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xff); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + break; +#endif + case MODE_NONE: debug("MODE_NONE"); @@ -448,32 +497,42 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels) } void +PX4FMU::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +void PX4FMU::task_main() { - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; #ifdef HRT_PPM_CHANNEL // rc input, published to ORB @@ -491,6 +550,12 @@ PX4FMU::task_main() /* loop until killed */ while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } /* * Adjust actuator topic update rate to keep up with @@ -515,20 +580,23 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - orb_set_interval(_t_actuators, update_rate_in_ms); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } // set to current max rate, even if we are actually checking slower/faster _current_update_rate = max_rate; } /* sleep waiting for data, stopping to check for PPM - * input at 100Hz */ - int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); + * input at 50Hz */ + int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } else if (ret == 0) { @@ -537,89 +605,101 @@ PX4FMU::task_main() } else { - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { + /* get controls for required topics */ + unsigned poll_id = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + } + poll_id++; + } + } - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); + /* can we mix? */ + if (_mixers != nullptr) { - /* can we mix? */ - if (_mixers != nullptr) { + unsigned num_outputs; - unsigned num_outputs; + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; + case MODE_4PWM: + num_outputs = 4; + break; - case MODE_4PWM: - num_outputs = 4; - break; + case MODE_6PWM: + num_outputs = 6; + break; - case MODE_6PWM: - num_outputs = 6; - break; + case MODE_8PWM: + num_outputs = 8; + break; + default: + num_outputs = 0; + break; + } - default: - num_outputs = 0; - break; + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; } + } - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } - } + uint16_t pwm_limited[num_outputs]; - uint16_t pwm_limited[num_outputs]; + pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } - /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } + /* publish mixed control outputs */ + if (_outputs_pub < 0) { + _outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + } else { - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs); } } + } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - /* update the armed status and check that we're not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; - if (_armed != set_armed) - _armed = set_armed; + if (_servo_armed != set_armed) + _servo_armed = set_armed; - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (aa.armed || _num_disarmed_set > 0); + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); - } + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); } } @@ -661,8 +741,13 @@ PX4FMU::task_main() } - ::close(_t_actuators); - ::close(_t_actuator_armed); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + ::close(_armed_sub); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -684,7 +769,7 @@ PX4FMU::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = controls->control[control_index]; + input = controls[control_group].control[control_index]; return 0; } @@ -707,6 +792,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) case MODE_2PWM: case MODE_4PWM: case MODE_6PWM: +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: +#endif ret = pwm_ioctl(filp, cmd, arg); break; @@ -736,6 +824,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_OFF: // these are no-ops, as no safety switch break; @@ -935,6 +1024,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_SET(7): + case PWM_SERVO_SET(6): + if (_mode < MODE_8PWM) { + ret = -EINVAL; + break; + } +#endif + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { @@ -962,6 +1060,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET(7): + case PWM_SERVO_GET(6): + if (_mode < MODE_8PWM) { + ret = -EINVAL; + break; + } +#endif + case PWM_SERVO_GET(5): case PWM_SERVO_GET(4): if (_mode < MODE_6PWM) { @@ -989,12 +1096,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(3): case PWM_SERVO_GET_RATEGROUP(4): case PWM_SERVO_GET_RATEGROUP(5): +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET_RATEGROUP(6): + case PWM_SERVO_GET_RATEGROUP(7): +#endif *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: + *(unsigned *)arg = 8; + break; +#endif + case MODE_6PWM: *(unsigned *)arg = 6; break; @@ -1040,6 +1157,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_6PWM); break; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + case 8: + set_mode(MODE_8PWM); + break; +#endif default: ret = -EINVAL; @@ -1052,6 +1174,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; + _groups_required = 0; } break; @@ -1060,18 +1183,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) mixer_simple_s *mixinfo = (mixer_simple_s *)arg; SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); + (uintptr_t)_controls, mixinfo); if (mixer->check()) { delete mixer; + _groups_required = 0; ret = -EINVAL; } else { if (_mixers == nullptr) _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); + (uintptr_t)_controls); _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); } break; @@ -1082,9 +1207,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); if (_mixers == nullptr) { + _groups_required = 0; ret = -ENOMEM; } else { @@ -1095,7 +1221,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) debug("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; + _groups_required = 0; ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); } } @@ -1122,10 +1252,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) unsigned count = len / 2; uint16_t values[6]; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + if (count > 8) { + // we have at most 8 outputs + count = 8; + } +#else if (count > 6) { // we have at most 6 outputs count = 6; } +#endif // allow for misaligned values memcpy(values, buffer, count * 2); @@ -1400,6 +1537,9 @@ fmu_new_mode(PortMode new_mode) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) servo_mode = PX4FMU::MODE_6PWM; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + servo_mode = PX4FMU::MODE_8PWM; +#endif break; /* mixed modes supported on v1 board only */ @@ -1717,7 +1857,7 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #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\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n"); #endif exit(1); diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index 05bc7a5b3..eeb59e1a1 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = fmu SRCS = fmu.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 2054faa12..c14f1f783 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -44,3 +44,5 @@ SRCS = px4io.cpp \ # XXX prune to just get UART registers INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7c7b3dcb7..4099e5522 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -91,6 +91,8 @@ #include "uploader.h" +#include "modules/dataman/dataman.h" + extern device::Device *PX4IO_i2c_interface() weak_function; extern device::Device *PX4IO_serial_interface() weak_function; @@ -527,6 +529,11 @@ PX4IO::~PX4IO() if (_interface != nullptr) delete _interface; + /* deallocate perfs */ + perf_free(_perf_update); + perf_free(_perf_write); + perf_free(_perf_chan_count); + g_dev = nullptr; } @@ -568,9 +575,15 @@ int PX4IO::init() { int ret; + param_t sys_restart_param; + int sys_restart_val = DM_INIT_REASON_VOLATILE; ASSERT(_task == -1); + sys_restart_param = param_find("SYS_RESTART_TYPE"); + /* Indicate restart type is unknown */ + param_set(sys_restart_param, &sys_restart_val); + /* do regular cdev init */ ret = CDev::init(); @@ -675,6 +688,25 @@ PX4IO::init() /* send command to arm system via command API */ vehicle_command_s cmd; + /* send this to itself */ + param_t sys_id_param = param_find("MAV_SYS_ID"); + param_t comp_id_param = param_find("MAV_COMP_ID"); + + int32_t sys_id; + int32_t comp_id; + + if (param_get(sys_id_param, &sys_id)) { + errx(1, "PRM SYSID"); + } + + if (param_get(comp_id_param, &comp_id)) { + errx(1, "PRM CMPID"); + } + + cmd.target_system = sys_id; + cmd.target_component = comp_id; + cmd.source_system = sys_id; + cmd.source_component = comp_id; /* request arming */ cmd.param1 = 1.0f; cmd.param2 = 0; @@ -684,10 +716,7 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - // cmd.target_system = status.system_id; - // cmd.target_component = status.component_id; - // cmd.source_system = status.system_id; - // cmd.source_component = status.component_id; + /* ask to confirm command */ cmd.confirmation = 1; @@ -720,6 +749,11 @@ PX4IO::init() /* keep waiting for state change for 2 s */ } while (!safety.armed); + /* Indicate restart type is in-flight */ + sys_restart_val = DM_INIT_REASON_IN_FLIGHT; + param_set(sys_restart_param, &sys_restart_val); + + /* regular boot, no in-air restart, init IO */ } else { @@ -745,6 +779,10 @@ PX4IO::init() } } + /* Indicate restart type is power on */ + sys_restart_val = DM_INIT_REASON_POWER_ON; + param_set(sys_restart_param, &sys_restart_val); + } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ @@ -756,7 +794,12 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_spawn_cmd("px4io", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 2000, + (main_t)&PX4IO::task_main_trampoline, + nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -944,8 +987,27 @@ PX4IO::task_main() int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); if (pret != OK) { - log("voltage scaling upload failed"); + log("vscale upload failed"); } + + /* send RC throttle failsafe value to IO */ + int32_t failsafe_param_val; + param_t failsafe_param = param_find("RC_FAILS_THR"); + + if (failsafe_param != PARAM_INVALID) { + + param_get(failsafe_param, &failsafe_param_val); + + if (failsafe_param_val > 0) { + + uint16_t failsafe_thr = failsafe_param_val; + pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + if (pret != OK) { + log("failsafe upload failed, FS: %d us", (int)failsafe_thr); + } + } + } + } } @@ -1332,12 +1394,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + /* the announced battery status would conflict with the simulated battery status in HIL */ + if (!(_pub_blocked)) { + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } } } @@ -1397,7 +1462,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; /* @@ -1405,8 +1470,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - input_rc.timestamp_publication = hrt_absolute_time(); - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) @@ -1418,23 +1481,38 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ channel_count = regs[PX4IO_P_RAW_RC_COUNT]; - if (channel_count != _rc_chan_count) + /* limit the channel count */ + if (channel_count > RC_INPUT_MAX_CHANNELS) { + channel_count = RC_INPUT_MAX_CHANNELS; + } + + /* count channel count changes to identify signal integrity issues */ + if (channel_count != _rc_chan_count) { perf_count(_perf_chan_count); + } _rc_chan_count = channel_count; + input_rc.timestamp_publication = hrt_absolute_time(); + input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; + input_rc.channel_count = channel_count; /* rc_lost has to be set before the call to this function */ - if (!input_rc.rc_lost && !input_rc.rc_failsafe) + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { _rc_last_valid = input_rc.timestamp_publication; + } input_rc.timestamp_last_signal = _rc_last_valid; + /* FIELDS NOT SET HERE */ + /* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */ + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); @@ -1442,8 +1520,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) return ret; } - input_rc.channel_count = channel_count; - memcpy(input_rc.values, ®s[prolog], channel_count * 2); + /* last thing set are the actual channel values as 16 bit values */ + for (unsigned i = 0; i < channel_count; i++) { + input_rc.values[i] = regs[prolog + i]; + } return ret; } @@ -1476,10 +1556,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ return OK; + } } /* lazily advertise on first publication */ @@ -1921,12 +2002,14 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - printf("controls"); + for (unsigned group = 0; group < 4; group++) { + printf("controls %u:", group); - for (unsigned i = 0; i < _max_controls; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + for (unsigned i = 0; i < _max_controls; i++) + printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); - printf("\n"); + printf("\n"); + } for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; @@ -1957,8 +2040,7 @@ PX4IO::print_status() } int -PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) -/* Make it obvious that file * isn't used here */ +PX4IO::ioctl(file * filep, int cmd, unsigned long arg) { int ret = OK; @@ -2109,6 +2191,10 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_GET_DISABLE_LOCKDOWN: *(unsigned *)arg = _lockdown_override; + + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + /* force safety swith off */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; case DSM_BIND_START: @@ -2370,8 +2456,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; default: - /* not a recognized value */ - ret = -ENOTTY; + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); + break; } return ret; diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index dd8abbac5..28ec62356 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -201,9 +201,14 @@ PX4IO_Uploader::upload(const char *filenames[]) continue; } - if (bl_rev <= 2) + if (bl_rev <= 2) { ret = verify_rev2(fw_size); - else if(bl_rev == 3) { + } else if(bl_rev == 3) { + ret = verify_rev3(fw_size); + } else { + /* verify rev 4 and higher still uses the same approach and + * every version *needs* to be verified. + */ ret = verify_rev3(fw_size); } diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 4f58891ed..13cbfdfa8 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); break; } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index d65a9be36..dd5e4d3e0 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -53,7 +53,7 @@ #include <arch/board/board.h> #include <mavlink/mavlink_log.h> -#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/Publication.hpp> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_hrt.h> diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index e9f35cf95..58994d6fa 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,7 +45,7 @@ #include <poll.h> #include <stdio.h> -#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/Subscription.hpp> #include <uORB/topics/actuator_controls.h> #include <drivers/device/i2c.h> @@ -169,7 +169,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription<actuator_controls_s> _actuators; + uORB::Subscription<actuator_controls_s> _actuators; // private data float _motor1Position; diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk new file mode 100644 index 000000000..dc93a90e7 --- /dev/null +++ b/src/drivers/sf0x/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +# +# Makefile to build the Lightware laser range finder driver. +# + +MODULE_COMMAND = sf0x + +SRCS = sf0x.cpp diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp new file mode 100644 index 000000000..a0cf98340 --- /dev/null +++ b/src/drivers/sf0x/sf0x.cpp @@ -0,0 +1,1020 @@ +/**************************************************************************** + * + * Copyright (c) 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 sf0x.cpp + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Greg Hulands + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <termios.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_range_finder.h> +#include <drivers/device/device.h> +#include <drivers/device/ringbuffer.h> + +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> + +#include <board_config.h> + +/* Configuration Constants */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +#define SF0X_CONVERSION_INTERVAL 83334 +#define SF0X_TAKE_RANGE_REG 'd' +#define SF02F_MIN_DISTANCE 0.0f +#define SF02F_MAX_DISTANCE 40.0f +#define SF0X_DEFAULT_PORT "/dev/ttyS2" + +class SF0X : public device::CDev +{ +public: + SF0X(const char *port = SF0X_DEFAULT_PORT); + virtual ~SF0X(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _fd; + char _linebuf[10]; + unsigned _linebuf_index; + hrt_abstime _last_read; + + orb_advert_t _range_finder_topic; + + unsigned _consecutive_fail_count; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE + * and SF0X_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); + +SF0X::SF0X(const char *port) : + CDev("SF0X", RANGE_FINDER_DEVICE_PATH), + _min_distance(SF02F_MIN_DISTANCE), + _max_distance(SF02F_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _fd(-1), + _linebuf_index(0), + _last_read(0), + _range_finder_topic(-1), + _consecutive_fail_count(0), + _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), + _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) +{ + /* open fd */ + _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (_fd < 0) { + warnx("FAIL: laser fd"); + } + + /* tell it to stop auto-triggering */ + char stop_auto = ' '; + (void)::write(_fd, &stop_auto, 1); + usleep(100); + (void)::write(_fd, &stop_auto, 1); + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = B9600; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d OSPD\n", termios_state); + } + + if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR baud %d ATTR", termios_state); + } + + // disable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +SF0X::~SF0X() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } +} + +int +SF0X::init() +{ + int ret = ERROR; + unsigned i = 0; + + /* do regular cdev init */ + if (CDev::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + warnx("mem err"); + goto out; + } + + /* get a publish handle on the range finder topic */ + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + + if (_range_finder_topic < 0) { + warnx("advert err"); + } + + /* close the fd */ + ::close(_fd); + _fd = -1; +out: + return OK; +} + +int +SF0X::probe() +{ + return measure(); +} + +void +SF0X::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +SF0X::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +SF0X::get_minimum_distance() +{ + return _min_distance; +} + +float +SF0X::get_maximum_distance() +{ + return _max_distance; +} + +int +SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(SF0X_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(SF0X_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +SF0X::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(SF0X_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +SF0X::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + char cmd = SF0X_TAKE_RANGE_REG; + ret = ::write(_fd, &cmd, 1); + + if (ret != sizeof(cmd)) { + perf_count(_comms_errors); + log("write fail %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +SF0X::collect() +{ + int ret; + + perf_begin(_sample_perf); + + /* clear buffer if last read was too long ago */ + uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { + _linebuf_index = 0; + } else if (_linebuf_index > 0) { + /* increment to next read position */ + _linebuf_index++; + } + + /* the buffer for read chars is buflen minus null termination */ + unsigned readlen = sizeof(_linebuf) - 1; + + /* read from the sensor (uart buffer) */ + ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index); + + if (ret < 0) { + _linebuf[sizeof(_linebuf) - 1] = '\0'; + debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); + perf_count(_comms_errors); + perf_end(_sample_perf); + + /* only throw an error if we time out */ + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { + return ret; + + } else { + return -EAGAIN; + } + } else if (ret == 0) { + return -EAGAIN; + } + + /* we did increment the index to the next position already, so just add the additional fields */ + _linebuf_index += (ret - 1); + + _last_read = hrt_absolute_time(); + + if (_linebuf_index < 1) { + /* we need at least the two end bytes to make sense of this string */ + return -EAGAIN; + + } else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') { + + if (_linebuf_index >= readlen - 1) { + /* we have a full buffer, but no line ending - abort */ + _linebuf_index = 0; + perf_count(_comms_errors); + return -ENOMEM; + } else { + /* incomplete read, reschedule ourselves */ + return -EAGAIN; + } + } + + char *end; + float si_units; + bool valid; + + /* enforce line ending */ + unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1); + + _linebuf[lend] = '\0'; + + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { + si_units = -1.0f; + valid = false; + + } else { + + /* we need to find a dot in the string, as we're missing the meters part else */ + valid = false; + + /* wipe out partially read content from last cycle(s), check for dot */ + for (int i = 0; i < (lend - 2); i++) { + if (_linebuf[i] == '\n') { + char buf[sizeof(_linebuf)]; + memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); + memcpy(_linebuf, buf, (lend + 1) - (i + 1)); + } + + if (_linebuf[i] == '.') { + valid = true; + } + } + + if (valid) { + si_units = strtod(_linebuf, &end); + + /* we require at least 3 characters for a valid number */ + if (end > _linebuf + 3) { + valid = true; + } else { + si_units = -1.0f; + valid = false; + } + } + } + + debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO")); + + /* done with this chunk, resetting - even if invalid */ + _linebuf_index = 0; + + /* if its invalid, there is no reason to forward the value */ + if (!valid) { + perf_count(_comms_errors); + return -EINVAL; + } + + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +SF0X::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1); + + // /* notify about state change */ + // struct subsystem_info_s info = { + // true, + // true, + // true, + // SUBSYSTEM_TYPE_RANGEFINDER + // }; + // static orb_advert_t pub = -1; + + // if (pub > 0) { + // orb_publish(ORB_ID(subsystem_info), pub, &info); + + // } else { + // pub = orb_advertise(ORB_ID(subsystem_info), &info); + // } +} + +void +SF0X::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +SF0X::cycle_trampoline(void *arg) +{ + SF0X *dev = static_cast<SF0X *>(arg); + + dev->cycle(); +} + +void +SF0X::cycle() +{ + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK); + } + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + int collect_ret = collect(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + USEC2TICK(1100)); + return; + } + + if (OK != collect_ret) { + + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + log("collection error #%u", _consecutive_fail_count); + } + _consecutive_fail_count++; + + /* restart the measurement state machine */ + start(); + return; + } else { + /* apparently success */ + _consecutive_fail_count = 0; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + USEC2TICK(SF0X_CONVERSION_INTERVAL)); +} + +void +SF0X::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %d ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace sf0x +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +SF0X *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(const char *port) +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new SF0X(port); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("device open fail"); + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("val: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2 Hz rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + int ret = poll(&fds, 1, 2000); + + if (ret != 1) { + warnx("timed out"); + break; + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warnx("read failed: got %d vs exp. %d", sz, sizeof(report)); + break; + } + + warnx("read #%u", i); + warnx("val: %0.3f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to the default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "ERR: DEF RATE"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +sf0x_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + if (argc > 2) { + sf0x::start(argv[2]); + + } else { + sf0x::start(SF0X_DEFAULT_PORT); + } + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + sf0x::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + sf0x::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + sf0x::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + sf0x::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 00e46d6b8..de13b8969 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -41,6 +41,7 @@ */ #include <nuttx/config.h> +#include <board_config.h> #include <drivers/device/device.h> #include <sys/types.h> @@ -64,6 +65,8 @@ #include <systemlib/err.h> #include <systemlib/perf_counter.h> +#include <uORB/topics/system_power.h> + /* * Register accessors. * For now, no reason not to just use ADC1. @@ -119,6 +122,8 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ + orb_advert_t _to_system_power; + /** work trampoline */ static void _tick_trampoline(void *arg); @@ -134,13 +139,16 @@ private: */ uint16_t _sample(unsigned channel); + // update system_power ORB topic, only on FMUv2 + void update_system_power(void); }; ADC::ADC(uint32_t channels) : CDev("adc", ADC_DEVICE_PATH), _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), _channel_count(0), - _samples(nullptr) + _samples(nullptr), + _to_system_power(0) { _debug_enabled = true; @@ -290,6 +298,43 @@ ADC::_tick() /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) _samples[i].am_data = _sample(_samples[i].am_channel); + update_system_power(); +} + +void +ADC::update_system_power(void) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + 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 low + system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID); + + // OC pins are active low + system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC); + system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC); + + /* lazily publish */ + if (_to_system_power > 0) { + 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_V2 } uint16_t @@ -341,7 +386,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[10]; + adc_msg_s data[12]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) @@ -374,6 +419,10 @@ adc_main(int argc, char *argv[]) g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE + /* XXX this hardcodes the default channel set for AeroCore - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index b7c9b89a4..281f918d7 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -94,7 +94,7 @@ #elif HRT_TIMER == 3 # define HRT_TIMER_BASE STM32_TIM3_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM3 # define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN # if CONFIG_STM32_TIM3 @@ -141,7 +141,7 @@ # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP -# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN # if CONFIG_STM32_TIM10 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 # endif @@ -150,7 +150,7 @@ # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM -# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN # if CONFIG_STM32_TIM11 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 # endif @@ -354,6 +354,9 @@ __EXPORT uint16_t ppm_frame_length = 0; __EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; +#define PPM_DEBUG 0 + +#if PPM_DEBUG /* PPM edge history */ __EXPORT uint16_t ppm_edge_history[32]; unsigned ppm_edge_next; @@ -361,6 +364,7 @@ unsigned ppm_edge_next; /* PPM pulse history */ __EXPORT uint16_t ppm_pulse_history[32]; unsigned ppm_pulse_next; +#endif static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; @@ -455,10 +459,12 @@ hrt_ppm_decode(uint32_t status) /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; +#if PPM_DEBUG ppm_edge_history[ppm_edge_next++] = width; if (ppm_edge_next >= 32) ppm_edge_next = 0; +#endif /* * if this looks like a start pulse, then push the last set of values @@ -546,10 +552,12 @@ hrt_ppm_decode(uint32_t status) interval = count - ppm.last_mark; ppm.last_mark = count; +#if PPM_DEBUG ppm_pulse_history[ppm_pulse_next++] = interval; if (ppm_pulse_next >= 32) ppm_pulse_next = 0; +#endif /* if the mark-mark timing is out of bounds, abandon the frame */ if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index f36f2091e..810f4aacc 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -334,6 +334,8 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow + _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP"; + _default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -344,6 +346,8 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow _tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast _tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning + _tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm + _tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release } ToneAlarm::~ToneAlarm() |