fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

added terrain estimator to inav

Browse Source
This commit is contained in:
Andreas Antener 2015-11-23 23:17:44 +01:00
parent b7dfa3a9d0
commit a87ffe9bf3
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -74,6 +74,7 @@
#include <drivers/drv_hrt.h>
#include <platforms/px4_defines.h>
#include <terrain_estimation/terrain_estimator.h>
#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);