/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* 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 position_estimator_main.c
* Model-identification based position estimator for multirotors
*/
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <string.h>
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
#include <systemlib/geo/geo.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#include "position_estimator_mc_params.h"
//#include <uORB/topics/debug_key_value.h>
#include "codegen/kalman_dlqe2.h"
#include "codegen/kalman_dlqe2_initialize.h"
#include "codegen/kalman_dlqe3.h"
#include "codegen/kalman_dlqe3_initialize.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_mc_task; /**< Handle of deamon task / thread */
__EXPORT int position_estimator_mc_main(int argc, char *argv[]);
int position_estimator_mc_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
warnx("%s\n", reason);
warnx("usage: position_estimator_mc {start|stop|status}");
exit(1);
}
/**
* The position_estimator_mc_thread 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 position_estimator_mc_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("position_estimator_mc already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
position_estimator_mc_task = task_spawn("position_estimator_mc",
SCHED_RR,
SCHED_PRIORITY_MAX - 5,
4096,
position_estimator_mc_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("position_estimator_mc is running");
} else {
warnx("position_estimator_mc not started");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_mc_thread_main(int argc, char *argv[])
{
/* welcome user */
warnx("[position_estimator_mc] started");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[position_estimator_mc] started");
/* initialize values */
float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */
// float rotMatrix[4] = {1.0f, 0.0f, 0.0f, 1.0f};
float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f};
float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f};
float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f};
// XXX this is terribly wrong and should actual dT instead
const float dT_const_50 = 1.0f/50.0f;
float addNoise = 0.0f;
float sigma = 0.0f;
//computed from dlqe in matlab
const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f};
// XXX implement baro filter
const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f};
float K[3] = {0.0f, 0.0f, 0.0f};
int baro_loop_cnt = 0;
int baro_loop_end = 70; /* measurement for 1 second */
float p0_Pa = 0.0f; /* to determin while start up */
float rho0 = 1.293f; /* standard pressure */
const float const_earth_gravity = 9.81f;
float posX = 0.0f;
float posY = 0.0f;
float posZ = 0.0f;
double lat_current;
double lon_current;
float alt_current;
float gps_origin_altitude = 0.0f;
/* Initialize filter */
kalman_dlqe2_initialize();
kalman_dlqe3_initialize();
/* declare and safely initialize all structs */
struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */
struct vehicle_vicon_position_s vicon_pos;
memset(&vicon_pos, 0, sizeof(vicon_pos));
struct actuator_controls_effective_s act_eff;
memset(&act_eff, 0, sizeof(act_eff));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
struct vehicle_local_position_s local_pos_est;
memset(&local_pos_est, 0, sizeof(local_pos_est));
struct vehicle_global_position_s global_pos_est;
memset(&global_pos_est, 0, sizeof(global_pos_est));
/* subscribe */
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
int sub_params = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0));
int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* advertise */
orb_advert_t local_pos_est_pub = 0;
orb_advert_t global_pos_est_pub = 0;
struct position_estimator_mc_params pos1D_params;
struct position_estimator_mc_param_handles pos1D_param_handles;
/* initialize parameter handles */
parameters_init(&pos1D_param_handles);
bool flag_use_gps = false;
bool flag_use_baro = false;
bool flag_baro_initialized = false; /* in any case disable baroINITdone */
/* FIRST PARAMETER READ at START UP*/
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */
/* FIRST PARAMETER UPDATE */
parameters_update(&pos1D_param_handles, &pos1D_params);
flag_use_baro = pos1D_params.baro;
sigma = pos1D_params.sigma;
addNoise = pos1D_params.addNoise;
/* END FIRST PARAMETER UPDATE */
/* try to grab a vicon message - if it fails, go for GPS. */
/* make sure the next orb_check() can't return true unless we get a timely update */
orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
/* allow 200 ms for vicon to come in */
usleep(200000);
/* check if we got vicon */
bool update_check;
orb_check(vicon_pos_sub, &update_check);
/* if no update was available, use GPS */
flag_use_gps = !update_check;
if (flag_use_gps) {
mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked");
/* wait until gps signal turns valid, only then can we initialize the projection */
// XXX magic number
float hdop_threshold_m = 4.0f;
float vdop_threshold_m = 8.0f;
/*
* If horizontal dilution of precision (hdop / eph)
* and vertical diluation of precision (vdop / epv)
* are below a certain threshold (e.g. 4 m), AND
* home position is not yet set AND the last GPS
* GPS measurement is not older than two seconds AND
* the system is currently not armed, set home
* position to the current position.
*/
while (!(gps.fix_type == 3
&& (gps.eph_m < hdop_threshold_m)
&& (gps.epv_m < vdop_threshold_m)
&& (hrt_absolute_time() - gps.timestamp_position < 2000000))) {
struct pollfd fds1[2] = {
{ .fd = vehicle_gps_sub, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN },
};
/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
* this choice is critical, since the vehicle status might not
* actually change, if this app is started after GPS lock was
* aquired.
*/
if (poll(fds1, 2, 5000)) {
if (fds1[0].revents & POLLIN){
/* Read gps position */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
}
if (fds1[1].revents & POLLIN){
/* Read out parameters to check for an update there, e.g. useGPS variable */
/* read from param to clear updated flag */
struct parameter_update_s updated;
orb_copy(ORB_ID(parameter_update), sub_params, &updated);
/* update parameters */
parameters_update(&pos1D_param_handles, &pos1D_params);
}
}
static int printcounter = 0;
if (printcounter == 100) {
printcounter = 0;
warnx("[pos_est_mc] wait for GPS fix");
}
printcounter++;
}
/* get gps value for first initialization */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
lat_current = ((double)(gps.lat)) * 1e-7d;
lon_current = ((double)(gps.lon)) * 1e-7d;
alt_current = gps.alt * 1e-3f;
gps_origin_altitude = alt_current;
/* initialize coordinates */
map_projection_init(lat_current, lon_current);
/* publish global position messages only after first GPS message */
printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current);
} else {
mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
/* onboard calculated position estimations */
}
thread_running = true;
struct pollfd fds2[3] = {
{ .fd = vehicle_gps_sub, .events = POLLIN },
{ .fd = vicon_pos_sub, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN },
};
bool vicon_updated = false;
bool gps_updated = false;
/**< main_loop */
while (!thread_should_exit) {
int ret = poll(fds2, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate
if (ret < 0) {
/* poll error */
} else {
if (fds2[2].revents & POLLIN){
/* new parameter */
/* read from param to clear updated flag */
struct parameter_update_s updated;
orb_copy(ORB_ID(parameter_update), sub_params, &updated);
/* update parameters */
parameters_update(&pos1D_param_handles, &pos1D_params);
flag_use_baro = pos1D_params.baro;
sigma = pos1D_params.sigma;
addNoise = pos1D_params.addNoise;
}
vicon_updated = false; /* default is no vicon_updated */
if (fds2[1].revents & POLLIN) {
/* new vicon position */
orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
posX = vicon_pos.x;
posY = vicon_pos.y;
posZ = vicon_pos.z;
vicon_updated = true; /* set flag for vicon update */
} /* end of poll call for vicon updates */
gps_updated = false;
if (fds2[0].revents & POLLIN) {
/* new GPS value */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
/* Project gps lat lon (Geographic coordinate system) to plane*/
map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1]));
posX = z[0];
posY = z[1];
posZ = (float)(gps.alt * 1e-3f);
gps_updated = true;
}
/* Main estimator loop */
orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff);
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor);
// barometric pressure estimation at start up
if (!flag_baro_initialized){
// mean calculation over several measurements
if (baro_loop_cnt<baro_loop_end) {
p0_Pa += (sensor.baro_pres_mbar*100);
baro_loop_cnt++;
} else {
p0_Pa /= (float)(baro_loop_cnt);
flag_baro_initialized = true;
char *baro_m_start = "barometer initialized with p0 = ";
char p0_char[15];
sprintf(p0_char, "%8.2f", (double)(p0_Pa/100));
char *baro_m_end = " mbar";
char str[80];
strcpy(str,baro_m_start);
strcat(str,p0_char);
strcat(str,baro_m_end);
mavlink_log_info(mavlink_fd, str);
}
}
if (flag_use_gps) {
/* initialize map projection with the last estimate (not at full rate) */
if (gps.fix_type > 2) {
/* x-y-position/velocity estimation in earth frame = gps frame */
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori);
memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori);
memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
/* z-position/velocity estimation in earth frame = vicon frame */
float z_est = 0.0f;
if (flag_baro_initialized && flag_use_baro) {
z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity);
K[0] = K_vicon_50Hz[0];
K[1] = K_vicon_50Hz[1];
K[2] = K_vicon_50Hz[2];
gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */
} else {
z_est = posZ;
K[0] = K_vicon_50Hz[0];
K[1] = K_vicon_50Hz[1];
K[2] = K_vicon_50Hz[2];
}
kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori);
memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
local_pos_est.x = x_x_aposteriori_k[0];
local_pos_est.vx = x_x_aposteriori_k[1];
local_pos_est.y = x_y_aposteriori_k[0];
local_pos_est.vy = x_y_aposteriori_k[1];
local_pos_est.z = x_z_aposteriori_k[0];
local_pos_est.vz = x_z_aposteriori_k[1];
local_pos_est.timestamp = hrt_absolute_time();
if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) {
/* publish local position estimate */
if (local_pos_est_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
} else {
local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
}
/* publish on GPS updates */
if (gps_updated) {
double lat, lon;
float alt = z_est + gps_origin_altitude;
map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon);
global_pos_est.lat = lat;
global_pos_est.lon = lon;
global_pos_est.alt = alt;
if (global_pos_est_pub > 0) {
orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est);
} else {
global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est);
}
}
}
}
} else {
/* x-y-position/velocity estimation in earth frame = vicon frame */
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori);
memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori);
memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
/* z-position/velocity estimation in earth frame = vicon frame */
float z_est = 0.0f;
float local_sigma = 0.0f;
if (flag_baro_initialized && flag_use_baro) {
z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity);
K[0] = K_vicon_50Hz[0];
K[1] = K_vicon_50Hz[1];
K[2] = K_vicon_50Hz[2];
vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */
local_sigma = 0.0f; /* don't add noise on barometer in any case */
} else {
z_est = posZ;
K[0] = K_vicon_50Hz[0];
K[1] = K_vicon_50Hz[1];
K[2] = K_vicon_50Hz[2];
local_sigma = sigma;
}
kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori);
memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
local_pos_est.x = x_x_aposteriori_k[0];
local_pos_est.vx = x_x_aposteriori_k[1];
local_pos_est.y = x_y_aposteriori_k[0];
local_pos_est.vy = x_y_aposteriori_k[1];
local_pos_est.z = x_z_aposteriori_k[0];
local_pos_est.vz = x_z_aposteriori_k[1];
local_pos_est.timestamp = hrt_absolute_time();
if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
}
}
} /* end of poll return value check */
}
printf("[pos_est_mc] exiting.\n");
mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting");
thread_running = false;
return 0;
}