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

Merge branch 'master' of https://github.com/PX4/Firmware into fmuv2_bringup

Browse Source
This commit is contained in:
px4dev
2013-07-07 12:22:56 -07:00
parent 9fe257c4d1 cf2dbdf9a1
commit 43f1843cc7
2168 changed files with 13072 additions and 486290 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ardrone_interface/ardrone_interface.c
+1 -1
View File
@@ -114,7 +114,7 @@ int ardrone_interface_main(int argc, char *argv[])
}
thread_should_exit = false;
ardrone_interface_task = task_spawn("ardrone_interface",
ardrone_interface_task = task_spawn_cmd("ardrone_interface",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
2048,
src/drivers/boards/px4fmu/px4fmu_init.c
+1 -1
View File
@@ -58,7 +58,7 @@
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
#include "stm32_internal.h"
#include "stm32.h"
#include "px4fmu_internal.h"
#include "stm32_uart.h"
src/drivers/boards/px4fmu/px4fmu_internal.h
+1 -1
View File
@@ -50,7 +50,7 @@
__BEGIN_DECLS
/* these headers are not C++ safe */
#include <stm32_internal.h>
#include <stm32.h>
/****************************************************************************************************
src/drivers/boards/px4fmu/px4fmu_led.c
+1 -1
View File
@@ -41,7 +41,7 @@
#include <stdbool.h>
#include "stm32_internal.h"
#include "stm32.h"
#include "px4fmu_internal.h"
#include <arch/board/board.h>
src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+1 -1
View File
@@ -46,7 +46,7 @@
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <stm32_internal.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
src/drivers/boards/px4fmu/px4fmu_spi.c
+1 -1
View File
@@ -52,7 +52,7 @@
#include "up_arch.h"
#include "chip.h"
#include "stm32_internal.h"
#include "stm32.h"
#include "px4fmu_internal.h"
/************************************************************************************
src/drivers/boards/px4fmu/px4fmu_usb.c
+1 -1
View File
@@ -52,7 +52,7 @@
#include <nuttx/usb/usbdev_trace.h>
#include "up_arch.h"
#include "stm32_internal.h"
#include "stm32.h"
#include "px4fmu_internal.h"
/************************************************************************************
src/drivers/boards/px4io/px4io_init.c
+1 -1
View File
@@ -54,7 +54,7 @@
#include <nuttx/arch.h>
#include "stm32_internal.h"
#include "stm32.h"
#include "px4io_internal.h"
#include "stm32_uart.h"
src/drivers/boards/px4io/px4io_internal.h
+1 -1
View File
@@ -47,7 +47,7 @@
#include <nuttx/compiler.h>
#include <stdint.h>
#include <stm32_internal.h>
#include <stm32.h>
/************************************************************************************
* Definitions
src/drivers/boards/px4io/px4io_pwm_servo.c
+1 -1
View File
@@ -46,7 +46,7 @@
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <stm32_internal.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
src/drivers/ets_airspeed/ets_airspeed.cpp
+80 -76
View File
@@ -37,7 +37,7 @@
*
* Driver for the Eagle Tree Airspeed V3 connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@@ -77,15 +77,16 @@
#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
/* I2C bus address */
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
/* Register address */
#define READ_CMD 0x07 /* Read the data */
#define READ_CMD 0x07 /* Read the data */
/**
* The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
* The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
* You can set this value to 12 if you want a zero reading below 15km/h.
*/
#define MIN_ACCURATE_DIFF_PRES_PA 12
#define MIN_ACCURATE_DIFF_PRES_PA 0
/* Measurement rate is 100Hz */
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
@@ -105,38 +106,38 @@ class ETSAirspeed : public device::I2C
public:
ETSAirspeed(int bus, int address = I2C_ADDRESS);
virtual ~ETSAirspeed();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_info();
protected:
virtual int probe();
virtual int probe();
private:
work_s _work;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
differential_pressure_s *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _diff_pres_offset;
orb_advert_t _airspeed_pub;
work_s _work;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
differential_pressure_s *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _diff_pres_offset;
orb_advert_t _airspeed_pub;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
@@ -144,28 +145,28 @@ private:
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
@@ -173,9 +174,9 @@ private:
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
static void cycle_trampoline(void *arg);
};
/* helper macro for handling report buffer indices */
@@ -203,7 +204,7 @@ ETSAirspeed::ETSAirspeed(int bus, int address) :
{
// enable debug() calls
_debug_enabled = true;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
@@ -230,6 +231,7 @@ ETSAirspeed::init()
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct differential_pressure_s[_num_reports];
for (unsigned i = 0; i < _num_reports; i++)
_reports[i].max_differential_pressure_pa = 0;
@@ -351,11 +353,11 @@ ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@@ -432,14 +434,14 @@ ETSAirspeed::measure()
uint8_t cmd = READ_CMD;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret)
{
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
@@ -447,30 +449,31 @@ int
ETSAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
log("error reading from sensor: %d", ret);
return ret;
}
uint16_t diff_pres_pa = val[1] << 8 | val[0];
// XXX move the parameter read out of the driver.
param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa -= _diff_pres_offset;
diff_pres_pa -= _diff_pres_offset;
}
// XXX we may want to smooth out the readings to remove noise.
// XXX we may want to smooth out the readings to remove noise.
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].differential_pressure_pa = diff_pres_pa;
@@ -498,7 +501,7 @@ ETSAirspeed::collect()
ret = OK;
perf_end(_sample_perf);
return ret;
}
@@ -511,17 +514,19 @@ ETSAirspeed::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_DIFFPRESSURE};
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);
}
@@ -653,8 +658,7 @@ start(int i2c_bus)
fail:
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
@@ -668,15 +672,14 @@ fail:
void
stop()
{
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
else
{
} else {
errx(1, "driver not running");
}
exit(0);
}
@@ -773,10 +776,10 @@ info()
} // namespace
static void
ets_airspeed_usage()
static void
ets_airspeed_usage()
{
fprintf(stderr, "usage: ets_airspeed [options] command\n");
fprintf(stderr, "usage: ets_airspeed command [options]\n");
fprintf(stderr, "options:\n");
fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT);
fprintf(stderr, "command:\n");
@@ -789,6 +792,7 @@ ets_airspeed_main(int argc, char *argv[])
int i2c_bus = PX4_I2C_BUS_DEFAULT;
int i;
for (i = 1; i < argc; i++) {
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
if (argc > i + 1) {
@@ -802,12 +806,12 @@ ets_airspeed_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "start"))
ets_airspeed::start(i2c_bus);
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
ets_airspeed::stop();
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
ets_airspeed::stop();
/*
* Test the driver/device.
src/drivers/hil/hil.cpp
+1 -1
View File
@@ -224,7 +224,7 @@ HIL::init()
// gpio_reset();
/* start the HIL interface task */
_task = task_spawn("fmuhil",
_task = task_spawn_cmd("fmuhil",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
src/drivers/hott_telemetry/hott_telemetry_main.c
+1 -1
View File
@@ -271,7 +271,7 @@ hott_telemetry_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn(daemon_name,
deamon_task = task_spawn_cmd(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
2048,
src/drivers/hott_telemetry/messages.c
+13
View File
@@ -44,6 +44,7 @@
#include <string.h>
#include <systemlib/geo/geo.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/sensor_combined.h>
@@ -56,6 +57,7 @@ static int battery_sub = -1;
static int gps_sub = -1;
static int home_sub = -1;
static int sensor_sub = -1;
static int airspeed_sub = -1;
static bool home_position_set = false;
static double home_lat = 0.0d;
@@ -68,6 +70,7 @@ messages_init(void)
gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
home_sub = orb_subscribe(ORB_ID(home_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
}
void
@@ -100,6 +103,16 @@ build_eam_response(uint8_t *buffer, size_t *size)
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* get a local copy of the airspeed data */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
src/drivers/md25/md25_main.cpp
+1 -1
View File
@@ -109,7 +109,7 @@ int md25_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("md25",
deamon_task = task_spawn_cmd("md25",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
src/drivers/mkblctrl/mkblctrl.cpp
+1 -1
View File
@@ -313,7 +313,7 @@ MK::init(unsigned motors)
gpio_reset();
/* start the IO interface task */
_task = task_spawn("mkblctrl",
_task = task_spawn_cmd("mkblctrl",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
src/drivers/px4fmu/fmu.cpp
+1 -1
View File
@@ -264,7 +264,7 @@ PX4FMU::init()
gpio_reset();
/* start the IO interface task */
_task = task_spawn("fmuservo",
_task = task_spawn_cmd("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
src/drivers/stm32/adc/adc.cpp
+1 -1
View File
@@ -58,7 +58,7 @@
#include <drivers/drv_adc.h>
#include <arch/stm32/chip.h>
#include <stm32_internal.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <systemlib/err.h>
src/drivers/stm32/drv_hrt.c
+4 -2
View File
@@ -66,7 +66,7 @@
#include "up_internal.h"
#include "up_arch.h"
#include "stm32_internal.h"
#include "stm32.h"
#include "stm32_gpio.h"
#include "stm32_tim.h"
@@ -278,8 +278,10 @@ static void hrt_call_invoke(void);
#ifdef CONFIG_HRT_PPM
/*
* If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
*
* Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
*/
# ifndef GTIM_CCER_CC1NP
# ifdef CONFIG_ARCH_CORTEXM3
# define GTIM_CCER_CC1NP 0
# define GTIM_CCER_CC2NP 0
# define GTIM_CCER_CC3NP 0
src/drivers/stm32/drv_pwm_servo.c
+1 -1
View File
@@ -64,7 +64,7 @@
#include <up_internal.h>
#include <up_arch.h>
#include <stm32_internal.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+1 -1
View File
@@ -111,7 +111,7 @@
#include <up_internal.h>
#include <up_arch.h>
#include <stm32_internal.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
src/examples/fixedwing_control/main.c
+1 -1
View File
@@ -528,7 +528,7 @@ int ex_fixedwing_control_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("ex_fixedwing_control",
deamon_task = task_spawn_cmd("ex_fixedwing_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
src/examples/flow_position_control/flow_position_control_main.c
+589
View File
@@ -0,0 +1,589 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_position_control.c
*
* Optical flow position controller
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <math.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include "flow_position_control_params.h"
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 */
__EXPORT int flow_position_control_main(int argc, char *argv[]);
/**
* Mainloop of position controller.
*/
static int flow_position_control_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);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\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_spawn().
*/
int flow_position_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
printf("flow position control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("flow_position_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
4096,
flow_position_control_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)
printf("\tflow position control app is running\n");
else
printf("\tflow position control app not started\n");
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int
flow_position_control_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf("[flow position control] starting\n");
uint32_t counter = 0;
const float time_scale = powf(10.0f,-6.0f);
/* structures */
struct vehicle_status_s vstatus;
struct vehicle_attitude_s att;
struct manual_control_setpoint_s manual;
struct filtered_bottom_flow_s filtered_flow;
struct vehicle_local_position_s local_pos;
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_advert_t speed_sp_pub;
bool speed_setpoint_adverted = false;
/* parameters init*/
struct flow_position_control_params params;
struct flow_position_control_param_handles param_handles;
parameters_init(&param_handles);
parameters_update(&param_handles, &params);
/* init flow sum setpoint */
float flow_sp_sumx = 0.0f;
float flow_sp_sumy = 0.0f;
/* init yaw setpoint */
float yaw_sp = 0.0f;
/* init height setpoint */
float height_sp = params.height_min;
/* height controller states */
bool start_phase = true;
bool landing_initialized = false;
float landing_thrust_start = 0.0f;
/* states */
float integrated_h_error = 0.0f;
float last_local_pos_z = 0.0f;
bool update_flow_sp_sumx = false;
bool update_flow_sp_sumy = false;
uint64_t last_time = 0.0f;
float dt = 0.0f; // s
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
static bool sensors_ready = false;
while (!thread_should_exit)
{
/* wait for first attitude msg to be sure all data are available */
if (sensors_ready)
{
/* polling */
struct pollfd fds[2] = {
{ .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator
{ .fd = parameter_update_sub, .events = POLLIN }
};
/* wait for a position 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(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
// printf("[flow position control] no filtered flow updates\n");
}
else
{
/* parameter update available? */
if (fds[1].revents & POLLIN)
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
printf("[flow position control] parameters updated.\n");
}
/* only run controller if position/speed changed */
if (fds[0].revents & POLLIN)
{
perf_begin(mc_loop_perf);
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
/* get a local copy of filtered bottom flow */
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
if (vstatus.state_machine == SYSTEM_STATE_AUTO)
{
float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
/* calc dt */
if(last_time == 0)
{
last_time = hrt_absolute_time();
continue;
}
dt = ((float) (hrt_absolute_time() - last_time)) * time_scale;
last_time = hrt_absolute_time();
/* update flow sum setpoint */
if (update_flow_sp_sumx)
{
flow_sp_sumx = filtered_flow.sumx;
update_flow_sp_sumx = false;
}
if (update_flow_sp_sumy)
{
flow_sp_sumy = filtered_flow.sumy;
update_flow_sp_sumy = false;
}
/* calc new bodyframe speed setpoints */
float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d;
float speed_limit_height_factor = height_sp; // the settings are for 1 meter
/* overwrite with rc input if there is any */
if(isfinite(manual_pitch) && isfinite(manual_roll))
{
if(fabsf(manual_pitch) > params.manual_threshold)
{
speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor;
update_flow_sp_sumx = true;
}
if(fabsf(manual_roll) > params.manual_threshold)
{
speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor;
update_flow_sp_sumy = true;
}
}
/* limit speed setpoints */
if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) &&
(speed_body_x >= -params.limit_speed_x * speed_limit_height_factor))
{
speed_sp.vx = speed_body_x;
}
else
{
if(speed_body_x > params.limit_speed_x * speed_limit_height_factor)
speed_sp.vx = params.limit_speed_x * speed_limit_height_factor;
if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor)
speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor;
}
if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) &&
(speed_body_y >= -params.limit_speed_y * speed_limit_height_factor))
{
speed_sp.vy = speed_body_y;
}
else
{
if(speed_body_y > params.limit_speed_y * speed_limit_height_factor)
speed_sp.vy = params.limit_speed_y * speed_limit_height_factor;
if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor)
speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor;
}
/* manual yaw change */
if(isfinite(manual_yaw) && isfinite(manual.throttle))
{
if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f)
{
yaw_sp += manual_yaw * params.limit_yaw_step;
/* modulo for rotation -pi +pi */
if(yaw_sp < -M_PI_F)
yaw_sp = yaw_sp + M_TWOPI_F;
else if(yaw_sp > M_PI_F)
yaw_sp = yaw_sp - M_TWOPI_F;
}
}
/* forward yaw setpoint */
speed_sp.yaw_sp = yaw_sp;
/* manual height control
* 0-20%: thrust linear down
* 20%-40%: down
* 40%-60%: stabilize altitude
* 60-100%: up
*/
float thrust_control = 0.0f;
if (isfinite(manual.throttle))
{
if (start_phase)
{
/* control start thrust with stick input */
if (manual.throttle < 0.4f)
{
/* first 40% for up to feedforward */
thrust_control = manual.throttle / 0.4f * params.thrust_feedforward;
}
else
{
/* second 60% for up to feedforward + 10% */
thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward;
}
/* exit start phase if setpoint is reached */
if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower)
{
start_phase = false;
/* switch to stabilize */
thrust_control = params.thrust_feedforward;
}
}
else
{
if (manual.throttle < 0.2f)
{
/* landing initialization */
if (!landing_initialized)
{
/* consider last thrust control to avoid steps */
landing_thrust_start = speed_sp.thrust_sp;
landing_initialized = true;
}
/* set current height as setpoint to avoid steps */
if (-local_pos.z > params.height_min)
height_sp = -local_pos.z;
else
height_sp = params.height_min;
/* lower 20% stick range controls thrust down */
thrust_control = manual.throttle / 0.2f * landing_thrust_start;
/* assume ground position here */
if (thrust_control < 0.1f)
{
/* reset integral if on ground */
integrated_h_error = 0.0f;
/* switch to start phase */
start_phase = true;
/* reset height setpoint */
height_sp = params.height_min;
}
}
else
{
/* stabilized mode */
landing_initialized = false;
/* calc new thrust with PID */
float height_error = (local_pos.z - (-height_sp));
/* update height setpoint if needed*/
if (manual.throttle < 0.4f)
{
/* down */
if (height_sp > params.height_min + params.height_rate &&
fabsf(height_error) < params.limit_height_error)
height_sp -= params.height_rate * dt;
}
if (manual.throttle > 0.6f)
{
/* up */
if (height_sp < params.height_max &&
fabsf(height_error) < params.limit_height_error)
height_sp += params.height_rate * dt;
}
/* instead of speed limitation, limit height error (downwards) */
if(height_error > params.limit_height_error)
height_error = params.limit_height_error;
else if(height_error < -params.limit_height_error)
height_error = -params.limit_height_error;
integrated_h_error = integrated_h_error + height_error;
float integrated_thrust_addition = integrated_h_error * params.height_i;
if(integrated_thrust_addition > params.limit_thrust_int)
integrated_thrust_addition = params.limit_thrust_int;
if(integrated_thrust_addition < -params.limit_thrust_int)
integrated_thrust_addition = -params.limit_thrust_int;
float height_speed = last_local_pos_z - local_pos.z;
float thrust_diff = height_error * params.height_p - height_speed * params.height_d;
thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition;
/* add attitude component
* F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM
*/
// // TODO problem with attitude
// if (att.R_valid && att.R[2][2] > 0)
// thrust_control = thrust_control / att.R[2][2];
/* set thrust lower limit */
if(thrust_control < params.limit_thrust_lower)
thrust_control = params.limit_thrust_lower;
}
}
/* set thrust upper limit */
if(thrust_control > params.limit_thrust_upper)
thrust_control = params.limit_thrust_upper;
}
/* store actual height for speed estimation */
last_local_pos_z = local_pos.z;
speed_sp.thrust_sp = thrust_control;
speed_sp.timestamp = hrt_absolute_time();
/* publish new speed setpoint */
if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp))
{
if(speed_setpoint_adverted)
{
orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp);
}
else
{
speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp);
speed_setpoint_adverted = true;
}
}
else
{
warnx("NaN in flow position controller!");
}
}
else
{
/* in manual or stabilized state just reset speed and flow sum setpoint */
speed_sp.vx = 0.0f;
speed_sp.vy = 0.0f;
flow_sp_sumx = filtered_flow.sumx;
flow_sp_sumy = filtered_flow.sumy;
if(isfinite(att.yaw))
{
yaw_sp = att.yaw;
speed_sp.yaw_sp = att.yaw;
}
if(isfinite(manual.throttle))
speed_sp.thrust_sp = manual.throttle;
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
}
}
counter++;
}
else
{
/* sensors not ready waiting for first attitude msg */
/* polling */
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
/* wait for a flow msg, check for exit condition every 5 s */
int ret = poll(fds, 1, 5000);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
printf("[flow position control] no attitude received.\n");
}
else
{
if (fds[0].revents & POLLIN)
{
sensors_ready = true;
printf("[flow position control] initialized.\n");
}
}
}
}
printf("[flow position control] ending now...\n");
thread_running = false;
close(parameter_update_sub);
close(vehicle_attitude_sub);
close(vehicle_local_position_sub);
close(vehicle_status_sub);
close(manual_control_setpoint_sub);
close(speed_sp_pub);
perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);
fflush(stdout);
return 0;
}
src/examples/flow_position_control/flow_position_control_params.c
+124
View File
@@ -0,0 +1,124 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_position_control_params.c
*/
#include "flow_position_control_params.h"
/* controller parameters */
// Position control P gain
PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f);
// Position control D / damping gain
PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f);
// Altitude control P gain
PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f);
// Altitude control I (integrator) gain
PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f);
// Altitude control D gain
PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f);
// Altitude control rate limiter
PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f);
// Altitude control minimum altitude
PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f);
// Altitude control maximum altitude (higher than 1.5m is untested)
PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f);
// Altitude control feed forward throttle - adjust to the
// throttle position (0..1) where the copter hovers in manual flight
PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight
PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f);
PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f);
PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f);
PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f);
PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f);
PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f);
PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f);
PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f);
int parameters_init(struct flow_position_control_param_handles *h)
{
/* PID parameters */
h->pos_p = param_find("FPC_POS_P");
h->pos_d = param_find("FPC_POS_D");
h->height_p = param_find("FPC_H_P");
h->height_i = param_find("FPC_H_I");
h->height_d = param_find("FPC_H_D");
h->height_rate = param_find("FPC_H_RATE");
h->height_min = param_find("FPC_H_MIN");
h->height_max = param_find("FPC_H_MAX");
h->thrust_feedforward = param_find("FPC_T_FFWD");
h->limit_speed_x = param_find("FPC_L_S_X");
h->limit_speed_y = param_find("FPC_L_S_Y");
h->limit_height_error = param_find("FPC_L_H_ERR");
h->limit_thrust_int = param_find("FPC_L_TH_I");
h->limit_thrust_upper = param_find("FPC_L_TH_U");
h->limit_thrust_lower = param_find("FPC_L_TH_L");
h->limit_yaw_step = param_find("FPC_L_YAW_STEP");
h->manual_threshold = param_find("FPC_MAN_THR");
h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
h->rc_scale_roll = param_find("RC_SCALE_ROLL");
h->rc_scale_yaw = param_find("RC_SCALE_YAW");
return OK;
}
int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p)
{
param_get(h->pos_p, &(p->pos_p));
param_get(h->pos_d, &(p->pos_d));
param_get(h->height_p, &(p->height_p));
param_get(h->height_i, &(p->height_i));
param_get(h->height_d, &(p->height_d));
param_get(h->height_rate, &(p->height_rate));
param_get(h->height_min, &(p->height_min));
param_get(h->height_max, &(p->height_max));
param_get(h->thrust_feedforward, &(p->thrust_feedforward));
param_get(h->limit_speed_x, &(p->limit_speed_x));
param_get(h->limit_speed_y, &(p->limit_speed_y));
param_get(h->limit_height_error, &(p->limit_height_error));
param_get(h->limit_thrust_int, &(p->limit_thrust_int));
param_get(h->limit_thrust_upper, &(p->limit_thrust_upper));
param_get(h->limit_thrust_lower, &(p->limit_thrust_lower));
param_get(h->limit_yaw_step, &(p->limit_yaw_step));
param_get(h->manual_threshold, &(p->manual_threshold));
param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
param_get(h->rc_scale_roll, &(p->rc_scale_roll));
param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
return OK;
}
src/examples/flow_position_control/flow_position_control_params.h
+100
View File
@@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_position_control_params.h
*
* Parameters for position controller
*/
#include <systemlib/param/param.h>
struct flow_position_control_params {
float pos_p;
float pos_d;
float height_p;
float height_i;
float height_d;
float height_rate;
float height_min;
float height_max;
float thrust_feedforward;
float limit_speed_x;
float limit_speed_y;
float limit_height_error;
float limit_thrust_int;
float limit_thrust_upper;
float limit_thrust_lower;
float limit_yaw_step;
float manual_threshold;
float rc_scale_pitch;
float rc_scale_roll;
float rc_scale_yaw;
};
struct flow_position_control_param_handles {
param_t pos_p;
param_t pos_d;
param_t height_p;
param_t height_i;
param_t height_d;
param_t height_rate;
param_t height_min;
param_t height_max;
param_t thrust_feedforward;
param_t limit_speed_x;
param_t limit_speed_y;
param_t limit_height_error;
param_t limit_thrust_int;
param_t limit_thrust_upper;
param_t limit_thrust_lower;
param_t limit_yaw_step;
param_t manual_threshold;
param_t rc_scale_pitch;
param_t rc_scale_roll;
param_t rc_scale_yaw;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct flow_position_control_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p);
src/examples/flow_position_control/module.mk
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# 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.
#
############################################################################
#
# Build multirotor position control
#
MODULE_COMMAND = flow_position_control
SRCS = flow_position_control_main.c \
flow_position_control_params.c
src/examples/flow_position_estimator/flow_position_estimator_main.c
+458
View File
@@ -0,0 +1,458 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_position_estimator_main.c
*
* Optical flow position estimator
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/perf_counter.h>
#include <poll.h>
#include "flow_position_estimator_params.h"
__EXPORT int flow_position_estimator_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Daemon exit flag */
static bool thread_running = false; /**< Daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
int flow_position_estimator_thread_main(int argc, char *argv[]);
static void usage(const char *reason);
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The daemon 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 flow_position_estimator_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
printf("flow position estimator already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
daemon_task = task_spawn("flow_position_estimator",
SCHED_RR,
SCHED_PRIORITY_MAX - 5,
4096,
flow_position_estimator_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)
printf("\tflow position estimator is running\n");
else
printf("\tflow position estimator not started\n");
exit(0);
}
usage("unrecognized command");
exit(1);
}
int flow_position_estimator_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf("[flow position estimator] starting\n");
/* rotation matrix for transformation of optical flow speed vectors */
static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 },
{ -1, 0, 0 },
{ 0, 0, 1 }}; // 90deg rotated
const float time_scale = powf(10.0f,-6.0f);
static float speed[3] = {0.0f, 0.0f, 0.0f};
static float flow_speed[3] = {0.0f, 0.0f, 0.0f};
static float global_speed[3] = {0.0f, 0.0f, 0.0f};
static uint32_t counter = 0;
static uint64_t time_last_flow = 0; // in ms
static float dt = 0.0f; // seconds
static float sonar_last = 0.0f;
static bool sonar_valid = false;
static float sonar_lp = 0.0f;
/* subscribe to vehicle status, attitude, sensors and flow*/
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
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 optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
/* subscribe to parameter changes */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to vehicle status */
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to attitude */
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
/* subscribe to attitude setpoint */
int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
/* subscribe to optical flow*/
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
/* init local position and filtered flow struct */
struct vehicle_local_position_s local_pos = {
.x = 0.0f,
.y = 0.0f,
.z = 0.0f,
.vx = 0.0f,
.vy = 0.0f,
.vz = 0.0f
};
struct filtered_bottom_flow_s filtered_flow = {
.sumx = 0.0f,
.sumy = 0.0f,
.vx = 0.0f,
.vy = 0.0f
};
/* advert pub messages */
orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow);
/* vehicle flying status parameters */
bool vehicle_liftoff = false;
bool sensors_ready = false;
/* parameters init*/
struct flow_position_estimator_params params;
struct flow_position_estimator_param_handles param_handles;
parameters_init(&param_handles);
parameters_update(&param_handles, &params);
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err");
while (!thread_should_exit)
{
if (sensors_ready)
{
/*This runs at the rate of the sensors */
struct pollfd fds[2] = {
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = parameter_update_sub, .events = POLLIN }
};
/* 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(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
// printf("[flow position estimator] no bottom flow.\n");
}
else
{
/* parameter update available? */
if (fds[1].revents & POLLIN)
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
printf("[flow position estimator] parameters updated.\n");
}
/* only if flow data changed */
if (fds[0].revents & POLLIN)
{
perf_begin(mc_loop_perf);
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
/* got flow, updating attitude and status as well */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
/* vehicle state estimation */
float sonar_new = flow.ground_distance_m;
/* set liftoff boolean
* -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m)
* -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
* -> minimum sonar value 0.3m
*/
if (!vehicle_liftoff)
{
if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
vehicle_liftoff = true;
}
else
{
if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
vehicle_liftoff = false;
}
/* calc dt between flow timestamps */
/* ignore first flow msg */
if(time_last_flow == 0)
{
time_last_flow = flow.timestamp;
continue;
}
dt = (float)(flow.timestamp - time_last_flow) * time_scale ;
time_last_flow = flow.timestamp;
/* only make position update if vehicle is lift off or DEBUG is activated*/
if (vehicle_liftoff || params.debug)
{
/* copy flow */
flow_speed[0] = flow.flow_comp_x_m;
flow_speed[1] = flow.flow_comp_y_m;
flow_speed[2] = 0.0f;
/* convert to bodyframe velocity */
for(uint8_t i = 0; i < 3; i++)
{
float sum = 0.0f;
for(uint8_t j = 0; j < 3; j++)
sum = sum + flow_speed[j] * rotM_flow_sensor[j][i];
speed[i] = sum;
}
/* update filtered flow */
filtered_flow.sumx = filtered_flow.sumx + speed[0] * dt;
filtered_flow.sumy = filtered_flow.sumy + speed[1] * dt;
filtered_flow.vx = speed[0];
filtered_flow.vy = speed[1];
// TODO add yaw rotation correction (with distance to vehicle zero)
/* convert to globalframe velocity
* -> local position is currently not used for position control
*/
for(uint8_t i = 0; i < 3; i++)
{
float sum = 0.0f;
for(uint8_t j = 0; j < 3; j++)
sum = sum + speed[j] * att.R[i][j];
global_speed[i] = sum;
}
local_pos.x = local_pos.x + global_speed[0] * dt;
local_pos.y = local_pos.y + global_speed[1] * dt;
local_pos.vx = global_speed[0];
local_pos.vy = global_speed[1];
}
else
{
/* set speed to zero and let position as it is */
filtered_flow.vx = 0;
filtered_flow.vy = 0;
local_pos.vx = 0;
local_pos.vy = 0;
}
/* filtering ground distance */
if (!vehicle_liftoff || !vstatus.flag_system_armed)
{
/* not possible to fly */
sonar_valid = false;
local_pos.z = 0.0f;
}
else
{
sonar_valid = true;
}
if (sonar_valid || params.debug)
{
/* simple lowpass sonar filtering */
/* if new value or with sonar update frequency */
if (sonar_new != sonar_last || counter % 10 == 0)
{
sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp;
sonar_last = sonar_new;
}
float height_diff = sonar_new - sonar_lp;
/* if over 1/2m spike follow lowpass */
if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold)
{
local_pos.z = -sonar_lp;
}
else
{
local_pos.z = -sonar_new;
}
}
filtered_flow.timestamp = hrt_absolute_time();
local_pos.timestamp = hrt_absolute_time();
/* publish local position */
if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z)
&& isfinite(local_pos.vx) && isfinite(local_pos.vy))
{
orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
}
/* publish filtered flow */
if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy))
{
orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow);
}
/* measure in what intervals the position estimator runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
}
}
}
else
{
/* sensors not ready waiting for first attitude msg */
/* polling */
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
/* wait for a attitude message, check for exit condition every 5 s */
int ret = poll(fds, 1, 5000);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
printf("[flow position estimator] no attitude received.\n");
}
else
{
if (fds[0].revents & POLLIN){
sensors_ready = true;
printf("[flow position estimator] initialized.\n");
}
}
}
counter++;
}
printf("[flow position estimator] exiting.\n");
thread_running = false;
close(vehicle_attitude_setpoint_sub);
close(vehicle_attitude_sub);
close(vehicle_status_sub);
close(parameter_update_sub);
close(optical_flow_sub);
perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);
fflush(stdout);
return 0;
}
src/examples/flow_position_estimator/flow_position_estimator_params.c
+72
View File
@@ -0,0 +1,72 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_position_estimator_params.c
*
* Parameters for position estimator
*/
#include "flow_position_estimator_params.h"
/* Extended Kalman Filter covariances */
/* controller parameters */
PARAM_DEFINE_FLOAT(FPE_LO_THRUST, 0.4f);
PARAM_DEFINE_FLOAT(FPE_SONAR_LP_U, 0.5f);
PARAM_DEFINE_FLOAT(FPE_SONAR_LP_L, 0.2f);
PARAM_DEFINE_INT32(FPE_DEBUG, 0);
int parameters_init(struct flow_position_estimator_param_handles *h)
{
/* PID parameters */
h->minimum_liftoff_thrust = param_find("FPE_LO_THRUST");
h->sonar_upper_lp_threshold = param_find("FPE_SONAR_LP_U");
h->sonar_lower_lp_threshold = param_find("FPE_SONAR_LP_L");
h->debug = param_find("FPE_DEBUG");
return OK;
}
int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p)
{
param_get(h->minimum_liftoff_thrust, &(p->minimum_liftoff_thrust));
param_get(h->sonar_upper_lp_threshold, &(p->sonar_upper_lp_threshold));
param_get(h->sonar_lower_lp_threshold, &(p->sonar_lower_lp_threshold));
param_get(h->debug, &(p->debug));
return OK;
}
src/examples/flow_position_estimator/flow_position_estimator_params.h
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_position_estimator_params.h
*
* Parameters for position estimator
*/
#include <systemlib/param/param.h>
struct flow_position_estimator_params {
float minimum_liftoff_thrust;
float sonar_upper_lp_threshold;
float sonar_lower_lp_threshold;
int debug;
};
struct flow_position_estimator_param_handles {
param_t minimum_liftoff_thrust;
param_t sonar_upper_lp_threshold;
param_t sonar_lower_lp_threshold;
param_t debug;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct flow_position_estimator_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p);
src/examples/flow_position_estimator/module.mk
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# 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.
#
############################################################################
#
# Build position estimator
#
MODULE_COMMAND = flow_position_estimator
SRCS = flow_position_estimator_main.c \
flow_position_estimator_params.c
src/examples/flow_speed_control/flow_speed_control_main.c
+361
View File
@@ -0,0 +1,361 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_speed_control.c
*
* Optical flow speed controller
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <math.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include "flow_speed_control_params.h"
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 */
__EXPORT int flow_speed_control_main(int argc, char *argv[]);
/**
* Mainloop of position controller.
*/
static int flow_speed_control_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);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\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_spawn().
*/
int flow_speed_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
printf("flow speed control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("flow_speed_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
4096,
flow_speed_control_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)
printf("\tflow speed control app is running\n");
else
printf("\tflow speed control app not started\n");
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int
flow_speed_control_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf("[flow speed control] starting\n");
uint32_t counter = 0;
/* structures */
struct vehicle_status_s vstatus;
struct filtered_bottom_flow_s filtered_flow;
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
struct vehicle_attitude_setpoint_s att_sp;
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
orb_advert_t att_sp_pub;
bool attitude_setpoint_adverted = false;
/* parameters init*/
struct flow_speed_control_params params;
struct flow_speed_control_param_handles param_handles;
parameters_init(&param_handles);
parameters_update(&param_handles, &params);
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err");
static bool sensors_ready = false;
while (!thread_should_exit)
{
/* wait for first attitude msg to be sure all data are available */
if (sensors_ready)
{
/* polling */
struct pollfd fds[2] = {
{ .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller
{ .fd = parameter_update_sub, .events = POLLIN }
};
/* wait for a position update, check for exit condition every 5000 ms */
int ret = poll(fds, 2, 500);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
// printf("[flow speed control] no bodyframe speed setpoints updates\n");
}
else
{
/* parameter update available? */
if (fds[1].revents & POLLIN)
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
printf("[flow speed control] parameters updated.\n");
}
/* only run controller if position/speed changed */
if (fds[0].revents & POLLIN)
{
perf_begin(mc_loop_perf);
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
/* get a local copy of filtered bottom flow */
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of bodyframe speed setpoint */
orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
if (vstatus.state_machine == SYSTEM_STATE_AUTO)
{
/* calc new roll/pitch */
float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p;
/* limit roll and pitch corrections */
if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
{
att_sp.pitch_body = pitch_body;
}
else
{
if(pitch_body > params.limit_pitch)
att_sp.pitch_body = params.limit_pitch;
if(pitch_body < -params.limit_pitch)
att_sp.pitch_body = -params.limit_pitch;
}
if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll))
{
att_sp.roll_body = roll_body;
}
else
{
if(roll_body > params.limit_roll)
att_sp.roll_body = params.limit_roll;
if(roll_body < -params.limit_roll)
att_sp.roll_body = -params.limit_roll;
}
/* set yaw setpoint forward*/
att_sp.yaw_body = speed_sp.yaw_sp;
/* add trim from parameters */
att_sp.roll_body = att_sp.roll_body + params.trim_roll;
att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch;
att_sp.thrust = speed_sp.thrust_sp;
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust))
{
if (attitude_setpoint_adverted)
{
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
else
{
att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
attitude_setpoint_adverted = true;
}
}
else
{
warnx("NaN in flow speed controller!");
}
}
else
{
/* reset attitude setpoint */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.thrust = 0.0f;
att_sp.yaw_body = 0.0f;
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
}
}
counter++;
}
else
{
/* sensors not ready waiting for first attitude msg */
/* polling */
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
/* wait for a flow msg, check for exit condition every 5 s */
int ret = poll(fds, 1, 5000);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
printf("[flow speed control] no attitude received.\n");
}
else
{
if (fds[0].revents & POLLIN)
{
sensors_ready = true;
printf("[flow speed control] initialized.\n");
}
}
}
}
printf("[flow speed control] ending now...\n");
thread_running = false;
close(parameter_update_sub);
close(vehicle_attitude_sub);
close(vehicle_bodyframe_speed_setpoint_sub);
close(filtered_bottom_flow_sub);
close(vehicle_status_sub);
close(att_sp_pub);
perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);
fflush(stdout);
return 0;
}
src/examples/flow_speed_control/flow_speed_control_params.c
+70
View File
@@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_speed_control_params.c
*
*/
#include "flow_speed_control_params.h"
/* controller parameters */
PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f);
PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f);
PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f);
int parameters_init(struct flow_speed_control_param_handles *h)
{
/* PID parameters */
h->speed_p = param_find("FSC_S_P");
h->limit_pitch = param_find("FSC_L_PITCH");
h->limit_roll = param_find("FSC_L_ROLL");
h->trim_roll = param_find("TRIM_ROLL");
h->trim_pitch = param_find("TRIM_PITCH");
return OK;
}
int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p)
{
param_get(h->speed_p, &(p->speed_p));
param_get(h->limit_pitch, &(p->limit_pitch));
param_get(h->limit_roll, &(p->limit_roll));
param_get(h->trim_roll, &(p->trim_roll));
param_get(h->trim_pitch, &(p->trim_pitch));
return OK;
}
src/examples/flow_speed_control/flow_speed_control_params.h
+70
View File
@@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.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 flow_speed_control_params.h
*
* Parameters for speed controller
*/
#include <systemlib/param/param.h>
struct flow_speed_control_params {
float speed_p;
float limit_pitch;
float limit_roll;
float trim_roll;
float trim_pitch;
};
struct flow_speed_control_param_handles {
param_t speed_p;
param_t limit_pitch;
param_t limit_roll;
param_t trim_roll;
param_t trim_pitch;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct flow_speed_control_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p);
src/examples/flow_speed_control/module.mk
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# 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.
#
############################################################################
#
# Build flow speed control
#
MODULE_COMMAND = flow_speed_control
SRCS = flow_speed_control_main.c \
flow_speed_control_params.c
src/examples/px4_daemon_app/px4_daemon_app.c
+1 -1
View File
@@ -95,7 +95,7 @@ int px4_daemon_app_main(int argc, char *argv[])
}
thread_should_exit = false;
daemon_task = task_spawn("daemon",
daemon_task = task_spawn_cmd("daemon",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+110 -88
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -40,6 +40,7 @@
#include <poll.h>
#include "KalmanNav.hpp"
#include <systemlib/err.h>
// constants
// Titterton pg. 52
@@ -58,14 +59,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
P0(9, 9),
V(6, 6),
// attitude measurement ekf matrices
HAtt(6, 9),
RAtt(6, 6),
HAtt(4, 9),
RAtt(4, 4),
// position measurement ekf matrices
HPos(6, 9),
RPos(6, 6),
// attitude representations
C_nb(),
q(),
_accel_sub(-1),
_gyro_sub(-1),
_mag_sub(-1),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
@@ -123,8 +127,19 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
lon = 0.0f;
alt = 0.0f;
// initialize quaternions
q = Quaternion(EulerAngles(phi, theta, psi));
// gyro, accel and mag subscriptions
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
struct accel_report accel;
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
struct mag_report mag;
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
// initialize rotation quaternion with a single raw sensor measurement
q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
// initialize dcm
C_nb = Dcm(q);
@@ -141,6 +156,45 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
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;
@@ -181,8 +235,8 @@ void KalmanNav::update()
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
printf("[kalman_demo] initialized EKF attitude\n");
printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
warnx("initialized EKF attitude\n");
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
@@ -202,13 +256,13 @@ void KalmanNav::update()
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
_positionInitialized = true;
printf("[kalman_demo] initialized EKF state with GPS\n");
printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
warnx("initialized EKF state with GPS\n");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
lat, lon, alt);
}
// prediciton step
// 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));
@@ -233,7 +287,7 @@ void KalmanNav::update()
// attitude correction step
if (_attitudeInitialized // initialized
&& sensorsUpdate // new data
&& _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz
&& _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz
) {
_attTimeStamp = _sensors.timestamp;
correctAtt();
@@ -480,26 +534,32 @@ int KalmanNav::correctAtt()
// trig
float cosPhi = cosf(phi);
float cosTheta = cosf(theta);
float cosPsi = cosf(psi);
// float cosPsi = cosf(psi);
float sinPhi = sinf(phi);
float sinTheta = sinf(theta);
float sinPsi = sinf(psi);
// mag measurement
Vector3 zMag(_sensors.magnetometer_ga);
//float magNorm = zMag.norm();
zMag = zMag.unit();
// 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 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
float bN = cosf(dip) * cosf(dec);
float bE = cosf(dip) * sinf(dec);
float bD = sinf(dip);
Vector3 bNav(bN, bE, bD);
Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
// compensate roll and pitch, but not yaw
// XXX take the vectors out of the C_nb matrix to avoid singularities
math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
// mag measurement
Vector3 magBody(_sensors.magnetometer_ga);
// transform to earth frame
Vector3 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
Vector3 zAccel(_sensors.accelerometer_m_s2);
@@ -512,9 +572,9 @@ int KalmanNav::correctAtt()
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;
RAttAdjust(4, 4) = 1.0e10;
RAttAdjust(5, 5) = 1.0e10;
} else {
//printf("correcting attitude with accel\n");
@@ -523,58 +583,25 @@ int KalmanNav::correctAtt()
// accel predicted measurement
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
// combined measurement
Vector zAtt(6);
Vector zAttHat(6);
// calculate residual
Vector y(4);
y(0) = yMag;
y(1) = zAccel(0) - zAccelHat(0);
y(2) = zAccel(1) - zAccelHat(1);
y(3) = zAccel(2) - zAccelHat(2);
for (int i = 0; i < 3; i++) {
zAtt(i) = zMag(i);
zAtt(i + 3) = zAccel(i);
zAttHat(i) = zMagHat(i);
zAttHat(i + 3) = zAccelHat(i);
}
// HMag
HAtt(0, 2) = 1;
// HMag , HAtt (0-2,:)
float tmp1 =
cosPsi * cosTheta * bN +
sinPsi * cosTheta * bE -
sinTheta * bD;
HAtt(0, 1) = -(
cosPsi * sinTheta * bN +
sinPsi * sinTheta * bE +
cosTheta * bD
);
HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
HAtt(1, 0) =
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
cosPhi * cosTheta * bD;
HAtt(1, 1) = sinPhi * tmp1;
HAtt(1, 2) = -(
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
);
HAtt(2, 0) = -(
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
(sinPhi * cosTheta) * bD
);
HAtt(2, 1) = cosPhi * tmp1;
HAtt(2, 2) = -(
(cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
);
// HAccel , HAtt (3-5,:)
HAtt(3, 1) = cosTheta;
HAtt(4, 0) = -cosPhi * cosTheta;
HAtt(4, 1) = sinPhi * sinTheta;
HAtt(5, 0) = sinPhi * cosTheta;
HAtt(5, 1) = cosPhi * sinTheta;
// 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
Vector y = zAtt - zAttHat; // residual
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
Matrix K = P * HAtt.transpose() * S.inverse();
Vector xCorrect = K * y;
@@ -585,7 +612,7 @@ int KalmanNav::correctAtt()
if (isnan(val) || isinf(val)) {
// abort correction and return
printf("[kalman_demo] numerical failure in att correction\n");
warnx("numerical failure in att correction\n");
// reset P matrix to P0
P = P0;
return ret_error;
@@ -615,11 +642,8 @@ int KalmanNav::correctAtt()
float beta = y.dot(S.inverse() * y);
if (beta > _faultAtt.get()) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
printf("y:\n"); y.print();
printf("zMagHat:\n"); zMagHat.print();
printf("zMag:\n"); zMag.print();
printf("bNav:\n"); bNav.print();
warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:\n"); y.print();
}
// update quaternions from euler
@@ -652,9 +676,9 @@ int KalmanNav::correctPos()
for (size_t i = 0; i < xCorrect.getRows(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
if (!isfinite(val)) {
// abort correction and return
printf("[kalman_demo] numerical failure in gps correction\n");
warnx("numerical failure in gps correction\n");
// fallback to GPS
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
@@ -685,8 +709,8 @@ int KalmanNav::correctPos()
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
printf("fault in gps: beta = %8.4f\n", (double)beta);
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
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",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
@@ -722,8 +746,6 @@ void KalmanNav::updateParams()
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
RAtt(0, 0) = noiseMagSq; // normalized direction
RAtt(1, 1) = noiseMagSq;
RAtt(2, 2) = noiseMagSq;
// accelerometer noise
float noiseAccelSq = _rAccel.get() * _rAccel.get();
@@ -731,9 +753,9 @@ void KalmanNav::updateParams()
// bound noise to prevent singularities
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
RAtt(3, 3) = noiseAccelSq; // normalized direction
RAtt(4, 4) = noiseAccelSq;
RAtt(5, 5) = noiseAccelSq;
RAtt(1, 1) = noiseAccelSq; // normalized direction
RAtt(2, 2) = noiseAccelSq;
RAtt(3, 3) = noiseAccelSq;
// gps noise
float R = R0 + float(alt);
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+15 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -56,6 +56,10 @@
#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>
@@ -78,6 +82,9 @@ public:
*/
virtual ~KalmanNav() {};
math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
/**
* The main callback function for the class
*/
@@ -136,6 +143,11 @@ protected:
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
int _accel_sub; /**< Accelerometer subscription */
int _gyro_sub; /**< Gyroscope subscription */
int _mag_sub; /**< Magnetometer subscription */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
@@ -151,7 +163,8 @@ protected:
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, alt; /**< lat, lon, alt, radians */
double lat, lon; /**< lat, lon radians */
float alt; /**< altitude, meters */
// parameters
control::BlockParam<float> _vGyro; /**< gyro process noise */
control::BlockParam<float> _vAccel; /**< accelerometer process noise */
src/modules/att_pos_estimator_ekf/kalman_main.cpp
+12 -9
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
* 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
@@ -33,8 +33,10 @@
****************************************************************************/
/**
* @file kalman_demo.cpp
* Demonstration of control library
* @file kalman_main.cpp
* Combined attitude / position estimator.
*
* @author James Goppert
*/
#include <nuttx/config.h>
@@ -51,7 +53,7 @@
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 */
static int daemon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
@@ -101,9 +103,10 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("att_pos_estimator_ekf",
daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
SCHED_PRIORITY_MAX - 30,
4096,
kalman_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -133,7 +136,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
int kalman_demo_thread_main(int argc, char *argv[])
{
warnx("starting\n");
warnx("starting");
using namespace math;
@@ -145,7 +148,7 @@ int kalman_demo_thread_main(int argc, char *argv[])
nav.update();
}
printf("exiting.\n");
warnx("exiting.");
thread_running = false;
src/modules/att_pos_estimator_ekf/module.mk
-3
View File
@@ -37,9 +37,6 @@
MODULE_COMMAND = att_pos_estimator_ekf
# XXX this might be intended for the spawned deamon, validate
MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
SRCS = kalman_main.cpp \
KalmanNav.cpp \
params.c
src/modules/att_pos_estimator_ekf/params.c
+37 -4
View File
@@ -1,12 +1,45 @@
/****************************************************************************
*
* 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, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.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);
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+2 -2
View File
@@ -123,10 +123,10 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
}
thread_should_exit = false;
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12400,
14000,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+1 -1
View File
@@ -117,7 +117,7 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
}
thread_should_exit = false;
attitude_estimator_so3_comp_task = task_spawn("attitude_estimator_so3_comp",
attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12400,
src/modules/commander/accelerometer_calibration.c
+42 -8
View File
@@ -1,12 +1,45 @@
/*
* accelerometer_calibration.c
/****************************************************************************
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* Transform acceleration vector to true orientation and scale
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * * * Model * * *
* 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 accelerometer_calibration.c
*
* Implementation of accelerometer calibration.
*
* Transform acceleration vector to true orientation, scale and offset
*
* ===== Model =====
* accel_corr = accel_T * (accel_raw - accel_offs)
*
* accel_corr[3] - fully corrected acceleration vector in body frame
@@ -14,7 +47,7 @@
* accel_raw[3] - raw acceleration vector
* accel_offs[3] - acceleration offset vector
*
* * * * Calibration * * *
* ===== Calibration =====
*
* Reference vectors
* accel_corr_ref[6][3] = [ g 0 0 ] // nose up
@@ -34,7 +67,6 @@
*
* accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
*
*
* Find accel_T
*
* 9 unknown constants
@@ -67,6 +99,8 @@
*
* accel_T = A^-1 * g
* g = 9.80665
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "accelerometer_calibration.h"
src/modules/commander/accelerometer_calibration.h
+39 -4
View File
@@ -1,8 +1,43 @@
/*
* accelerometer_calibration.h
/****************************************************************************
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* 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 accelerometer_calibration.h
*
* Definition of accelerometer calibration.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef ACCELEROMETER_CALIBRATION_H_
src/modules/commander/commander.c
+1 -1
View File
@@ -1200,7 +1200,7 @@ int commander_main(int argc, char *argv[])
}
thread_should_exit = false;
daemon_task = task_spawn("commander",
daemon_task = task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
3000,
src/modules/controllib/block/UOrbPublication.hpp
+6 -6
View File
@@ -60,11 +60,15 @@ public:
List<UOrbPublicationBase *> * list,
const struct orb_metadata *meta) :
_meta(meta),
_handle() {
_handle(-1) {
if (list != NULL) list->add(this);
}
void update() {
orb_publish(getMeta(), getHandle(), getDataVoidPtr());
if (_handle > 0) {
orb_publish(getMeta(), getHandle(), getDataVoidPtr());
} else {
setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
}
}
virtual void *getDataVoidPtr() = 0;
virtual ~UOrbPublicationBase() {
@@ -99,10 +103,6 @@ public:
const struct orb_metadata *meta) :
T(), // initialize data structure to zero
UOrbPublicationBase(list, meta) {
// It is important that we call T()
// before we publish the data, so we
// call this here instead of the base class
setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
}
virtual ~UOrbPublication() {}
/*
src/modules/controllib/fixedwing.cpp
+5 -5
View File
@@ -130,7 +130,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
@@ -213,7 +213,7 @@ void BlockMultiModeBacksideAutopilot::update()
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
}
// XXX handle STABILIZED (loiter on spot) as well
@@ -225,7 +225,7 @@ void BlockMultiModeBacksideAutopilot::update()
_status.state_machine == SYSTEM_STATE_STABILIZED) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
@@ -239,7 +239,7 @@ void BlockMultiModeBacksideAutopilot::update()
float vCmd = _vLimit.update(_vCmd.get());
// altitude hold
float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
// heading hold
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
@@ -328,7 +328,7 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
// currenlty using manual throttle
// currently using manual throttle
// XXX if you enable this watch out, vz might be very noisy
//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.control[CH_THR] = _manual.throttle;
src/modules/controllib/fixedwing.hpp
+3 -3
View File
@@ -43,7 +43,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
@@ -280,7 +280,7 @@ protected:
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
UOrbSubscription<vehicle_global_position_s> _pos;
UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
UOrbSubscription<manual_control_setpoint_s> _manual;
UOrbSubscription<vehicle_status_s> _status;
UOrbSubscription<parameter_update_s> _param_update;
@@ -328,7 +328,7 @@ private:
BlockParam<float> _crMax;
struct pollfd _attPoll;
vehicle_global_position_setpoint_s _lastPosCmd;
vehicle_global_position_set_triplet_s _lastPosCmd;
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
uint64_t _timeStamp;
public:
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+2 -2
View File
@@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
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); //P Controller
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
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;
}
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+1 -1
View File
@@ -336,7 +336,7 @@ int fixedwing_att_control_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("fixedwing_att_control",
deamon_task = task_spawn_cmd("fixedwing_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+3 -3
View File
@@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
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); // 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); // 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); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
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;
}
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+2 -1
View File
@@ -108,7 +108,8 @@ int fixedwing_backside_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("fixedwing_backside",
deamon_task = task_spawn_cmd("fixedwing_backside",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+5 -5
View File
@@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
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); //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);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
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");
@@ -448,7 +448,7 @@ int fixedwing_pos_control_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("fixedwing_pos_control",
deamon_task = task_spawn_cmd("fixedwing_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
src/modules/mathlib/math/Dcm.cpp
+9
View File
@@ -69,6 +69,15 @@ Dcm::Dcm(float c00, float c01, float c02,
dcm(2, 2) = c22;
}
Dcm::Dcm(const float data[3][3]) :
Matrix(3, 3)
{
Dcm &dcm = *this;
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
dcm(i, j) = data[i][j];
}
Dcm::Dcm(const float *data) :
Matrix(3, 3, data)
{
src/modules/mathlib/math/Dcm.hpp
+5
View File
@@ -76,6 +76,11 @@ public:
*/
Dcm(const float *data);
/**
* array ctor
*/
Dcm(const float data[3][3]);
/**
* quaternion ctor
*/
src/modules/mathlib/math/Limits.cpp
+146
View File
@@ -0,0 +1,146 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Limits.cpp
*
* Limiting / constrain helper functions
*/
#include <math.h>
#include <stdint.h>
#include "Limits.hpp"
namespace math {
float __EXPORT min(float val1, float val2)
{
return (val1 < val2) ? val1 : val2;
}
int __EXPORT min(int val1, int val2)
{
return (val1 < val2) ? val1 : val2;
}
unsigned __EXPORT min(unsigned val1, unsigned val2)
{
return (val1 < val2) ? val1 : val2;
}
uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
{
return (val1 < val2) ? val1 : val2;
}
double __EXPORT min(double val1, double val2)
{
return (val1 < val2) ? val1 : val2;
}
float __EXPORT max(float val1, float val2)
{
return (val1 > val2) ? val1 : val2;
}
int __EXPORT max(int val1, int val2)
{
return (val1 > val2) ? val1 : val2;
}
unsigned __EXPORT max(unsigned val1, unsigned val2)
{
return (val1 > val2) ? val1 : val2;
}
uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
{
return (val1 > val2) ? val1 : val2;
}
double __EXPORT max(double val1, double val2)
{
return (val1 > val2) ? val1 : val2;
}
float __EXPORT constrain(float val, float min, float max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
int __EXPORT constrain(int val, int min, int max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
double __EXPORT constrain(double val, double min, double max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
float __EXPORT radians(float degrees)
{
return (degrees / 180.0f) * M_PI_F;
}
double __EXPORT radians(double degrees)
{
return (degrees / 180.0) * M_PI;
}
float __EXPORT degrees(float radians)
{
return (radians / M_PI_F) * 180.0f;
}
double __EXPORT degrees(double radians)
{
return (radians / M_PI) * 180.0;
}
}
src/modules/mathlib/math/Limits.hpp
+87
View File
@@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Limits.hpp
*
* Limiting / constrain helper functions
*/
#pragma once
#include <nuttx/config.h>
#include <stdint.h>
namespace math {
float __EXPORT min(float val1, float val2);
int __EXPORT min(int val1, int val2);
unsigned __EXPORT min(unsigned val1, unsigned val2);
uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
double __EXPORT min(double val1, double val2);
float __EXPORT max(float val1, float val2);
int __EXPORT max(int val1, int val2);
unsigned __EXPORT max(unsigned val1, unsigned val2);
uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
double __EXPORT max(double val1, double val2);
float __EXPORT constrain(float val, float min, float max);
int __EXPORT constrain(int val, int min, int max);
unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
double __EXPORT constrain(double val, double min, double max);
float __EXPORT radians(float degrees);
double __EXPORT radians(double degrees);
float __EXPORT degrees(float radians);
double __EXPORT degrees(double radians);
}
src/modules/mathlib/math/Quaternion.hpp
+1 -1
View File
@@ -37,7 +37,7 @@
* math quaternion lib
*/
//#pragma once
#pragma once
#include "Vector.hpp"
#include "Matrix.hpp"
src/modules/mathlib/math/Vector2f.cpp
+103
View File
@@ -0,0 +1,103 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Vector2f.cpp
*
* math vector
*/
#include "test/test.hpp"
#include "Vector2f.hpp"
namespace math
{
Vector2f::Vector2f() :
Vector(2)
{
}
Vector2f::Vector2f(const Vector &right) :
Vector(right)
{
#ifdef VECTOR_ASSERT
ASSERT(right.getRows() == 2);
#endif
}
Vector2f::Vector2f(float x, float y) :
Vector(2)
{
setX(x);
setY(y);
}
Vector2f::Vector2f(const float *data) :
Vector(2, data)
{
}
Vector2f::~Vector2f()
{
}
float Vector2f::cross(const Vector2f &b) const
{
const Vector2f &a = *this;
return a(0)*b(1) - a(1)*b(0);
}
float Vector2f::operator %(const Vector2f &v) const
{
return cross(v);
}
float Vector2f::operator *(const Vector2f &v) const
{
return dot(v);
}
int __EXPORT vector2fTest()
{
printf("Test Vector2f\t\t: ");
// test float ctor
Vector2f v(1, 2);
ASSERT(equal(v(0), 1));
ASSERT(equal(v(1), 2));
printf("PASS\n");
return 0;
}
} // namespace math
src/modules/mathlib/math/Vector2f.hpp
+79
View File
@@ -0,0 +1,79 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Vector2f.hpp
*
* math 3 vector
*/
#pragma once
#include "Vector.hpp"
namespace math
{
class __EXPORT Vector2f :
public Vector
{
public:
Vector2f();
Vector2f(const Vector &right);
Vector2f(float x, float y);
Vector2f(const float *data);
virtual ~Vector2f();
float cross(const Vector2f &b) const;
float operator %(const Vector2f &v) const;
float operator *(const Vector2f &v) const;
inline Vector2f operator*(const float &right) const {
return Vector::operator*(right);
}
/**
* accessors
*/
void setX(float x) { (*this)(0) = x; }
void setY(float y) { (*this)(1) = y; }
const float &getX() const { return (*this)(0); }
const float &getY() const { return (*this)(1); }
};
class __EXPORT Vector2 :
public Vector2f
{
};
int __EXPORT vector2fTest();
} // math
src/modules/mathlib/math/Vector3.cpp
+2 -2
View File
@@ -74,9 +74,9 @@ Vector3::~Vector3()
{
}
Vector3 Vector3::cross(const Vector3 &b)
Vector3 Vector3::cross(const Vector3 &b) const
{
Vector3 &a = *this;
const Vector3 &a = *this;
Vector3 result;
result(0) = a(1) * b(2) - a(2) * b(1);
result(1) = a(2) * b(0) - a(0) * b(2);
src/modules/mathlib/math/Vector3.hpp
+6 -1
View File
@@ -53,7 +53,7 @@ public:
Vector3(float x, float y, float z);
Vector3(const float *data);
virtual ~Vector3();
Vector3 cross(const Vector3 &b);
Vector3 cross(const Vector3 &b) const;
/**
* accessors
@@ -65,6 +65,11 @@ public:
const float &getY() const { return (*this)(1); }
const float &getZ() const { return (*this)(2); }
};
class __EXPORT Vector3f :
public Vector3
{
};
int __EXPORT vector3Test();
} // math
src/modules/mathlib/math/arm/Vector.hpp
+19 -3
View File
@@ -178,8 +178,15 @@ public:
getRows());
return result;
}
inline Vector operator-(void) const {
Vector result(getRows());
arm_negate_f32((float *)getData(),
result.getData(),
getRows());
return result;
}
// other functions
inline float dot(const Vector &right) {
inline float dot(const Vector &right) const {
float result = 0;
arm_dot_prod_f32((float *)getData(),
(float *)right.getData(),
@@ -187,12 +194,21 @@ public:
&result);
return result;
}
inline float norm() {
inline float norm() const {
return sqrtf(dot(*this));
}
inline Vector unit() {
inline float length() const {
return norm();
}
inline Vector unit() const {
return (*this) / norm();
}
inline Vector normalized() const {
return unit();
}
inline void normalize() {
(*this) = (*this) / norm();
}
inline static Vector zero(size_t rows) {
Vector result(rows);
// calloc returns zeroed memory
src/modules/mathlib/math/generic/Vector.hpp
+21 -3
View File
@@ -184,8 +184,17 @@ public:
return result;
}
inline Vector operator-(void) const {
Vector result(getRows());
for (size_t i = 0; i < getRows(); i++) {
result(i) = -((*this)(i));
}
return result;
}
// other functions
inline float dot(const Vector &right) {
inline float dot(const Vector &right) const {
float result = 0;
for (size_t i = 0; i < getRows(); i++) {
@@ -194,12 +203,21 @@ public:
return result;
}
inline float norm() {
inline float norm() const {
return sqrtf(dot(*this));
}
inline Vector unit() {
inline float length() const {
return norm();
}
inline Vector unit() const {
return (*this) / norm();
}
inline Vector normalized() const {
return unit();
}
inline void normalize() {
(*this) = (*this) / norm();
}
inline static Vector zero(size_t rows) {
Vector result(rows);
// calloc returns zeroed memory
src/modules/mathlib/mathlib.h
+4
View File
@@ -39,12 +39,16 @@
#ifdef __cplusplus
#pragma once
#include "math/Dcm.hpp"
#include "math/EulerAngles.hpp"
#include "math/Matrix.hpp"
#include "math/Quaternion.hpp"
#include "math/Vector.hpp"
#include "math/Vector3.hpp"
#include "math/Vector2f.hpp"
#include "math/Limits.hpp"
#endif
src/modules/mathlib/module.mk
+3 -1
View File
@@ -36,11 +36,13 @@
#
SRCS = math/test/test.cpp \
math/Vector.cpp \
math/Vector2f.cpp \
math/Vector3.cpp \
math/EulerAngles.cpp \
math/Quaternion.cpp \
math/Dcm.cpp \
math/Matrix.cpp
math/Matrix.cpp \
math/Limits.cpp
#
# In order to include .config we first have to save off the
src/modules/mavlink/mavlink.c
+6 -9
View File
@@ -144,14 +144,6 @@ set_hil_on_off(bool hil_enabled)
/* Enable HIL */
if (hil_enabled && !mavlink_hil_enabled) {
/* Advertise topics */
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
/* sensore level hil */
pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
mavlink_hil_enabled = true;
/* ramp up some HIL-related subscriptions */
@@ -714,6 +706,8 @@ int mavlink_thread_main(int argc, char *argv[])
lowspeed_counter++;
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
/* sleep quarter the time */
usleep(25000);
@@ -725,10 +719,13 @@ int mavlink_thread_main(int argc, char *argv[])
/* send parameters at 20 Hz (if queued for sending) */
mavlink_pm_queued_send();
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
/* sleep quarter the time */
usleep(25000);
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
if (baudrate > 57600) {
mavlink_pm_queued_send();
}
@@ -787,7 +784,7 @@ int mavlink_main(int argc, char *argv[])
errx(0, "mavlink already running\n");
thread_should_exit = false;
mavlink_task = task_spawn("mavlink",
mavlink_task = task_spawn_cmd("mavlink",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
src/modules/mavlink/mavlink_hil.h
-9
View File
@@ -41,15 +41,6 @@
extern bool mavlink_hil_enabled;
extern struct vehicle_global_position_s hil_global_pos;
extern struct vehicle_attitude_s hil_attitude;
extern struct sensor_combined_s hil_sensors;
extern struct vehicle_gps_position_s hil_gps;
extern orb_advert_t pub_hil_global_pos;
extern orb_advert_t pub_hil_attitude;
extern orb_advert_t pub_hil_sensors;
extern orb_advert_t pub_hil_gps;
/**
* Enable / disable Hardware in the Loop simulation mode.
*
src/modules/mavlink/mavlink_receiver.c → src/modules/mavlink/mavlink_receiver.cpp
+142 -69
View File
@@ -49,7 +49,6 @@
#include <fcntl.h>
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
@@ -62,10 +61,17 @@
#include <stdlib.h>
#include <poll.h>
#include <mathlib/mathlib.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
#include <mavlink/mavlink_log.h>
__BEGIN_DECLS
#include "mavlink_bridge_header.h"
#include "waypoints.h"
#include "orb_topics.h"
#include "missionlib.h"
@@ -73,8 +79,12 @@
#include "mavlink_parameters.h"
#include "util.h"
extern bool gcs_link;
__END_DECLS
/* XXX should be in a header somewhere */
pthread_t receive_start(int uart);
extern "C" pthread_t receive_start(int uart);
static void handle_message(mavlink_message_t *msg);
static void *receive_thread(void *arg);
@@ -88,18 +98,18 @@ struct vehicle_global_position_s hil_global_pos;
struct vehicle_attitude_s hil_attitude;
struct vehicle_gps_position_s hil_gps;
struct sensor_combined_s hil_sensors;
orb_advert_t pub_hil_global_pos = -1;
orb_advert_t pub_hil_attitude = -1;
orb_advert_t pub_hil_gps = -1;
orb_advert_t pub_hil_sensors = -1;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t pub_hil_attitude = -1;
static orb_advert_t pub_hil_gps = -1;
static orb_advert_t pub_hil_sensors = -1;
static orb_advert_t pub_hil_airspeed = -1;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
static orb_advert_t offboard_control_sp_pub = -1;
static orb_advert_t vicon_position_pub = -1;
extern bool gcs_link;
static orb_advert_t telemetry_status_pub = -1;
static void
handle_message(mavlink_message_t *msg)
@@ -141,10 +151,10 @@ handle_message(mavlink_message_t *msg)
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* publish */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
/* publish */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
}
@@ -281,7 +291,7 @@ handle_message(mavlink_message_t *msg)
}
offboard_control_sp.armed = ml_armed;
offboard_control_sp.mode = ml_mode;
offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
offboard_control_sp.timestamp = hrt_absolute_time();
@@ -296,6 +306,33 @@ handle_message(mavlink_message_t *msg)
}
}
/* handle status updates of the radio */
if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
struct telemetry_status_s tstatus;
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
/* publish telemetry status topic */
tstatus.timestamp = hrt_absolute_time();
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
tstatus.txbuf = rstatus.txbuf;
tstatus.noise = rstatus.noise;
tstatus.remote_noise = rstatus.remnoise;
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (telemetry_status_pub == 0) {
telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus);
}
}
/*
* Only decode hil messages in HIL mode.
*
@@ -308,10 +345,10 @@ handle_message(mavlink_message_t *msg)
uint64_t timestamp = hrt_absolute_time();
if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) {
mavlink_highres_imu_t imu;
mavlink_msg_highres_imu_decode(msg, &imu);
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
/* packet counter */
static uint16_t hil_counter = 0;
@@ -370,8 +407,34 @@ handle_message(mavlink_message_t *msg)
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.accelerometer_counter = hil_counter;
/* differential pressure */
hil_sensors.differential_pressure_pa = imu.diff_pressure;
hil_sensors.differential_pressure_counter = hil_counter;
/* airspeed from differential pressure, ambient pressure and temp */
struct airspeed_s airspeed;
airspeed.timestamp = hrt_absolute_time();
float ias = calc_indicated_airspeed(imu.diff_pressure);
// XXX need to fix this
float tas = ias;
airspeed.indicated_airspeed_m_s = ias;
airspeed.true_airspeed_m_s = tas;
if (pub_hil_airspeed < 0) {
pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
/* publish */
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
if (pub_hil_sensors > 0) {
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
} else {
pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
}
// increment counters
hil_counter++;
@@ -379,21 +442,16 @@ handle_message(mavlink_message_t *msg)
// output
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil imu at %d hz\n", hil_frames/10);
printf("receiving hil sensor at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
}
if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) {
mavlink_gps_raw_int_t gps;
mavlink_msg_gps_raw_int_decode(msg, &gps);
/* packet counter */
static uint16_t hil_counter = 0;
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
mavlink_hil_gps_t gps;
mavlink_msg_hil_gps_decode(msg, &gps);
/* gps */
hil_gps.timestamp_position = gps.time_usec;
@@ -412,54 +470,40 @@ handle_message(mavlink_message_t *msg)
/* go back to -PI..PI */
if (heading_rad > M_PI_F)
heading_rad -= 2.0f * M_PI_F;
hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad);
hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
hil_gps.vel_d_m_s = 0.0f;
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
hil_gps.vel_ned_valid = true;
/* COG (course over ground) is speced as -PI..+PI */
/* COG (course over ground) is spec'ed as -PI..+PI */
hil_gps.cog_rad = heading_rad;
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
/* publish */
orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
/* publish GPS measurement data */
if (pub_hil_gps > 0) {
orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
} else {
pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
}
// increment counters
hil_counter += 1 ;
hil_frames += 1 ;
// output
// if ((timestamp - old_timestamp) > 10000000) {
// printf("receiving hil gps at %d hz\n", hil_frames/10);
// old_timestamp = timestamp;
// hil_frames = 0;
// }
}
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) {
mavlink_hil_state_t hil_state;
mavlink_msg_hil_state_decode(msg, &hil_state);
mavlink_hil_state_quaternion_t hil_state;
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
/* Calculate Rotation Matrix */
//TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
struct airspeed_s airspeed;
airspeed.timestamp = hrt_absolute_time();
airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
if (mavlink_system.type == MAV_TYPE_FIXED_WING) {
//TODO: assuming low pitch and roll values for now
hil_attitude.R[0][0] = cosf(hil_state.yaw);
hil_attitude.R[0][1] = sinf(hil_state.yaw);
hil_attitude.R[0][2] = 0.0f;
hil_attitude.R[1][0] = -sinf(hil_state.yaw);
hil_attitude.R[1][1] = cosf(hil_state.yaw);
hil_attitude.R[1][2] = 0.0f;
hil_attitude.R[2][0] = 0.0f;
hil_attitude.R[2][1] = 0.0f;
hil_attitude.R[2][2] = 1.0f;
hil_attitude.R_valid = true;
if (pub_hil_airspeed < 0) {
pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
@@ -468,21 +512,48 @@ handle_message(mavlink_message_t *msg)
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
/* set timestamp and notify processes (broadcast) */
hil_global_pos.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
hil_attitude.roll = hil_state.roll;
hil_attitude.pitch = hil_state.pitch;
hil_attitude.yaw = hil_state.yaw;
if (pub_hil_global_pos > 0) {
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
}
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Dcm C_nb(q);
math::EulerAngles euler(C_nb);
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
hil_attitude.R[i][j] = C_nb(i, j);
hil_attitude.R_valid = true;
/* set quaternion */
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
hil_attitude.q[2] = q(2);
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
hil_attitude.roll = euler.getPhi();
hil_attitude.pitch = euler.getTheta();
hil_attitude.yaw = euler.getPsi();
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
/* set timestamp and notify processes (broadcast) */
hil_attitude.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
if (pub_hil_attitude > 0) {
orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
} else {
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
}
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
@@ -553,7 +624,9 @@ receive_thread(void *arg)
while (!thread_should_exit) {
struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
if (poll(fds, 1, timeout) > 0) {
/* non-blocking read. read may return negative values */
@@ -592,7 +665,7 @@ receive_start(int uart)
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
pthread_attr_setstacksize(&receiveloop_attr, 3000);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
src/modules/mavlink/missionlib.c
+174 -1
View File
@@ -60,6 +60,7 @@
#include <stdlib.h>
#include <poll.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <mavlink/mavlink_log.h>
@@ -73,6 +74,10 @@
#include "mavlink_parameters.h"
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
static uint64_t loiter_start_time;
static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
struct vehicle_global_position_setpoint_s *sp);
int
mavlink_missionlib_send_message(mavlink_message_t *msg)
@@ -122,6 +127,52 @@ uint64_t mavlink_missionlib_get_system_timestamp()
return hrt_absolute_time();
}
/**
* Set special vehicle setpoint fields based on current mission item.
*
* @return true if the mission item could be interpreted
* successfully, it return false on failure.
*/
bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
struct vehicle_global_position_setpoint_s *sp)
{
switch (command) {
case MAV_CMD_NAV_LOITER_UNLIM:
sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
break;
case MAV_CMD_NAV_LOITER_TIME:
sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
loiter_start_time = hrt_absolute_time();
break;
// case MAV_CMD_NAV_LOITER_TURNS:
// sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT;
// break;
case MAV_CMD_NAV_WAYPOINT:
sp->nav_cmd = NAV_CMD_WAYPOINT;
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
break;
case MAV_CMD_NAV_LAND:
sp->nav_cmd = NAV_CMD_LAND;
break;
case MAV_CMD_NAV_TAKEOFF:
sp->nav_cmd = NAV_CMD_TAKEOFF;
break;
default:
/* abort */
return false;
}
sp->loiter_radius = param3;
sp->loiter_direction = (param3 >= 0) ? 1 : -1;
sp->param1 = param1;
sp->param1 = param2;
sp->param1 = param3;
sp->param1 = param4;
}
/**
* This callback is executed each time a waypoint changes.
*
@@ -133,9 +184,13 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
{
static orb_advert_t global_position_setpoint_pub = -1;
static orb_advert_t global_position_set_triplet_pub = -1;
static orb_advert_t local_position_setpoint_pub = -1;
static unsigned last_waypoint_index = -1;
char buf[50] = {0};
// XXX include check if WP is supported, jump to next if not
/* Update controller setpoints */
if (frame == (int)MAV_FRAME_GLOBAL) {
/* global, absolute waypoint */
@@ -145,8 +200,9 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.altitude = param7_alt_z;
sp.altitude_is_relative = false;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize publication if necessary */
/* Initialize setpoint publication if necessary */
if (global_position_setpoint_pub < 0) {
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
@@ -154,6 +210,113 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
}
/* fill triplet: previous, current, next waypoint */
struct vehicle_global_position_set_triplet_s triplet;
/* current waypoint is same as sp */
memcpy(&(triplet.current), &sp, sizeof(sp));
/*
* Check if previous WP (in mission, not in execution order)
* is available and identify correct index
*/
int last_setpoint_index = -1;
bool last_setpoint_valid = false;
/* at first waypoint, but cycled once through mission */
if (index == 0 && last_waypoint_index > 0) {
last_setpoint_index = last_waypoint_index;
} else {
last_setpoint_index = index - 1;
}
while (last_setpoint_index >= 0) {
if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL &&
(wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
last_setpoint_valid = true;
break;
}
last_setpoint_index--;
}
/*
* Check if next WP (in mission, not in execution order)
* is available and identify correct index
*/
int next_setpoint_index = -1;
bool next_setpoint_valid = false;
/* at last waypoint, try to re-loop through mission as default */
if (index == (wpm->size - 1) && wpm->size > 1) {
next_setpoint_index = 0;
} else if (wpm->size > 1) {
next_setpoint_index = index + 1;
}
while (next_setpoint_index < wpm->size - 1) {
if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
next_setpoint_valid = true;
break;
}
next_setpoint_index++;
}
/* populate last and next */
triplet.previous_valid = false;
triplet.next_valid = false;
if (last_setpoint_valid) {
triplet.previous_valid = true;
struct vehicle_global_position_setpoint_s sp;
sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[last_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
set_special_fields(wpm->waypoints[last_setpoint_index].param1,
wpm->waypoints[last_setpoint_index].param2,
wpm->waypoints[last_setpoint_index].param3,
wpm->waypoints[last_setpoint_index].param4,
wpm->waypoints[last_setpoint_index].command, &sp);
memcpy(&(triplet.previous), &sp, sizeof(sp));
}
if (next_setpoint_valid) {
triplet.next_valid = true;
struct vehicle_global_position_setpoint_s sp;
sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[next_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
set_special_fields(wpm->waypoints[next_setpoint_index].param1,
wpm->waypoints[next_setpoint_index].param2,
wpm->waypoints[next_setpoint_index].param3,
wpm->waypoints[next_setpoint_index].param4,
wpm->waypoints[next_setpoint_index].command, &sp);
memcpy(&(triplet.next), &sp, sizeof(sp));
}
/* Initialize triplet publication if necessary */
if (global_position_set_triplet_pub < 0) {
global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
} else {
orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
}
sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
@@ -164,6 +327,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.altitude = param7_alt_z;
sp.altitude_is_relative = true;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize publication if necessary */
if (global_position_setpoint_pub < 0) {
@@ -173,6 +337,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
}
sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
@@ -192,8 +358,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
}
sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} else {
warnx("non-navigation WP, ignoring");
mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring.");
return;
}
/* only set this for known waypoint types (non-navigation types would have returned earlier) */
last_waypoint_index = index;
mavlink_missionlib_send_gcs_string(buf);
printf("%s\n", buf);
//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
src/modules/mavlink/module.mk
+1 -1
View File
@@ -40,7 +40,7 @@ SRCS += mavlink.c \
missionlib.c \
mavlink_parameters.c \
mavlink_log.c \
mavlink_receiver.c \
mavlink_receiver.cpp \
orb_listener.c \
waypoints.c
src/modules/mavlink/orb_listener.c
+30 -11
View File
@@ -72,6 +72,8 @@ struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_0;
struct vehicle_attitude_s att;
struct mavlink_subscriptions mavlink_subs;
@@ -116,6 +118,7 @@ static void l_debug_key_value(const struct listener *l);
static void l_optical_flow(const struct listener *l);
static void l_vehicle_rates_setpoint(const struct listener *l);
static void l_home(const struct listener *l);
static void l_airspeed(const struct listener *l);
static const struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@@ -140,6 +143,7 @@ static const struct listener listeners[] = {
{l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
{l_home, &mavlink_subs.home_sub, 0},
{l_airspeed, &mavlink_subs.airspeed_sub, 0},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -192,7 +196,7 @@ l_sensor_combined(const struct listener *l)
raw.gyro_rad_s[1], raw.gyro_rad_s[2],
raw.magnetometer_ga[0],
raw.magnetometer_ga[1], raw.magnetometer_ga[2],
raw.baro_pres_mbar, 0 /* no diff pressure yet */,
raw.baro_pres_mbar, raw.differential_pressure_pa,
raw.baro_alt_meter, raw.baro_temp_celcius,
fields_updated);
@@ -202,9 +206,6 @@ l_sensor_combined(const struct listener *l)
void
l_vehicle_attitude(const struct listener *l)
{
struct vehicle_attitude_s att;
/* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
@@ -564,28 +565,26 @@ l_manual_control_setpoint(const struct listener *l)
void
l_vehicle_attitude_controls(const struct listener *l)
{
struct actuator_controls_effective_s actuators;
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl0 ",
actuators.control_effective[0]);
actuators_0.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl1 ",
actuators.control_effective[1]);
actuators_0.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl2 ",
actuators.control_effective[2]);
actuators_0.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl3 ",
actuators.control_effective[3]);
actuators_0.control_effective[3]);
}
}
@@ -626,6 +625,22 @@ l_home(const struct listener *l)
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
}
void
l_airspeed(const struct listener *l)
{
struct airspeed_s airspeed;
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
float alt = global_pos.alt;
float climb = global_pos.vz;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
}
static void *
uorb_receive_thread(void *arg)
{
@@ -765,6 +780,10 @@ uorb_receive_start(void)
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
/* --- AIRSPEED / VFR / HUD --- */
mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */
/* start the listener loop */
pthread_attr_t uorb_attr;
pthread_attr_init(&uorb_attr);
src/modules/mavlink/orb_topics.h
+4
View File
@@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/optical_flow.h>
@@ -59,7 +60,9 @@
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/airspeed.h>
#include <drivers/drv_rc_input.h>
struct mavlink_subscriptions {
@@ -83,6 +86,7 @@ struct mavlink_subscriptions {
int optical_flow;
int rates_setpoint_sub;
int home_sub;
int airspeed_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
src/modules/mavlink/waypoints.c
+47 -21
View File
@@ -347,15 +347,24 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
{
static uint16_t counter;
// Do not flood the precious wireless link with debug data
// if (wpm->size > 0 && counter % 10 == 0) {
// printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id);
// }
if (wpm->current_active_wp_id < wpm->size) {
float orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
float orbit;
if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
} else {
// XXX set default orbit via param
orbit = 15.0f;
}
int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
float dist = -1.0f;
@@ -374,10 +383,9 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
}
if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
wpm->pos_reached = true;
if (counter % 100 == 0)
printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit);
}
// else
@@ -394,29 +402,47 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
if (wpm->timestamp_firstinside_orbit == 0) {
// Announce that last waypoint was reached
printf("Reached waypoint %u for the first time \n", cur_wp->seq);
mavlink_wpm_send_waypoint_reached(cur_wp->seq);
wpm->timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) {
printf("Reached waypoint %u long enough \n", cur_wp->seq);
bool time_elapsed = false;
if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
}
} else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
time_elapsed = true;
}
if (time_elapsed) {
if (cur_wp->autocontinue) {
cur_wp->current = 0;
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
* activated restart the waypoint list from the beginning
*/
wpm->current_active_wp_id = 0;
/* only accept supported navigation waypoints, skip unknown ones */
do {
} else {
if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
wpm->current_active_wp_id++;
}
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
* activated restart the waypoint list from the beginning
*/
wpm->current_active_wp_id = 0;
} else {
if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
wpm->current_active_wp_id++;
}
} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
// Fly to next waypoint
wpm->timestamp_firstinside_orbit = 0;
src/modules/mavlink_onboard/mavlink.c
+1 -1
View File
@@ -495,7 +495,7 @@ int mavlink_onboard_main(int argc, char *argv[])
errx(0, "mavlink already running\n");
thread_should_exit = false;
mavlink_task = task_spawn("mavlink_onboard",
mavlink_task = task_spawn_cmd("mavlink_onboard",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
src/modules/multirotor_att_control/multirotor_att_control_main.c
+1 -1
View File
@@ -466,7 +466,7 @@ int multirotor_att_control_main(int argc, char *argv[])
if (!strcmp(argv[1 + optioncount], "start")) {
thread_should_exit = false;
mc_task = task_spawn("multirotor_att_control",
mc_task = task_spawn_cmd("multirotor_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
2048,
src/modules/multirotor_att_control/multirotor_attitude_control.c
+17 -15
View File
@@ -1,12 +1,12 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
/*
* @file multirotor_attitude_control.c
* Implementation of attitude controller
*
* Implementation of attitude controller for multirotors.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include "multirotor_attitude_control.h"
@@ -163,16 +171,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
static uint64_t last_run = 0;
static uint64_t last_input = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
last_run = hrt_absolute_time();
if (last_input != att_sp->timestamp) {
last_input = att_sp->timestamp;
}
static int sensor_delay;
sensor_delay = hrt_absolute_time() - att->timestamp;
static int motor_skip_counter = 0;
static PID_t pitch_controller;
@@ -190,10 +194,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
1000.0f, PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
1000.0f, PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
initialized = true;
}
src/modules/multirotor_att_control/multirotor_attitude_control.h
+15 -7
View File
@@ -1,12 +1,12 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
/*
* @file multirotor_attitude_control.h
* Attitude control for multi rotors.
*
* Definition of attitude controller for multirotors.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
src/modules/multirotor_att_control/multirotor_rate_control.c
+25 -30
View File
@@ -1,8 +1,10 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,10 +38,12 @@
/**
* @file multirotor_rate_control.c
*
* Implementation of rate controller
* Implementation of rate controller for multirotors.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include "multirotor_rate_control.h"
@@ -150,14 +154,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
{
static float roll_control_last = 0;
static float pitch_control_last = 0;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
static uint64_t last_input = 0;
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
if (last_input != rate_sp->timestamp) {
last_input = rate_sp->timestamp;
}
@@ -166,6 +166,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static int motor_skip_counter = 0;
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
@@ -176,43 +179,35 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
parameters_init(&h);
parameters_update(&h, &p);
initialized = true;
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
// (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
}
/* calculate current control outputs */
/* reset integral if on ground */
if (rate_sp->thrust < 0.01f) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
}
/* control pitch (forward) output */
float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
/* increase resilience to faulty control inputs */
if (isfinite(pitch_control)) {
pitch_control_last = pitch_control;
} else {
pitch_control = 0.0f;
warnx("rej. NaN ctrl pitch");
}
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT);
/* increase resilience to faulty control inputs */
if (isfinite(roll_control)) {
roll_control_last = roll_control;
} else {
roll_control = 0.0f;
warnx("rej. NaN ctrl roll");
}
/* control yaw rate */
/* control yaw rate */ //XXX use library here
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
/* increase resilience to faulty control inputs */
src/modules/multirotor_att_control/multirotor_rate_control.h
+16 -8
View File
@@ -1,12 +1,12 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
/*
* @file multirotor_attitude_control.h
* Attitude control for multi rotors.
*
* Definition of rate controller for multirotors.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef MULTIROTOR_RATE_CONTROL_H_
src/modules/multirotor_pos_control/multirotor_pos_control.c
+2 -2
View File
@@ -94,7 +94,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_spawn().
* to task_spawn_cmd().
*/
int multirotor_pos_control_main(int argc, char *argv[])
{
@@ -110,7 +110,7 @@ int multirotor_pos_control_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("multirotor pos control",
deamon_task = task_spawn_cmd("multirotor pos control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
src/modules/position_estimator_mc/position_estimator_mc_main.c
+1 -1
View File
@@ -123,7 +123,7 @@ int position_estimator_mc_main(int argc, char *argv[])
}
thread_should_exit = false;
position_estimator_mc_task = task_spawn("position_estimator_mc",
position_estimator_mc_task = task_spawn_cmd("position_estimator_mc",
SCHED_RR,
SCHED_PRIORITY_MAX - 5,
4096,
src/modules/px4iofirmware/adc.c
+1 -1
View File
@@ -41,7 +41,7 @@
#include <nuttx/arch.h>
#include <arch/stm32/chip.h>
#include <stm32_internal.h>
#include <stm32.h>
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
src/modules/sdlog/sdlog.c
+2 -2
View File
@@ -161,7 +161,7 @@ bool logging_enabled = true;
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
* to task_spawn_cmd().
*/
int sdlog_main(int argc, char *argv[])
{
@@ -177,7 +177,7 @@ int sdlog_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("sdlog",
deamon_task = task_spawn_cmd("sdlog",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
4096,
src/modules/sdlog2/logbuffer.c
+3 -3
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* 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
@@ -37,7 +37,7 @@
*
* Ring FIFO buffer for binary log data.
*
* @author Anton Babushkin <rk3dov@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <string.h>
@@ -121,7 +121,7 @@ int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part)
} else {
// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
n = lb->size - lb->read_ptr;
*is_part = true;
*is_part = lb->write_ptr > 0;
}
*ptr = &(lb->data[lb->read_ptr]);
src/modules/sdlog2/logbuffer.h
+2 -2
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* 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
@@ -37,7 +37,7 @@
*
* Ring FIFO buffer for binary log data.
*
* @author Anton Babushkin <rk3dov@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef SDLOG2_RINGBUFFER_H_
src/modules/sdlog2/sdlog2.c
+131 -69
View File
@@ -2,7 +2,7 @@
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <rk3dov@gmail.com>
* 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
@@ -40,7 +40,7 @@
* does the heavy SD I/O in a low-priority worker thread.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <rk3dov@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@@ -64,6 +64,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
@@ -102,7 +103,7 @@
//#define SDLOG2_DEBUG
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */
@@ -113,35 +114,34 @@ static const int MAX_WRITE_CHUNK = 512;
static const int MIN_BYTES_TO_WRITE = 512;
static const char *mountpoint = "/fs/microsd";
int log_file = -1;
int mavlink_fd = -1;
static int mavlink_fd = -1;
struct logbuffer_s lb;
/* mutex / condition to synchronize threads */
pthread_mutex_t logbuffer_mutex;
pthread_cond_t logbuffer_cond;
static pthread_mutex_t logbuffer_mutex;
static pthread_cond_t logbuffer_cond;
char folder_path[64];
static char folder_path[64];
/* statistics counters */
unsigned long log_bytes_written = 0;
uint64_t start_time = 0;
unsigned long log_msgs_written = 0;
unsigned long log_msgs_skipped = 0;
static unsigned long log_bytes_written = 0;
static uint64_t start_time = 0;
static unsigned long log_msgs_written = 0;
static unsigned long log_msgs_skipped = 0;
/* current state of logging */
bool logging_enabled = false;
static bool logging_enabled = false;
/* enable logging on start (-e option) */
bool log_on_start = false;
static bool log_on_start = false;
/* enable logging when armed (-a option) */
bool log_when_armed = false;
static bool log_when_armed = false;
/* delay = 1 / rate (rate defined by -r option) */
useconds_t sleep_delay = 0;
static useconds_t sleep_delay = 0;
/* helper flag to track system state changes */
bool flag_system_armed = false;
static bool flag_system_armed = false;
pthread_t logwriter_pthread = 0;
static pthread_t logwriter_pthread = 0;
/**
* Log buffer writing thread. Open and close file here.
@@ -171,17 +171,17 @@ static void sdlog2_status(void);
/**
* Start logging: create new file and start log writer thread.
*/
void sdlog2_start_log();
static void sdlog2_start_log(void);
/**
* Stop logging: stop log writer thread and close log file.
*/
void sdlog2_stop_log();
static void sdlog2_stop_log(void);
/**
* Write a header to log file: list of message formats.
*/
void write_formats(int fd);
static void write_formats(int fd);
static bool file_exist(const char *filename);
@@ -195,12 +195,12 @@ static void handle_status(struct vehicle_status_s *cmd);
/**
* Create folder for current logging session. Store folder name in 'log_folder'.
*/
static int create_logfolder();
static int create_logfolder(void);
/**
* Select first free log file name and open it.
*/
static int open_logfile();
static int open_logfile(void);
static void
sdlog2_usage(const char *reason)
@@ -236,11 +236,11 @@ int sdlog2_main(int argc, char *argv[])
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("sdlog2",
main_thread_should_exit = false;
deamon_task = task_spawn_cmd("sdlog2",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
2048,
3000,
sdlog2_thread_main,
(const char **)argv);
exit(0);
@@ -251,7 +251,7 @@ int sdlog2_main(int argc, char *argv[])
printf("\tsdlog2 is not started\n");
}
thread_should_exit = true;
main_thread_should_exit = true;
exit(0);
}
@@ -285,22 +285,6 @@ int create_logfolder()
if (mkdir_ret == 0) {
/* folder does not exist, success */
/* copy parser script file */
// TODO
/*
char mfile_out[100];
sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber);
int ret = file_copy(mfile_in, mfile_out);
if (!ret) {
warnx("copied m file to %s", mfile_out);
} else {
warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out);
}
*/
break;
} else if (mkdir_ret == -1) {
@@ -346,10 +330,10 @@ int open_logfile()
fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC);
if (fd == 0) {
errx(1, "opening %s failed.", path_buf);
warn("opening %s failed", path_buf);
}
warnx("logging to: %s", path_buf);
warnx("logging to: %s.", path_buf);
mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf);
return fd;
@@ -357,7 +341,7 @@ int open_logfile()
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
warn("all %d possible files exist already", MAX_NO_LOGFILE);
warnx("all %d possible files exist already.", MAX_NO_LOGFILE);
return -1;
}
@@ -383,8 +367,7 @@ static void *logwriter_thread(void *arg)
bool should_wait = false;
bool is_part = false;
while (!thread_should_exit && !logwriter_should_exit) {
while (true) {
/* make sure threads are synchronized */
pthread_mutex_lock(&logbuffer_mutex);
@@ -394,7 +377,7 @@ static void *logwriter_thread(void *arg)
}
/* only wait if no data is available to process */
if (should_wait) {
if (should_wait && !logwriter_should_exit) {
/* blocking wait for new data at this line */
pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex);
}
@@ -402,6 +385,11 @@ static void *logwriter_thread(void *arg)
/* only get pointer to thread-safe data, do heavy I/O a few lines down */
int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part);
#ifdef SDLOG2_DEBUG
int rp = logbuf->read_ptr;
int wp = logbuf->write_ptr;
#endif
/* continue */
pthread_mutex_unlock(&logbuffer_mutex);
@@ -418,11 +406,11 @@ static void *logwriter_thread(void *arg)
should_wait = (n == available) && !is_part;
#ifdef SDLOG2_DEBUG
printf("%i wrote: %i of %i, is_part=%i, should_wait=%i", poll_count, n, available, (int)is_part, (int)should_wait);
printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait);
#endif
if (n < 0) {
thread_should_exit = true;
main_thread_should_exit = true;
err(1, "error writing log file");
}
@@ -431,19 +419,33 @@ static void *logwriter_thread(void *arg)
}
} else {
n = 0;
#ifdef SDLOG2_DEBUG
printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit);
#endif
/* exit only with empty buffer */
if (main_thread_should_exit || logwriter_should_exit) {
#ifdef SDLOG2_DEBUG
printf("break logwriter thread\n");
#endif
break;
}
should_wait = true;
}
if (poll_count % 10 == 0) {
if (++poll_count == 10) {
fsync(log_file);
poll_count = 0;
}
poll_count++;
}
fsync(log_file);
close(log_file);
#ifdef SDLOG2_DEBUG
printf("logwriter thread exit\n");
#endif
return OK;
}
@@ -470,10 +472,9 @@ void sdlog2_start_log()
pthread_attr_setstacksize(&receiveloop_attr, 2048);
logwriter_should_exit = false;
pthread_t thread;
/* start log buffer emptying thread */
if (0 != pthread_create(&thread, &receiveloop_attr, logwriter_thread, &lb)) {
if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) {
errx(1, "error creating logwriter thread");
}
@@ -487,16 +488,20 @@ void sdlog2_stop_log()
mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
logging_enabled = false;
logwriter_should_exit = true;
/* wake up write thread one last time */
pthread_mutex_lock(&logbuffer_mutex);
logwriter_should_exit = true;
pthread_cond_signal(&logbuffer_cond);
/* unlock, now the writer thread may return */
pthread_mutex_unlock(&logbuffer_mutex);
/* wait for write thread to return */
(void)pthread_join(logwriter_pthread, NULL);
int ret;
if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) {
warnx("error joining logwriter thread: %i", ret);
}
logwriter_pthread = 0;
sdlog2_status();
}
@@ -517,7 +522,7 @@ void write_formats(int fd)
for (i = 0; i < log_formats_num; i++) {
log_format_packet.body = log_formats[i];
write(fd, &log_format_packet, sizeof(log_format_packet));
log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet));
}
fsync(fd);
@@ -607,12 +612,9 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "can't allocate log buffer, exiting.");
}
/* file descriptors to wait for */
struct pollfd fds_control[2];
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 15;
const ssize_t fdsc = 17;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@@ -627,6 +629,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct sensor_combined_s sensor;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_effective_s act_controls_effective;
@@ -648,6 +651,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int sensor_sub;
int att_sub;
int att_sp_sub;
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int act_controls_effective_sub;
@@ -658,6 +662,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int vicon_pos_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
} subs;
/* log message buffer: header + body */
@@ -677,6 +682,10 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_STAT_s log_STAT;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS;
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -720,6 +729,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- RATES SETPOINT --- */
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
fds[fdsc_count].fd = subs.rates_sp_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR OUTPUTS --- */
subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
fds[fdsc_count].fd = subs.act_outputs_sub;
@@ -774,6 +789,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- AIRSPEED --- */
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
fds[fdsc_count].fd = subs.airspeed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -806,7 +827,7 @@ int sdlog2_thread_main(int argc, char *argv[])
if (log_on_start)
sdlog2_start_log();
while (!thread_should_exit) {
while (!main_thread_should_exit) {
/* decide use usleep() or blocking poll() */
bool use_sleep = sleep_delay > 0 && logging_enabled;
@@ -816,7 +837,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* handle the poll result */
if (poll_ret < 0) {
warnx("ERROR: poll error, stop logging.");
thread_should_exit = true;
main_thread_should_exit = true;
} else if (poll_ret > 0) {
@@ -883,7 +904,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_GPS.epv = buf.gps_pos.epv_m;
log_msg.body.log_GPS.lat = buf.gps_pos.lat;
log_msg.body.log_GPS.lon = buf.gps_pos.lon;
log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001;
log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f;
log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s;
log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s;
@@ -953,6 +974,9 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATT.roll = buf.att.roll;
log_msg.body.log_ATT.pitch = buf.att.pitch;
log_msg.body.log_ATT.yaw = buf.att.yaw;
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
@@ -963,9 +987,20 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body;
log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust;
LOGBUFFER_WRITE_AND_COUNT(ATSP);
}
/* --- RATES SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp);
log_msg.msg_type = LOG_ARSP_MSG;
log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw;
LOGBUFFER_WRITE_AND_COUNT(ARSP);
}
/* --- ACTUATOR OUTPUTS --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs);
@@ -1022,7 +1057,14 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
// TODO not implemented yet
log_msg.msg_type = LOG_GPOS_MSG;
log_msg.body.log_GPOS.lat = buf.global_pos.lat;
log_msg.body.log_GPOS.lon = buf.global_pos.lon;
log_msg.body.log_GPOS.alt = buf.global_pos.alt;
log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
/* --- VICON POSITION --- */
@@ -1034,7 +1076,15 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- FLOW --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
// TODO not implemented yet
log_msg.msg_type = LOG_FLOW_MSG;
log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.quality = buf.flow.quality;
log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
LOGBUFFER_WRITE_AND_COUNT(FLOW);
}
/* --- RC CHANNELS --- */
@@ -1046,10 +1096,22 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(RC);
}
/* --- AIRSPEED --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
#ifdef SDLOG2_DEBUG
printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
#endif
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
#ifdef SDLOG2_DEBUG
printf("signal %i", logbuffer_count(&lb));
printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
#endif
/* only request write if several packets can be written at once */
pthread_cond_signal(&logbuffer_cond);
src/modules/sdlog2/sdlog2_format.h
+2 -2
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* 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
@@ -37,7 +37,7 @@
*
* General log format structures and macro.
*
* @author Anton Babushkin <rk3dov@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
/*
src/modules/sdlog2/sdlog2_messages.h
+50 -4
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* 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
@@ -37,7 +37,7 @@
*
* Log messages and structures definition.
*
* @author Anton Babushkin <rk3dov@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef SDLOG2_MESSAGES_H_
@@ -60,6 +60,9 @@ struct log_ATT_s {
float roll;
float pitch;
float yaw;
float roll_rate;
float pitch_rate;
float yaw_rate;
};
/* --- ATSP - ATTITUDE SET POINT --- */
@@ -68,6 +71,7 @@ struct log_ATSP_s {
float roll_sp;
float pitch_sp;
float yaw_sp;
float thrust_sp;
};
/* --- IMU - IMU SENSORS --- */
@@ -167,14 +171,52 @@ struct log_RC_s {
struct log_OUT0_s {
float output[8];
};
/* --- AIRS - AIRSPEED --- */
#define LOG_AIRS_MSG 13
struct log_AIRS_s {
float indicated_airspeed;
float true_airspeed;
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
#define LOG_ARSP_MSG 14
struct log_ARSP_s {
float roll_rate_sp;
float pitch_rate_sp;
float yaw_rate_sp;
};
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
struct log_FLOW_s {
int16_t flow_raw_x;
int16_t flow_raw_y;
float flow_comp_x;
float flow_comp_y;
float distance;
uint8_t quality;
uint8_t sensor_id;
};
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
#define LOG_GPOS_MSG 16
struct log_GPOS_s {
int32_t lat;
int32_t lon;
float alt;
float vel_n;
float vel_e;
float vel_d;
};
#pragma pack(pop)
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(ATT, "fff", "Roll,Pitch,Yaw"),
LOG_FORMAT(ATSP, "fff", "RollSP,PitchSP,YawSP"),
LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
@@ -184,6 +226,10 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
src/modules/sensors/sensor_params.c
+4
View File
@@ -48,6 +48,10 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
src/modules/sensors/sensors.cpp
+14 -5
View File
@@ -194,6 +194,7 @@ private:
float scaling_factor[_rc_max_chan_count];
float gyro_offset[3];
float gyro_scale[3];
float mag_offset[3];
float mag_scale[3];
float accel_offset[3];
@@ -243,6 +244,7 @@ private:
param_t rc_demix;
param_t gyro_offset[3];
param_t gyro_scale[3];
param_t accel_offset[3];
param_t accel_scale[3];
param_t mag_offset[3];
@@ -486,6 +488,9 @@ Sensors::Sensors() :
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
_parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
_parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE");
_parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE");
_parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE");
/* accel offsets */
_parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
@@ -696,6 +701,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
param_get(_parameter_handles.gyro_scale[0], &(_parameters.gyro_scale[0]));
param_get(_parameter_handles.gyro_scale[1], &(_parameters.gyro_scale[1]));
param_get(_parameter_handles.gyro_scale[2], &(_parameters.gyro_scale[2]));
/* accel offsets */
param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0]));
@@ -983,11 +991,11 @@ Sensors::parameter_update_poll(bool forced)
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
_parameters.gyro_offset[0],
1.0f,
_parameters.gyro_scale[0],
_parameters.gyro_offset[1],
1.0f,
_parameters.gyro_scale[1],
_parameters.gyro_offset[2],
1.0f,
_parameters.gyro_scale[2],
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
@@ -1445,7 +1453,7 @@ Sensors::start()
ASSERT(_sensors_task == -1);
/* start the task */
_sensors_task = task_spawn("sensors_task",
_sensors_task = task_spawn_cmd("sensors_task",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
@@ -1502,6 +1510,7 @@ int sensors_main(int argc, char *argv[])
}
}
errx(1, "unrecognized command");
warnx("unrecognized command");
return 1;
}
src/modules/systemlib/conversions.h
+1 -5
View File
@@ -43,11 +43,7 @@
#define CONVERSIONS_H_
#include <float.h>
#include <stdint.h>
#define CONSTANTS_ONE_G 9.80665f // m/s^2
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3
#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K)
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C
#include <systemlib/geo/geo.h>
__BEGIN_DECLS
src/modules/systemlib/cpuload.c
+24 -30
View File
@@ -1,9 +1,8 @@
/****************************************************************************
* configs/px4fmu/src/up_leds.c
* arch/arm/src/board/up_leds.c
*
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -15,7 +14,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 NuttX 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.
*
@@ -34,18 +33,26 @@
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
/**
* @file cpuload.c
*
* Measurement of CPU load of each individual task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <arch/arch.h>
#include <debug.h>
#include <sys/time.h>
#include <sched.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
@@ -54,26 +61,13 @@
#ifdef CONFIG_SCHED_INSTRUMENTATION
/****************************************************************************
* Definitions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
__EXPORT void sched_note_start(FAR _TCB *tcb);
__EXPORT void sched_note_stop(FAR _TCB *tcb);
__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb);
/****************************************************************************
* Name:
****************************************************************************/
__EXPORT void sched_note_start(FAR struct tcb_s *tcb);
__EXPORT void sched_note_stop(FAR struct tcb_s *tcb);
__EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb);
__EXPORT struct system_load_s system_load;
extern FAR _TCB *sched_gettcb(pid_t pid);
extern FAR struct _TCB *sched_gettcb(pid_t pid);
void cpuload_initialize_once()
{
@@ -109,7 +103,7 @@ void cpuload_initialize_once()
// }
}
void sched_note_start(FAR _TCB *tcb)
void sched_note_start(FAR struct tcb_s *tcb)
{
/* search first free slot */
int i;
@@ -128,7 +122,7 @@ void sched_note_start(FAR _TCB *tcb)
}
}
void sched_note_stop(FAR _TCB *tcb)
void sched_note_stop(FAR struct tcb_s *tcb)
{
int i;
@@ -145,7 +139,7 @@ void sched_note_stop(FAR _TCB *tcb)
}
}
void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb)
{
uint64_t new_time = hrt_absolute_time();
src/modules/systemlib/cpuload.h
+4 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -43,7 +43,7 @@ struct system_load_taskinfo_s {
uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
uint64_t curr_start_time; ///< Start time of the current scheduling slot
uint64_t start_time; ///< FIRST start time of task
FAR struct _TCB *tcb; ///<
FAR struct tcb_s *tcb; ///<
bool valid; ///< Task is currently active / valid
};
@@ -60,4 +60,6 @@ __EXPORT extern struct system_load_s system_load;
__EXPORT void cpuload_initialize_once(void);
__END_DECLS
#endif
src/modules/systemlib/err.c
+2 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -61,7 +61,7 @@ const char *
getprogname(void)
{
#if CONFIG_TASK_NAME_SIZE > 0
_TCB *thisproc = sched_self();
FAR struct tcb_s *thisproc = sched_self();
return thisproc->name;
#else
src/modules/systemlib/geo/geo.h
+16
View File
@@ -45,9 +45,23 @@
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/
#pragma once
__BEGIN_DECLS
#include <stdbool.h>
#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */
#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */
#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */
/* compatibility aliases */
#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH
#define GRAVITY_MSS CONSTANTS_ONE_G
// XXX remove
struct crosstrack_error_s {
bool past_end; // Flag indicating we are past the end of the line/arc segment
float distance; // Distance in meters to closest point on line/arc
@@ -111,3 +125,5 @@ __EXPORT float _wrap_180(float bearing);
__EXPORT float _wrap_360(float bearing);
__EXPORT float _wrap_pi(float bearing);
__EXPORT float _wrap_2pi(float bearing);
__END_DECLS
src/modules/systemlib/pid/pid.c
+41 -26
View File
@@ -1,9 +1,11 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,14 +38,21 @@
/**
* @file pid.c
* Implementation of generic PID control interface
*
* Implementation of generic PID control interface.
*
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include "pid.h"
#include <math.h>
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
float limit, uint8_t mode)
float limit, uint8_t mode, float dt_min)
{
pid->kp = kp;
pid->ki = ki;
@@ -51,13 +60,13 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
pid->intmax = intmax;
pid->limit = limit;
pid->mode = mode;
pid->count = 0;
pid->saturated = 0;
pid->last_output = 0;
pid->sp = 0;
pid->error_previous = 0;
pid->integral = 0;
pid->dt_min = dt_min;
pid->count = 0.0f;
pid->saturated = 0.0f;
pid->last_output = 0.0f;
pid->sp = 0.0f;
pid->error_previous = 0.0f;
pid->integral = 0.0f;
}
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
{
@@ -136,14 +145,14 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
// Calculated current error value
float error = pid->sp - val;
if (isfinite(error)) { // Why is this necessary? DEW
pid->error_previous = error;
}
// Calculate or measured current error derivative
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / dt;
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
d = -val_dot;
@@ -152,6 +161,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
d = 0.0f;
}
if (!isfinite(d)) {
d = 0.0f;
}
// Calculate the error integral and check for saturation
i = pid->integral + (error * dt);
@@ -162,7 +175,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
} else {
if (!isfinite(i)) {
i = 0;
i = 0.0f;
}
pid->integral = i;
@@ -170,17 +183,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
}
// Calculate the output. Limit output magnitude to pid->limit
float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (output > pid->limit) output = pid->limit;
if (output < -pid->limit) output = -pid->limit;
float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (isfinite(output)) {
if (output > pid->limit) {
output = pid->limit;
} else if (output < -pid->limit) {
output = -pid->limit;
}
pid->last_output = output;
}
return pid->last_output;
}
src/modules/systemlib/pid/pid.h
+23 -7
View File
@@ -1,9 +1,11 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,7 +38,14 @@
/**
* @file pid.h
* Definition of generic PID control interface
*
* Definition of generic PID control interface.
*
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef PID_H_
@@ -44,11 +53,16 @@
#include <stdint.h>
__BEGIN_DECLS
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC 0
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC_NO_SP 1
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
#define PID_MODE_DERIVATIV_SET 1
#define PID_MODE_DERIVATIV_SET 2
// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
#define PID_MODE_DERIVATIV_NONE 9
@@ -62,17 +76,19 @@ typedef struct {
float error_previous;
float last_output;
float limit;
float dt_min;
uint8_t mode;
uint8_t count;
uint8_t saturated;
} PID_t;
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
//void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
__EXPORT void pid_reset_integral(PID_t *pid);
__END_DECLS
#endif /* PID_H_ */
src/modules/systemlib/systemlib.c
+3 -3
View File
@@ -50,7 +50,7 @@
#include "systemlib.h"
static void kill_task(FAR _TCB *tcb, FAR void *arg);
static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
void killall()
{
@@ -60,12 +60,12 @@ void killall()
sched_foreach(kill_task, NULL);
}
static void kill_task(FAR _TCB *tcb, FAR void *arg)
static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
{
kill(tcb->pid, SIGUSR1);
}
int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
{
int pid;
src/modules/systemlib/systemlib.h
+3 -3
View File
@@ -42,11 +42,11 @@
#include <float.h>
#include <stdint.h>
__BEGIN_DECLS
/** Reboots the board */
extern void up_systemreset(void) noreturn_function;
__BEGIN_DECLS
/** Sends SIGUSR1 to all processes */
__EXPORT void killall(void);
@@ -58,7 +58,7 @@ __EXPORT void killall(void);
#endif
/** Starts a task and performs any specific accounting, scheduler setup, etc. */
__EXPORT int task_spawn(const char *name,
__EXPORT int task_spawn_cmd(const char *name,
int priority,
int scheduler,
int stack_size,

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