diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-26 16:07:29 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-26 16:07:29 +0200 |
| commit | 13dfe0447ccfa4f75b551d02b5c979a6ade4c81a (patch) | |
| tree | 8e5a1bc812056cb634beaacbea4ca8f4f205943f /src/modules/commander | |
| parent | 0d50b3ea866a9ca0b9271b8c861f1d9e2a61a24a (diff) | |
| download | px4-firmware-13dfe0447ccfa4f75b551d02b5c979a6ade4c81a.tar.gz px4-firmware-13dfe0447ccfa4f75b551d02b5c979a6ade4c81a.tar.bz2 px4-firmware-13dfe0447ccfa4f75b551d02b5c979a6ade4c81a.zip | |
Send current local position estimate as well
Diffstat (limited to 'src/modules/commander')
| -rw-r--r-- | src/modules/commander/commander.cpp | 24 |
1 files changed, 16 insertions, 8 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 352711734..dd2614c1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -927,6 +927,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* update local position estimate */ + orb_check(local_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + } + /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ bool eph_epv_good; @@ -952,6 +960,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -968,14 +980,6 @@ int commander_thread_main(int argc, char *argv[]) tune_positive(true); } - /* update local position estimate */ - orb_check(local_position_sub, &updated); - - if (updated) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); @@ -1338,6 +1342,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); |