X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=8676a670fd86a092bdaf5fcdd8d7b8220ab92ac3;hb=8d9c79f5c162e07d57d42c6ba5825a3327a911d5;hp=e91683485a6c5a36166e04d715e816feea66f9ed;hpb=5c4b3658a96f1a64ccebf7bddda06b15b4ac4a6f;p=fw%2Faltos diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index e9168348..8676a670 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -21,10 +21,11 @@ #include "ao_gps_ublox.h" -#define AO_UBLOX_DEBUG 1 +#define AO_UBLOX_DEBUG 0 #include +__xdata uint8_t ao_gps_new; __xdata uint8_t ao_gps_mutex; __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; @@ -712,7 +713,7 @@ ao_gps(void) __reentrant ao_gps_data.flags |= AO_GPS_RUNNING; if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) { uint8_t nsat = nav_sol.nsat; - ao_gps_data.flags |= AO_GPS_VALID; + ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID; if (nsat > 15) nsat = 15; ao_gps_data.flags |= nsat; @@ -745,23 +746,25 @@ ao_gps(void) __reentrant ao_gps_tracking_data.channels = 0; struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0]; + struct nav_svinfo_sat *src = &nav_svinfo_sat[0]; for (i = 0; i < nav_svinfo_nsat; i++) { - struct nav_svinfo_sat *src = &nav_svinfo_sat[i]; - if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) && src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED) { - dst->svid = src->svid; - dst->c_n_1 = src->cno; - dst++; - ao_gps_tracking_data.channels++; + if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) { + dst->svid = src->svid; + dst->c_n_1 = src->cno; + dst++; + ao_gps_tracking_data.channels++; + } } + src++; } ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); break; } break;