From b7dfa3a9d0a592e178d3f57e8c3966bccbf22afc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 22:58:14 +0100 Subject: [PATCH 1/4] made inav compile with c++ --- src/modules/position_estimator_inav/CMakeLists.txt | 9 ++++++--- .../{inertial_filter.c => inertial_filter.cpp} | 0 ...nav_main.c => position_estimator_inav_main.cpp} | 14 +++++++------- ...params.c => position_estimator_inav_params.cpp} | 0 4 files changed, 13 insertions(+), 10 deletions(-) rename src/modules/position_estimator_inav/{inertial_filter.c => inertial_filter.cpp} (100%) rename src/modules/position_estimator_inav/{position_estimator_inav_main.c => position_estimator_inav_main.cpp} (99%) rename src/modules/position_estimator_inav/{position_estimator_inav_params.c => position_estimator_inav_params.cpp} (100%) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 60429f5d22..2f14727509 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -30,18 +30,21 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +set(MODULE_CFLAGS ) if(${OS} STREQUAL "nuttx") list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) endif() + px4_add_module( MODULE modules__position_estimator_inav MAIN position_estimator_inav STACK 1200 COMPILE_FLAGS ${MODULE_CFLAGS} SRCS - position_estimator_inav_main.c - position_estimator_inav_params.c - inertial_filter.c + position_estimator_inav_main.cpp + position_estimator_inav_params.cpp + inertial_filter.cpp DEPENDS platforms__common ) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.cpp similarity index 100% rename from src/modules/position_estimator_inav/inertial_filter.c rename to src/modules/position_estimator_inav/inertial_filter.cpp diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp similarity index 99% rename from src/modules/position_estimator_inav/position_estimator_inav_main.c rename to src/modules/position_estimator_inav/position_estimator_inav_main.cpp index d8f29f1082..7316da727f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -95,7 +95,7 @@ static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distan static const unsigned updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s -__EXPORT int position_estimator_inav_main(int argc, char *argv[]); +extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]); int position_estimator_inav_thread_main(int argc, char *argv[]); @@ -390,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); - px4_pollfd_struct_t fds_init[1] = { - { .fd = sensor_combined_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds_init[1]; + fds_init[0].fd = sensor_combined_sub; + fds_init[0].events = POLLIN; /* wait for initial baro value */ bool wait_baro = true; @@ -434,9 +434,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* main loop */ - px4_pollfd_struct_t fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds[1]; + fds[0].fd = vehicle_attitude_sub; + fds[0].events = POLLIN; while (!thread_should_exit) { int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp similarity index 100% rename from src/modules/position_estimator_inav/position_estimator_inav_params.c rename to src/modules/position_estimator_inav/position_estimator_inav_params.cpp From a87ffe9bf3e7dbc81859e7e7297acfdfb2f81d1a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 23:17:44 +0100 Subject: [PATCH 2/4] added terrain estimator to inav --- .../position_estimator_inav_main.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 7316da727f..f97b7e944e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -74,6 +74,7 @@ #include #include +#include #include "position_estimator_inav_params.h" #include "inertial_filter.h" @@ -397,6 +398,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* wait for initial baro value */ bool wait_baro = true; + TerrainEstimator *terrain_estimator = new TerrainEstimator(); + thread_running = true; while (wait_baro && !thread_should_exit) { @@ -1228,6 +1231,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } + /* run terrain estimator */ + terrain_estimator->predict(dt, &att, &sensor, &lidar); + terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att); + if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { @@ -1318,6 +1325,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.eph = eph; global_pos.epv = epv; + if (terrain_estimator->is_valid()) { + global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground(); + global_pos.terrain_alt_valid = true; + } else { + global_pos.terrain_alt_valid = false; + } + if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); From 67dd28e2c4e4e6fb39c345e74c40759baa3f5500 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 24 Nov 2015 10:30:18 +0100 Subject: [PATCH 3/4] update distance sensor separate from flow --- .../position_estimator_inav_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index f97b7e944e..57a118a9e4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -526,9 +526,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_check(optical_flow_sub, &updated); orb_check(distance_sensor_sub, &updated2); + /* update lidar separately, needed by terrain estimator */ + if (updated2) { + orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); + } + if (updated && updated2) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; From 6bec773423747f24fa815fdbea7eb9c279dd8e33 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 24 Nov 2015 13:37:06 +0100 Subject: [PATCH 4/4] changed isfinite to PX4_ISFINITE --- .../position_estimator_inav_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 57a118a9e4..72b250ebe9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1048,7 +1048,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += R_gps[j][i] * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1088,7 +1088,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1113,7 +1113,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1121,7 +1121,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1148,7 +1148,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } - if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1167,7 +1167,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1214,7 +1214,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);