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

Send current local position estimate as well

Browse Source
This commit is contained in:
Lorenz Meier
2014-04-26 16:07:29 +02:00
parent 0d50b3ea86
commit 13dfe0447c
2 changed files with 20 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+16 -8
View File
@@ -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);