diff options
| -rw-r--r-- | ao-view/aoview.glade | 25 | ||||
| -rw-r--r-- | ao-view/aoview.h | 22 | ||||
| -rw-r--r-- | ao-view/aoview_monitor.c | 34 | ||||
| -rw-r--r-- | ao-view/aoview_state.c | 24 | ||||
| -rw-r--r-- | ao-view/aoview_table.c | 2 | ||||
| -rw-r--r-- | src/Makefile | 4 | ||||
| -rw-r--r-- | src/ao.h | 32 | ||||
| -rw-r--r-- | src/ao_dbg.c | 2 | ||||
| -rw-r--r-- | src/ao_gps.c | 126 | ||||
| -rw-r--r-- | src/ao_gps_print.c | 36 | ||||
| -rw-r--r-- | src/ao_gps_report.c | 31 | ||||
| -rw-r--r-- | src/ao_gps_test.c | 181 | ||||
| -rw-r--r-- | src/ao_host.h | 4 | ||||
| -rw-r--r-- | src/ao_monitor.c | 3 | ||||
| -rw-r--r-- | src/ao_telemetry.c | 1 |
15 files changed, 454 insertions, 73 deletions
diff --git a/ao-view/aoview.glade b/ao-view/aoview.glade index 3481a779..9a746110 100644 --- a/ao-view/aoview.glade +++ b/ao-view/aoview.glade @@ -3,7 +3,7 @@ <!-- interface-requires gtk+ 2.16 --> <!-- interface-naming-policy project-wide --> <widget class="GtkWindow" id="aoview"> - <property name="width_request">550</property> + <property name="width_request">900</property> <property name="height_request">700</property> <property name="visible">True</property> <property name="title" translatable="yes">AltOS View</property> @@ -412,6 +412,17 @@ <property name="position">1</property> </packing> </child> + <child> + <widget class="GtkTreeView" id="dataview_2"> + <property name="visible">True</property> + <property name="can_focus">True</property> + <property name="show_expanders">False</property> + <property name="enable_grid_lines">both</property> + </widget> + <packing> + <property name="position">2</property> + </packing> + </child> </widget> <packing> <property name="position">2</property> @@ -453,7 +464,7 @@ <property name="layout_style">end</property> <child> <widget class="GtkButton" id="cancel_button"> - <property name="label" translatable="yes">gtk-cancel</property> + <property name="label">gtk-cancel</property> <property name="response_id">1</property> <property name="visible">True</property> <property name="can_focus">True</property> @@ -470,7 +481,7 @@ </child> <child> <widget class="GtkButton" id="connect_button"> - <property name="label" translatable="yes">gtk-connect</property> + <property name="label">gtk-connect</property> <property name="visible">True</property> <property name="can_focus">True</property> <property name="can_default">True</property> @@ -511,7 +522,7 @@ <property name="layout_style">end</property> <child> <widget class="GtkButton" id="file_configure_cancel"> - <property name="label" translatable="yes">gtk-cancel</property> + <property name="label">gtk-cancel</property> <property name="visible">True</property> <property name="can_focus">True</property> <property name="receives_default">True</property> @@ -526,7 +537,7 @@ </child> <child> <widget class="GtkButton" id="file_configure_ok"> - <property name="label" translatable="yes">gtk-ok</property> + <property name="label">gtk-ok</property> <property name="visible">True</property> <property name="can_focus">True</property> <property name="can_default">True</property> @@ -693,7 +704,7 @@ You should have received a copy of the GNU General Public License along with AoV <property name="layout_style">end</property> <child> <widget class="GtkButton" id="ao_replay_cancel"> - <property name="label" translatable="yes">gtk-cancel</property> + <property name="label">gtk-cancel</property> <property name="visible">True</property> <property name="can_focus">True</property> <property name="receives_default">True</property> @@ -708,7 +719,7 @@ You should have received a copy of the GNU General Public License along with AoV </child> <child> <widget class="GtkButton" id="ao_replay_ok"> - <property name="label" translatable="yes">gtk-ok</property> + <property name="label">gtk-ok</property> <property name="visible">True</property> <property name="can_focus">True</property> <property name="receives_default">True</property> diff --git a/ao-view/aoview.h b/ao-view/aoview.h index e8334e5b..9ca65298 100644 --- a/ao-view/aoview.h +++ b/ao-view/aoview.h @@ -75,6 +75,26 @@ struct aogps { int v_error; /* m */ }; +#define SIRF_SAT_STATE_ACQUIRED (1 << 0) +#define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) +#define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) +#define SIRF_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3) +#define SIRF_SAT_CARRIER_PULLIN_COMPLETE (1 << 4) +#define SIRF_SAT_CODE_LOCKED (1 << 5) +#define SIRF_SAT_ACQUISITION_FAILED (1 << 6) +#define SIRF_SAT_EPHEMERIS_AVAILABLE (1 << 7) + +struct aogps_sat { + int svid; + int state; + int c_n0; +}; + +struct aogps_tracking { + int channels; + struct aogps_sat sats[12]; +}; + struct aodata { char callsign[16]; int serial; @@ -93,6 +113,7 @@ struct aodata { int flight_pres; int ground_pres; struct aogps gps; + struct aogps_tracking gps_tracking; }; struct aostate { @@ -121,6 +142,7 @@ struct aostate { double max_speed; struct aogps gps; + struct aogps_tracking gps_tracking; int gps_valid; double pad_lat; diff --git a/ao-view/aoview_monitor.c b/ao-view/aoview_monitor.c index 9265a199..1f4c8f72 100644 --- a/ao-view/aoview_monitor.c +++ b/ao-view/aoview_monitor.c @@ -48,6 +48,12 @@ aoview_parse_int(int *target, char *source) } static void +aoview_parse_hex(int *target, char *source) +{ + *target = strtol(source, NULL, 16); +} + +static void aoview_parse_pos(double *target, char *source) { int deg; @@ -65,20 +71,23 @@ aoview_parse_pos(double *target, char *source) *target = r; } +#define PARSE_MAX_WORDS 256 + gboolean aoview_monitor_parse(const char *input_line) { char *saveptr; - char *words[64]; + char *words[PARSE_MAX_WORDS]; int nword; char line_buf[8192], *line; struct aodata data; + int tracking_pos; /* avoid smashing our input parameter */ strncpy (line_buf, input_line, sizeof (line_buf)-1); line_buf[sizeof(line_buf) - 1] = '\0'; line = line_buf; - for (nword = 0; nword < 64; nword++) { + for (nword = 0; nword < PARSE_MAX_WORDS; nword++) { words[nword] = strtok_r(line, " \t\n", &saveptr); line = NULL; if (words[nword] == NULL) @@ -112,6 +121,7 @@ aoview_monitor_parse(const char *input_line) data.gps.gps_time.hour = data.gps.gps_time.minute = data.gps.gps_time.second = 0; data.gps.lat = data.gps.lon = 0; data.gps.alt = 0; + tracking_pos = 37; } else if (nword >= 40) { data.gps.gps_locked = 1; data.gps.gps_connected = 1; @@ -119,6 +129,7 @@ aoview_monitor_parse(const char *input_line) aoview_parse_pos(&data.gps.lat, words[37]); aoview_parse_pos(&data.gps.lon, words[38]); sscanf(words[39], "%dm", &data.gps.alt); + tracking_pos = 46; } else { data.gps.gps_connected = 0; data.gps.gps_locked = 0; @@ -143,6 +154,25 @@ aoview_monitor_parse(const char *input_line) data.gps.h_error = 0; data.gps.v_error = 0; } + if (nword >= tracking_pos + 2 && strcmp(words[tracking_pos], "SAT") == 0) { + int c, n, pos; + aoview_parse_int(&n, words[tracking_pos + 1]); + pos = tracking_pos + 2; + if (nword >= pos + n * 3) { + data.gps_tracking.channels = n; + for (c = 0; c < n; c++) { + aoview_parse_int(&data.gps_tracking.sats[c].svid, + words[pos + 0]); + aoview_parse_hex(&data.gps_tracking.sats[c].state, + words[pos + 1]); + aoview_parse_int(&data.gps_tracking.sats[c].c_n0, + words[pos + 2]); + pos += 3; + } + } else { + data.gps_tracking.channels = 0; + } + } aoview_state_notify(&data); return TRUE; } diff --git a/ao-view/aoview_state.c b/ao-view/aoview_state.c index 7efd33b0..f75066dd 100644 --- a/ao-view/aoview_state.c +++ b/ao-view/aoview_state.c @@ -168,6 +168,8 @@ aoview_state_derive(struct aodata *data, struct aostate *state) aoview_great_circle(state->pad_lat, state->pad_lon, state->gps.lat, state->gps.lon, &state->distance, &state->bearing); } + if (data->gps_tracking.channels) + state->gps_tracking = data->gps_tracking; if (state->npad) { state->gps_height = state->gps.alt - state->pad_alt; } else { @@ -308,6 +310,7 @@ aoview_state_notify(struct aodata *data) if (state->gps_valid) { aoview_state_add_deg(1, "Latitude", state->gps.lat, 'N', 'S'); aoview_state_add_deg(1, "Longitude", state->gps.lon, 'E', 'W'); + aoview_table_add_row(1, "GPS altitude", "%d", state->gps.alt); aoview_table_add_row(1, "GPS height", "%d", state->gps_height); aoview_table_add_row(1, "GPS time", "%02d:%02d:%02d", state->gps.gps_time.hour, @@ -330,6 +333,27 @@ aoview_state_notify(struct aodata *data) aoview_state_add_deg(1, "Pad longitude", state->pad_lon, 'E', 'W'); aoview_table_add_row(1, "Pad GPS alt", "%gm", state->pad_alt); } + if (state->gps.gps_connected) { + int nsat_vis = 0; + int nsat_locked = 0; + int c; + + for (c = 0; c < state->gps_tracking.channels; c++) { + if ((state->gps_tracking.sats[c].state & 0xff) == 0xbf) + nsat_locked++; + } + aoview_table_add_row(2, "Satellites Visible", "%d", state->gps_tracking.channels); + aoview_table_add_row(2, "Satellites Locked", "%d", nsat_locked); + for (c = 0; c < state->gps_tracking.channels; c++) { + aoview_table_add_row(2, "Satellite id,state,C/N0", + "%3d,%02x,%2d%s", + state->gps_tracking.sats[c].svid, + state->gps_tracking.sats[c].state, + state->gps_tracking.sats[c].c_n0, + (state->gps_tracking.sats[c].state & 0xff) == 0xbf ? + " LOCKED" : ""); + } + } aoview_table_finish(); aoview_label_show(state); aoview_speak_state(state); diff --git a/ao-view/aoview_table.c b/ao-view/aoview_table.c index 93143009..e75ae670 100644 --- a/ao-view/aoview_table.c +++ b/ao-view/aoview_table.c @@ -17,7 +17,7 @@ #include "aoview.h" -#define NCOL 2 +#define NCOL 3 static GtkTreeView *dataview[NCOL]; static GtkListStore *datalist[NCOL]; diff --git a/src/Makefile b/src/Makefile index 5a8afe0b..96049c65 100644 --- a/src/Makefile +++ b/src/Makefile @@ -262,8 +262,8 @@ clean: install: -ao_flight_test: ao_flight.c ao_flight_test.c +ao_flight_test: ao_flight.c ao_flight_test.c ao_host.h cc -g -o $@ ao_flight_test.c -ao_gps_test: ao_gps.c ao_gps_test.c ao_host.h +ao_gps_test: ao_gps.c ao_gps_test.c ao_gps_print.c ao_host.h cc -g -o $@ ao_gps_test.c @@ -458,6 +458,7 @@ ao_ee_init(void); #define AO_LOG_GPS_LAT 'N' #define AO_LOG_GPS_LON 'W' #define AO_LOG_GPS_ALT 'H' +#define AO_LOG_GPS_SAT 'V' #define AO_LOG_POS_NONE (~0UL) @@ -499,6 +500,12 @@ struct ao_log_record { uint16_t unused; } gps_altitude; struct { + uint16_t svid; + uint8_t state; + uint8_t c_n; + uint8_t unused; + } gps_sat; + struct { uint16_t d0; uint16_t d1; } anon; @@ -696,8 +703,29 @@ struct ao_gps_data { uint16_t v_error; /* m */ }; +#define SIRF_SAT_STATE_ACQUIRED (1 << 0) +#define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) +#define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) +#define SIRF_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3) +#define SIRF_SAT_CARRIER_PULLIN_COMPLETE (1 << 4) +#define SIRF_SAT_CODE_LOCKED (1 << 5) +#define SIRF_SAT_ACQUISITION_FAILED (1 << 6) +#define SIRF_SAT_EPHEMERIS_AVAILABLE (1 << 7) + +struct ao_gps_sat_data { + uint8_t svid; + uint8_t state; + uint8_t c_n_1; +}; + +struct ao_gps_tracking_data { + uint8_t channels; + struct ao_gps_sat_data sats[12]; +}; + extern __xdata uint8_t ao_gps_mutex; extern __xdata struct ao_gps_data ao_gps_data; +extern __xdata struct ao_gps_tracking_data ao_gps_tracking_data; void ao_gps(void); @@ -706,6 +734,9 @@ void ao_gps_print(__xdata struct ao_gps_data *gps_data); void +ao_gps_tracking_print(__xdata struct ao_gps_tracking_data *gps_tracking_data); + +void ao_gps_init(void); /* @@ -735,6 +766,7 @@ struct ao_telemetry { struct ao_adc adc; struct ao_gps_data gps; char callsign[AO_MAX_CALLSIGN]; + struct ao_gps_tracking_data gps_tracking; }; /* Set delay between telemetry reports (0 to disable) */ diff --git a/src/ao_dbg.c b/src/ao_dbg.c index c8dc6ddc..b218897c 100644 --- a/src/ao_dbg.c +++ b/src/ao_dbg.c @@ -26,7 +26,7 @@ #define DBG_RESET_N_PIN (P0_5) static void -ao_dbg_send_bits(uint8_t msk, uint8_t val) +ao_dbg_send_bits(uint8_t msk, uint8_t val) __reentrant { P0 = (P0 & ~msk) | (val & msk); _asm diff --git a/src/ao_gps.c b/src/ao_gps.c index c4c434fd..7d68b325 100644 --- a/src/ao_gps.c +++ b/src/ao_gps.c @@ -21,6 +21,7 @@ __xdata uint8_t ao_gps_mutex; __xdata struct ao_gps_data ao_gps_data; +__xdata struct ao_gps_tracking_data ao_gps_tracking_data; static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n"; @@ -105,6 +106,21 @@ struct sirf_geodetic_nav_data { static __xdata struct sirf_geodetic_nav_data ao_sirf_data; +struct sirf_measured_sat_data { + uint8_t svid; + uint16_t state; + uint8_t c_n_1; +}; + +struct sirf_measured_tracker_data { + int16_t gps_week; + uint32_t gps_tow; + uint8_t channels; + struct sirf_measured_sat_data sats[12]; +}; + +static __xdata struct sirf_measured_tracker_data ao_sirf_tracker_data; + static __pdata uint16_t ao_sirf_cksum; static __pdata uint16_t ao_sirf_len; @@ -118,9 +134,11 @@ static uint8_t data_byte(void) return c; } +static char __xdata *sirf_target; + static void sirf_u16(uint8_t offset) { - uint16_t __xdata *ptr = (uint16_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint16_t __xdata *ptr = (uint16_t __xdata *) (sirf_target + offset); uint16_t val; val = data_byte() << 8; @@ -130,16 +148,16 @@ static void sirf_u16(uint8_t offset) static void sirf_u8(uint8_t offset) { - uint8_t __xdata *ptr = (uint8_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint8_t __xdata *ptr = (uint8_t __xdata *) (sirf_target + offset); uint8_t val; val = data_byte (); *ptr = val; } -static void sirf_u32(uint8_t offset) +static void sirf_u32(uint8_t offset) __reentrant { - uint32_t __xdata *ptr = (uint32_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint32_t __xdata *ptr = (uint32_t __xdata *) (sirf_target + offset); uint32_t val; val = ((uint32_t) data_byte ()) << 24; @@ -160,12 +178,44 @@ static void sirf_discard(uint8_t len) #define SIRF_U8 2 #define SIRF_U16 3 #define SIRF_U32 4 +#define SIRF_U8X10 5 struct sirf_packet_parse { uint8_t type; uint8_t offset; }; +static void +ao_sirf_parse(void __xdata *target, const struct sirf_packet_parse *parse) __reentrant +{ + uint8_t i, offset, j; + + sirf_target = target; + for (i = 0; ; i++) { + offset = parse[i].offset; + switch (parse[i].type) { + case SIRF_END: + return; + case SIRF_DISCARD: + sirf_discard(offset); + break; + case SIRF_U8: + sirf_u8(offset); + break; + case SIRF_U16: + sirf_u16(offset); + break; + case SIRF_U32: + sirf_u32(offset); + break; + case SIRF_U8X10: + for (j = 10; j--;) + sirf_u8(offset++); + break; + } + } +} + static const struct sirf_packet_parse geodetic_nav_data_packet[] = { { SIRF_DISCARD, 2 }, /* 1 nav valid */ { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) }, /* 3 */ @@ -200,29 +250,34 @@ static const struct sirf_packet_parse geodetic_nav_data_packet[] = { }; static void -ao_sirf_parse_41(void) +ao_sirf_parse_41(void) __reentrant { - uint8_t i, offset; + ao_sirf_parse(&ao_sirf_data, geodetic_nav_data_packet); +} - for (i = 0; ; i++) { - offset = geodetic_nav_data_packet[i].offset; - switch (geodetic_nav_data_packet[i].type) { - case SIRF_END: - return; - case SIRF_DISCARD: - sirf_discard(offset); - break; - case SIRF_U8: - sirf_u8(offset); - break; - case SIRF_U16: - sirf_u16(offset); - break; - case SIRF_U32: - sirf_u32(offset); - break; - } - } +static const struct sirf_packet_parse measured_tracker_data_packet[] = { + { SIRF_U16, offsetof (struct sirf_measured_tracker_data, gps_week) }, /* 1 week */ + { SIRF_U32, offsetof (struct sirf_measured_tracker_data, gps_tow) }, /* 3 time of week */ + { SIRF_U8, offsetof (struct sirf_measured_tracker_data, channels) }, /* 7 channels */ + { SIRF_END, 0 }, +}; + +static const struct sirf_packet_parse measured_sat_data_packet[] = { + { SIRF_U8, offsetof (struct sirf_measured_sat_data, svid) }, /* 0 SV id */ + { SIRF_DISCARD, 2 }, /* 1 azimuth, 2 elevation */ + { SIRF_U16, offsetof (struct sirf_measured_sat_data, state) }, /* 2 state */ + { SIRF_U8, offsetof (struct sirf_measured_sat_data, c_n_1) }, /* C/N0 1 */ + { SIRF_DISCARD, 9 }, /* C/N0 2-10 */ + { SIRF_END, 0 }, +}; + +static void +ao_sirf_parse_4(void) __reentrant +{ + uint8_t i; + ao_sirf_parse(&ao_sirf_tracker_data, measured_tracker_data_packet); + for (i = 0; i < 12; i++) + ao_sirf_parse(&ao_sirf_tracker_data.sats[i], measured_sat_data_packet); } static void @@ -267,7 +322,6 @@ ao_sirf_set_message_rate(uint8_t msg, uint8_t rate) static const uint8_t sirf_disable[] = { 2, - 4, 9, 10, 27, @@ -289,6 +343,7 @@ ao_gps(void) __reentrant for (i = 0; i < sizeof (sirf_disable); i++) ao_sirf_set_message_rate(sirf_disable[i], 0); ao_sirf_set_message_rate(41, 1); + ao_sirf_set_message_rate(4, 1); } for (;;) { /* Locate the begining of the next record */ @@ -314,6 +369,11 @@ ao_gps(void) __reentrant break; ao_sirf_parse_41(); break; + case 4: + if (ao_sirf_len < 187) + break; + ao_sirf_parse_4(); + break; } if (ao_sirf_len != 0) continue; @@ -356,6 +416,17 @@ ao_gps(void) __reentrant ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_data); break; + case 4: + ao_mutex_get(&ao_gps_mutex); + ao_gps_tracking_data.channels = ao_sirf_tracker_data.channels; + for (i = 0; i < 12; i++) { + ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid; + ao_gps_tracking_data.sats[i].state = (uint8_t) ao_sirf_tracker_data.sats[i].state; + ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1; + } + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_tracking_data); + break; } } } @@ -367,6 +438,9 @@ gps_dump(void) __reentrant { ao_mutex_get(&ao_gps_mutex); ao_gps_print(&ao_gps_data); + putchar('\n'); + ao_gps_tracking_print(&ao_gps_tracking_data); + putchar('\n'); ao_mutex_put(&ao_gps_mutex); } diff --git a/src/ao_gps_print.c b/src/ao_gps_print.c index 8cc07c85..cc751337 100644 --- a/src/ao_gps_print.c +++ b/src/ao_gps_print.c @@ -82,14 +82,44 @@ ao_gps_print(__xdata struct ao_gps_data *gps_data) __reentrant gps_data->course * 2, climb / 100, climb % 100); - printf(" %d.%d(hdop) %5u(herr) %5u(verr)\n", + printf(" %d.%d(hdop) %5u(herr) %5u(verr)", gps_data->hdop / 5, (gps_data->hdop * 2) % 10, gps_data->h_error, gps_data->v_error); } else if (gps_data->flags & AO_GPS_RUNNING) { - printf(" unlocked\n"); + printf(" unlocked"); } else { - printf (" not-connected\n"); + printf (" not-connected"); + } +} + +void +ao_gps_tracking_print(__xdata struct ao_gps_tracking_data *gps_tracking_data) __reentrant +{ + uint8_t c, n, v; + __xdata struct ao_gps_sat_data *sat; + printf("SAT "); + n = gps_tracking_data->channels; + if (n == 0) { + printf("not-connected"); + return; + } + sat = gps_tracking_data->sats; + v = 0; + for (c = 0; c < n; c++) { + if (sat->svid && sat->state) + v++; + sat++; + } + printf("%d ", v); + sat = gps_tracking_data->sats; + for (c = 0; c < n; c++) { + if (sat->svid && sat->state) + printf (" %3d %02x %3d", + sat->svid, + sat->state, + sat->c_n_1); + sat++; } } diff --git a/src/ao_gps_report.c b/src/ao_gps_report.c index dce12adb..acf8bb40 100644 --- a/src/ao_gps_report.c +++ b/src/ao_gps_report.c @@ -52,10 +52,41 @@ ao_gps_report(void) } } +void +ao_gps_tracking_report(void) +{ + static __xdata struct ao_log_record gps_log; + static __xdata struct ao_gps_tracking_data gps_tracking_data; + uint8_t c, n; + + for (;;) { + ao_sleep(&ao_gps_tracking_data); + ao_mutex_get(&ao_gps_mutex); + memcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (struct ao_gps_tracking_data)); + ao_mutex_put(&ao_gps_mutex); + + if (!(n = gps_tracking_data.channels)) + continue; + + gps_log.tick = ao_time(); + gps_log.type = AO_LOG_GPS_SAT; + for (c = 0; c < n; c++) + if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid) && + (gps_log.u.gps_sat.state = gps_tracking_data.sats[c].state)) + { + gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1; + gps_log.u.gps_sat.unused = 0; + ao_log_data(&gps_log); + } + } +} + __xdata struct ao_task ao_gps_report_task; +__xdata struct ao_task ao_gps_tracking_report_task; void ao_gps_report_init(void) { ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report"); + ao_add_task(&ao_gps_tracking_report_task, ao_gps_tracking_report, "gps_tracking_report"); } diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c index fb9b0d10..c94128d9 100644 --- a/src/ao_gps_test.c +++ b/src/ao_gps_test.c @@ -44,6 +44,26 @@ struct ao_gps_data { uint16_t v_error; /* m */ }; +#define SIRF_SAT_STATE_ACQUIRED (1 << 0) +#define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) +#define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) +#define SIRF_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3) +#define SIRF_SAT_CARRIER_PULLIN_COMPLETE (1 << 4) +#define SIRF_SAT_CODE_LOCKED (1 << 5) +#define SIRF_SAT_ACQUISITION_FAILED (1 << 6) +#define SIRF_SAT_EPHEMERIS_AVAILABLE (1 << 7) + +struct ao_gps_sat_data { + uint8_t svid; + uint8_t state; + uint8_t c_n_1; +}; + +struct ao_gps_tracking_data { + uint8_t channels; + struct ao_gps_sat_data sats[12]; +}; + void ao_mutex_get(uint8_t *mutex) { @@ -78,6 +98,15 @@ ao_dbg_char(char c) static char input_queue[QUEUE_LEN]; int input_head, input_tail; +#include <sys/time.h> + +int +get_millis(void) +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return tv.tv_sec * 1000 + tv.tv_usec / 1000; +} static void check_sirf_message(char *from, uint8_t *msg, int len) @@ -101,9 +130,11 @@ check_sirf_message(char *from, uint8_t *msg, int len) } encoded_len = (msg[2] << 8) | msg[3]; id = msg[4]; +/* printf ("%9d: %3d\n", get_millis(), id); */ if (encoded_len != len - 8) { - printf ("length mismatch (got %d, wanted %d)\n", - len - 8, encoded_len); + if (id != 52) + printf ("length mismatch (got %d, wanted %d)\n", + len - 8, encoded_len); return; } encoded_cksum = (msg[len - 4] << 8) | msg[len-3]; @@ -116,7 +147,8 @@ check_sirf_message(char *from, uint8_t *msg, int len) return; } id = msg[4]; - if (id == 41) { + switch (id) { + case 41:{ int off = 4; uint8_t id; @@ -181,29 +213,83 @@ check_sirf_message(char *from, uint8_t *msg, int len) printf ("Geodetic Navigation Data (41):\n"); printf ("\tNav valid %04x\n", nav_valid); printf ("\tNav type %04x\n", nav_type); - printf ("\tWeek %d\n", week); - printf ("\tTOW %d\n", tow); - printf ("\tyear %d\n", year); - printf ("\tmonth %d\n", month); - printf ("\tday %d\n", day); - printf ("\thour %d\n", hour); - printf ("\tminute %d\n", minute); - printf ("\tsecond %g\n", second / 1000.0); + printf ("\tWeek %5d", week); + printf (" TOW %9d", tow); + printf (" %4d-%2d-%2d %02d:%02d:%07.4f\n", + year, month, day, + hour, minute, second / 1000.0); printf ("\tsats: %08x\n", sat_list); - printf ("\tlat: %g\n", lat / 1.0e7); - printf ("\tlon: %g\n", lon / 1.0e7); - printf ("\talt_ell: %g\n", alt_ell / 100.0); - printf ("\talt_msll: %g\n", alt_msl / 100.0); - printf ("\tdatum: %d\n", datum); - printf ("\tground speed: %g\n", sog / 100.0); - printf ("\tcourse: %g\n", cog / 100.0); - printf ("\tclimb: %g\n", climb_rate / 100.0); - printf ("\theading rate: %g\n", heading_rate / 100.0); - printf ("\th error: %g\n", h_error / 100.0); - printf ("\tv error: %g\n", v_error / 100.0); - printf ("\tt error: %g\n", t_error / 100.0); - printf ("\th vel error: %g\n", h_v_error / 100.0); - } else { + printf ("\tlat: %g", lat / 1.0e7); + printf (" lon: %g", lon / 1.0e7); + printf (" alt_ell: %g", alt_ell / 100.0); + printf (" alt_msll: %g", alt_msl / 100.0); + printf (" datum: %d\n", datum); + printf ("\tground speed: %g", sog / 100.0); + printf (" course: %g", cog / 100.0); + printf (" climb: %g", climb_rate / 100.0); + printf (" heading rate: %g\n", heading_rate / 100.0); + printf ("\th error: %g", h_error / 100.0); + printf (" v error: %g", v_error / 100.0); + printf (" t error: %g", t_error / 100.0); + printf (" h vel error: %g\n", h_v_error / 100.0); + break; + } + case 4: { + int off = 4; + uint8_t id; + int16_t gps_week; + uint32_t gps_tow; + uint8_t channels; + int j, k; + + get_u8(id); + get_u16(gps_week); + get_u32(gps_tow); + get_u8(channels); + + printf ("Measured Tracker Data (4):\n"); + printf ("GPS week: %d\n", gps_week); + printf ("GPS time of week: %d\n", gps_tow); + printf ("channels: %d\n", channels); + for (j = 0; j < 12; j++) { + uint8_t svid, azimuth, elevation; + uint16_t state; + uint8_t c_n[10]; + get_u8(svid); + get_u8(azimuth); + get_u8(elevation); + get_u16(state); + for (k = 0; k < 10; k++) { + get_u8(c_n[k]); + } + printf ("Sat %3d:", svid); + printf (" aziumuth: %6.1f", azimuth * 1.5); + printf (" elevation: %6.1f", elevation * 0.5); + printf (" state: 0x%02x", state); + printf (" c_n:"); + for (k = 0; k < 10; k++) + printf(" %3d", c_n[k]); + if (state & SIRF_SAT_STATE_ACQUIRED) + printf(" acq,"); + if (state & SIRF_SAT_STATE_CARRIER_PHASE_VALID) + printf(" car,"); + if (state & SIRF_SAT_BIT_SYNC_COMPLETE) + printf(" bit,"); + if (state & SIRF_SAT_SUBFRAME_SYNC_COMPLETE) + printf(" sub,"); + if (state & SIRF_SAT_CARRIER_PULLIN_COMPLETE) + printf(" pullin,"); + if (state & SIRF_SAT_CODE_LOCKED) + printf(" code,"); + if (state & SIRF_SAT_ACQUISITION_FAILED) + printf(" fail,"); + if (state & SIRF_SAT_EPHEMERIS_AVAILABLE) + printf(" ephem,"); + printf ("\n"); + } + break; + } + default: return; printf ("%s %4d:", from, encoded_len); for (i = 4; i < len - 4; i++) { @@ -315,9 +401,16 @@ ao_serial_set_speed(uint8_t speed) #include "ao_gps.c" void -ao_dump_state(void) +ao_dump_state(void *wchan) { double lat, lon; + int i; + if (wchan == &ao_gps_data) + ao_gps_print(&ao_gps_data); + else + ao_gps_tracking_print(&ao_gps_tracking_data); + putchar('\n'); + return; printf ("%02d:%02d:%02d", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); @@ -336,6 +429,12 @@ ao_dump_state(void) ao_gps_data.hdop / 5.0, ao_gps_data.h_error, ao_gps_data.v_error); printf("\n"); + printf ("\t"); + for (i = 0; i < 12; i++) + printf (" %2d(%02x)", + ao_gps_tracking_data.sats[i].svid, + ao_gps_tracking_data.sats[i].state); + printf ("\n"); } int @@ -358,14 +457,38 @@ ao_gps_open(const char *tty) return fd; } +#include <getopt.h> + +static const struct option options[] = { + { .name = "tty", .has_arg = 1, .val = 'T' }, + { 0, 0, 0, 0}, +}; + +static void usage(char *program) +{ + fprintf(stderr, "usage: %s [--tty <tty-name>]\n", program); + exit(1); +} + int main (int argc, char **argv) { - char *gps_file = "/dev/ttyUSB0"; + char *tty = "/dev/ttyUSB0"; + int c; - ao_gps_fd = ao_gps_open(gps_file); + while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) { + switch (c) { + case 'T': + tty = optarg; + break; + default: + usage(argv[0]); + break; + } + } + ao_gps_fd = ao_gps_open(tty); if (ao_gps_fd < 0) { - perror (gps_file); + perror (tty); exit (1); } ao_gps_setup(); diff --git a/src/ao_host.h b/src/ao_host.h index 38ff84ac..6b42f9f9 100644 --- a/src/ao_host.h +++ b/src/ao_host.h @@ -65,7 +65,7 @@ uint8_t ao_adc_head; #define ao_led_on(l) #define ao_led_off(l) #define ao_timer_set_adc_interval(i) -#define ao_wakeup(wchan) ao_dump_state() +#define ao_wakeup(wchan) ao_dump_state(wchan) #define ao_cmd_register(c) #define ao_usb_disable() #define ao_telemetry_set_interval(x) @@ -100,7 +100,7 @@ struct ao_adc ao_adc_static; FILE *emulator_in; void -ao_dump_state(void); +ao_dump_state(void *wchan); void ao_sleep(void *wchan); diff --git a/src/ao_monitor.c b/src/ao_monitor.c index 5997d427..e57ea145 100644 --- a/src/ao_monitor.c +++ b/src/ao_monitor.c @@ -55,6 +55,9 @@ ao_monitor(void) recv.telemetry.flight_pres, recv.telemetry.ground_pres); ao_gps_print(&recv.telemetry.gps); + putchar(' '); + ao_gps_tracking_print(&recv.telemetry.gps_tracking); + putchar('\n'); ao_rssi_set((int) recv.rssi - 74); } else { printf("CRC INVALID RSSI %3d\n", (int) recv.rssi - 74); diff --git a/src/ao_telemetry.c b/src/ao_telemetry.c index 463bcd91..9f57f3a9 100644 --- a/src/ao_telemetry.c +++ b/src/ao_telemetry.c @@ -39,6 +39,7 @@ ao_telemetry(void) ao_adc_get(&telemetry.adc); ao_mutex_get(&ao_gps_mutex); memcpy(&telemetry.gps, &ao_gps_data, sizeof (struct ao_gps_data)); + memcpy(&telemetry.gps_tracking, &ao_gps_tracking_data, sizeof (struct ao_gps_tracking_data)); ao_mutex_put(&ao_gps_mutex); ao_radio_send(&telemetry); ao_delay(ao_telemetry_interval); |