fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 12:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

position_estimator_inav: fixed global_pos topic publishing

Browse Source
This commit is contained in:
Anton Babushkin
2013-08-16 21:24:19 +02:00
parent 63af0a80ee
commit 4882bc0f1c
1 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+6 -1
View File
@@ -116,6 +116,7 @@ int position_estimator_inav_main(int argc, char *argv[])
} }
verbose_mode = false; verbose_mode = false;
if (argc > 1) if (argc > 1)
if (!strcmp(argv[2], "-v")) if (!strcmp(argv[2], "-v"))
verbose_mode = true; verbose_mode = true;
@@ -375,6 +376,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (att.R_valid) { if (att.R_valid) {
/* correct accel bias, now only for Z */ /* correct accel bias, now only for Z */
sensor.accelerometer_m_s2[2] -= accel_bias[2]; sensor.accelerometer_m_s2[2] -= accel_bias[2];
/* transform acceleration vector from body frame to NED frame */ /* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
accel_NED[i] = 0.0f; accel_NED[i] = 0.0f;
@@ -551,9 +553,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
if (params.use_gps) { if (params.use_gps) {
global_pos.valid = local_pos.valid;
double est_lat, est_lon; double est_lat, est_lon;
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.valid = local_pos.valid;
global_pos.timestamp = t;
global_pos.time_gps_usec = gps.time_gps_usec;
global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.lat = (int32_t)(est_lat * 1e7);
global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.lon = (int32_t)(est_lon * 1e7);
global_pos.alt = local_pos.home_alt - local_pos.z; global_pos.alt = local_pos.home_alt - local_pos.z;