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

position_estimator_inav: GPS topic timeout detection fixed, messages about GPS signal state in mavlink added

Browse Source
This commit is contained in:
Anton Babushkin
2013-09-22 16:53:48 +02:00
parent 8a9a230e0d
commit 4124417934
1 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+17 -4
View File
@@ -438,12 +438,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* vehicle GPS position */
if (fds[6].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
if (gps.fix_type >= 3) {
/* hysteresis for GPS quality */
if (gps_valid) {
gps_valid = gps.eph_m < 10.0f;
if (gps.eph_m > 10.0f) {
gps_valid = false;
warnx("GPS signal lost");
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
gps_valid = gps.eph_m < 5.0f;
if (gps.eph_m < 5.0f) {
gps_valid = true;
warnx("GPS signal found");
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
}
}
} else {
gps_valid = false;
@@ -501,7 +509,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
/* end of poll return value check */
/* check for timeout on GPS topic */
if (gps_valid && t > gps.timestamp_position + gps_timeout) {
gps_valid = false;
warnx("GPS timeout");
mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
t_prev = t;