Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 18:27:05 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Merge pull request #3256 from UAVenture/inav_terrain

Browse Source 
Added terrain estimator to INAV
This commit is contained in:
Lorenz Meier
2015-11-24 14:01:07 +01:00
parent fff75f6029 6bec773423
commit da59e632b2
4 changed files with 39 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/CMakeLists.txt
+6 -3
View File
@@ -30,18 +30,21 @@
# POSSIBILITY OF SUCH DAMAGE. # POSSIBILITY OF SUCH DAMAGE.
# #
############################################################################ ############################################################################
set(MODULE_CFLAGS )
if(${OS} STREQUAL "nuttx") if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000)
endif() endif()
px4_add_module( px4_add_module(
MODULE modules__position_estimator_inav MODULE modules__position_estimator_inav
MAIN position_estimator_inav MAIN position_estimator_inav
STACK 1200 STACK 1200
COMPILE_FLAGS ${MODULE_CFLAGS} COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS SRCS
position_estimator_inav_main.c position_estimator_inav_main.cpp
position_estimator_inav_params.c position_estimator_inav_params.cpp
inertial_filter.c inertial_filter.cpp
DEPENDS DEPENDS
platforms__common platforms__common
) )
src/modules/position_estimator_inav/inertial_filter.c → src/modules/position_estimator_inav/inertial_filter.cpp
View File
src/modules/position_estimator_inav/position_estimator_inav_main.c → src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+33 -15
View File
@@ -74,6 +74,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <platforms/px4_defines.h> #include <platforms/px4_defines.h>
#include <terrain_estimation/terrain_estimator.h>
#include "position_estimator_inav_params.h" #include "position_estimator_inav_params.h"
#include "inertial_filter.h" #include "inertial_filter.h"
@@ -95,7 +96,7 @@ static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distan
static const unsigned updates_counter_len = 1000000; static const unsigned updates_counter_len = 1000000;
static const float max_flow = 1.0f; // max flow value that can be used, rad/s 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[]); int position_estimator_inav_thread_main(int argc, char *argv[]);
@@ -390,13 +391,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* first parameters update */ /* first parameters update */
inav_parameters_update(&pos_inav_param_handles, &params); inav_parameters_update(&pos_inav_param_handles, &params);
px4_pollfd_struct_t fds_init[1] = { px4_pollfd_struct_t fds_init[1];
{ .fd = sensor_combined_sub, .events = POLLIN }, fds_init[0].fd = sensor_combined_sub;
}; fds_init[0].events = POLLIN;
/* wait for initial baro value */ /* wait for initial baro value */
bool wait_baro = true; bool wait_baro = true;
TerrainEstimator *terrain_estimator = new TerrainEstimator();
thread_running = true; thread_running = true;
while (wait_baro && !thread_should_exit) { while (wait_baro && !thread_should_exit) {
@@ -434,9 +437,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
/* main loop */ /* main loop */
px4_pollfd_struct_t fds[1] = { px4_pollfd_struct_t fds[1];
{ .fd = vehicle_attitude_sub, .events = POLLIN }, fds[0].fd = vehicle_attitude_sub;
}; fds[0].events = POLLIN;
while (!thread_should_exit) { while (!thread_should_exit) {
int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
@@ -523,9 +526,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_check(optical_flow_sub, &updated); orb_check(optical_flow_sub, &updated);
orb_check(distance_sensor_sub, &updated2); 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) { if (updated && updated2) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); 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 */ /* calculate time from previous update */
// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
@@ -1041,7 +1048,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
c += R_gps[j][i] * accel_bias_corr[j]; 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; acc_bias[i] += c * params.w_acc_bias * dt;
} }
} }
@@ -1081,7 +1088,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
c += PX4_R(att.R, j, i) * accel_bias_corr[j]; 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; acc_bias[i] += c * params.w_acc_bias * dt;
} }
} }
@@ -1106,7 +1113,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
c += PX4_R(att.R, j, i) * accel_bias_corr[j]; 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; acc_bias[i] += c * params.w_acc_bias * dt;
} }
} }
@@ -1114,7 +1121,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter prediction for altitude */ /* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est, acc[2]); 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, 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, 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); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1141,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); 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, 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, 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); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1160,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, x_est, acc[0]);
inertial_filter_predict(dt, y_est, acc[1]); 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, 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, 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); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1207,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); 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, 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, 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); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1228,6 +1235,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); 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) { if (inav_verbose_mode) {
/* print updates rate */ /* print updates rate */
if (t > updates_counter_start + updates_counter_len) { if (t > updates_counter_start + updates_counter_len) {
@@ -1318,6 +1329,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.eph = eph; global_pos.eph = eph;
global_pos.epv = epv; 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) { if (vehicle_global_position_pub == NULL) {
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
src/modules/position_estimator_inav/position_estimator_inav_params.c → src/modules/position_estimator_inav/position_estimator_inav_params.cpp
View File