diff options
Diffstat (limited to 'src/venus_api.c')
-rw-r--r-- | src/venus_api.c | 176 |
1 files changed, 85 insertions, 91 deletions
diff --git a/src/venus_api.c b/src/venus_api.c index 52a4a96..26e2787 100644 --- a/src/venus_api.c +++ b/src/venus_api.c @@ -60,110 +60,116 @@ static const struct baud_map __venus_baud_map[] = { {B115200, VENUS_115200}, }; -speed_t value_to_baud(speed_t value) { +speed_t value_to_baud(speed_t value) +{ int n = sizeof(__baud_map) / sizeof(struct baud_map); int i; - for(i = 0; i < n; ++i) { - if(__baud_map[i].value == value) { + for (i = 0; i < n; ++i) { + if (__baud_map[i].value == value) { return __baud_map[i].baud; } } log_warning("baud rate not valid: %lu", (unsigned long) value); - return (speed_t) -1; + return (speed_t) - 1; } -speed_t baud_to_venus(speed_t baud) { +speed_t baud_to_venus(speed_t baud) +{ int n = sizeof(__venus_baud_map) / sizeof(struct baud_map); int i; - for(i = 0; i < n; ++i) { - if(__venus_baud_map[i].baud == baud) { + for (i = 0; i < n; ++i) { + if (__venus_baud_map[i].baud == baud) { return __venus_baud_map[i].value; } } log_warning("baud rate not valid: %lu", (unsigned long) baud); - return (speed_t) -1; + return (speed_t) - 1; } -static uint8_t seq_start[] = {0xA0, 0xA1}; -static uint8_t seq_end[] = {0x0D, 0x0A}; +static uint8_t seq_start[] = { 0xA0, 0xA1 }; +static uint8_t seq_end[] = { 0x0D, 0x0A }; -void free_venus_msg_data(struct venus_msg *msg) { - if(!msg) { +void free_venus_msg_data(struct venus_msg *msg) +{ + if (!msg) { return; } - if(msg->data) { + if (msg->data) { free(msg->data); } msg->data = NULL; } -uint8_t checksum(void *data, uint16_t len) { +uint8_t checksum(void *data, uint16_t len) +{ int i; uint8_t cs = 0; - for(i = 0; i < len; i++) { + for (i = 0; i < len; i++) { cs ^= ((char *) data)[i]; } return cs; } -int venus_write_msg(int fd, struct venus_msg *msg) { +int venus_write_msg(int fd, struct venus_msg *msg) +{ int err; uint8_t cs = 0; log_debug("id: %d", msg->data[0]); err = full_write(fd, seq_start, sizeof(seq_start)); - if(err != sizeof(seq_start)) { - log_error("failed to write seq_start: %d %d", err, errno); + if (err != sizeof(seq_start)) { + log_error("failed to write seq_start: %d %m", err); } msg->len = __cpu_to_be16(msg->len); err = full_write(fd, &msg->len, 2); - if(err != 2) { - log_error("failed to write len: %d %d", err, errno); + if (err != 2) { + log_error("failed to write len: %d %m", err); } msg->len = __be16_to_cpu(msg->len); err = full_write(fd, msg->data, msg->len); - if(err != msg->len) { - log_error("failed to write data: %d %d", err, errno); + if (err != msg->len) { + log_error("failed to write data: %d %m", err); } cs = checksum(msg->data, msg->len); err = full_write(fd, &cs, 1); - if(err != 1) { - log_error("failed to write checksum: %d %d", err, errno); + if (err != 1) { + log_error("failed to write checksum: %d %m", err); } err = full_write(fd, seq_end, sizeof(seq_end)); - if(err != sizeof(seq_end)) { - log_error("failed to write seq_end: %d %d", err, errno); + if (err != sizeof(seq_end)) { + log_error("failed to write seq_end: %d %m", err); } return 0; } -int venus_read_msg(int fd, struct venus_msg *msg) { +int venus_read_msg(int fd, struct venus_msg *msg) +{ uint8_t cs; uint8_t *data; uint16_t len; uint8_t seq[2]; int attempts = 0; -again: - while(1) { + again: + while (1) { safe_readn(fd, seq, 2); //log_error("start seq %c %c", seq[0], seq[1]); - if(!memcmp(seq, seq_start, 2)) { + if (!memcmp(seq, seq_start, 2)) { break; } attempts++; - if(attempts > 1024) { + if (attempts > 1024) { log_error("max attempts reached"); return -1; } @@ -174,17 +180,17 @@ again: data = malloc(len); safe_readn(fd, data, len); safe_readn(fd, &cs, 1); - if(checksum(data, len) != cs) { + if (checksum(data, len) != cs) { log_error("checksum mismatch"); return -1; } safe_readn(fd, seq, 2); - if(memcmp(seq, seq_end, 2)) { + if (memcmp(seq, seq_end, 2)) { log_error("expected seq end"); return -1; } - if(len == 2 && data[0] == ID_ACK && data[1] == ID_NONE) { + if (len == 2 && data[0] == ID_ACK && data[1] == ID_NONE) { log_debug("read ACK ID_NONE"); free(data); goto again; @@ -195,19 +201,20 @@ again: #if DEBUG { - int i; - printf("READ MSG: "); - for(i = 0; i < msg->len; i++) { - printf("%02X ", msg->data[i]); - } - printf("\n"); + int i; + printf("READ MSG: "); + for (i = 0; i < msg->len; i++) { + printf("%02X ", msg->data[i]); + } + printf("\n"); } #endif return 0; } -ssize_t read_nmea_sentence(int fd, void *buf, size_t count) { +ssize_t read_nmea_sentence(int fd, void *buf, size_t count) +{ int err; char c; ssize_t total = 0; @@ -215,25 +222,25 @@ ssize_t read_nmea_sentence(int fd, void *buf, size_t count) { int start = 0; - while(1) { + while (1) { err = safe_read(fd, &c, 1); - if(err <= 0) { - log_error("read from gps failed: %d", errno); + if (err <= 0) { + log_error("read from gps failed: %m"); return -1; } - if(!start) { - if(c != '$') { + if (!start) { + if (c != '$') { continue; } start = 1; } - if(total < count) { + if (total < count) { *cp++ = c; total++; } - if(c == '\n') { + if (c == '\n') { break; } } @@ -241,13 +248,7 @@ ssize_t read_nmea_sentence(int fd, void *buf, size_t count) { return total; } -int venus_system_restart( - int fd, - uint8_t mode, - time_t utc, - int16_t latitude, - int16_t longitude, - int16_t altitude) +int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, int16_t longitude, int16_t altitude) { int err; uint8_t data[15]; @@ -258,7 +259,7 @@ int venus_system_restart( tmp = gmtime_r(&utc, &broken); if (tmp == NULL) { - log_error("gmtime_r: %d", errno); + log_error("gmtime_r: %m"); return -1; } @@ -285,13 +286,13 @@ int venus_system_restart( msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -300,7 +301,8 @@ int venus_system_restart( return 0; } -int venus_factory_defaults(int fd, uint8_t type) { +int venus_factory_defaults(int fd, uint8_t type) +{ int err; uint8_t data[2]; struct venus_msg msg; @@ -311,13 +313,13 @@ int venus_factory_defaults(int fd, uint8_t type) { msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -326,7 +328,8 @@ int venus_factory_defaults(int fd, uint8_t type) { return 0; } -int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) { +int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) +{ int err; uint8_t data[4]; struct venus_msg msg; @@ -339,13 +342,13 @@ int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) { msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -354,7 +357,8 @@ int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) { return 0; } -int venus_conf_format(int fd, uint8_t type, uint8_t attr) { +int venus_conf_format(int fd, uint8_t type, uint8_t attr) +{ int err; uint8_t data[3]; struct venus_msg msg; @@ -366,13 +370,13 @@ int venus_conf_format(int fd, uint8_t type, uint8_t attr) { msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -381,7 +385,8 @@ int venus_conf_format(int fd, uint8_t type, uint8_t attr) { return 0; } -int venus_query_sw_version(int fd, uint8_t type) { +int venus_query_sw_version(int fd, uint8_t type) +{ int err; uint8_t data[2]; struct venus_msg msg; @@ -392,20 +397,20 @@ int venus_query_sw_version(int fd, uint8_t type) { msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } free_venus_msg_data(&msg); err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -414,16 +419,7 @@ int venus_query_sw_version(int fd, uint8_t type) { return 0; } -int venus_conf_nmea( - int fd, - uint8_t gga, - uint8_t gsa, - uint8_t gsv, - uint8_t gll, - uint8_t rmc, - uint8_t vtg, - uint8_t zda, - uint8_t attr) +int venus_conf_nmea(int fd, uint8_t gga, uint8_t gsa, uint8_t gsv, uint8_t gll, uint8_t rmc, uint8_t vtg, uint8_t zda, uint8_t attr) { int err; uint8_t data[9]; @@ -442,13 +438,13 @@ int venus_conf_nmea( msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -457,7 +453,8 @@ int venus_conf_nmea( return 0; } -int venus_tty_configure(int fd, speed_t baud_rate) { +int venus_tty_configure(int fd, speed_t baud_rate) +{ struct termios tio; tcgetattr(fd, &tio); cfmakeraw(&tio); @@ -467,12 +464,13 @@ int venus_tty_configure(int fd, speed_t baud_rate) { return 0; } -int venus_open(const char *dev, speed_t baud_rate) { +int venus_open(const char *dev, speed_t baud_rate) +{ int tty; tty = open(dev, O_RDWR | O_NOCTTY); - if(tty < 0) { - log_error("failed to open %s: errno: %d", dev, errno); + if (tty < 0) { + log_error("failed to open %s: %m", dev); exit(1); } @@ -481,7 +479,3 @@ int venus_open(const char *dev, speed_t baud_rate) { return tty; } - - - - |