diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-06-10 23:16:04 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-06-10 23:16:04 +0400 |
commit | 4256e43de7ea4c9cad98e8bfc9a811310bfb3d77 (patch) | |
tree | 5e024a235dc682cb7323cf16f3e1871e44599cb1 /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
parent | afb34950a38644f4a06bc0621bcd4c95b70b1fa6 (diff) | |
download | px4-firmware-4256e43de7ea4c9cad98e8bfc9a811310bfb3d77.tar.gz px4-firmware-4256e43de7ea4c9cad98e8bfc9a811310bfb3d77.tar.bz2 px4-firmware-4256e43de7ea4c9cad98e8bfc9a811310bfb3d77.zip |
Complete position estimator implemented (GPS + Baro + Accel)
Diffstat (limited to 'src/modules/position_estimator_inav/position_estimator_inav_main.c')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 242 |
1 files changed, 133 insertions, 109 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6ecdfe01d..306ebe5cc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -66,7 +66,7 @@ #include <drivers/drv_hrt.h> #include "position_estimator_inav_params.h" -#include "kalman_filter_inertial.h" +#include "inertial_filter.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -82,13 +82,15 @@ static void usage(const char *reason); /** * 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: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); - } + + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + exit(1); +} /** * The position_estimator_inav_thread only briefly exists to start @@ -98,7 +100,8 @@ static void usage(const char *reason) { * The actual stack size should be set in the call * to task_create(). */ -int position_estimator_inav_main(int argc, char *argv[]) { +int position_estimator_inav_main(int argc, char *argv[]) +{ if (argc < 1) usage("missing command"); @@ -108,17 +111,19 @@ int position_estimator_inav_main(int argc, char *argv[]) { /* this is not an error */ exit(0); } + if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; thread_should_exit = false; position_estimator_inav_task = task_spawn("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, - position_estimator_inav_thread_main, - (argv) ? (const char **) &argv[2] : (const char **) NULL ); + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); } + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); @@ -127,9 +132,11 @@ int position_estimator_inav_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tposition_estimator_inav is running\n"); + } else { printf("\tposition_estimator_inav not started\n"); } + exit(0); } @@ -140,10 +147,10 @@ int position_estimator_inav_main(int argc, char *argv[]) { /**************************************************************************** * main ****************************************************************************/ -int position_estimator_inav_thread_main(int argc, char *argv[]) { - /* welcome user */ - printf("[position_estimator_inav] started\n"); - static int mavlink_fd; +int position_estimator_inav_thread_main(int argc, char *argv[]) +{ + warnx("started."); + int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); @@ -156,9 +163,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determine while start up */ - static double lat_current = 0.0; //[°]] --> 47.0 - static double lon_current = 0.0; //[°]] -->8.5 - static double alt_current = 0.0; //[m] above MSL + double lat_current = 0.0; //[°]] --> 47.0 + double lon_current = 0.0; //[°]] -->8.5 + double alt_current = 0.0; //[m] above MSL /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; @@ -188,40 +195,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); - bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */ - /* FIRST PARAMETER READ at START UP*/ + bool local_flag_baroINITdone = false; + /* first parameters read at start up */ struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ - /* FIRST PARAMETER UPDATE */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ + /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - /* END FIRST PARAMETER UPDATE */ - /* wait until gps signal turns valid, only then can we initialize the projection */ + /* wait for GPS fix, only then can we initialize the projection */ if (params.use_gps) { struct pollfd fds_init[1] = { - { .fd = vehicle_gps_position_sub, .events = POLLIN } + { .fd = vehicle_gps_position_sub, .events = POLLIN } }; while (gps.fix_type < 3) { - if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (poll(fds_init, 1, 5000)) { if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); } } + static int printcounter = 0; + if (printcounter == 100) { printcounter = 0; - printf("[position_estimator_inav] wait for GPS fix type 3\n"); + warnx("waiting for GPS fix type 3..."); } + printcounter++; } /* get GPS position for first initialization */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double) (gps.lat)) * 1e-7; - lon_current = ((double) (gps.lon)) * 1e-7; + lat_current = ((double)(gps.lat)) * 1e-7; + lon_current = ((double)(gps.lon)) * 1e-7; alt_current = gps.alt * 1e-3; pos.home_lat = lat_current * 1e7; @@ -232,15 +241,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf( - "[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", - lat_current, lon_current); - hrt_abstime last_time = 0; + warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current); + + hrt_abstime t_prev = 0; thread_running = true; - uint32_t accelerometer_counter = 0; + uint32_t accel_counter = 0; + hrt_abstime accel_t = 0; + float accel_dt = 0.0f; uint32_t baro_counter = 0; - uint16_t accelerometer_updates = 0; + hrt_abstime baro_t = 0; + hrt_abstime gps_t = 0; + uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; @@ -251,173 +263,184 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* main loop */ struct pollfd fds[5] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, - { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); + while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; bool gps_updated = false; - float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; + float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate + hrt_abstime t = hrt_absolute_time(); + if (ret < 0) { /* poll error */ - printf("[position_estimator_inav] subscriptions poll error\n"); + warnx("subscriptions poll error."); thread_should_exit = true; continue; + } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, - &update); + &update); /* update parameters */ parameters_update(&pos_inav_param_handles, ¶ms); } + /* vehicle status */ if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, - &vehicle_status); + &vehicle_status); } + /* vehicle attitude */ if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; } + /* sensor combined */ if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter > accelerometer_counter) { + + if (sensor.accelerometer_counter > accel_counter) { accelerometer_updated = true; - accelerometer_counter = sensor.accelerometer_counter; - accelerometer_updates++; + accel_counter = sensor.accelerometer_counter; + accel_updates++; + accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f; + accel_t = t; } + if (sensor.baro_counter > baro_counter) { baro_updated = true; baro_counter = sensor.baro_counter; baro_updates++; } - // barometric pressure estimation at start up + + /* barometric pressure estimation at start up */ if (!local_flag_baroINITdone && baro_updated) { - // mean calculation over several measurements + /* mean calculation over several measurements */ if (baro_loop_cnt < baro_loop_end) { baro_alt0 += sensor.baro_alt_meter; baro_loop_cnt++; + } else { - baro_alt0 /= (float) (baro_loop_cnt); + baro_alt0 /= (float)(baro_loop_cnt); local_flag_baroINITdone = true; - char str[80]; - sprintf(str, - "[position_estimator_inav] baro_alt0 = %.2f", - baro_alt0); - printf("%s\n", str); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0); } } } + if (params.use_gps) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - /* Project gps lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double) (gps.lat)) * 1e-7, - ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), - &(local_pos_gps[1])); - local_pos_gps[2] = (float) (gps.alt * 1e-3); + /* project GPS lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double)(gps.lat)) * 1e-7, + ((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]), + &(proj_pos_gps[1])); + proj_pos_gps[2] = (float)(gps.alt * 1e-3); gps_updated = true; pos.valid = gps.fix_type >= 3; gps_updates++; } + } else { pos.valid = true; } - } /* end of poll return value check */ + } + + /* end of poll return value check */ + + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f; + t_prev = t; - hrt_abstime t = hrt_absolute_time(); - float dt = (t - last_time) / 1000000.0; - last_time = t; if (att.R_valid) { /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; + for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; - /* kalman filter for altitude */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // position, acceleration - bool use_z[2] = { false, false }; + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); + + /* inertial filter correction for altitude */ if (local_flag_baroINITdone && baro_updated) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; + if (baro_t > 0) { + inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro); + } + + baro_t = t; } + if (accelerometer_updated) { - z_meas[1] = accel_NED[2]; - use_z[1] = true; - } - if (use_z[0] || use_z[1]) { - /* correction */ - kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); + inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc); } if (params.use_gps) { - /* kalman filter for position */ - kalman_filter_inertial_predict(dt, x_est); - kalman_filter_inertial_predict(dt, y_est); - /* prepare vectors for kalman filter correction */ - float x_meas[3]; // position, velocity, acceleration - float y_meas[3]; // position, velocity, acceleration - bool use_xy[3] = { false, false, false }; + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); + + /* inertial filter correction for position */ if (gps_updated) { - x_meas[0] = local_pos_gps[0]; - y_meas[0] = local_pos_gps[1]; - x_meas[1] = gps.vel_n_m_s; - y_meas[1] = gps.vel_e_m_s; - use_xy[0] = true; - use_xy[1] = true; + if (gps_t > 0) { + float gps_dt = (t - gps_t) / 1000000.0f; + inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p); + inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v); + inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p); + inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v); + } + + gps_t = t; } + if (accelerometer_updated) { - x_meas[2] = accel_NED[0]; - y_meas[2] = accel_NED[1]; - use_xy[2] = true; - } - if (use_xy[0] || use_xy[2]) { - /* correction */ - kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); + inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc); + inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc); } } } + if (verbose_mode) { /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", - accelerometer_updates / updates_dt, - baro_updates / updates_dt, - gps_updates / updates_dt, - attitude_updates / updates_dt); + "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + accel_updates / updates_dt, + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt); updates_counter_start = t; - accelerometer_updates = 0; + accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; } } + if (t - pub_last > pub_interval) { pub_last = t; pos.x = x_est[0]; @@ -427,17 +450,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { pos.z = z_est[0]; pos.vz = z_est[1]; pos.timestamp = hrt_absolute_time(); + if ((isfinite(pos.x)) && (isfinite(pos.vx)) - && (isfinite(pos.y)) - && (isfinite(pos.vy)) - && (isfinite(pos.z)) - && (isfinite(pos.vz))) { + && (isfinite(pos.y)) + && (isfinite(pos.vy)) + && (isfinite(pos.z)) + && (isfinite(pos.vz))) { orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); } } } - printf("[position_estimator_inav] exiting.\n"); + warnx("exiting."); mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); thread_running = false; return 0; |