From 82e92a29e22d9602c5a11508d5bf8117cd229cfc Mon Sep 17 00:00:00 2001 From: James Maki Date: Thu, 13 May 2010 13:49:07 -0500 Subject: check for ACKs --- src/venus_api.c | 123 +++++++++++++++++++++++++++++++++++--------------------- src/venus_api.h | 3 ++ src/venus_gps.c | 46 ++++++++++++++------- 3 files changed, 112 insertions(+), 60 deletions(-) diff --git a/src/venus_api.c b/src/venus_api.c index 839d9ed..ea5a6bc 100644 --- a/src/venus_api.c +++ b/src/venus_api.c @@ -222,6 +222,49 @@ again: return 0; } +int venus_read_ack(int fd) +{ + int ret = -1; + int err; + + struct venus_msg msg; + + err = venus_read_msg(fd, &msg); + if (err < 0) { + log_error("venus_read_msg failed"); + return ret; + } + + if (msg.len == 2 && msg.data[0] == ID_ACK) { + ret = 0; + } else { + log_error("msg is not ACK"); + } + + venus_msg_data_free(&msg); + + return ret; +} + +int venus_write_msg_read_ack(int fd, struct venus_msg *msg) +{ + int err; + + err = venus_write_msg(fd, msg); + if (err < 0) { + log_error("venus_write_msg failed"); + return -1; + } + + err = venus_read_ack(fd); + if (err < 0) { + log_error("venus_read_ack failed"); + return -1; + } + + return 0; +} + ssize_t venus_read_nmea_sentence(int fd, void *buf, size_t count) { int err; @@ -266,6 +309,8 @@ int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, int struct tm broken; struct tm *tmp; + log_debug("begin"); + tmp = gmtime_r(&utc, &broken); if (tmp == NULL) { log_error("gmtime_r: %m"); @@ -294,18 +339,13 @@ int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, int msg.data = data; msg.len = sizeof(data); - err = venus_write_msg(fd, &msg); + err = venus_write_msg_read_ack(fd, &msg); if (err < 0) { - log_error("venus_write_msg: %d", err); + log_error("venus_write_msg_read_ack failed"); return -1; } - err = venus_read_msg(fd, &msg); - if (err < 0) { - log_error("venus_read_msg: %d", err); - return -1; - } - venus_msg_data_free(&msg); + log_debug("end"); return 0; } @@ -319,20 +359,17 @@ int venus_factory_defaults(int fd, uint8_t type) data[0] = ID_FACTORY_DEFAULTS; data[1] = type; + log_debug("begin"); + msg.data = data; msg.len = sizeof(data); - err = venus_write_msg(fd, &msg); + err = venus_write_msg_read_ack(fd, &msg); if (err < 0) { - log_error("venus_write_msg: %d", err); + log_error("venus_write_msg_read_ack failed"); return -1; } - err = venus_read_msg(fd, &msg); - if (err < 0) { - log_error("venus_read_msg: %d", err); - return -1; - } - venus_msg_data_free(&msg); + log_debug("end"); return 0; } @@ -348,20 +385,17 @@ int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) data[2] = baud; data[3] = attr; + log_debug("begin"); + msg.data = data; msg.len = sizeof(data); - err = venus_write_msg(fd, &msg); + err = venus_write_msg_read_ack(fd, &msg); if (err < 0) { - log_error("venus_write_msg: %d", err); + log_error("venus_write_msg_read_ack failed"); return -1; } - err = venus_read_msg(fd, &msg); - if (err < 0) { - log_error("venus_read_msg: %d", err); - return -1; - } - venus_msg_data_free(&msg); + log_debug("end"); return 0; } @@ -376,20 +410,17 @@ int venus_conf_format(int fd, uint8_t type, uint8_t attr) data[1] = type; data[2] = attr; + log_debug("begin"); + msg.data = data; msg.len = sizeof(data); - err = venus_write_msg(fd, &msg); + err = venus_write_msg_read_ack(fd, &msg); if (err < 0) { - log_error("venus_write_msg: %d", err); + log_error("venus_write_msg_read_ack failed"); return -1; } - err = venus_read_msg(fd, &msg); - if (err < 0) { - log_error("venus_read_msg: %d", err); - return -1; - } - venus_msg_data_free(&msg); + log_debug("end"); return 0; } @@ -402,11 +433,13 @@ int venus_query_sw_version(int fd, uint8_t type, struct venus_msg *msg) data[0] = ID_QUERY_SW_VERSION; data[1] = type; + log_debug("begin"); + msg->data = data; msg->len = sizeof(data); - err = venus_write_msg(fd, msg); + err = venus_write_msg_read_ack(fd, msg); if (err < 0) { - log_error("venus_write_msg: %d", err); + log_error("venus_write_msg_read_ack failed"); return -1; } @@ -415,14 +448,15 @@ int venus_query_sw_version(int fd, uint8_t type, struct venus_msg *msg) log_error("venus_read_msg: %d", err); return -1; } - venus_msg_data_free(msg); - err = venus_read_msg(fd, msg); - if (err < 0) { - log_error("venus_read_msg: %d", err); + if (msg->len != 14 || msg->data[0] != ID_SW_VERSION) { + venus_msg_data_free(msg); + log_error("expected sw version message"); return -1; } + log_debug("end"); + return 0; } @@ -442,20 +476,17 @@ int venus_conf_nmea(int fd, uint8_t gga, uint8_t gsa, uint8_t gsv, uint8_t gll, data[7] = zda; data[8] = attr; + log_debug("begin"); + msg.data = data; msg.len = sizeof(data); - err = venus_write_msg(fd, &msg); + err = venus_write_msg_read_ack(fd, &msg); if (err < 0) { - log_error("venus_write_msg: %d", err); + log_error("venus_write_msg_read_ack failed"); return -1; } - err = venus_read_msg(fd, &msg); - if (err < 0) { - log_error("venus_read_msg: %d", err); - return -1; - } - venus_msg_data_free(&msg); + log_debug("end"); return 0; } diff --git a/src/venus_api.h b/src/venus_api.h index 79a1729..8212487 100644 --- a/src/venus_api.h +++ b/src/venus_api.h @@ -84,6 +84,9 @@ void venus_msg_free(struct venus_msg *msg); uint8_t venus_checksum(void *data, uint16_t len); int venus_write_msg(int fd, struct venus_msg *msg); int venus_read_msg(int fd, struct venus_msg *msg); +int venus_read_ack(int fd); +int venus_write_msg_read_ack(int fd, struct venus_msg *msg); + ssize_t venus_read_nmea_sentence(int fd, void *buf, size_t count); int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, int16_t longitude, int16_t altitude); diff --git a/src/venus_gps.c b/src/venus_gps.c index c11bed1..1136d07 100644 --- a/src/venus_gps.c +++ b/src/venus_gps.c @@ -710,42 +710,60 @@ int main(int argc, char *argv[]) exit(1); } - venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM); + tmp = venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM); + if (tmp < 0) { + log_error("failed to change to binary format"); + exit(1); + } tmp = venus_query_sw_version(tty, SW_TYPE_RESERVED, &msg); if (tmp < 0) { log_error("failed to query venus sw version info"); exit(1); } - if (msg.len == 14 && msg.data[0] == ID_SW_VERSION) { - log_notice("Venus Software Type: 0x%02X", msg.data[1]); - log_notice("Venus Kernel Version: 0x%02X%02X%02X%02X", - msg.data[2], msg.data[3], msg.data[4], msg.data[5]); - log_notice("Venus ODM version: 0x%02X%02X%02X%02X", - msg.data[6], msg.data[7], msg.data[8], msg.data[9]); - log_notice("Venus Revision: 0x%02X%02X%02X%02X", - msg.data[10], msg.data[11], msg.data[12], msg.data[13]); - } + log_notice("Venus Software Type: 0x%02X", msg.data[1]); + log_notice("Venus Kernel Version: 0x%02X%02X%02X%02X", + msg.data[2], msg.data[3], msg.data[4], msg.data[5]); + log_notice("Venus ODM version: 0x%02X%02X%02X%02X", + msg.data[6], msg.data[7], msg.data[8], msg.data[9]); + log_notice("Venus Revision: 0x%02X%02X%02X%02X", + msg.data[10], msg.data[11], msg.data[12], msg.data[13]); venus_msg_data_free(&msg); #if CONFIG_CAN_DEFAULT if (factory_defaults) { - venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT); log_notice("setting factory defaults"); + tmp = venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT); + if (tmp < 0) { + log_error("failed to set factory defaults"); + exit(1); + } exit(0); } #endif #if CONFIG_CAN_RESET if (start_mode != (typeof(start_mode)) - 1) { - venus_system_restart(tty, start_mode, time(NULL), latitude, longitude, altitude); log_notice("issuing system restart"); + tmp = venus_system_restart(tty, start_mode, time(NULL), latitude, longitude, altitude); + if (tmp < 0) { + log_error("system restart failed"); + exit(1); + } exit(0); } #endif - venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM); + tmp = venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM); + if (tmp < 0) { + log_error("failed to configure nmea sentences"); + exit(1); + } - venus_conf_format(tty, MSG_TYPE_NMEA, UPDATE_ATTR_SRAM); + tmp = venus_conf_format(tty, MSG_TYPE_NMEA, UPDATE_ATTR_SRAM); + if (tmp < 0) { + log_error("failed to change to nmea format"); + exit(1); + } close(tty); -- cgit v1.2.3