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

Merge branch 'master' into offboard2

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-16 12:12:43 +02:00
parent d2553bfd29 f892a7278a
commit 8c4b35cc23
511 changed files with 23010 additions and 11290 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/modules/sdlog/sdlog_ringbuffer.c → src/drivers/boards/aerocore/aerocore_led.c
+70 -40
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 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
@@ -33,59 +32,90 @@
****************************************************************************/
/**
* @file sdlog_log.c
* MAVLink text logging.
* @file aerocore_led.c
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* AeroCore LED backend.
*/
#include <string.h>
#include <stdlib.h>
#include <nuttx/config.h>
#include "sdlog_ringbuffer.h"
#include <stdbool.h>
void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size)
#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()
{
lb->size = size;
lb->start = 0;
lb->count = 0;
lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector));
stm32_configgpio(GPIO_LED0);
stm32_configgpio(GPIO_LED1);
}
int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb)
__EXPORT void led_on(int led)
{
return lb->count == (int)lb->size;
}
switch (led) {
case 0:
stm32_gpiowrite(GPIO_LED0, true);
break;
int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb)
{
return lb->count == 0;
}
case 1:
stm32_gpiowrite(GPIO_LED1, true);
break;
// XXX make these functions thread-safe
void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem)
{
int end = (lb->start + lb->count) % lb->size;
memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector));
if (sdlog_logbuffer_is_full(lb)) {
lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
} else {
++lb->count;
default:
warnx("LED ID not recognized\n");
}
}
int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem)
__EXPORT void led_off(int led)
{
if (!sdlog_logbuffer_is_empty(lb)) {
memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector));
lb->start = (lb->start + 1) % lb->size;
--lb->count;
return 0;
switch (led) {
case 0:
stm32_gpiowrite(GPIO_LED0, false);
break;
} else {
return 1;
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/modules/fixedwing_att_control/fixedwing_att_control_att.h → src/drivers/boards/aerocore/aerocore_pwm_servo.c
+80 -14
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,19 +31,87 @@
*
****************************************************************************/
/* @file Fixed Wing Attitude Control */
/*
* @file aerocore_pwm_servo.c
*
* Configuration data for the stm32 pwm_servo driver.
*
* Note that these arrays must always be fully-sized.
*/
#ifndef FIXEDWING_ATT_CONTROL_ATT_H_
#define FIXEDWING_ATT_CONTROL_ATT_H_
#include <stdint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp);
#include <drivers/stm32/drv_pwm_servo.h>
#include <drivers/drv_pwm_output.h>
#endif /* FIXEDWING_ATT_CONTROL_ATT_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/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_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
+15 -4
View File
@@ -176,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;
@@ -204,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;
}
@@ -235,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;
src/drivers/frsky_telemetry/frsky_data.c
+1 -1
View File
@@ -225,7 +225,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/gps/gps.cpp
+13 -4
View File
@@ -63,6 +63,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <board_config.h>
#include "ubx.h"
#include "mtk.h"
@@ -276,14 +278,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;
@@ -421,7 +423,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/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/meas_airspeed/meas_airspeed.cpp
+125 -16
View File
@@ -87,6 +87,7 @@
#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>
@@ -121,6 +122,14 @@ protected:
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;
};
/*
@@ -129,10 +138,11 @@ 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),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
_t_system_power(-1)
{
memset(&system_power, 0, sizeof(system_power));
}
int
@@ -194,19 +204,48 @@ 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;
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
if (diff_press_pa < 0.0f) {
diff_press_pa = 0.0f;
}
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). */
@@ -219,6 +258,13 @@ MEASAirspeed::collect()
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;
@@ -242,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;
}
@@ -272,10 +322,13 @@ 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;
@@ -287,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.
*/
@@ -409,7 +518,7 @@ test()
}
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)) {
@@ -437,7 +546,7 @@ test()
}
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);
}
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/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
+94 -16
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,23 @@ 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 > 0) {
param_get(failsafe_param, &failsafe_param_val);
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");
}
}
}
}
@@ -1400,7 +1458,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];
/*
@@ -1408,8 +1466,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)
@@ -1421,23 +1477,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);
@@ -1445,8 +1516,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;
}
@@ -1479,10 +1552,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 */
@@ -2113,6 +2187,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:
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/sf0x/sf0x.cpp
+91 -48
View File
@@ -124,6 +124,8 @@ private:
orb_advert_t _range_finder_topic;
unsigned _consecutive_fail_count;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
@@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) :
_linebuf_index(0),
_last_read(0),
_range_finder_topic(-1),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
@@ -276,34 +279,11 @@ SF0X::init()
warnx("advert err");
}
/* attempt to get a measurement 5 times */
while (ret != OK && i < 5) {
if (measure()) {
ret = ERROR;
_sensor_ok = false;
}
usleep(100000);
if (collect()) {
ret = ERROR;
_sensor_ok = false;
} else {
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
}
i++;
}
/* close the fd */
::close(_fd);
_fd = -1;
out:
return ret;
return OK;
}
int
@@ -376,6 +356,7 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
@@ -544,10 +525,16 @@ SF0X::collect()
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
_linebuf_index = 0;
} else if (_linebuf_index > 0) {
/* increment to next read position */
_linebuf_index++;
}
/* the buffer for read chars is buflen minus null termination */
unsigned readlen = sizeof(_linebuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
if (ret < 0) {
_linebuf[sizeof(_linebuf) - 1] = '\0';
@@ -562,19 +549,30 @@ SF0X::collect()
} else {
return -EAGAIN;
}
} else if (ret == 0) {
return -EAGAIN;
}
_linebuf_index += ret;
if (_linebuf_index >= sizeof(_linebuf)) {
_linebuf_index = 0;
}
/* we did increment the index to the next position already, so just add the additional fields */
_linebuf_index += (ret - 1);
_last_read = hrt_absolute_time();
if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
/* incomplete read, reschedule ourselves */
if (_linebuf_index < 1) {
/* we need at least the two end bytes to make sense of this string */
return -EAGAIN;
} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
if (_linebuf_index >= readlen - 1) {
/* we have a full buffer, but no line ending - abort */
_linebuf_index = 0;
perf_count(_comms_errors);
return -ENOMEM;
} else {
/* incomplete read, reschedule ourselves */
return -EAGAIN;
}
}
char *end;
@@ -582,22 +580,56 @@ SF0X::collect()
bool valid;
/* enforce line ending */
_linebuf[sizeof(_linebuf) - 1] = '\0';
unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
_linebuf[lend] = '\0';
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
si_units = -1.0f;
valid = false;
} else {
si_units = strtod(_linebuf, &end);
valid = true;
/* we need to find a dot in the string, as we're missing the meters part else */
valid = false;
/* wipe out partially read content from last cycle(s), check for dot */
for (int i = 0; i < (lend - 2); i++) {
if (_linebuf[i] == '\n') {
char buf[sizeof(_linebuf)];
memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
memcpy(_linebuf, buf, (lend + 1) - (i + 1));
}
if (_linebuf[i] == '.') {
valid = true;
}
}
if (valid) {
si_units = strtod(_linebuf, &end);
/* we require at least 3 characters for a valid number */
if (end > _linebuf + 3) {
valid = true;
} else {
si_units = -1.0f;
valid = false;
}
}
}
debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));
/* done with this chunk, resetting */
/* done with this chunk, resetting - even if invalid */
_linebuf_index = 0;
/* if its invalid, there is no reason to forward the value */
if (!valid) {
perf_count(_comms_errors);
return -EINVAL;
}
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
@@ -689,10 +721,19 @@ SF0X::cycle()
}
if (OK != collect_ret) {
log("collection error");
/* we know the sensor needs about four seconds to initialize */
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
log("collection error #%u", _consecutive_fail_count);
}
_consecutive_fail_count++;
/* restart the measurement state machine */
start();
return;
} else {
/* apparently success */
_consecutive_fail_count = 0;
}
/* next phase is measurement */
@@ -848,10 +889,10 @@ test()
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("val: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
/* start the sensor polling at 2 Hz rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
@@ -866,24 +907,26 @@ test()
int ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
warnx("timed out");
break;
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
break;
}
warnx("periodic read %u", i);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("read #%u", i);
warnx("val: %0.3f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
}
/* reset the sensor polling to default rate */
/* reset the sensor polling to the default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
errx(1, "ERR: DEF RATE");
}
errx(0, "PASS");
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/jetdrive_control/jetdrive_control.h
-207
View File
@@ -1,207 +0,0 @@
#ifndef JETDRIVE_CONTROL_H
#define JETDRIVE_CONTROL_H
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
/**
* Multicopter attitude control app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int jetdrive_control_main(int argc, char *argv[]);
#define MIN_TAKEOFF_THROTTLE 0.3f
#define YAW_DEADZONE 0.05f
#define RATES_I_LIMIT 0.5f
class JetdriveControl
{
public:
/**
* Constructor
*/
JetdriveControl();
/**
* Destructor, also kills the sensors task.
*/
~JetdriveControl();
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
int _v_att_sub; /**< vehicle attitude subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< parameter updates subscription */
int _manual_control_sp_sub; /**< manual control setpoint subscription */
int _armed_sub; /**< arming status subscription */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator controls */
struct actuator_armed_s _armed; /**< actuator arming status */
perf_counter_t _loop_perf; /**< loop performance counter */
math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */
math::Matrix<3, 3> _R; /**< rotation matrix for current state */
math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_sp; /**< angular rates setpoint */
math::Vector<3> _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
math::Vector<3> _att_control; /**< attitude control vector */
math::Matrix<3, 3> I; /**< identity matrix */
bool _reset_yaw_sp; /**< reset yaw setpoint flag */
struct {
param_t roll_p;
param_t roll_rate_p;
param_t roll_rate_i;
param_t roll_rate_d;
param_t pitch_p;
param_t pitch_rate_p;
param_t pitch_rate_i;
param_t pitch_rate_d;
param_t yaw_p;
param_t yaw_rate_p;
param_t yaw_rate_i;
param_t yaw_rate_d;
param_t yaw_ff;
param_t rc_scale_yaw;
} _params_handles; /**< handles for interesting parameters */
struct {
math::Vector<3> att_p; /**< P gain for angular error */
math::Vector<3> rate_p; /**< P gain for angular rate error */
math::Vector<3> rate_i; /**< I gain for angular rate error */
math::Vector<3> rate_d; /**< D gain for angular rate error */
float yaw_ff; /**< yaw control feed-forward */
float rc_scale_yaw;
} _params;
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Check for parameter update and handle it.
*/
void parameter_update_poll();
/**
* Check for changes in vehicle control mode.
*/
void vehicle_control_mode_poll();
/**
* Check for changes in manual inputs.
*/
void vehicle_manual_poll();
/**
* Check for attitude setpoint updates.
*/
void vehicle_attitude_setpoint_poll();
/**
* Check for rates setpoint updates.
*/
void vehicle_rates_setpoint_poll();
/**
* Check for arming status updates.
*/
void arming_status_poll();
/**
* Attitude controller.
*/
void control_attitude(float dt);
/**
* Attitude rates controller.
*/
void control_attitude_rates(float dt);
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
};
#ifndef jetdrive_control
#define jetdrive_control
namespace jetdrive_control
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
JetdriveControl *g_control;
}
#endif // jetdrive_control
#endif // JETDRIVE_CONTROL_H
src/examples/jetdrive_control/jetdrive_control_main.cpp
-707
View File
@@ -1,707 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* 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 mc_att_control_main.c
* Multicopter attitude controller.
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
* so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
* These two approaches fused seamlessly with weight depending on angular error.
* When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include "jetdrive_control.h"
JetdriveControl::JetdriveControl() :
_task_should_exit(false),
_control_task(-1),
/* subscriptions */
_v_att_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sp_sub(-1),
_armed_sub(-1),
/* publications */
_att_sp_pub(-1),
_v_rates_sp_pub(-1),
_actuators_0_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control"))
{
memset(&_v_att, 0, sizeof(_v_att));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
memset(&_v_control_mode, 0, sizeof(_v_control_mode));
memset(&_armed, 0, sizeof(_armed));
_params.att_p.zero();
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_d.zero();
_R_sp.identity();
_R.identity();
_rates_prev.zero();
_rates_sp.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
I.identity();
_params_handles.roll_p = param_find("JC_ROLL_P");
_params_handles.roll_rate_p = param_find("JC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("JC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("JC_ROLLRATE_D");
_params_handles.pitch_p = param_find("JC_PITCH_P");
_params_handles.pitch_rate_p = param_find("JC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("JC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("JC_PITCHRATE_D");
_params_handles.yaw_p = param_find("JC_YAW_P");
_params_handles.yaw_rate_p = param_find("JC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("JC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("JC_YAWRATE_D");
_params_handles.yaw_ff = param_find("JC_YAW_FF");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
/* fetch initial parameter values */
parameters_update();
}
JetdriveControl::~JetdriveControl()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
jetdrive_control::g_control = nullptr;
}
int
JetdriveControl::parameters_update()
{
float v;
/* roll */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v;
param_get(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
/* pitch */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v;
param_get(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v;
param_get(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
/* yaw */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
param_get(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
return OK;
}
void
JetdriveControl::parameter_update_poll()
{
bool updated;
/* Check HIL state if vehicle status has changed */
orb_check(_params_sub, &updated);
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
parameters_update();
}
}
void
JetdriveControl::vehicle_control_mode_poll()
{
bool updated;
/* Check HIL state if vehicle status has changed */
orb_check(_v_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
}
}
void
JetdriveControl::vehicle_manual_poll()
{
bool updated;
/* get pilots inputs */
orb_check(_manual_control_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
}
}
void
JetdriveControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}
void
JetdriveControl::vehicle_rates_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_rates_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
}
}
void
JetdriveControl::arming_status_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
}
/*
* Attitude controller.
* Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
*/
void
JetdriveControl::control_attitude(float dt)
{
float yaw_sp_move_rate = 0.0f;
bool publish_att_sp = false;
if (_v_control_mode.flag_control_manual_enabled) {
/* manual input, set or modify attitude setpoint */
if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
vehicle_attitude_setpoint_poll();
}
if (!_v_control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude stabilized mode */
_v_att_sp.thrust = _manual_control_sp.throttle;
publish_att_sp = true;
}
if (!_armed.armed) {
/* reset yaw setpoint when disarmed */
_reset_yaw_sp = true;
}
/* move yaw setpoint in all modes */
if (_v_att_sp.thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
if (_manual_control_sp.yaw > 0.0f) {
yaw_sp_move_rate -= YAW_DEADZONE;
} else {
yaw_sp_move_rate += YAW_DEADZONE;
}
yaw_sp_move_rate *= _params.rc_scale_yaw;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
}
/* reset yaw setpint to current position if needed */
if (_reset_yaw_sp) {
_reset_yaw_sp = false;
_v_att_sp.yaw_body = _v_att.yaw;
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
if (!_v_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_v_att_sp.roll_body = _manual_control_sp.roll;
_v_att_sp.pitch_body = _manual_control_sp.pitch;
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
} else {
/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
vehicle_attitude_setpoint_poll();
/* reset yaw setpoint after non-manual control mode */
_reset_yaw_sp = true;
}
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
if (_v_att_sp.R_valid) {
/* rotation matrix in _att_sp is valid, use it */
_R_sp.set(&_v_att_sp.R_body[0][0]);
} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
_R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
/* copy rotation matrix back to setpoint struct */
memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body));
_v_att_sp.R_valid = true;
}
/* publish the attitude setpoint if needed */
if (publish_att_sp) {
_v_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
}
}
/* rotation matrix for current state */
_R.set(_v_att.R);
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2));
math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = _R_sp(2, 2) * _R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = _R;
}
/* R_rp and _R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q;
q.from_dcm(_R.transposed() * _R_sp);
math::Vector<3> e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* feed forward yaw setpoint rate */
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
JetdriveControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
// _att_control will be loaded to controls
//
// _att_control = (JC_XXXRATE_P * rates_err) + (JC_XXXRATE_D * (_rates_prev - rate) / dt) + _rates_int;
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
_rates_prev = rates;
/* update integral only if not saturated on low limit */
if (_thrust_sp > 0.1f) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}
}
}
}
void
JetdriveControl::task_main_trampoline(int argc, char *argv[])
{
jetdrive_control::g_control->task_main();
}
void
JetdriveControl::task_main()
{
warnx("started");
fflush(stdout);
/*
* do subscriptions
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */
orb_set_interval(_v_att_sub, 5);
/* initialize parameters cache */
parameters_update();
/* wakeup source: vehicle attitude */
struct pollfd fds[1];
fds[0].fd = _v_att_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
perf_begin(_loop_perf);
/* run controller on attitude changes */
if (fds[0].revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* check for updates in other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
if(!_v_control_mode.flag_control_offboard_enabled)
{
/* manual/direct control */
_actuators.control[0] = _manual_control_sp .roll;
_actuators.control[1] = _manual_control_sp.pitch;
_actuators.control[2] = _manual_control_sp.yaw;
_actuators.control[3] = _manual_control_sp.throttle;
_actuators.timestamp = hrt_absolute_time();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
} else {
// FIX this: This is the atitude control logic for mc's
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
} else {
/* attitude controller disabled */
// TODO poll 'attitude_rates_setpoint' topic
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
// _rates_sp.zero();
// _thrust_sp = 0.0f;
}
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
/* publish actuator controls */
_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
}
}
}
perf_end(_loop_perf);
}
warnx("exit");
_control_task = -1;
_exit(0);
}
int
JetdriveControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = task_spawn_cmd("jetdrive_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
(main_t)&JetdriveControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int jetdrive_control_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: jetdrive_control {start|stop|status}");
if (!strcmp(argv[1], "start")) {
if (jetdrive_control::g_control != nullptr)
errx(1, "already running");
jetdrive_control::g_control = new JetdriveControl;
if (jetdrive_control::g_control == nullptr)
errx(1, "alloc failed");
if (OK != jetdrive_control::g_control->start()) {
delete jetdrive_control::g_control;
jetdrive_control::g_control = nullptr;
err(1, "start failed");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (jetdrive_control::g_control == nullptr)
errx(1, "not running");
delete jetdrive_control::g_control;
jetdrive_control::g_control = nullptr;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (jetdrive_control::g_control) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
return 1;
}
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/lib/geo/geo.c
+73 -126
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;
@@ -443,15 +374,19 @@ __EXPORT float _wrap_pi(float bearing)
int c = 0;
while (bearing >= M_PI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < -M_PI_F) {
bearing += M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
@@ -467,15 +402,19 @@ __EXPORT float _wrap_2pi(float bearing)
int c = 0;
while (bearing >= M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
@@ -491,15 +430,19 @@ __EXPORT float _wrap_180(float bearing)
int c = 0;
while (bearing >= 180.0f) {
bearing -= 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < -180.0f) {
bearing += 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
@@ -515,15 +458,19 @@ __EXPORT float _wrap_360(float bearing)
int c = 0;
while (bearing >= 360.0f) {
bearing -= 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
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/fixedwing_att_control/fixedwing_att_control_rate.h → src/lib/geo/geo_mag_declination.h
+12 -13
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* 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
@@ -14,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.
*
@@ -33,16 +31,17 @@
*
****************************************************************************/
/* @file Fixed Wing Attitude Rate Control */
/**
* @file geo_mag_declination.h
*
* Calculation / lookup table for earth magnetic field declination.
*
*/
#ifndef FIXEDWING_ATT_CONTROL_RATE_H_
#define FIXEDWING_ATT_CONTROL_RATE_H_
#pragma once
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
__BEGIN_DECLS
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators);
__EXPORT float get_mag_declination(float lat, float lon);
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
__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
+6 -5
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,9 +33,10 @@
/**
* @file CatapultLaunchMethod.cpp
* Catpult Launch detection
* Catapult Launch detection
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Authors and acknowledgements in header.
*/
#include "CatapultLaunchMethod.h"
src/lib/launchdetection/CatapultLaunchMethod.h
+3 -3
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.
*
src/lib/launchdetection/LaunchDetector.cpp
+3 -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
@@ -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,12 +30,11 @@
* 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"
src/lib/launchdetection/LaunchDetector.h
+3 -3
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.
*
src/lib/launchdetection/LaunchMethod.h
+3 -3
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.
*
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 <uORB/Subscription.hpp>
#include <uORB/Publication.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
uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */
uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */
uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */
uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */
uORB::Publication<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/module.mk
-42
View File
@@ -1,42 +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.
#
############################################################################
#
# Full attitude / position Extended Kalman Filter
#
MODULE_COMMAND = att_pos_estimator_ekf
SRCS = kalman_main.cpp \
KalmanNav.cpp \
params.c
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
+7 -6
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[])
{
@@ -407,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;
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
+5 -4
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[])
{
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
+383 -323
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
+2 -2
View File
@@ -12,8 +12,8 @@
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_OFFBOARD,
};
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
+86 -91
View File
@@ -69,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
*/
@@ -85,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;
@@ -94,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;
}
@@ -191,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;
}
@@ -234,7 +222,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 ||
@@ -245,7 +233,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 ||
@@ -266,7 +254,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
case MAIN_STATE_OFFBOARD:
/* need global position estimate */
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
}
@@ -351,6 +339,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
@@ -362,26 +351,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
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;
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/dataman/dataman.c
+77 -27
View File
@@ -1,10 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Jean Cyr
* Lorenz Meier
* Julian Oes
* Thomas Gubler
* 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
@@ -37,6 +33,11 @@
/**
* @file dataman.c
* DATAMANAGER driver.
*
* @author Jean Cyr
* @author Lorenz Meier
* @author Julian Oes
* @author Thomas Gubler
*/
#include <nuttx/config.h>
@@ -44,7 +45,9 @@
#include <stdlib.h>
#include <fcntl.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <queue.h>
#include <string.h>
#include "dataman.h"
@@ -60,7 +63,7 @@ __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t
__EXPORT int dm_clear(dm_item_t item);
__EXPORT int dm_restart(dm_reset_reason restart_type);
/* Types of function calls supported by the worker task */
/** Types of function calls supported by the worker task */
typedef enum {
dm_write_func = 0,
dm_read_func,
@@ -69,11 +72,12 @@ typedef enum {
dm_number_of_funcs
} dm_function_t;
/* Work task work item */
/** Work task work item */
typedef struct {
sq_entry_t link; /**< list linkage */
sem_t wait_sem;
dm_function_t func;
unsigned char first;
unsigned char func;
ssize_t result;
union {
struct {
@@ -98,6 +102,8 @@ typedef struct {
};
} work_q_item_t;
const size_t k_work_item_allocation_chunk_size = 8;
/* Usage statistics */
static unsigned g_func_counts[dm_number_of_funcs];
@@ -175,9 +181,23 @@ create_work_item(void)
unlock_queue(&g_free_q);
/* If we there weren't any free items then obtain memory for a new one */
if (item == NULL)
item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
/* If we there weren't any free items then obtain memory for a new ones */
if (item == NULL) {
item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t));
if (item) {
item->first = 1;
lock_queue(&g_free_q);
for (int i = 1; i < k_work_item_allocation_chunk_size; i++) {
(item + i)->first = 0;
sq_addfirst(&(item + i)->link, &(g_free_q.q));
}
/* Update the queue size and potentially the maximum queue size */
g_free_q.size += k_work_item_allocation_chunk_size - 1;
if (g_free_q.size > g_free_q.max_size)
g_free_q.max_size = g_free_q.size;
unlock_queue(&g_free_q);
}
}
/* If we got one then lock the item*/
if (item)
@@ -409,31 +429,31 @@ _clear(dm_item_t item)
return result;
}
/* Tell the data manager about the type of the last reset */
/** Tell the data manager about the type of the last reset */
static int
_restart(dm_reset_reason reason)
{
unsigned char buffer[2];
int offset, result = 0;
int offset = 0, result = 0;
/* We need to scan the entire file and invalidate and data that should not persist after the last reset */
/* Loop through all of the data segments and delete those that are not persistent */
offset = 0;
while (1) {
size_t len;
/* Get data segment at current offset */
if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
result = -1;
/* must be at eof */
break;
}
len = read(g_task_fd, buffer, sizeof(buffer));
if (len == 0)
if (len != sizeof(buffer)) {
/* must be at eof */
break;
}
/* check if segment contains data */
if (buffer[0]) {
@@ -441,12 +461,12 @@ _restart(dm_reset_reason reason)
/* Whether data gets deleted depends on reset type and data segment's persistence setting */
if (reason == DM_INIT_REASON_POWER_ON) {
if (buffer[1] != DM_PERSIST_POWER_ON_RESET) {
if (buffer[1] > DM_PERSIST_POWER_ON_RESET) {
clear_entry = 1;
}
} else {
if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) {
if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) {
clear_entry = 1;
}
}
@@ -478,7 +498,7 @@ _restart(dm_reset_reason reason)
return result;
}
/* write to the data manager file */
/** Write to the data manager file */
__EXPORT ssize_t
dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
{
@@ -503,7 +523,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
return (ssize_t)enqueue_work_item_and_wait_for_result(work);
}
/* Retrieve from the data manager file */
/** Retrieve from the data manager file */
__EXPORT ssize_t
dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
{
@@ -594,6 +614,20 @@ task_main(int argc, char *argv[])
sem_init(&g_work_queued_sema, 1, 0);
/* See if the data manage file exists and is a multiple of the sector size */
g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
if (g_task_fd >= 0) {
/* File exists, check its size */
int file_size = lseek(g_task_fd, 0, SEEK_END);
if ((file_size % k_sector_size) != 0) {
warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
close(g_task_fd);
unlink(k_data_manager_device_path);
}
else
close(g_task_fd);
}
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
@@ -603,7 +637,7 @@ task_main(int argc, char *argv[])
return -1;
}
if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
warnx("Could not seek data manager file %s", k_data_manager_device_path);
sem_post(&g_init_sema); /* Don't want to hang startup */
@@ -612,6 +646,23 @@ task_main(int argc, char *argv[])
fsync(g_task_fd);
/* see if we need to erase any items based on restart type */
int sys_restart_val;
if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
warnx("Power on restart");
_restart(DM_INIT_REASON_POWER_ON);
}
else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
warnx("In flight restart");
_restart(DM_INIT_REASON_IN_FLIGHT);
}
else
warnx("Unknown restart");
}
else
warnx("Unknown restart");
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
/* worker thread is shutting down but still processing requests */
@@ -684,8 +735,8 @@ task_main(int argc, char *argv[])
for (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
break;
free(work);
if (work->first)
free(work);
}
destroy_q(&g_work_q);
@@ -703,12 +754,12 @@ start(void)
sem_init(&g_init_sema, 1, 0);
/* start the worker thread */
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
/* wait for the thread to actuall initialize */
/* wait for the thread to actually initialize */
sem_wait(&g_init_sema);
sem_destroy(&g_init_sema);
@@ -776,4 +827,3 @@ dataman_main(int argc, char *argv[])
exit(1);
}
src/modules/dataman/dataman.h
+13 -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
@@ -46,7 +46,7 @@
extern "C" {
#endif
/* Types of items that the data manager can store */
/** Types of items that the data manager can store */
typedef enum {
DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */
DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */
@@ -56,7 +56,7 @@ extern "C" {
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
/* The maximum number of instances for each item type */
/** The maximum number of instances for each item type */
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
@@ -65,23 +65,24 @@ extern "C" {
DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
};
/* Data persistence levels */
/** Data persistence levels */
typedef enum {
DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */
DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */
DM_PERSIST_VOLATILE /* Data does not survive resets */
} dm_persitence_t;
/* The reason for the last reset */
/** The reason for the last reset */
typedef enum {
DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */
DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */
DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */
DM_INIT_REASON_VOLATILE /* Data does not survive reset */
} dm_reset_reason;
/* Maximum size in bytes of a single item instance */
#define DM_MAX_DATA_SIZE 126
/** Maximum size in bytes of a single item instance */
#define DM_MAX_DATA_SIZE 124
/* Retrieve from the data manager store */
/** Retrieve from the data manager store */
__EXPORT ssize_t
dm_read(
dm_item_t item, /* The item type to retrieve */
@@ -90,7 +91,7 @@ extern "C" {
size_t buflen /* Length in bytes of data to retrieve */
);
/* write to the data manager store */
/** write to the data manager store */
__EXPORT ssize_t
dm_write(
dm_item_t item, /* The item type to store */
@@ -100,13 +101,13 @@ extern "C" {
size_t buflen /* Length in bytes of data to retrieve */
);
/* Retrieve from the data manager store */
/** Erase all items of this type */
__EXPORT int
dm_clear(
dm_item_t item /* The item type to clear */
);
/* Tell the data manager about the type of the last reset */
/** Tell the data manager about the type of the last reset */
__EXPORT int
dm_restart(
dm_reset_reason restart_type /* The last reset type */
src/modules/dataman/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = dataman
SRCS = dataman.c
MODULE_STACKSIZE = 1200
src/modules/ekf_att_pos_estimator/estimator.cpp
+2609
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf_att_pos_estimator/estimator.h
+352
View File
@@ -0,0 +1,352 @@
#include <math.h>
#include <stdint.h>
#pragma once
#define GRAVITY_MSS 9.80665f
#define deg2rad 0.017453292f
#define rad2deg 57.295780f
#define pi 3.141592657f
#define earthRate 0.000072921f
#define earthRadius 6378145.0f
#define earthRadiusInv 1.5678540e-7f
class Vector3f
{
private:
public:
float x;
float y;
float z;
float length(void) const;
void zero(void);
};
class Mat3f
{
private:
public:
Vector3f x;
Vector3f y;
Vector3f z;
Mat3f();
void identity();
Mat3f transpose(void) const;
};
Vector3f operator*(float sclIn1, Vector3f vecIn1);
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*( Mat3f matIn, Vector3f vecIn);
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*(Vector3f vecIn1, float sclIn1);
void swap_var(float &d1, float &d2);
const unsigned int n_states = 23;
const unsigned int data_buffer_size = 50;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
GPS_FIX_3D = 3
};
struct ekf_status_report {
bool velHealth;
bool posHealth;
bool hgtHealth;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;
float states[n_states];
bool statesNaN;
bool covarianceNaN;
bool kalmanGainsNaN;
};
class AttPosEKF {
public:
AttPosEKF();
~AttPosEKF();
/* ##############################################
*
* M A I N F I L T E R P A R A M E T E R S
*
* ########################################### */
/*
* parameters are defined here and initialised in
* the InitialiseParameters() (which is just 20 lines down)
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float yawVarScale;
float windVelSigma;
float dAngBiasSigma;
float dVelBiasSigma;
float magEarthSigma;
float magBodySigma;
float gndHgtSigma;
float vneSigma;
float vdSigma;
float posNeSigma;
float posDSigma;
float magMeasurementSigma;
float airspeedMeasurementSigma;
float gyroProcessNoise;
float accelProcessNoise;
float EAS2TAS; // ratio f true to equivalent airspeed
void InitialiseParameters()
{
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
EAS2TAS = 1.0f;
yawVarScale = 1.0f;
windVelSigma = 0.1f;
dAngBiasSigma = 5.0e-7f;
dVelBiasSigma = 1e-4f;
magEarthSigma = 3.0e-4f;
magBodySigma = 3.0e-4f;
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
vneSigma = 0.2f;
vdSigma = 0.3f;
posNeSigma = 2.0f;
posDSigma = 2.0f;
magMeasurementSigma = 0.05;
airspeedMeasurementSigma = 1.4f;
gyroProcessNoise = 1.4544411e-2f;
accelProcessNoise = 0.5f;
}
struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
float q0;
float q1;
float q2;
float q3;
float magN;
float magE;
float magD;
float magXbias;
float magYbias;
float magZbias;
float R_MAG;
Mat3f DCM;
} magstate;
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float statesAtRngTime[n_states]; // filter states at the effective measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Mat3f Tbn; // transformation matrix from body to NED coordinates
Mat3f Tnb; // transformation amtrix from NED to body coordinates
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output
float innovRng; ///< Range finder innovation
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
bool refSet; ///< flag to indicate if the reference position has been set
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
// GPS input data variables
float gpsCourse;
float gpsVelD;
double gpsLat;
double gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised;
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool fuseRngData; ///< true when range data is fused
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection;
unsigned storeIndex;
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
void FuseVelposNED();
void FuseMagnetometer();
void FuseAirspeed();
void FuseRangeFinder();
void FuseOpticalFlow();
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
*
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static float sq(float valIn);
void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float max);
void ConstrainVariances();
void ConstrainStates();
void ForceSymmetry();
int CheckAndBound();
void ResetPosition();
void ResetVelocity();
void ZeroVariables();
void GetFilterState(struct ekf_status_report *state);
void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN(struct ekf_status_report *err_report);
void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
};
uint32_t millis();
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+1523
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c
+271
View File
@@ -0,0 +1,271 @@
/****************************************************************************
*
* 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 fw_att_pos_estimator_params.c
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Estimator parameters, accessible via MAVLink
*
*/
/**
* Velocity estimate delay
*
* The delay in milliseconds of the velocity estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
/**
* Position estimate delay
*
* The delay in milliseconds of the position estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
/**
* Height estimate delay
*
* The delay in milliseconds of the height estimate from the barometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
/**
* Mag estimate delay
*
* The delay in milliseconds of the magnetic field estimate from
* the magnetometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
/**
* True airspeeed estimate delay
*
* The delay in milliseconds of the airspeed estimate.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
/**
* GPS vs. barometric altitude update weight
*
* RE-CHECK this.
*
* @min 0.0
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
/**
* Airspeed measurement noise.
*
* Increasing this value will make the filter trust this sensor
* less and trust other sensors more.
*
* @min 0.5
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
/**
* Velocity measurement noise in north-east (horizontal) direction.
*
* Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
/**
* Velocity noise in down (vertical) direction
*
* Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f);
/**
* Position noise in north-east (horizontal) direction
*
* Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
/**
* Position noise in down (vertical) direction
*
* Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f);
/**
* Magnetometer measurement noise
*
* Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
/**
* Gyro process noise
*
* Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
* This noise controls how much the filter trusts the gyro measurements.
* Increasing it makes the filter trust the gyro less and other sensors more.
*
* @min 0.001
* @max 0.05
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
/**
* Accelerometer process noise
*
* Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
* Increasing this value makes the filter trust the accelerometer less
* and other sensors more.
*
* @min 0.05
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f);
/**
* Gyro bias estimate process noise
*
* Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
* Increasing this value will make the gyro bias converge faster but noisier.
*
* @min 0.0000001
* @max 0.00001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
/**
* Accelerometer bias estimate process noise
*
* Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
* Increasing this value makes the bias estimation faster and noisier.
*
* @min 0.0001
* @max 0.001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f);
/**
* Magnetometer earth frame offsets process noise
*
* Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
* Increasing this value makes the magnetometer earth bias estimate converge
* faster but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
/**
* Magnetometer body frame offsets process noise
*
* Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
* Increasing this value makes the magnetometer body bias estimate converge faster
* but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
/**
* Threshold for filter initialization.
*
* If the standard deviation of the GPS position estimate is below this threshold
* in meters, the filter will initialize.
*
* @min 0.3
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
src/examples/jetdrive_control/module.mk → src/modules/ekf_att_pos_estimator/module.mk
+5 -4
View File
@@ -32,10 +32,11 @@
############################################################################
#
# Multirotor attitude controller (vector based, no Euler singularities)
# Main Attitude and Position Estimator for Fixed Wing Aircraft
#
MODULE_COMMAND = jetdrive_control
MODULE_COMMAND = ekf_att_pos_estimator
SRCS = jetdrive_control_main.cpp \
jetdrive_control_params.c
SRCS = fw_att_pos_estimator_main.cpp \
fw_att_pos_estimator_params.c \
estimator.cpp
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
-169
View File
@@ -1,169 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* 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 fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_att.h"
struct fw_att_control_params {
float roll_p;
float rollrate_lim;
float pitch_p;
float pitchrate_lim;
float yawrate_lim;
float pitch_roll_compensation_p;
};
struct fw_pos_control_param_handles {
param_t roll_p;
param_t rollrate_lim;
param_t pitch_p;
param_t pitchrate_lim;
param_t yawrate_lim;
param_t pitch_roll_compensation_p;
};
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p);
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->roll_p = param_find("FW_ROLL_P");
h->rollrate_lim = param_find("FW_ROLLR_LIM");
h->pitch_p = param_find("FW_PITCH_P");
h->pitchrate_lim = param_find("FW_PITCHR_LIM");
h->yawrate_lim = param_find("FW_YAWR_LIM");
h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP");
return OK;
}
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p)
{
param_get(h->roll_p, &(p->roll_p));
param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p));
return OK;
}
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp)
{
static int counter = 0;
static bool initialized = false;
static struct fw_att_control_params p;
static struct fw_pos_control_param_handles h;
static PID_t roll_controller;
static PID_t pitch_controller;
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
/* Pitch (P) */
/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
/* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller,
att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
counter++;
return 0;
}
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
-367
View File
@@ -1,367 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
*
* 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 fixedwing_att_control.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/debug_key_value.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_rate.h"
#include "fixedwing_att_control_att.h"
/* Prototypes */
/**
* Deamon management function.
*/
__EXPORT int fixedwing_att_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_att_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/* Main Thread */
int fixedwing_att_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing att control] started\n");
/* declare and safely initialize all structs */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
/* output structs */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
float speed_body[3] = {0.0f, 0.0f, 0.0f};
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
/* Check if there is a new position measurement or attitude setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool att_sp_updated;
orb_check(att_sp_sub, &att_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (att_sp_updated)
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (att.R_valid) {
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
} else {
speed_body[0] = 0;
speed_body[1] = 0;
speed_body[2] = 0;
printf("FW ATT CONTROL: Did not get a valid R\n");
}
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* set manual setpoints if required */
if (control_mode.flag_control_manual_enabled) {
if (control_mode.flag_control_attitude_enabled) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
/* execute attitude control if requested */
if (control_mode.flag_control_attitude_enabled) {
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
}
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* sanity check and publish actuator outputs */
if (isfinite(actuators.control[0]) &&
isfinite(actuators.control[1]) &&
isfinite(actuators.control[2]) &&
isfinite(actuators.control[3])) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
thread_running = false;
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
close(att_sub);
close(actuator_pub);
close(rates_pub);
fflush(stdout);
exit(0);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
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 fixedwing_att_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_att_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("fixedwing_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_att_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_att_control is running\n");
} else {
printf("\tfixedwing_att_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
-211
View File
@@ -1,211 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
*
* 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 fixedwing_att_control_rate.c
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Implementation of a fixed wing attitude controller.
*
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_rate.h"
/*
* Controller parameters, accessible via MAVLink
*
*/
// Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
//Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
/* feedforward compensation */
PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */
struct fw_rate_control_params {
float rollrate_p;
float rollrate_i;
float rollrate_awu;
float pitchrate_p;
float pitchrate_i;
float pitchrate_awu;
float yawrate_p;
float yawrate_i;
float yawrate_awu;
float pitch_thr_ff;
};
struct fw_rate_control_param_handles {
param_t rollrate_p;
param_t rollrate_i;
param_t rollrate_awu;
param_t pitchrate_p;
param_t pitchrate_i;
param_t pitchrate_awu;
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_awu;
param_t pitch_thr_ff;
};
/* Internal Prototypes */
static int parameters_init(struct fw_rate_control_param_handles *h);
static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p);
static int parameters_init(struct fw_rate_control_param_handles *h)
{
/* PID parameters */
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
h->rollrate_i = param_find("FW_ROLLR_I");
h->rollrate_awu = param_find("FW_ROLLR_AWU");
h->pitchrate_p = param_find("FW_PITCHR_P");
h->pitchrate_i = param_find("FW_PITCHR_I");
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
h->yawrate_p = param_find("FW_YAWR_P");
h->yawrate_i = param_find("FW_YAWR_I");
h->yawrate_awu = param_find("FW_YAWR_AWU");
h->pitch_thr_ff = param_find("FW_PITCH_THR_P");
return OK;
}
static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p)
{
param_get(h->rollrate_p, &(p->rollrate_p));
param_get(h->rollrate_i, &(p->rollrate_i));
param_get(h->rollrate_awu, &(p->rollrate_awu));
param_get(h->pitchrate_p, &(p->pitchrate_p));
param_get(h->pitchrate_i, &(p->pitchrate_i));
param_get(h->pitchrate_awu, &(p->pitchrate_awu));
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
return OK;
}
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators)
{
static int counter = 0;
static bool initialized = false;
static struct fw_rate_control_params p;
static struct fw_rate_control_param_handles h;
static PID_t roll_rate_controller;
static PID_t pitch_rate_controller;
static PID_t yaw_rate_controller;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
}
/* roll rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
/* pitch rate (PI) */
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
/* yaw rate (PI) */
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
counter++;
return 0;
}
src/modules/fixedwing_att_control/module.mk
-42
View File
@@ -1,42 +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.
#
############################################################################
#
# Fixedwing Attitude Control application
#
MODULE_COMMAND = fixedwing_att_control
SRCS = fixedwing_att_control_main.c \
fixedwing_att_control_att.c \
fixedwing_att_control_rate.c
src/modules/fixedwing_backside/fixedwing.cpp
+2 -2
View File
@@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.main_state == MAIN_STATE_SEATBELT ||
_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
} else if (_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
-479
View File
@@ -1,479 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
*
* 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 fixedwing_pos_control.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
/*
* Controller parameters, accessible via MAVLink
*
*/
PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value
PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track
PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */
struct fw_pos_control_params {
float heading_p;
float headingr_p;
float headingr_i;
float headingr_lim;
float xtrack_p;
float altitude_p;
float roll_lim;
float pitch_lim;
};
struct fw_pos_control_param_handles {
param_t heading_p;
param_t headingr_p;
param_t headingr_i;
param_t headingr_lim;
param_t xtrack_p;
param_t altitude_p;
param_t roll_lim;
param_t pitch_lim;
};
struct planned_path_segments_s {
bool segment_type;
double start_lat; // Start of line or center of arc
double start_lon;
double end_lat;
double end_lon;
float radius; // Radius of arc
float arc_start_bearing; // Bearing from center to start of arc
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
};
/* Prototypes */
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
/**
* Deamon management function.
*/
__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_pos_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Parameter management
*/
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->heading_p = param_find("FW_HEAD_P");
h->headingr_p = param_find("FW_HEADR_P");
h->headingr_i = param_find("FW_HEADR_I");
h->headingr_lim = param_find("FW_HEADR_LIM");
h->xtrack_p = param_find("FW_XTRACK_P");
h->altitude_p = param_find("FW_ALT_P");
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_lim = param_find("FW_PITCH_LIM");
return OK;
}
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
{
param_get(h->heading_p, &(p->heading_p));
param_get(h->headingr_p, &(p->headingr_p));
param_get(h->headingr_i, &(p->headingr_i));
param_get(h->headingr_lim, &(p->headingr_lim));
param_get(h->xtrack_p, &(p->xtrack_p));
param_get(h->altitude_p, &(p->altitude_p));
param_get(h->roll_lim, &(p->roll_lim));
param_get(h->pitch_lim, &(p->pitch_lim));
return OK;
}
/* Main Thread */
int fixedwing_pos_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing pos control] started\n");
/* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
struct vehicle_global_position_setpoint_s global_setpoint;
memset(&global_setpoint, 0, sizeof(global_setpoint));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct crosstrack_error_s xtrack_err;
memset(&xtrack_err, 0, sizeof(xtrack_err));
struct parameter_update_s param_update;
memset(&param_update, 0, sizeof(param_update));
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint;
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
/* publish attitude setpoint */
attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f;
attitude_setpoint.thrust = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* subscribe */
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
struct pollfd fds[2] = {
{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }
};
bool global_sp_updated_set_once = false;
float psi_track = 0.0f;
int counter = 0;
struct fw_pos_control_params p;
struct fw_pos_control_param_handles h;
PID_t heading_controller;
PID_t heading_rate_controller;
PID_t offtrack_controller;
PID_t altitude_controller;
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit
pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value
/* error and performance monitoring */
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(fw_err_perf);
} else if (ret == 0) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
}
if (global_sp_updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("next wp direction: %0.4f\n", (double)psi_track);
}
/* Simple Horizontal Control */
if (global_sp_updated_set_once) {
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
// XXX what is xtrack_err.past_end?
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* wrap difference back onto -pi..pi range */
psi_e = _wrap_pi(psi_e);
if (verbose) {
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
printf("delta_psi_c %.4f ", (double)delta_psi_c);
printf("psi_c %.4f ", (double)psi_c);
printf("att.yaw %.4f ", (double)att.yaw);
printf("psi_e %.4f ", (double)psi_e);
}
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
/* limit turn rate */
if (psi_rate_c > p.headingr_lim) {
psi_rate_c = p.headingr_lim;
} else if (psi_rate_c < -p.headingr_lim) {
psi_rate_c = -p.headingr_lim;
}
float psi_rate_e = psi_rate_c - att.yawspeed;
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
if (ground_speed < 10.0f) {
ground_speed = 10.0f;
}
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
if (verbose) {
printf("psi_rate_c %.4f ", (double)psi_rate_c);
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
}
if (verbose && counter % 100 == 0)
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
} else {
if (verbose && counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
}
/* Very simple Altitude Control */
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
// XXX need speed control
attitude_setpoint.thrust = 0.7f;
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
/* measure in what intervals the controller runs */
perf_count(fw_interval_perf);
counter++;
} else {
// XXX no setpoint, decent default needed (loiter?)
}
}
}
}
printf("[fixedwing_pos_control] exiting.\n");
thread_running = false;
close(attitude_setpoint_pub);
fflush(stdout);
exit(0);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
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 fixedwing_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_pos_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("fixedwing_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_pos_control is running\n");
} else {
printf("\tfixedwing_pos_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
src/modules/fixedwing_pos_control/module.mk
-40
View File
@@ -1,40 +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.
#
############################################################################
#
# Fixedwing PositionControl application
#
MODULE_COMMAND = fixedwing_pos_control
SRCS = fixedwing_pos_control_main.c
src/modules/fw_att_control/fw_att_control_main.cpp
+40 -22
View File
@@ -173,6 +173,8 @@ private:
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
float man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
} _parameters; /**< local copies of interesting parameters */
@@ -211,6 +213,8 @@ private:
param_t trim_yaw;
param_t rollsp_offset_deg;
param_t pitchsp_offset_deg;
param_t man_roll_max;
param_t man_pitch_max;
} _parameter_handles; /**< handles for interesting parameters */
@@ -269,7 +273,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace att_control
@@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
/* fetch initial parameter values */
parameters_update();
}
@@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
/* pitch control parameters */
@@ -660,18 +671,24 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is smaller than min, the sensor is not giving good readings */
if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s) ||
/* if airspeed is not updating, we assume the normal average speed */
if (!isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
} else {
airspeed = _airspeed.indicated_airspeed_m_s;
airspeed = _airspeed.true_airspeed_m_s;
}
float airspeed_scaling = _parameters.airspeed_trim / airspeed;
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
@@ -689,20 +706,21 @@ FixedwingAttitudeControl::task_main()
} else {
/*
* Scale down roll and pitch as the setpoints are radians
* and a typical remote can only do 45 degrees, the mapping is
* -1..+1 to -45..+45 degrees or -0.75..+0.75 radians.
* and a typical remote can only do around 45 degrees, the mapping is
* -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
*
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude. If more than 45 degrees are desired,
* a scaling parameter can be applied to the remote.
* the commanded attitude.
*
* The trim gets subtracted here from the manual setpoint to get
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
throttle_sp = _manual.throttle;
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+ _parameters.pitchsp_offset_rad;
throttle_sp = _manual.z;
_actuators.control[4] = _manual.flaps;
/*
@@ -765,7 +783,7 @@ FixedwingAttitudeControl::task_main()
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body);
}
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -774,16 +792,16 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
if (!isfinite(yaw_u)) {
warnx("yaw_u %.4f", yaw_u);
warnx("yaw_u %.4f", (double)yaw_u);
}
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
warnx("throttle_sp %.4f", throttle_sp);
warnx("throttle_sp %.4f", (double)throttle_sp);
}
} else {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
/*
@@ -808,10 +826,10 @@ FixedwingAttitudeControl::task_main()
} else {
/* manual/direct control */
_actuators.control[0] = _manual.roll;
_actuators.control[1] = _manual.pitch;
_actuators.control[2] = _manual.yaw;
_actuators.control[3] = _manual.throttle;
_actuators.control[0] = _manual.y;
_actuators.control[1] = -_manual.x;
_actuators.control[2] = _manual.r;
_actuators.control[3] = _manual.z;
_actuators.control[4] = _manual.flaps;
}

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