dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into sdlog2

This commit is contained in:
Anton Babushkin 2013-06-17 14:44:45 +04:00
parent 138ce117ab badaa5e4a2
commit 22a925adeb
21 changed files with 2294 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
makefiles/config_px4fmu_default.mk
View File

@ -63,9 +63,9 @@ MODULES += modules/gpio_led
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3_comp
#MODULES += modules/position_estimator_mc
MODULES += modules/position_estimator
MODULES += modules/att_pos_estimator_ekf
MODULES += examples/flow_position_estimator
#
# Vehicle Control
@ -75,11 +75,12 @@ MODULES += modules/fixedwing_att_control
MODULES += modules/fixedwing_pos_control
MODULES += modules/multirotor_att_control
MODULES += modules/multirotor_pos_control
MODULES += examples/flow_position_control
MODULES += examples/flow_speed_control
#
# Logging
#
MODULES += modules/sdlog
MODULES += modules/sdlog2
#

153
src/drivers/ets_airspeed/ets_airspeed.cpp
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,13 +77,13 @@
#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.
*/
#define MIN_ACCURATE_DIFF_PRES_PA 12
@ -105,38 +105,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 +144,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 +173,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 +203,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 +230,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 +352,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 +433,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 +448,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];
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 +500,7 @@ ETSAirspeed::collect()
ret = OK;
perf_end(_sample_perf);
return ret;
}
@ -511,17 +513,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 +657,7 @@ start(int i2c_bus)
fail:
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
@ -668,15 +671,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 +775,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 +791,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 +805,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.

13
src/drivers/hott_telemetry/messages.c
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);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}

589
src/examples/flow_position_control/flow_position_control_main.c Normal file
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;
}

113
src/examples/flow_position_control/flow_position_control_params.c Normal file
View File

@ -0,0 +1,113 @@
/****************************************************************************
*
* 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 */
PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f);
PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f);
PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f);
PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f);
PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f);
PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f);
PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f);
PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f);
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;
}

100
src/examples/flow_position_control/flow_position_control_params.h Normal file
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);

41
src/examples/flow_position_control/module.mk Normal file
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

458
src/examples/flow_position_estimator/flow_position_estimator_main.c Normal file
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;
}

72
src/examples/flow_position_estimator/flow_position_estimator_params.c Normal file
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;
}

68
src/examples/flow_position_estimator/flow_position_estimator_params.h Normal file
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);

41
src/examples/flow_position_estimator/module.mk Normal file
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

361
src/examples/flow_speed_control/flow_speed_control_main.c Normal file
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;
}

70
src/examples/flow_speed_control/flow_speed_control_params.c Normal file
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;
}

70
src/examples/flow_speed_control/flow_speed_control_params.h Normal file
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);

41
src/examples/flow_speed_control/module.mk Normal file
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

2
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -126,7 +126,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
attitude_estimator_ekf_task = task_spawn("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);

19
src/modules/sdlog2/sdlog2.c
View File

@ -241,7 +241,7 @@ int sdlog2_main(int argc, char *argv[])
deamon_task = task_spawn("sdlog2",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
2048,
3000,
sdlog2_thread_main,
(const char **)argv);
exit(0);
@ -661,6 +661,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 */
@ -680,6 +681,7 @@ 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;
} body;
} log_msg = {
@ -784,6 +786,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.
@ -1070,6 +1078,15 @@ 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);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
#ifdef SDLOG2_DEBUG

8
src/modules/sdlog2/sdlog2_messages.h
View File

@ -172,6 +172,13 @@ 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 13
struct log_ARSP_s {
@ -196,6 +203,7 @@ 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"),
};

6
src/modules/uORB/objects_common.cpp
View File

@ -101,6 +101,9 @@ ORB_DEFINE(vehicle_command, struct vehicle_command_s);
#include "topics/vehicle_local_position_setpoint.h"
ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
#include "topics/vehicle_bodyframe_speed_setpoint.h"
ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
#include "topics/vehicle_global_position_setpoint.h"
ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
@ -119,6 +122,9 @@ ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
#include "topics/optical_flow.h"
ORB_DEFINE(optical_flow, struct optical_flow_s);
#include "topics/filtered_bottom_flow.h"
ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s);
#include "topics/omnidirectional_flow.h"
ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s);

74
src/modules/uORB/topics/filtered_bottom_flow.h Normal file
View File

@ -0,0 +1,74 @@
/****************************************************************************
*
* 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 filtered_bottom_flow.h
* Definition of the filtered bottom flow uORB topic.
*/
#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_
#define TOPIC_FILTERED_BOTTOM_FLOW_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* Filtered bottom flow in bodyframe.
*/
struct filtered_bottom_flow_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
float sumx; /**< Integrated bodyframe x flow in meters */
float sumy; /**< Integrated bodyframe y flow in meters */
float vx; /**< Flow bodyframe x speed, m/s */
float vy; /**< Flow bodyframe y Speed, m/s */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(filtered_bottom_flow);
#endif

69
src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h Normal file
View File

@ -0,0 +1,69 @@
/****************************************************************************
*
* 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 vehicle_bodyframe_speed_setpoint.h
* Definition of the bodyframe speed setpoint uORB topic.
*/
#ifndef TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_
#define TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
struct vehicle_bodyframe_speed_setpoint_s
{
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
float vx; /**< in m/s */
float vy; /**< in m/s */
// float vz; /**< in m/s */
float thrust_sp;
float yaw_sp; /**< in radian -PI +PI */
}; /**< Speed in bodyframe to go to */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_bodyframe_speed_setpoint);
#endif