diff options
Diffstat (limited to 'src/venus_api.c')
| -rw-r--r-- | src/venus_api.c | 123 |
1 files changed, 77 insertions, 46 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; } |