fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-06 01:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into acro2

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-20 00:03:00 +02:00
parent b165e6ba20 b250e28abf
commit b12928548c
590 changed files with 32230 additions and 16046 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/airspeed/airspeed.cpp
+26 -18
View File
@@ -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
@@ -368,12 +353,35 @@ Airspeed::stop()
work_cancel(HPWORK, &_work);
}
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
src/drivers/airspeed/airspeed.h
+7
View File
@@ -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;
src/drivers/blinkm/blinkm.cpp
+135 -71
View File
@@ -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;
}
}
src/drivers/boards/aerocore/aerocore_init.c
+282
View File
@@ -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;
}
src/drivers/boards/aerocore/aerocore_led.c
+121
View File
@@ -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");
}
}
src/drivers/boards/aerocore/aerocore_pwm_servo.c
+117
View File
@@ -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,
}
};
src/drivers/boards/aerocore/aerocore_spi.c
+183
View File
@@ -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
src/drivers/boards/aerocore/board_config.h
+176
View File
@@ -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
src/drivers/boards/aerocore/module.mk
+8
View File
@@ -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
src/drivers/boards/px4fmu-v1/board_config.h
+2
View File
@@ -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
src/drivers/boards/px4fmu-v2/board_config.h
+2
View File
@@ -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
src/drivers/device/cdev.cpp
+1 -1
View File
@@ -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;
}
src/drivers/drv_gpio.h
+9 -1
View File
@@ -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 */
#endif /* _DRV_GPIO_H */
src/drivers/drv_gps.h
+4
View File
@@ -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"
src/drivers/drv_hrt.h
+1
View File
@@ -41,6 +41,7 @@
#include <sys/types.h>
#include <stdbool.h>
#include <inttypes.h>
#include <time.h>
#include <queue.h>
src/drivers/drv_pwm_output.h
+3
View File
@@ -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)
/*
*
*
src/drivers/drv_px4flow.h
+84
View File
@@ -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 */
src/drivers/drv_range_finder.h
+9 -2
View File
@@ -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 */
};
/*
src/drivers/drv_tone_alarm.h
+2
View File
@@ -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
};
src/drivers/ets_airspeed/ets_airspeed.cpp
+16 -6
View File
@@ -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");
}
/**
src/drivers/frsky_telemetry/frsky_data.c
+3 -1
View File
@@ -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);
src/drivers/frsky_telemetry/frsky_telemetry.c
+1 -1
View File
@@ -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);
src/drivers/frsky_telemetry/module.mk
+2
View File
@@ -39,3 +39,5 @@ MODULE_COMMAND = frsky_telemetry
SRCS = frsky_data.c \
frsky_telemetry.c
MODULE_STACKSIZE = 1200
src/drivers/gps/gps.cpp
+22 -12
View File
@@ -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
src/drivers/gps/module.mk
+2
View File
@@ -41,3 +41,5 @@ SRCS = gps.cpp \
gps_helper.cpp \
mtk.cpp \
ubx.cpp
MODULE_STACKSIZE = 1200
src/drivers/gps/mtk.cpp
+8 -2
View File
@@ -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;
src/drivers/hil/hil.cpp
+1 -1
View File
@@ -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,
src/drivers/hmc5883/hmc5883.cpp
+27 -23
View File
@@ -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,32 +1049,29 @@ 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];
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
goto out;
}
ret = OK;
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
}
if (ret == OK) {
if (!check_scale()) {
warnx("mag scale calibration successfully finished.");
@@ -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,
src/drivers/hott/messages.cpp
+17 -15
View File
@@ -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
src/drivers/l3gd20/l3gd20.cpp
+38 -18
View File
@@ -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;
src/drivers/lsm303d/lsm303d.cpp
+1 -1
View File
@@ -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");
src/drivers/mb12xx/mb12xx.cpp
+140 -104
View File
@@ -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'");
}
src/drivers/md25/md25.cpp
+2 -2
View File
@@ -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
src/drivers/md25/md25.hpp
+2 -2
View File
@@ -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;
src/drivers/meas_airspeed/meas_airspeed.cpp
+195 -53
View File
@@ -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;
@@ -275,6 +340,62 @@ MEASAirspeed::cycle()
USEC2TICK(CONVERSION_INTERVAL));
}
/**
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.
*/
@@ -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);
src/drivers/mkblctrl/mkblctrl.cpp
+18 -22
View File
@@ -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;
src/drivers/ms5611/ms5611.cpp
+5
View File
@@ -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;
src/drivers/ms5611/ms5611_spi.cpp
+1 -1
View File
@@ -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) :
src/modules/fixedwing_pos_control/module.mk → src/drivers/px4flow/module.mk
+4 -4
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# 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
@@ -32,9 +32,9 @@
############################################################################
#
# Fixedwing PositionControl application
# Makefile to build the PX4FLOW driver.
#
MODULE_COMMAND = fixedwing_pos_control
MODULE_COMMAND = px4flow
SRCS = fixedwing_pos_control_main.c
SRCS = px4flow.cpp
src/drivers/px4flow/px4flow.cpp
+806
View File
@@ -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'");
}
src/drivers/px4fmu/fmu.cpp
+252 -112
View File
@@ -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");
@@ -447,33 +496,43 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels)
return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
}
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 - 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) {
unsigned num_outputs;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
default:
num_outputs = 0;
break;
/* 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]);
}
/* 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];
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]);
}
/* 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);
poll_id++;
}
}
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
actuator_armed_s aa;
/* can we mix? */
if (_mixers != nullptr) {
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
unsigned num_outputs;
/* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
if (_armed != set_armed)
_armed = set_armed;
case MODE_4PWM:
num_outputs = 4;
break;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0);
case MODE_6PWM:
num_outputs = 6;
break;
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
case MODE_8PWM:
num_outputs = 8;
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;
}
}
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);
/* 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 {
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs);
}
}
}
/* check arming state */
bool updated = false;
orb_check(_armed_sub, &updated);
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 = _armed.armed && !_armed.lockdown;
if (_servo_armed != set_armed)
_servo_armed = set_armed;
/* 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);
}
}
@@ -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);
@@ -1399,6 +1536,9 @@ fmu_new_mode(PortMode new_mode)
#endif
#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;
@@ -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);
src/drivers/px4fmu/module.mk
+2
View File
@@ -4,3 +4,5 @@
MODULE_COMMAND = fmu
SRCS = fmu.cpp
MODULE_STACKSIZE = 1200
src/drivers/px4io/module.mk
+2
View File
@@ -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
src/drivers/px4io/px4io.cpp
+116 -29
View File
@@ -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, &regs[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, &regs[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, &regs[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;
src/drivers/px4io/px4io_uploader.cpp
+7 -2
View File
@@ -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);
}
src/drivers/rgbled/rgbled.cpp
+2
View File
@@ -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;
}
src/drivers/roboclaw/RoboClaw.cpp
+1 -1
View File
@@ -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>
src/drivers/roboclaw/RoboClaw.hpp
+2 -2
View File
@@ -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;
src/modules/att_pos_estimator_ekf/module.mk → src/drivers/sf0x/module.mk
+4 -6
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# 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
@@ -32,11 +32,9 @@
############################################################################
#
# Full attitude / position Extended Kalman Filter
# Makefile to build the Lightware laser range finder driver.
#
MODULE_COMMAND = att_pos_estimator_ekf
MODULE_COMMAND = sf0x
SRCS = kalman_main.cpp \
KalmanNav.cpp \
params.c
SRCS = sf0x.cpp
src/drivers/sf0x/sf0x.cpp
+1020
View File
File diff suppressed because it is too large Load Diff
src/drivers/stm32/adc/adc.cpp
+51 -2
View File
@@ -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");
src/drivers/stm32/drv_hrt.c
+11 -3
View File
@@ -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))
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+4
View File
@@ -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()
src/examples/fixedwing_control/module.mk
+2
View File
@@ -39,3 +39,5 @@ MODULE_COMMAND = ex_fixedwing_control
SRCS = main.c \
params.c
MODULE_STACKSIZE = 1200
src/examples/flow_position_estimator/flow_position_estimator_main.c
+3 -2
View File
@@ -65,6 +65,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <poll.h>
#include "flow_position_estimator_params.h"
@@ -109,9 +110,9 @@ int flow_position_estimator_main(int argc, char *argv[])
thread_should_exit = false;
daemon_task = task_spawn_cmd("flow_position_estimator",
SCHED_RR,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4096,
4000,
flow_position_estimator_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
src/examples/px4_daemon_app/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = px4_daemon_app
SRCS = px4_daemon_app.c
MODULE_STACKSIZE = 1200
src/examples/px4_daemon_app/px4_daemon_app.c
+1 -1
View File
@@ -98,7 +98,7 @@ int px4_daemon_app_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("daemon",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
2000,
px4_daemon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
src/examples/px4_mavlink_debug/module.mk
+3 -1
View File
@@ -37,4 +37,6 @@
MODULE_COMMAND = px4_mavlink_debug
SRCS = px4_mavlink_debug.c
SRCS = px4_mavlink_debug.c
MODULE_STACKSIZE = 2000
src/modules/controllib/block/List.hpp → src/include/containers/List.hpp
+3 -3
View File
@@ -32,9 +32,9 @@
****************************************************************************/
/**
* @file Node.h
* @file List.hpp
*
* A node of a linked list.
* A linked list.
*/
#pragma once
@@ -43,7 +43,7 @@ template<class T>
class __EXPORT ListNode
{
public:
ListNode() : _sibling(NULL) {
ListNode() : _sibling(nullptr) {
}
void setSibling(T sibling) { _sibling = sibling; }
T getSibling() { return _sibling; }
src/include/mavlink/mavlink_log.h
+3
View File
@@ -100,6 +100,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
*/
#define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
struct mavlink_logmessage {
char text[MAVLINK_LOG_MAXLEN + 1];
unsigned char severity;
@@ -112,6 +113,7 @@ struct mavlink_logbuffer {
struct mavlink_logmessage *elems;
};
__BEGIN_DECLS
void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb);
@@ -125,6 +127,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
__END_DECLS
#endif
src/lib/geo/geo.c
+81 -134
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012, 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
@@ -42,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <geo/geo.h>
@@ -52,124 +50,58 @@
#include <math.h>
#include <stdbool.h>
/*
* Azimuthal Equidistant Projection
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
*/
/* values for map projection */
static double phi_1;
static double sin_phi_1;
static double cos_phi_1;
static double lambda_0;
static double scale;
__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
phi_1 = lat_0 / 180.0 * M_PI;
lambda_0 = lon_0 / 180.0 * M_PI;
sin_phi_1 = sin(phi_1);
cos_phi_1 = cos(phi_1);
/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
double lat1 = phi_1;
double lon1 = lambda_0;
double lat2 = phi_1 + 0.5 / 180 * M_PI;
double lon2 = lambda_0 + 0.5 / 180 * M_PI;
double sin_lat_2 = sin(lat2);
double cos_lat_2 = cos(lat2);
double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH;
/* 2) calculate distance rho on plane */
double k_bar = 0;
double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
if (0 != c)
k_bar = c / sin(c);
double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
double rho = sqrt(pow(x2, 2) + pow(y2, 2));
scale = d / rho;
ref->lat = lat_0 / 180.0 * M_PI;
ref->lon = lon_0 / 180.0 * M_PI;
ref->sin_lat = sin(ref->lat);
ref->cos_lat = cos(ref->lat);
}
__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double phi = lat / 180.0 * M_PI;
double lambda = lon / 180.0 * M_PI;
double lat_rad = lat / 180.0 * M_PI;
double lon_rad = lon / 180.0 * M_PI;
double sin_phi = sin(phi);
double cos_phi = cos(phi);
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref->lon);
double k_bar = 0;
/* using small angle approximation (formula in comment is without aproximation) */
double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
double k = (c == 0.0) ? 1.0 : (c / sin(c));
if (0 != c)
k_bar = c / sin(c);
/* using small angle approximation (formula in comment is without aproximation) */
*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
}
__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double x_descaled = x / scale;
double y_descaled = y / scale;
double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
float x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
float y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
double lat_sphere = 0;
double lat_rad;
double lon_rad;
if (c != 0)
lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
else
lat_sphere = asin(cos_c * sin_phi_1);
// printf("lat_sphere = %.10f\n",lat_sphere);
double lon_sphere = 0;
if (phi_1 == M_PI / 2) {
//using small angle approximation (formula in comment is without aproximation)
lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
} else if (phi_1 == -M_PI / 2) {
//using small angle approximation (formula in comment is without aproximation)
lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
if (c != 0.0) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
//using small angle approximation
// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
// if(denominator != 0)
// {
// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
// }
// else
// {
// ...
// }
lat_rad = ref->lat;
lon_rad = ref->lon;
}
// printf("lon_sphere = %.10f\n",lon_sphere);
*lat = lat_sphere * 180.0 / M_PI;
*lon = lon_sphere * 180.0 / M_PI;
*lat = lat_rad * 180.0 / M_PI;
*lon = lon_rad * 180.0 / M_PI;
}
@@ -197,7 +129,6 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
@@ -208,7 +139,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
return theta;
}
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -223,7 +154,7 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
}
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -249,7 +180,7 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
{
// This function returns the distance to the nearest point on the track line. Distance is positive if current
// position is right of the track and negative if left of the track as seen from a point on the track line
@@ -266,7 +197,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value;
if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -297,8 +228,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
}
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep)
__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep)
{
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
@@ -317,29 +248,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; }
if (arc_sweep >= 0) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; }
} else {
bearing_sector_end = arc_start_bearing;
bearing_sector_start = arc_start_bearing - arc_sweep;
if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F;
if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; }
}
in_sector = false;
// Case where sector does not span zero
if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; }
// Case where sector does span zero
if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; }
// If in the sector then calculate distance and bearing to closest point
if (in_sector) {
@@ -395,8 +326,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
}
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z)
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z)
{
double current_x_rad = lat_next / 180.0 * M_PI;
double current_y_rad = lon_next / 180.0 * M_PI;
@@ -420,8 +351,8 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z)
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z)
{
float dx = x_now - x_next;
float dy = y_now - y_next;
@@ -441,17 +372,21 @@ __EXPORT float _wrap_pi(float bearing)
}
int c = 0;
while (bearing > M_PI_F) {
while (bearing >= M_PI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing <= -M_PI_F) {
while (bearing < -M_PI_F) {
bearing += M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
@@ -465,17 +400,21 @@ __EXPORT float _wrap_2pi(float bearing)
}
int c = 0;
while (bearing > M_TWOPI_F) {
while (bearing >= M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing <= 0.0f) {
while (bearing < 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
@@ -489,17 +428,21 @@ __EXPORT float _wrap_180(float bearing)
}
int c = 0;
while (bearing > 180.0f) {
while (bearing >= 180.0f) {
bearing -= 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing <= -180.0f) {
while (bearing < -180.0f) {
bearing += 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
@@ -513,17 +456,21 @@ __EXPORT float _wrap_360(float bearing)
}
int c = 0;
while (bearing > 360.0f) {
while (bearing >= 360.0f) {
bearing -= 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing <= 0.0f) {
while (bearing < 0.0f) {
bearing += 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
src/lib/geo/geo.h
+24 -16
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012, 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
@@ -42,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/
@@ -52,6 +50,8 @@
__BEGIN_DECLS
#include "geo/geo_mag_declination.h"
#include <stdbool.h>
#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */
@@ -67,6 +67,14 @@ struct crosstrack_error_s {
float bearing; // Bearing in radians to closest point on line/arc
} ;
/* lat/lon are in radians */
struct map_projection_reference_s {
double lat;
double lon;
double sin_lat;
double cos_lat;
};
/**
* Initializes the map transformation.
*
@@ -74,7 +82,7 @@ struct crosstrack_error_s {
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_init(double lat_0, double lon_0);
__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
@@ -83,7 +91,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0);
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
@@ -93,7 +101,7 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
/**
* Returns the distance to the next waypoint in meters.
@@ -115,30 +123,30 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
*/
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep);
__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep);
/*
* Calculate distance in global frame
*/
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z);
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z);
/*
* Calculate distance in local frame (NED)
*/
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z);
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z);
__EXPORT float _wrap_180(float bearing);
__EXPORT float _wrap_360(float bearing);
src/lib/geo/geo_mag_declination.c
+136
View File
@@ -0,0 +1,136 @@
/****************************************************************************
*
* Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.c
*
* Calculation / lookup table for earth magnetic field declination.
*
* Lookup table from Scott Ferguson <scottfromscott@gmail.com>
*
* XXX Lookup table currently too coarse in resolution (only full degrees)
* and lat/lon res - needs extension medium term.
*
*/
#include <geo/geo.h>
/** set this always to the sampling in degrees for the table below */
#define SAMPLING_RES 10.0f
#define SAMPLING_MIN_LAT -60.0f
#define SAMPLING_MAX_LAT 60.0f
#define SAMPLING_MIN_LON -180.0f
#define SAMPLING_MAX_LON 180.0f
static const int8_t declination_table[13][37] = \
{
46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \
-66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \
-3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \
29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \
-40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \
8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \
10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \
-10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \
-14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \
9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \
7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \
0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \
0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \
0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \
-16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \
12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \
0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \
13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3
};
static float get_lookup_table_val(unsigned lat, unsigned lon);
__EXPORT float get_mag_declination(float lat, float lon)
{
/*
* If the values exceed valid ranges, return zero as default
* as we have no way of knowing what the closest real value
* would be.
*/
if (lat < -90.0f || lat > 90.0f ||
lon < -180.0f || lon > 180.0f) {
return 0.0f;
}
/* round down to nearest sampling resolution */
int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
/* for the rare case of hitting the bounds exactly
* the rounding logic wouldn't fit, so enforce it.
*/
/* limit to table bounds - required for maxima even when table spans full globe range */
if (lat <= SAMPLING_MIN_LAT) {
min_lat = SAMPLING_MIN_LAT;
}
if (lat >= SAMPLING_MAX_LAT) {
min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
}
if (lon <= SAMPLING_MIN_LON) {
min_lon = SAMPLING_MIN_LON;
}
if (lon >= SAMPLING_MAX_LON) {
min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
}
/* find index of nearest low sampling point */
unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES;
unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
/* perform bilinear interpolation on the four grid corners */
float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
}
float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
{
return declination_table[lat_index][lon_index];
}
src/modules/controllib/uorb/UOrbPublication.cpp → src/lib/geo/geo_mag_declination.h
+14 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
* 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
* 3. Neither the name MAVGEO nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -32,8 +32,16 @@
****************************************************************************/
/**
* @file UOrbPublication.cpp
*
*/
* @file geo_mag_declination.h
*
* Calculation / lookup table for earth magnetic field declination.
*
*/
#include "UOrbPublication.hpp"
#pragma once
__BEGIN_DECLS
__EXPORT float get_mag_declination(float lat, float lon);
__END_DECLS
src/lib/geo/module.mk
+2 -1
View File
@@ -35,4 +35,5 @@
# Geo library
#
SRCS = geo.c
SRCS = geo.c \
geo_mag_declination.c
src/lib/launchdetection/CatapultLaunchMethod.cpp
+15 -13
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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
@@ -10,9 +10,9 @@
* 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 documentation4 and/or other materials provided with the
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*
@@ -33,20 +33,25 @@
/**
* @file CatapultLaunchMethod.cpp
* Catpult Launch detection
* Catapult Launch detection
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Authors and acknowledgements in header.
*/
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
CatapultLaunchMethod::CatapultLaunchMethod() :
namespace launchdetection
{
CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
SuperBlock(parent, "CAT"),
last_timestamp(hrt_absolute_time()),
integrator(0.0f),
launchDetected(false),
threshold_accel(NULL, "LAUN_CAT_A", false),
threshold_time(NULL, "LAUN_CAT_T", false)
threshold_accel(this, "A"),
threshold_time(this, "T")
{
}
@@ -83,14 +88,11 @@ bool CatapultLaunchMethod::getLaunchDetected()
return launchDetected;
}
void CatapultLaunchMethod::updateParams()
{
threshold_accel.update();
threshold_time.update();
}
void CatapultLaunchMethod::reset()
{
integrator = 0.0f;
launchDetected = false;
}
}
src/lib/launchdetection/CatapultLaunchMethod.h
+11 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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
@@ -10,9 +10,9 @@
* 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 documentation4 and/or other materials provided with the
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*
@@ -44,17 +44,20 @@
#include "LaunchMethod.h"
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
class CatapultLaunchMethod : public LaunchMethod
namespace launchdetection
{
class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
{
public:
CatapultLaunchMethod();
CatapultLaunchMethod(SuperBlock *parent);
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
void updateParams();
void reset();
private:
@@ -68,3 +71,5 @@ private:
};
#endif /* CATAPULTLAUNCHMETHOD_H_ */
}
src/lib/launchdetection/LaunchDetector.cpp
+10 -15
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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
@@ -12,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*
@@ -30,24 +30,27 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file launchDetection.cpp
* Auto Detection for different launch methods (e.g. catapult)
*
* Authors and acknowledgements in header.
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "LaunchDetector.h"
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
namespace launchdetection
{
LaunchDetector::LaunchDetector() :
launchdetection_on(NULL, "LAUN_ALL_ON", false),
throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
SuperBlock(NULL, "LAUN"),
launchdetection_on(this, "ALL_ON"),
throttlePreTakeoff(this, "THR_PRE")
{
/* init all detectors */
launchMethods[0] = new CatapultLaunchMethod();
launchMethods[0] = new CatapultLaunchMethod(this);
/* update all parameters of all detectors */
@@ -87,12 +90,4 @@ bool LaunchDetector::getLaunchDetected()
return false;
}
void LaunchDetector::updateParams() {
launchdetection_on.update();
throttlePreTakeoff.update();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->updateParams();
}
}
src/lib/launchdetection/LaunchDetector.h
+9 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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
@@ -10,9 +10,9 @@
* 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 documentation4 and/or other materials provided with the
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*
@@ -45,10 +45,13 @@
#include <stdint.h>
#include "LaunchMethod.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
class __EXPORT LaunchDetector
namespace launchdetection
{
class __EXPORT LaunchDetector : public control::SuperBlock
{
public:
LaunchDetector();
@@ -57,7 +60,6 @@ public:
void update(float accel_x);
bool getLaunchDetected();
void updateParams();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -72,5 +74,6 @@ private:
};
}
#endif // LAUNCHDETECTOR_H
src/lib/launchdetection/LaunchMethod.h
+9 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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
@@ -10,9 +10,9 @@
* 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 documentation4 and/or other materials provided with the
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*
@@ -41,15 +41,20 @@
#ifndef LAUNCHMETHOD_H_
#define LAUNCHMETHOD_H_
namespace launchdetection
{
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
virtual void updateParams() = 0;
virtual void reset() = 0;
protected:
private:
};
}
#endif /* LAUNCHMETHOD_H_ */
src/lib/launchdetection/launchdetection_params.c
+2 -3
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* 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
@@ -18,7 +17,7 @@
* 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
* "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,
src/lib/launchdetection/module.mk
+1 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
# Copyright (c) 2012, 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
src/lib/mathlib/CMSIS/Include/arm_math.h
+1 -1
View File
@@ -5193,7 +5193,7 @@ void arm_rfft_fast_f32(
*pIa = Ialpha;
/* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
*pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
*pIb = (float32_t)-0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
}
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+6 -1
View File
@@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {
// don't allow bad values to propogate via the filter
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
return output;
}
float LowPassFilter2p::reset(float sample) {
_delay_element_1 = _delay_element_2 = sample;
return apply(sample);
}
} // namespace math
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+16 -4
View File
@@ -52,18 +52,30 @@ public:
_delay_element_1 = _delay_element_2 = 0;
}
// change parameters
/**
* Change filter parameters
*/
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
// apply - Add a new raw value to the filter
// and retrieve the filtered result
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
float apply(float sample);
// return the cutoff frequency
/**
* Return the cutoff frequency
*/
float get_cutoff_freq(void) const {
return _cutoff_freq;
}
/**
* Reset the filter state to this value
*/
float reset(float sample);
private:
float _cutoff_freq;
float _a1;
src/lib/version/version.h
+4
View File
@@ -59,4 +59,8 @@
#define HW_ARCH "PX4FMU_V2"
#endif
#ifdef CONFIG_ARCH_BOARD_AEROCORE
#define HW_ARCH "AEROCORE"
#endif
#endif /* VERSION_H_ */
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
-815
View File
@@ -1,815 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 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 KalmanNav.cpp
*
* Kalman filter navigation code
*/
#include <poll.h>
#include "KalmanNav.hpp"
#include <systemlib/err.h>
#include <geo/geo.h>
// constants
// Titterton pg. 52
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
static const float R0 = 6378137.0f; // earth radius, m
static const float g0 = 9.806f; // standard gravitational accel. m/s^2
static const int8_t ret_ok = 0; // no error in function
static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
_predictTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
// miss counts
_miss(0),
// accelerations
fN(0), fE(0), fD(0),
// state
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
lat0(0), lon0(0), alt0(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
_rMag(this, "R_MAG"),
_rGpsVel(this, "R_GPS_VEL"),
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rPressAlt(this, "R_PRESS_ALT"),
_rAccel(this, "R_ACCEL"),
_magDip(this, "ENV_MAG_DIP"),
_magDec(this, "ENV_MAG_DEC"),
_g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_attitudeInitialized(false),
_positionInitialized(false),
_attitudeInitCounter(0)
{
using namespace math;
F.zero();
G.zero();
V.zero();
HAtt.zero();
RAtt.zero();
HPos.zero();
RPos.zero();
// initial state covariance matrix
P0.identity();
P0 *= 0.01f;
P = P0;
// initial state
phi = 0.0f;
theta = 0.0f;
psi = 0.0f;
vN = 0.0f;
vE = 0.0f;
vD = 0.0f;
lat = 0.0f;
lon = 0.0f;
alt = 0.0f;
// initialize rotation quaternion with a single raw sensor measurement
_sensors.update();
q = init(
_sensors.accelerometer_m_s2[0],
_sensors.accelerometer_m_s2[1],
_sensors.accelerometer_m_s2[2],
_sensors.magnetometer_ga[0],
_sensors.magnetometer_ga[1],
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = q.to_dcm();
// HPos is constant
HPos(0, 3) = 1.0f;
HPos(1, 4) = 1.0f;
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(4, 8) = 1.0f;
HPos(5, 8) = 1.0f;
// initialize all parameters
updateParams();
}
math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
float magX, magY;
float initialHdg, cosHeading, sinHeading;
initialRoll = atan2(-ay, -az);
initialPitch = atan2(ax, -az);
cosRoll = cosf(initialRoll);
sinRoll = sinf(initialRoll);
cosPitch = cosf(initialPitch);
sinPitch = sinf(initialPitch);
magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX);
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
cosPitch = cosf(initialPitch * 0.5f);
sinPitch = sinf(initialPitch * 0.5f);
cosHeading = cosf(initialHdg * 0.5f);
sinHeading = sinf(initialHdg * 0.5f);
float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
return math::Quaternion(q0, q1, q2, q3);
}
void KalmanNav::update()
{
using namespace math;
struct pollfd fds[1];
fds[0].fd = _sensors.getHandle();
fds[0].events = POLLIN;
// poll for new data
int ret = poll(fds, 1, 1000);
if (ret < 0) {
// XXX this is seriously bad - should be an emergency
return;
} else if (ret == 0) { // timeout
return;
}
// get new timestamp
uint64_t newTimeStamp = hrt_absolute_time();
// check updated subscriptions
if (_param_update.updated()) updateParams();
bool gpsUpdate = _gps.updated();
bool sensorsUpdate = _sensors.updated();
// get new information from subscriptions
// this clears update flag
updateSubscriptions();
// initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
warnx("initialized EKF attitude");
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
}
// initialize position when gps received
if (!_positionInitialized &&
_attitudeInitialized && // wait for attitude first
gpsUpdate &&
_gps.fix_type > 2
//&& _gps.counter_pos_valid > 10
) {
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
vD = _gps.vel_d_m_s;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// set reference position for
// local position
lat0 = lat;
lon0 = lon;
alt0 = alt;
// XXX map_projection has internal global
// states that multiple things could change,
// should make map_projection take reference
// lat/lon and not have init
map_projection_init(lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
double(vN), double(vE), double(vD),
lat, lon, double(alt));
}
// prediction step
// using sensors timestamp so we can account for packet lag
float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
//printf("dt: %15.10f\n", double(dt));
_predictTimeStamp = _sensors.timestamp;
// don't predict if time greater than a second
if (dt < 1.0f) {
predictState(dt);
predictStateCovariance(dt);
// count fast frames
_navFrames += 1;
}
// count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// gps correction step
if (_positionInitialized && gpsUpdate) {
correctPos();
}
// attitude correction step
if (_attitudeInitialized // initialized
&& sensorsUpdate // new data
&& _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz
) {
_attTimeStamp = _sensors.timestamp;
correctAtt();
}
// publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp;
updatePublications();
}
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
//printf("nav: %4d Hz, miss #: %4d\n",
// _navFrames / 10, _miss / 10);
_navFrames = 0;
_miss = 0;
}
}
void KalmanNav::updatePublications()
{
using namespace math;
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.global_valid = true;
_pos.lat = lat * M_RAD_TO_DEG;
_pos.lon = lon * M_RAD_TO_DEG;
_pos.alt = float(alt);
_pos.vel_n = vN;
_pos.vel_e = vE;
_pos.vel_d = vD;
_pos.yaw = psi;
// local position publication
float x;
float y;
bool landed = alt < (alt0 + 0.1); // XXX improve?
map_projection_project(lat, lon, &x, &y);
_localPos.timestamp = _pubTimeStamp;
_localPos.xy_valid = true;
_localPos.z_valid = true;
_localPos.v_xy_valid = true;
_localPos.v_z_valid = true;
_localPos.x = x;
_localPos.y = y;
_localPos.z = alt0 - alt;
_localPos.vx = vN;
_localPos.vy = vE;
_localPos.vz = vD;
_localPos.yaw = psi;
_localPos.xy_global = true;
_localPos.z_global = true;
_localPos.ref_timestamp = _pubTimeStamp;
_localPos.ref_lat = getLatDegE7();
_localPos.ref_lon = getLonDegE7();
_localPos.ref_alt = 0;
_localPos.landed = landed;
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
_att.pitch = theta;
_att.yaw = psi;
_att.rollspeed = _sensors.gyro_rad_s[0];
_att.pitchspeed = _sensors.gyro_rad_s[1];
_att.yawspeed = _sensors.gyro_rad_s[2];
// TODO, add gyro offsets to filter
_att.rate_offsets[0] = 0.0f;
_att.rate_offsets[1] = 0.0f;
_att.rate_offsets[2] = 0.0f;
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = C_nb(i, j);
for (int i = 0; i < 4; i++) _att.q[i] = q(i);
_att.R_valid = true;
_att.q_valid = true;
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized) {
_pos.update();
_localPos.update();
}
if (_attitudeInitialized)
_att.update();
}
int KalmanNav::predictState(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
// attitude prediction
if (_attitudeInitialized) {
Vector<3> w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.length() - 1.0f) > 1e-4f) {
q.normalize();
}
// C_nb update
C_nb = q.to_dcm();
// euler update
Vector<3> euler = C_nb.to_euler();
phi = euler.data[0];
theta = euler.data[1];
psi = euler.data[2];
// specific acceleration in nav frame
Vector<3> accelB(_sensors.accelerometer_m_s2);
Vector<3> accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
}
// position prediction
if (_positionInitialized) {
// neglects angular deflections in local gravity
// see Titerton pg. 70
float R = R0 + float(alt);
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
// XXX position prediction using speed
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
// rectangular integration
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
lat += double(LDot * dt);
lon += double(lDot * dt);
alt += double(-vD * dt);
}
return ret_ok;
}
int KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSq = cosL * cosL;
float tanL = tanf(lat);
// prepare for matrix
float R = R0 + float(alt);
float RSq = R * R;
// F Matrix
// Titterton pg. 291
F(0, 1) = -(omega * sinL + vE * tanL / R);
F(0, 2) = vN / R;
F(0, 4) = 1.0f / R;
F(0, 6) = -omega * sinL;
F(0, 8) = -vE / RSq;
F(1, 0) = omega * sinL + vE * tanL / R;
F(1, 2) = omega * cosL + vE / R;
F(1, 3) = -1.0f / R;
F(1, 8) = vN / RSq;
F(2, 0) = -vN / R;
F(2, 1) = -omega * cosL - vE / R;
F(2, 4) = -tanL / R;
F(2, 6) = -omega * cosL - vE / (R * cosLSq);
F(2, 8) = vE * tanL / RSq;
F(3, 1) = -fD;
F(3, 2) = fE;
F(3, 3) = vD / R;
F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
F(3, 5) = vN / R;
F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
F(4, 0) = fD;
F(4, 2) = -fN;
F(4, 3) = 2 * omega * sinL + vE * tanL / R;
F(4, 4) = (vN * tanL + vD) / R;
F(4, 5) = 2 * omega * cosL + vE / R;
F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
vN * vE / (R * cosLSq);
F(4, 8) = -vE * (vN * tanL + vD) / RSq;
F(5, 0) = -fE;
F(5, 1) = fN;
F(5, 3) = -2 * vN / R;
F(5, 4) = -2 * (omega * cosL + vE / R);
F(5, 6) = 2 * omega * vE * sinL;
F(5, 8) = (vN * vN + vE * vE) / RSq;
F(6, 3) = 1 / R;
F(6, 8) = -vN / RSq;
F(7, 4) = 1 / (R * cosL);
F(7, 6) = vE * tanL / (R * cosL);
F(7, 8) = -vE / (cosL * RSq);
F(8, 5) = -1;
// G Matrix
// Titterton pg. 291
G(0, 0) = -C_nb(0, 0);
G(0, 1) = -C_nb(0, 1);
G(0, 2) = -C_nb(0, 2);
G(1, 0) = -C_nb(1, 0);
G(1, 1) = -C_nb(1, 1);
G(1, 2) = -C_nb(1, 2);
G(2, 0) = -C_nb(2, 0);
G(2, 1) = -C_nb(2, 1);
G(2, 2) = -C_nb(2, 2);
G(3, 3) = C_nb(0, 0);
G(3, 4) = C_nb(0, 1);
G(3, 5) = C_nb(0, 2);
G(4, 3) = C_nb(1, 0);
G(4, 4) = C_nb(1, 1);
G(4, 5) = C_nb(1, 2);
G(5, 3) = C_nb(2, 0);
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous prediction equations
// for discrete time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
return ret_ok;
}
int KalmanNav::correctAtt()
{
using namespace math;
// trig
float cosPhi = cosf(phi);
float cosTheta = cosf(theta);
// float cosPsi = cosf(psi);
float sinPhi = sinf(phi);
float sinTheta = sinf(theta);
// float sinPsi = sinf(psi);
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
// compensate roll and pitch, but not yaw
// XXX take the vectors out of the C_nb matrix to avoid singularities
math::Matrix<3,3> C_rp;
C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
// mag measurement
Vector<3> magBody(_sensors.magnetometer_ga);
// transform to earth frame
Vector<3> magNav = C_rp * magBody;
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
if (yMag > M_PI_F) yMag -= 2*M_PI_F;
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
// accel measurement
Vector<3> zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.length();
zAccel.normalize();
// ignore accel correction when accel mag not close to g
Matrix<4,4> RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
if (ignoreAccel) {
RAttAdjust(1, 1) = 1.0e10;
RAttAdjust(2, 2) = 1.0e10;
RAttAdjust(3, 3) = 1.0e10;
} else {
//printf("correcting attitude with accel\n");
}
// accel predicted measurement
Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
// calculate residual
Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
// HMag
HAtt(0, 2) = 1;
// HAccel
HAtt(1, 1) = cosTheta;
HAtt(2, 0) = -cosPhi * cosTheta;
HAtt(2, 1) = sinPhi * sinTheta;
HAtt(3, 0) = sinPhi * cosTheta;
HAtt(3, 1) = cosPhi * sinTheta;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
warnx("numerical failure in att correction");
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
if (!ignoreAccel) {
phi += xCorrect(PHI);
theta += xCorrect(THETA);
}
psi += xCorrect(PSI);
// attitude also affects nav velocities
if (_positionInitialized) {
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
}
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HAtt * P;
// fault detection
float beta = y * (S.inversed() * y);
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:"); y.print();
}
// update quaternions from euler
// angle correction
q.from_euler(phi, theta, psi);
return ret_ok;
}
int KalmanNav::correctPos()
{
using namespace math;
// residual
Vector<6> y;
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
y(4) = _gps.alt / 1.0e3f - alt;
y(5) = _sensors.baro_alt_meter - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
Matrix<9,6> K = P * HPos.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (!isfinite(val)) {
// abort correction and return
warnx("numerical failure in gps correction");
// fallback to GPS
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
vD = _gps.vel_d_m_s;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
alt += xCorrect(ALT);
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HPos * P;
// fault detetcion
float beta = y * (S.inversed() * y);
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
warnx("fault in gps: beta = %8.4f", (double)beta);
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
double(y(3) / sqrtf(RPos(3, 3))),
double(y(4) / sqrtf(RPos(4, 4))),
double(y(5) / sqrtf(RPos(5, 5))));
}
counter++;
return ret_ok;
}
void KalmanNav::updateParams()
{
using namespace math;
using namespace control;
SuperBlock::updateParams();
// gyro noise
V(0, 0) = _vGyro.get(); // gyro x, rad/s
V(1, 1) = _vGyro.get(); // gyro y
V(2, 2) = _vGyro.get(); // gyro z
// accel noise
V(3, 3) = _vAccel.get(); // accel x, m/s^2
V(4, 4) = _vAccel.get(); // accel y
V(5, 5) = _vAccel.get(); // accel z
// magnetometer noise
float noiseMin = 1e-6f;
float noiseMagSq = _rMag.get() * _rMag.get();
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
RAtt(0, 0) = noiseMagSq; // normalized direction
// accelerometer noise
float noiseAccelSq = _rAccel.get() * _rAccel.get();
// bound noise to prevent singularities
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
RAtt(1, 1) = noiseAccelSq; // normalized direction
RAtt(2, 2) = noiseAccelSq;
RAtt(3, 3) = noiseAccelSq;
// gps noise
float R = R0 + float(alt);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
float noiseVel = _rGpsVel.get();
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseGpsAlt = _rGpsAlt.get();
float noisePressAlt = _rPressAlt.get();
// bound noise to prevent singularities
if (noiseVel < noiseMin) noiseVel = noiseMin;
if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
RPos(0, 0) = noiseVel * noiseVel; // vn
RPos(1, 1) = noiseVel * noiseVel; // ve
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
RPos(5, 5) = noisePressAlt * noisePressAlt; // h
// XXX, note that RPos depends on lat, so updateParams should
// be called if lat changes significantly
}
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
-192
View File
@@ -1,192 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 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 KalmanNav.hpp
*
* kalman filter navigation code
*/
#pragma once
//#define MATRIX_ASSERT
//#define VECTOR_ASSERT
#include <nuttx/config.h>
#include <mathlib/mathlib.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <controllib/uorb/UOrbSubscription.hpp>
#include <controllib/uorb/UOrbPublication.hpp>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include <unistd.h>
/**
* Kalman filter navigation class
* http://en.wikipedia.org/wiki/Extended_Kalman_filter
* Discrete-time extended Kalman filter
*/
class KalmanNav : public control::SuperBlock
{
public:
/**
* Constructor
*/
KalmanNav(SuperBlock *parent, const char *name);
/**
* Deconstuctor
*/
virtual ~KalmanNav() {};
math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
/**
* The main callback function for the class
*/
void update();
/**
* Publication update
*/
virtual void updatePublications();
/**
* State prediction
* Continuous, non-linear
*/
int predictState(float dt);
/**
* State covariance prediction
* Continuous, linear
*/
int predictStateCovariance(float dt);
/**
* Attitude correction
*/
int correctAtt();
/**
* Position correction
*/
int correctPos();
/**
* Overloaded update parameters
*/
virtual void updateParams();
protected:
// kalman filter
math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */
math::Matrix<9,9> P; /**< state covariance matrix */
math::Matrix<9,9> P0; /**< initial state covariance matrix */
math::Matrix<6,6> V; /**< gyro/ accel noise matrix */
math::Matrix<4,9> HAtt; /**< attitude measurement matrix */
math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */
math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */
math::Matrix<6,6> RPos; /**< position measurement noise matrix */
// attitude
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _miss; /**< number of times fast prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon; /**< lat, lon radians */
// parameters
float alt; /**< altitude, meters */
double lat0, lon0; /**< reference latitude and longitude */
float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */
control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */
control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */
control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */
control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */
control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */
control::BlockParamFloat _magDip; /**< magnetic inclination with level */
control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */
control::BlockParamFloat _g; /**< gravitational constant */
control::BlockParamFloat _faultPos; /**< fault detection threshold for position */
control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */
// status
bool _attitudeInitialized;
bool _positionInitialized;
uint16_t _attitudeInitCounter;
// accessors
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
};
src/modules/att_pos_estimator_ekf/kalman_main.cpp
-157
View File
@@ -1,157 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: James Goppert
*
* 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 kalman_main.cpp
* Combined attitude / position estimator.
*
* @author James Goppert
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "KalmanNav.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int daemon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int kalman_demo_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int att_pos_estimator_ekf_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 30,
8192,
kalman_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running\n");
exit(0);
} else {
warnx("not started\n");
exit(1);
}
}
usage("unrecognized command");
exit(1);
}
int kalman_demo_thread_main(int argc, char *argv[])
{
warnx("starting");
using namespace math;
thread_running = true;
KalmanNav nav(NULL, "KF");
while (!thread_should_exit) {
nav.update();
}
warnx("exiting.");
thread_running = false;
return 0;
}
src/modules/att_pos_estimator_ekf/params.c
-49
View File
@@ -1,49 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 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.
*
****************************************************************************/
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f);
PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+15 -15
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2012-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
@@ -34,9 +32,12 @@
****************************************************************************/
/*
* @file attitude_estimator_ekf_main.c
* @file attitude_estimator_ekf_main.cpp
*
* Extended Kalman Filter for Attitude Estimation.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@@ -111,7 +112,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
{
@@ -277,7 +278,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// XXX write this out to perf regs
/* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_ekf_params ekf_params;
@@ -380,9 +380,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) {
if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
@@ -392,11 +391,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
hrt_abstime vel_t = 0;
@@ -410,7 +408,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
vel(2) = gps.vel_d_m_s;
}
} else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
} else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;
@@ -445,11 +443,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
z_k[6] = raw.magnetometer_ga[0];
@@ -477,6 +474,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+2
View File
@@ -40,6 +40,7 @@
*/
#include "attitude_estimator_ekf_params.h"
#include <math.h>
/* Extended Kalman Filter covariances */
@@ -113,6 +114,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->yaw_off, &(p->yaw_off));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI / 180.0f;
param_get(h->acc_comp, &(p->acc_comp));
src/modules/attitude_estimator_ekf/module.mk
+2
View File
@@ -50,3 +50,5 @@ SRCS = attitude_estimator_ekf_main.cpp \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/cross.c
MODULE_STACKSIZE = 1200
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+10 -13
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
* 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
@@ -36,6 +34,9 @@
/*
* @file attitude_estimator_so3_main.cpp
*
* @author Hyon Lim <limhyon@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
@@ -131,7 +132,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int attitude_estimator_so3_main(int argc, char *argv[])
{
@@ -445,7 +446,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
// XXX write this out to perf regs
/* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_so3_params so3_comp_params;
@@ -526,9 +526,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) {
if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
@@ -538,11 +537,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
acc[0] = raw.accelerometer_m_s2[0];
@@ -550,11 +548,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
acc[2] = raw.accelerometer_m_s2[2];
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
mag[0] = raw.magnetometer_ga[0];
src/modules/attitude_estimator_so3/module.mk
+2
View File
@@ -6,3 +6,5 @@ MODULE_COMMAND = attitude_estimator_so3
SRCS = attitude_estimator_so3_main.cpp \
attitude_estimator_so3_params.c
MODULE_STACKSIZE = 1200
src/modules/commander/accelerometer_calibration.cpp
+40 -26
View File
@@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd)
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3,3> board_rotation;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
@@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
}
}
if (old_done_count != done_count)
if (old_done_count != done_count) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
}
if (done)
if (done) {
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
@@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f)
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i])
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
@@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 0; // [ g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 0; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 1; // [ -g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 1; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 2; // [ 0, g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 2; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 3; // [ 0, -g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 3; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
return 4; // [ 0, 0, g ]
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return 4; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
return 5; // [ 0, 0, -g ]
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return 5; // [ 0, 0, -g ]
}
mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
@@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
struct sensor_combined_s sensor;
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
for (int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++) {
accel_sum[i] += sensor.accelerometer_m_s2[i];
}
count++;
@@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
continue;
}
if (errcount > samples_num / 10)
if (errcount > samples_num / 10) {
return ERROR;
}
}
for (int i = 0; i < 3; i++) {
@@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0f)
return ERROR; // Singular matrix
if (det == 0.0f) {
return ERROR; // Singular matrix
}
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
@@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
/* calculate inverse matrix for A */
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK)
if (mat_invert3(mat_A, mat_A_inv) != OK) {
return ERROR;
}
/* copy results to accel_T */
for (int i = 0; i < 3; i++) {
src/modules/commander/airspeed_calibration.cpp
+13 -7
View File
@@ -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
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
const int calibration_count = 2500;
const int calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@@ -82,16 +82,21 @@ int do_airspeed_calibration(int mavlink_fd)
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
warn("FAILED to set scale / offsets for airspeed");
mavlink_log_critical(mavlink_fd, "dpress reset failed");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
@@ -107,11 +112,12 @@ int do_airspeed_calibration(int mavlink_fd)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
src/modules/commander/commander.cpp
+371 -289
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_helper.cpp
+20 -18
View File
@@ -113,17 +113,22 @@ void buzzer_deinit()
close(buzzer);
}
void set_tune(int tune) {
void set_tune(int tune)
{
unsigned int new_tune_duration = tune_durations[tune];
/* don't interrupt currently playing non-repeating tune by repeating */
if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
/* allow interrupting current non-repeating tune by the same tune */
if (tune != tune_current || new_tune_duration != 0) {
ioctl(buzzer, TONE_SET_ALARM, tune);
}
tune_current = tune;
if (new_tune_duration != 0) {
tune_end = hrt_absolute_time() + new_tune_duration;
} else {
tune_end = 0;
}
@@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
}
@@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_WHITE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
@@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
}
@@ -198,16 +206,10 @@ int led_init()
return ERROR;
}
/* the blue LED is only available on FMUv1 but not FMUv2 */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (ioctl(leds, LED_ON, LED_BLUE)) {
warnx("Blue LED: ioctl fail\n");
return ERROR;
}
#endif
/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)ioctl(leds, LED_ON, LED_BLUE);
/* we consider the amber led mandatory */
if (ioctl(leds, LED_ON, LED_AMBER)) {
warnx("Amber LED: ioctl fail\n");
return ERROR;
@@ -217,11 +219,7 @@ int led_init()
rgbleds = open(RGBLED_DEVICE_PATH, 0);
if (rgbleds == -1) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
#else
warnx("No RGB LED found");
#endif
warnx("No RGB LED found at " RGBLED_DEVICE_PATH);
}
return 0;
@@ -254,22 +252,25 @@ int led_off(int led)
void rgbled_set_color(rgbled_color_t color)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
}
}
void rgbled_set_mode(rgbled_mode_t mode)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
}
}
void rgbled_set_pattern(rgbled_pattern_t *pattern)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
@@ -309,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;
src/modules/commander/commander_tests/commander_tests.cpp
+1 -2
View File
@@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
int commander_tests_main(int argc, char *argv[])
{
state_machine_helper_test();
//state_machine_test();
stateMachineHelperTest();
return 0;
}
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+273 -98
View File
@@ -49,13 +49,12 @@ public:
StateMachineHelperTest();
virtual ~StateMachineHelperTest();
virtual const char* run_tests();
virtual void runTests(void);
private:
const char* arming_state_transition_test();
const char* arming_state_transition_arm_disarm_test();
const char* main_state_transition_test();
const char* is_safe_test();
bool armingStateTransitionTest();
bool mainStateTransitionTest();
bool isSafeTest();
};
StateMachineHelperTest::StateMachineHelperTest() {
@@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() {
StateMachineHelperTest::~StateMachineHelperTest() {
}
const char*
StateMachineHelperTest::arming_state_transition_test()
bool StateMachineHelperTest::armingStateTransitionTest(void)
{
// These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
// to simulate machine state prior to testing an arming state transition. This structure is also
// use to represent the expected machine state after the transition has been requested.
typedef struct {
arming_state_t arming_state; // vehicle_status_s.arming_state
bool armed; // actuator_armed_s.armed
bool ready_to_arm; // actuator_armed_s.ready_to_arm
} ArmingTransitionVolatileState_t;
// This structure represents a test case for arming_state_transition. It contains the machine
// state prior to transtion, the requested state to transition to and finally the expected
// machine state after transition.
typedef struct {
const char* assertMsg; // Text to show when test case fails
ArmingTransitionVolatileState_t current_state; // Machine state prior to transtion
hil_state_t hil_state; // Current vehicle_status_s.hil_state
bool condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized
bool safety_switch_available; // Current safety_s.safety_switch_available
bool safety_off; // Current safety_s.safety_off
arming_state_t requested_state; // Requested arming state to transition to
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
transition_result_t expected_transition_result; // Expected result from arming_state_transition
} ArmingTransitionTest_t;
// We use these defines so that our test cases are more readable
#define ATT_ARMED true
#define ATT_DISARMED false
#define ATT_READY_TO_ARM true
#define ATT_NOT_READY_TO_ARM false
#define ATT_SENSORS_INITIALIZED true
#define ATT_SENSORS_NOT_INITIALIZED false
#define ATT_SAFETY_AVAILABLE true
#define ATT_SAFETY_NOT_AVAILABLE true
#define ATT_SAFETY_OFF true
#define ATT_SAFETY_ON false
// These are test cases for arming_state_transition
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{ "no transition: identical states",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
// TRANSITION_CHANGED tests
// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
{ "transition: init to standby",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to standby error",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to reboot",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to init",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to standby error",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to reboot",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to standby",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to armed error",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED_ERROR,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed error to standby error",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby error to reboot",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to armed",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to reboot",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// hil on tests, standby error to standby not normally allowed
{ "transition: standby error to standby, hil on",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// Safety switch arming tests
{ "transition: init to standby, no safety switch",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to standby, safety switch off",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// standby error
{ "transition: armed error to standby error requested standby",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// TRANSITION_DENIED tests
// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
{ "no transition: init to armed",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby to armed error",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED_ERROR,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to init",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to reboot",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to armed",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to reboot",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to armed",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to standby",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: reboot to armed",
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: in air restore to standby",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Sensor tests
{ "no transition: init to standby - sensors not initialized",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Safety switch arming tests
{ "no transition: init to standby, safety switch on",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
};
struct vehicle_status_s status;
struct safety_s safety;
arming_state_t new_arming_state;
struct safety_s safety;
struct actuator_armed_s armed;
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i=0; i<cArmingTransitionTests; i++) {
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
// Setup initial machine state
status.arming_state = test->current_state.arming_state;
status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
status.hil_state = test->hil_state;
safety.safety_switch_available = test->safety_switch_available;
safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
ut_assert(test->assertMsg, armed.armed == test->expected_state.armed);
ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm);
}
// Identical states.
status.arming_state = ARMING_STATE_INIT;
new_arming_state = ARMING_STATE_INIT;
mu_assert("no transition: identical states",
TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
// INIT to STANDBY.
armed.armed = false;
armed.ready_to_arm = false;
status.arming_state = ARMING_STATE_INIT;
status.condition_system_sensors_initialized = true;
new_arming_state = ARMING_STATE_STANDBY;
mu_assert("transition: init to standby",
TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
mu_assert("not armed", !armed.armed);
mu_assert("ready to arm", armed.ready_to_arm);
// INIT to STANDBY, sensors not initialized.
armed.armed = false;
armed.ready_to_arm = false;
status.arming_state = ARMING_STATE_INIT;
status.condition_system_sensors_initialized = false;
new_arming_state = ARMING_STATE_STANDBY;
mu_assert("no transition: sensors not initialized",
TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
mu_assert("not armed", !armed.armed);
mu_assert("not ready to arm", !armed.ready_to_arm);
return 0;
return true;
}
const char*
StateMachineHelperTest::arming_state_transition_arm_disarm_test()
{
struct vehicle_status_s status;
struct safety_s safety;
arming_state_t new_arming_state;
struct actuator_armed_s armed;
// TODO(sjwilks): ARM then DISARM.
return 0;
}
const char*
StateMachineHelperTest::main_state_transition_test()
bool StateMachineHelperTest::mainStateTransitionTest(void)
{
struct vehicle_status_s current_state;
main_state_t new_main_state;
@@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test()
// Identical states.
current_state.main_state = MAIN_STATE_MANUAL;
new_main_state = MAIN_STATE_MANUAL;
mu_assert("no transition: identical states",
ut_assert("no transition: identical states",
TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// AUTO to MANUAL.
current_state.main_state = MAIN_STATE_AUTO;
new_main_state = MAIN_STATE_MANUAL;
mu_assert("transition changed: auto to manual",
ut_assert("transition changed: auto to manual",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to SEATBELT.
// MANUAL to ALTCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = true;
new_main_state = MAIN_STATE_SEATBELT;
mu_assert("tranisition: manual to seatbelt",
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("tranisition: manual to altctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
// MANUAL to SEATBELT, invalid local altitude.
// MANUAL to ALTCTRL, invalid local altitude.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = false;
new_main_state = MAIN_STATE_SEATBELT;
mu_assert("no transition: invalid local altitude",
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("no transition: invalid local altitude",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to EASY.
// MANUAL to POSCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = true;
new_main_state = MAIN_STATE_EASY;
mu_assert("transition: manual to easy",
new_main_state = MAIN_STATE_POSCTL;
ut_assert("transition: manual to posctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
// MANUAL to EASY, invalid local position.
// MANUAL to POSCTRL, invalid local position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = false;
new_main_state = MAIN_STATE_EASY;
mu_assert("no transition: invalid position",
new_main_state = MAIN_STATE_POSCTL;
ut_assert("no transition: invalid position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to AUTO.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_global_position_valid = true;
new_main_state = MAIN_STATE_AUTO;
mu_assert("transition: manual to auto",
ut_assert("transition: manual to auto",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
// MANUAL to AUTO, invalid global position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_global_position_valid = false;
new_main_state = MAIN_STATE_AUTO;
mu_assert("no transition: invalid global position",
ut_assert("no transition: invalid global position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
return 0;
return true;
}
const char*
StateMachineHelperTest::is_safe_test()
bool StateMachineHelperTest::isSafeTest(void)
{
struct vehicle_status_s current_state;
struct safety_s safety;
@@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test()
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
ut_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
armed.armed = false;
armed.lockdown = true;
safety.safety_switch_available = true;
safety.safety_off = true;
mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
ut_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = true;
mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
ut_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
ut_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = false;
safety.safety_off = false;
mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
ut_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
return 0;
return true;
}
const char*
StateMachineHelperTest::run_tests()
void StateMachineHelperTest::runTests(void)
{
mu_run_test(arming_state_transition_test);
mu_run_test(arming_state_transition_arm_disarm_test);
mu_run_test(main_state_transition_test);
mu_run_test(is_safe_test);
return 0;
ut_run_test(armingStateTransitionTest);
ut_run_test(mainStateTransitionTest);
ut_run_test(isSafeTest);
}
void
state_machine_helper_test()
void stateMachineHelperTest(void)
{
StateMachineHelperTest* test = new StateMachineHelperTest();
test->UnitTest::print_results(test->run_tests());
test->runTests();
test->printResults();
}
src/modules/commander/commander_tests/state_machine_helper_test.h
+1 -1
View File
@@ -39,6 +39,6 @@
#ifndef STATE_MACHINE_HELPER_TEST_H_
#define STATE_MACHINE_HELPER_TEST_
void state_machine_helper_test();
void stateMachineHelperTest(void);
#endif /* STATE_MACHINE_HELPER_TEST_H_ */
src/modules/commander/gyro_calibration.cpp
+10 -8
View File
@@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd)
gyro_scale.z_offset += gyro_report.z;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else {
poll_errcount++;
@@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd)
/* apply new offsets */
fd = open(GYRO_DEVICE_PATH, 0);
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
warn("WARNING: failed to apply new offsets for gyro");
}
close(fd);
@@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd)
float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; }
if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; }
uint64_t last_time = hrt_absolute_time();
@@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd)
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2 * M_PI_F;
if (mag > M_PI_F) { mag -= 2 * M_PI_F; }
if (mag < -M_PI_F) mag += 2 * M_PI_F;
if (mag < -M_PI_F) { mag += 2 * M_PI_F; }
float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2 * M_PI_F;
if (diff > M_PI_F) { diff -= 2 * M_PI_F; }
if (diff < -M_PI_F) diff += 2 * M_PI_F;
if (diff < -M_PI_F) { diff += 2 * M_PI_F; }
baseline_integral += diff;
mag_last = mag;
src/modules/commander/mag_calibration.cpp
+36 -11
View File
@@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd)
uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 500 values */
const unsigned int calibration_maxcount = 500;
const unsigned int calibration_maxcount = 240;
unsigned int calibration_counter;
struct mag_scale mscale_null = {
@@ -121,9 +121,24 @@ int do_mag_calibration(int mavlink_fd)
if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
/* clean up */
if (x != NULL) {
free(x);
}
if (y != NULL) {
free(y);
}
if (z != NULL) {
free(z);
}
res = ERROR;
return res;
}
} else {
/* exit */
return ERROR;
@@ -163,8 +178,9 @@ int do_mag_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_maxcount / 20) == 0)
if (calibration_counter % (calibration_maxcount / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
}
} else {
poll_errcount++;
@@ -198,14 +214,17 @@ int do_mag_calibration(int mavlink_fd)
}
}
if (x != NULL)
if (x != NULL) {
free(x);
}
if (y != NULL)
if (y != NULL) {
free(y);
}
if (z != NULL)
if (z != NULL) {
free(z);
}
if (res == OK) {
/* apply calibration and set parameters */
@@ -234,23 +253,29 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
res = ERROR;
}
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
src/modules/commander/module.mk
+4
View File
@@ -47,3 +47,7 @@ SRCS = commander.cpp \
baro_calibration.cpp \
rc_calibration.cpp \
airspeed_calibration.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/modules/commander/px4_custom_mode.h
+4 -2
View File
@@ -8,10 +8,12 @@
#ifndef PX4_CUSTOM_MODE_H_
#define PX4_CUSTOM_MODE_H_
#include <stdint.h>
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_SEATBELT,
PX4_CUSTOM_MAIN_MODE_EASY,
PX4_CUSTOM_MAIN_MODE_ALTCTL,
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO,
};
src/modules/commander/rc_calibration.cpp
+3 -3
View File
@@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd)
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
float p = sp.y;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
p = sp.x;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
p = sp.r;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
src/modules/commander/state_machine_helper.cpp
+129 -99
View File
@@ -44,6 +44,7 @@
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -68,10 +69,44 @@ static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool failsafe_state_changed = true;
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
static const char *state_names[ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
"ARMING_STATE_ARMED_ERROR",
"ARMING_STATE_STANDBY_ERROR",
"ARMING_STATE_REBOOT",
"ARMING_STATE_IN_AIR_RESTORE",
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
/*
* Perform an atomic state update
*/
@@ -84,7 +119,6 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
ret = TRANSITION_NOT_CHANGED;
} else {
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
@@ -93,95 +127,43 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
armed->lockdown = false;
}
switch (new_arming_state) {
case ARMING_STATE_INIT:
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
/* allow going back from INIT for calibration */
if (status->arming_state == ARMING_STATE_STANDBY) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
// Fail transition if we need safety switch press
// Allow if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
}
break;
case ARMING_STATE_STANDBY:
/* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED
|| status->hil_state == HIL_STATE_ON) {
/* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = true;
valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
}
break;
case ARMING_STATE_ARMED:
/* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = true;
}
break;
case ARMING_STATE_ARMED_ERROR:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_ARMED) {
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_STANDBY_ERROR:
/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
if (status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_REBOOT:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
/* XXX implement */
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
valid_transition = false;
}
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
@@ -190,8 +172,15 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* end of atomic state update */
irqrestore(flags);
if (ret == TRANSITION_DENIED)
warnx("arming transition rejected");
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "Invalid arming transition from %s to %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
return ret;
}
@@ -237,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
if (!status->is_rotary_wing ||
@@ -248,7 +237,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case MAIN_STATE_EASY:
case MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
@@ -313,10 +302,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
bool valid_transition = false;
int ret = ERROR;
warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
if (current_status->hil_state == new_state) {
warnx("Hil state not changed");
valid_transition = true;
} else {
@@ -344,23 +330,67 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/* list directory */
DIR *d;
struct dirent *direntry;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
while ((direntry = readdir(d)) != NULL) {
int sensfd = ::open(direntry->d_name, 0);
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
/* skip serial ports */
if (!strncmp("tty", direntry->d_name, 3)) {
continue;
}
/* skip mtd devices */
if (!strncmp("mtd", direntry->d_name, 3)) {
continue;
}
/* skip ram devices */
if (!strncmp("ram", direntry->d_name, 3)) {
continue;
}
/* skip MMC devices */
if (!strncmp("mmc", direntry->d_name, 3)) {
continue;
}
/* skip mavlink */
if (!strcmp("mavlink", direntry->d_name)) {
continue;
}
/* skip console */
if (!strcmp("console", direntry->d_name)) {
continue;
}
/* skip null */
if (!strcmp("null", direntry->d_name)) {
continue;
}
snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
int sensfd = ::open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
close(sensfd);
printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
warnx("directory listing ok (FS mounted and readable)");
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -57,7 +57,7 @@ typedef enum {
} transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed);
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
src/modules/controllib/block/Block.cpp
+5 -4
View File
@@ -41,10 +41,11 @@
#include <string.h>
#include <stdio.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include "Block.hpp"
#include "BlockParam.hpp"
#include "../uorb/UOrbSubscription.hpp"
#include "../uorb/UOrbPublication.hpp"
namespace control
{
@@ -100,7 +101,7 @@ void Block::updateParams()
void Block::updateSubscriptions()
{
UOrbSubscriptionBase *sub = getSubscriptions().getHead();
uORB::SubscriptionBase *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
@@ -118,7 +119,7 @@ void Block::updateSubscriptions()
void Block::updatePublications()
{
UOrbPublicationBase *pub = getPublications().getHead();
uORB::PublicationBase *pub = getPublications().getHead();
int count = 0;
while (pub != NULL) {

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