2012-08-04 19:12:36 -03:00
/****************************************************************************
*
* Copyright ( C ) 2008 - 2012 PX4 Development Team . All rights reserved .
* Author : Tobias Naegeli < naegelit @ student . ethz . ch >
* Thomas Gubler < thomasgubler @ student . ethz . ch >
* Julian Oes < joes @ 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 Model - identification based position estimator for multirotors
*/
# include <nuttx/config.h>
# include <unistd.h>
# include <stdlib.h>
# include <stdio.h>
# include <stdbool.h>
# include <fcntl.h>
# include <v1.0/common/mavlink.h>
# include <float.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/vehicle_status.h>
# include <uORB/topics/vehicle_attitude.h>
# include <uORB/topics/vehicle_gps_position.h>
# include <uORB/topics/vehicle_global_position.h>
# include <poll.h>
# include "codegen/position_estimator.h"
# define N_STATES 6
# define ERROR_COVARIANCE_INIT 3
# define R_EARTH 6371000.0
# define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
# define REPROJECTION_COUNTER_LIMIT 125
__EXPORT int position_estimator_main ( int argc , char * argv [ ] ) ;
static uint16_t position_estimator_counter_position_information ;
/* values for map projection */
static double phi_1 ;
static double sin_phi_1 ;
static double cos_phi_1 ;
static double lambda_0 ;
static double scale ;
/**
* Initializes the map transformation .
*
* Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
* @ param lat in degrees ( 47.1234567 ° , not 471234567 ° )
* @ param lon in degrees ( 8.1234567 ° , not 81234567 ° )
*/
static void map_projection_init ( double lat_0 , double lon_0 ) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
phi_1 = lat_0 / 180.0 * M_PI ;
lambda_0 = lon_0 / 180.0 * M_PI ;
sin_phi_1 = sin ( phi_1 ) ;
cos_phi_1 = cos ( phi_1 ) ;
/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
const double r_earth = 6371000 ;
double lat1 = phi_1 ;
double lon1 = lambda_0 ;
double lat2 = phi_1 + 0.5 / 180 * M_PI ;
double lon2 = lambda_0 + 0.5 / 180 * M_PI ;
double sin_lat_2 = sin ( lat2 ) ;
double cos_lat_2 = cos ( lat2 ) ;
double d = acos ( sin ( lat1 ) * sin_lat_2 + cos ( lat1 ) * cos_lat_2 * cos ( lon2 - lon1 ) ) * r_earth ;
/* 2) calculate distance rho on plane */
double k_bar = 0 ;
double c = acos ( sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos ( lon2 - lambda_0 ) ) ;
if ( 0 ! = c )
k_bar = c / sin ( c ) ;
double x2 = k_bar * ( cos_lat_2 * sin ( lon2 - lambda_0 ) ) ; //Projection of point 2 on plane
double y2 = k_bar * ( ( cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos ( lon2 - lambda_0 ) ) ) ;
double rho = sqrt ( pow ( x2 , 2 ) + pow ( y2 , 2 ) ) ;
scale = d / rho ;
}
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
* @ param x north
* @ param y east
* @ param lat in degrees ( 47.1234567 ° , not 471234567 ° )
* @ param lon in degrees ( 8.1234567 ° , not 81234567 ° )
*/
static void map_projection_project ( double lat , double lon , float * x , float * y )
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double phi = lat / 180.0 * M_PI ;
double lambda = lon / 180.0 * M_PI ;
double sin_phi = sin ( phi ) ;
double cos_phi = cos ( phi ) ;
double k_bar = 0 ;
/* using small angle approximation (formula in comment is without aproximation) */
double c = acos ( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * ( 1 - pow ( ( lambda - lambda_0 ) , 2 ) / 2 ) ) ; //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
if ( 0 ! = c )
k_bar = c / sin ( c ) ;
/* using small angle approximation (formula in comment is without aproximation) */
* y = k_bar * ( cos_phi * ( lambda - lambda_0 ) ) * scale ; //*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
* x = k_bar * ( ( cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * ( 1 - pow ( ( lambda - lambda_0 ) , 2 ) / 2 ) ) ) * scale ; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
}
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
*
* @ param x north
* @ param y east
* @ param lat in degrees ( 47.1234567 ° , not 471234567 ° )
* @ param lon in degrees ( 8.1234567 ° , not 81234567 ° )
*/
static void map_projection_reproject ( float x , float y , double * lat , double * lon )
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double x_descaled = x / scale ;
double y_descaled = y / scale ;
double c = sqrt ( pow ( x_descaled , 2 ) + pow ( y_descaled , 2 ) ) ;
double sin_c = sin ( c ) ;
double cos_c = cos ( c ) ;
double lat_sphere = 0 ;
if ( c ! = 0 )
lat_sphere = asin ( cos_c * sin_phi_1 + ( x_descaled * sin_c * cos_phi_1 ) / c ) ;
else
lat_sphere = asin ( cos_c * sin_phi_1 ) ;
// printf("lat_sphere = %.10f\n",lat_sphere);
double lon_sphere = 0 ;
if ( phi_1 = = M_PI / 2 ) {
//using small angle approximation (formula in comment is without aproximation)
lon_sphere = ( lambda_0 - y_descaled / x_descaled ) ; //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
} else if ( phi_1 = = - M_PI / 2 ) {
//using small angle approximation (formula in comment is without aproximation)
lon_sphere = ( lambda_0 + y_descaled / x_descaled ) ; //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
} else {
lon_sphere = ( lambda_0 + atan2 ( y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c ) ) ;
//using small angle approximation
// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
// if(denominator != 0)
// {
// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
// }
// else
// {
// ...
// }
}
// printf("lon_sphere = %.10f\n",lon_sphere);
* lat = lat_sphere * 180.0 / M_PI ;
* lon = lon_sphere * 180.0 / M_PI ;
}
/****************************************************************************
* main
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
int position_estimator_main ( int argc , char * argv [ ] )
{
/* welcome user */
printf ( " [multirotor position_estimator] started \n " ) ;
/* initialize values */
static float u [ 2 ] = { 0 , 0 } ;
static float z [ 3 ] = { 0 , 0 , 0 } ;
static float xapo [ N_STATES ] = { 0 , 0 , 0 , 0 , 0 , 0 } ;
static float Papo [ N_STATES * N_STATES ] = { ERROR_COVARIANCE_INIT , 0 , 0 , 0 , 0 , 0 ,
ERROR_COVARIANCE_INIT , 0 , 0 , 0 , 0 , 0 ,
ERROR_COVARIANCE_INIT , 0 , 0 , 0 , 0 , 0 ,
ERROR_COVARIANCE_INIT , 0 , 0 , 0 , 0 , 0 ,
ERROR_COVARIANCE_INIT , 0 , 0 , 0 , 0 , 0 ,
ERROR_COVARIANCE_INIT , 0 , 0 , 0 , 0 , 0
} ;
static float xapo1 [ N_STATES ] ;
static float Papo1 [ 36 ] ;
static float gps_covariance [ 3 ] = { 0.0f , 0.0f , 0.0f } ;
static uint16_t counter = 0 ;
position_estimator_counter_position_information = 0 ;
uint8_t predict_only = 1 ;
bool gps_valid = false ;
bool new_initialization = true ;
static double lat_current = 0 ; //[°]] --> 47.0
static double lon_current = 0 ; //[°]] -->8.5
//TODO: handle flight without gps but with estimator
/* subscribe to vehicle status, attitude, gps */
struct vehicle_gps_position_s gps ;
struct vehicle_status_s vstatus ;
struct vehicle_attitude_s att ;
int vehicle_gps_sub = orb_subscribe ( ORB_ID ( vehicle_gps_position ) ) ;
int vehicle_status_sub = orb_subscribe ( ORB_ID ( vehicle_status ) ) ;
/* subscribe to attitude at 100 Hz */
int vehicle_attitude_sub = orb_subscribe ( ORB_ID ( vehicle_attitude ) ) ;
/* wait until gps signal turns valid, only then can we initialize the projection */
while ( ! gps_valid ) {
struct pollfd fds [ 1 ] = { { . fd = vehicle_gps_sub , . 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 ( fds , 1 , 5000 ) ) {
/* Wait for the GPS update to propagate (we have some time) */
usleep ( 5000 ) ;
/* Read wether the vehicle status changed */
orb_copy ( ORB_ID ( vehicle_status ) , vehicle_status_sub , & vstatus ) ;
gps_valid = vstatus . gps_valid ;
}
}
/* get gps value for first initialization */
orb_copy ( ORB_ID ( vehicle_gps_position ) , vehicle_gps_sub , & gps ) ;
lat_current = ( ( double ) ( gps . lat ) ) * 1e-7 ;
lon_current = ( ( double ) ( gps . lon ) ) * 1e-7 ;
/* publish global position messages only after first GPS message */
struct vehicle_global_position_s global_pos = {
. lat = lat_current * 1e7 ,
. lon = lon_current * 1e7 ,
. alt = gps . alt
} ;
2012-08-22 03:44:22 -03:00
orb_advert_t global_pos_pub = orb_advertise ( ORB_ID ( vehicle_global_position ) , & global_pos ) ;
2012-08-04 19:12:36 -03:00
printf ( " [multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f \n " , lat_current , lon_current ) ;
while ( 1 ) {
/*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */
struct pollfd fds [ 1 ] = { { . fd = vehicle_attitude_sub , . events = POLLIN } } ;
if ( poll ( fds , 1 , 5000 ) < = 0 ) {
/* error / timeout */
} else {
orb_copy ( ORB_ID ( vehicle_attitude ) , vehicle_attitude_sub , & att ) ;
/* got attitude, updating pos as well */
orb_copy ( ORB_ID ( vehicle_gps_position ) , vehicle_gps_sub , & gps ) ;
orb_copy ( ORB_ID ( vehicle_status ) , vehicle_status_sub , & vstatus ) ;
/*copy attitude */
u [ 0 ] = att . roll ;
u [ 1 ] = att . pitch ;
/* initialize map projection with the last estimate (not at full rate) */
if ( counter % PROJECTION_INITIALIZE_COUNTER_LIMIT = = 0 ) {
map_projection_init ( lat_current , lon_current ) ;
new_initialization = true ;
} else {
new_initialization = false ;
}
/*check if new gps values are available */
gps_valid = vstatus . gps_valid ;
if ( gps_valid ) { //we are safe to use the gps signal (it has good quality)
predict_only = 0 ;
/* Project gps lat lon (Geographic coordinate system) to plane*/
map_projection_project ( ( double ) ( gps . lat ) * 1e-7 , ( double ) ( gps . lon ) * 1e-7 , & ( z [ 0 ] ) , & ( z [ 1 ] ) ) ;
/* copy altitude */
z [ 2 ] = ( gps . alt ) * 1e-3 ;
gps_covariance [ 0 ] = gps . eph ; //TODO: needs scaling
gps_covariance [ 1 ] = gps . eph ;
gps_covariance [ 2 ] = gps . epv ;
} else {
/* we can not use the gps signal (it is of low quality) */
predict_only = 1 ;
}
// predict_only = 0; //TODO: only for testing, removeme, XXX
// z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX
// usleep(100000); //TODO: only for testing, removeme, XXX
/*Get new estimation (this is calculated in the plane) */
//TODO: if new_initialization == true: use 0,0,0, else use xapo
if ( true = = new_initialization ) { //TODO,XXX: uncomment!
xapo [ 0 ] = 0 ; //we have a new plane initialization. the current estimate is in the center of the plane
xapo [ 2 ] = 0 ;
xapo [ 4 ] = 0 ;
position_estimator ( u , z , xapo , Papo , gps_covariance , predict_only , xapo1 , Papo1 ) ;
} else {
position_estimator ( u , z , xapo , Papo , gps_covariance , predict_only , xapo1 , Papo1 ) ;
}
/* Copy values from xapo1 to xapo */
int i ;
for ( i = 0 ; i < N_STATES ; i + + ) {
xapo [ i ] = xapo1 [ i ] ;
}
if ( ( counter % REPROJECTION_COUNTER_LIMIT = = 0 ) | | ( counter % ( PROJECTION_INITIALIZE_COUNTER_LIMIT - 1 ) = = 0 ) ) {
/* Reproject from plane to geographic coordinate system */
// map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
map_projection_reproject ( z [ 0 ] , z [ 1 ] , & lat_current , & lon_current ) ; //do not use estimator for projection testing, removeme
// //DEBUG
// if(counter%500 == 0)
// {
// printf("phi_1: %.10f\n", phi_1);
// printf("lambda_0: %.10f\n", lambda_0);
// printf("lat_estimated: %.10f\n", lat_current);
// printf("lon_estimated: %.10f\n", lon_current);
// printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
// fflush(stdout);
//
// }
// if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
// {
/* send out */
global_pos . lat = lat_current ;
global_pos . lon = lon_current ;
global_pos . alt = xapo1 [ 4 ] ;
global_pos . vx = xapo1 [ 1 ] ;
global_pos . vy = xapo1 [ 3 ] ;
global_pos . vz = xapo1 [ 5 ] ;
/* publish current estimate */
orb_publish ( ORB_ID ( vehicle_global_position ) , global_pos_pub , & global_pos ) ;
// }
// else
// {
// printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]);
// fflush(stdout);
// }
}
counter + + ;
}
}
return 0 ;
}
