From a95aa1bbbae9b6d2a0b01d36e092fcc11a8f9bb8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 2 Oct 2012 13:50:59 +0200 Subject: [PATCH] Simplified pos estimator, ready for tests --- .../attitude_estimator_ekf_main.c | 8 +- apps/position_estimator/Makefile | 2 - .../position_estimator_main.c | 165 +++++++++--------- 3 files changed, 86 insertions(+), 89 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 46c1a66236..295cc4b542 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -285,10 +285,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (time_elapsed > loop_interval_alarm) { //TODO: add warning, cpu overload here - if (overloadcounter == 20) { - printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - overloadcounter = 0; - } + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } overloadcounter++; } diff --git a/apps/position_estimator/Makefile b/apps/position_estimator/Makefile index a766f1666b..3502cc402b 100644 --- a/apps/position_estimator/Makefile +++ b/apps/position_estimator/Makefile @@ -47,6 +47,4 @@ CSRCS = position_estimator_main.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c index 17482dc0a8..e061256fc8 100644 --- a/apps/position_estimator/position_estimator_main.c +++ b/apps/position_estimator/position_estimator_main.c @@ -35,8 +35,9 @@ * ****************************************************************************/ -/* - * @file Model-identification based position estimator for multirotors +/** + * @file position_estimator_main.c + * Model-identification based position estimator for multirotors */ #include @@ -45,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -58,6 +58,7 @@ #include #include #include +#include #include #include "codegen/position_estimator.h" @@ -251,8 +252,9 @@ int position_estimator_main(int argc, char *argv[]) bool new_initialization = true; - static double lat_current = 0;//[°]] --> 47.0 - static double lon_current = 0; //[°]] -->8.5 + static double lat_current = 0.0d;//[°]] --> 47.0 + static double lon_current = 0.0d; //[°]] -->8.5 + float alt_current = 0.0f; //TODO: handle flight without gps but with estimator @@ -269,7 +271,7 @@ int position_estimator_main(int argc, char *argv[]) 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) { + while (gps.fix_type < 3) { struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} }; /* wait for GPS updates, BUT READ VEHICLE STATUS (!) @@ -290,14 +292,18 @@ int position_estimator_main(int argc, char *argv[]) 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; + alt_current = gps.alt * 1e-3; + + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); /* 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 + struct vehicle_local_position_s local_pos = { + .x = 0, + .y = 0, + .z = 0 }; - orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); @@ -320,91 +326,84 @@ int position_estimator_main(int argc, char *argv[]) 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; + if (gps.fix_type > 2) { /* 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])); + 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 + local_pos.x = z[0]; + local_pos.y = z[1]; + /* negative offset from initialization altitude */ + local_pos.z = alt_current - (gps.alt) * 1e-3; - /*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); + orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); } + // gps_covariance[0] = gps.eph; //TODO: needs scaling + // gps_covariance[1] = gps.eph; + // gps_covariance[2] = gps.epv; - /* Copy values from xapo1 to xapo */ - int i; + // } else { + // /* we can not use the gps signal (it is of low quality) */ + // predict_only = 1; + // } - for (i = 0; i < N_STATES; i++) { - xapo[i] = xapo1[i]; - } + // // 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 - 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 */ + // /*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); - 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]; + // } 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); + // orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); // } // else // { @@ -412,7 +411,7 @@ int position_estimator_main(int argc, char *argv[]) // fflush(stdout); // } - } + // } counter++; }