summaryrefslogtreecommitdiff
path: root/src/venus_api.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/venus_api.c')
-rw-r--r--src/venus_api.c176
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;
}
-
-
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-07 14:04:18 +0000