/****************************************************************************
*
* 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(¶m_handles);
parameters_update(¶m_handles, ¶ms);
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(¶m_handles, ¶ms);
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;
}