diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-08-16 21:24:19 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-08-16 21:24:19 +0200 |
| commit | 4882bc0f1c74e9ddebbeaa73bc3e753ac43bdf9c (patch) | |
| tree | 8b6f5c092a35c8ba645db6cb1d27fee8771378c8 /src/modules/position_estimator_inav | |
| parent | 63af0a80ee19a73a94a3b46bbddffe1b80610a3c (diff) | |
| download | px4-firmware-4882bc0f1c74e9ddebbeaa73bc3e753ac43bdf9c.tar.gz px4-firmware-4882bc0f1c74e9ddebbeaa73bc3e753ac43bdf9c.tar.bz2 px4-firmware-4882bc0f1c74e9ddebbeaa73bc3e753ac43bdf9c.zip | |
position_estimator_inav: fixed global_pos topic publishing
Diffstat (limited to 'src/modules/position_estimator_inav')
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 |
1 files changed, 6 insertions, 1 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 d4b2f0e43..0530c2dea 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,6 +116,7 @@ int position_estimator_inav_main(int argc, char *argv[]) } verbose_mode = false; + if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; @@ -375,6 +376,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; + /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { 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); if (params.use_gps) { - global_pos.valid = local_pos.valid; double 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.lon = (int32_t)(est_lon * 1e7); global_pos.alt = local_pos.home_alt - local_pos.z; |