diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/drivers/hott_telemetry/hott_telemetry_main.c | 14 | ||||
| -rw-r--r-- | src/modules/gpio_led/gpio_led.c | 70 |
2 files changed, 58 insertions, 26 deletions
diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c index 4318244f8..1d2bdd92e 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -133,15 +133,14 @@ recv_req_id(int uart, uint8_t *id) if (poll(fds, 1, timeout_ms) > 0) { /* Get the mode: binary or text */ read(uart, &mode, sizeof(mode)); - /* Read the device ID being polled */ - read(uart, id, sizeof(*id)); /* if we have a binary mode request */ if (mode != BINARY_MODE_REQUEST_ID) { - warnx("Non binary request ID detected: %d", mode); return ERROR; } + /* Read the device ID being polled */ + read(uart, id, sizeof(*id)); } else { warnx("UART timeout on TX/RX port"); return ERROR; @@ -216,9 +215,15 @@ hott_telemetry_thread_main(int argc, char *argv[]) uint8_t buffer[MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; + bool connected = true; while (!thread_should_exit) { if (recv_req_id(uart, &id) == OK) { + if (!connected) { + connected = true; + warnx("OK"); + } + switch (id) { case EAM_SENSOR_ID: build_eam_response(buffer, &size); @@ -234,7 +239,8 @@ hott_telemetry_thread_main(int argc, char *argv[]) send_data(uart, buffer, size); } else { - warnx("NOK"); + connected = false; + warnx("syncing"); } } diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index a80bf9cb8..43cbe74c7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -48,6 +48,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <poll.h> @@ -64,6 +65,7 @@ struct gpio_led_s { }; static struct gpio_led_s gpio_led_data; +static bool gpio_led_started = false; __EXPORT int gpio_led_main(int argc, char *argv[]); @@ -75,31 +77,54 @@ int gpio_led_main(int argc, char *argv[]) { int pin = GPIO_EXT_1; - if (argc > 1) { - if (!strcmp(argv[1], "-p")) { - if (!strcmp(argv[2], "1")) { - pin = GPIO_EXT_1; + if (argc < 2) { + errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]"); - } else if (!strcmp(argv[2], "2")) { - pin = GPIO_EXT_2; + } else { - } else { - printf("[gpio_led] Unsupported pin: %s\n", argv[2]); + /* START COMMAND HANDLING */ + if (!strcmp(argv[1], "start")) { + + if (argc > 2) { + if (!strcmp(argv[1], "-p")) { + if (!strcmp(argv[2], "1")) { + pin = GPIO_EXT_1; + + } else if (!strcmp(argv[2], "2")) { + pin = GPIO_EXT_2; + + } else { + warnx("[gpio_led] Unsupported pin: %s\n", argv[2]); + exit(1); + } + } + } + + memset(&gpio_led_data, 0, sizeof(gpio_led_data)); + gpio_led_data.pin = pin; + int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); + + if (ret != 0) { + warnx("[gpio_led] Failed to queue work: %d\n", ret); exit(1); + + } else { + gpio_led_started = true; } - } - } - memset(&gpio_led_data, 0, sizeof(gpio_led_data)); - gpio_led_data.pin = pin; - int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); + exit(0); - if (ret != 0) { - printf("[gpio_led] Failed to queue work: %d\n", ret); - exit(1); - } + /* STOP COMMAND HANDLING */ + + } else if (!strcmp(argv[1], "stop")) { + gpio_led_started = false; - exit(0); + /* INVALID COMMAND */ + + } else { + errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]); + } + } } void gpio_led_start(FAR void *arg) @@ -110,7 +135,7 @@ void gpio_led_start(FAR void *arg) priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); if (priv->gpio_fd < 0) { - printf("[gpio_led] GPIO: open fail\n"); + warnx("[gpio_led] GPIO: open fail\n"); return; } @@ -125,11 +150,11 @@ void gpio_led_start(FAR void *arg) int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); if (ret != 0) { - printf("[gpio_led] Failed to queue work: %d\n", ret); + warnx("[gpio_led] Failed to queue work: %d\n", ret); return; } - printf("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin); + warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin); } void gpio_led_cycle(FAR void *arg) @@ -187,5 +212,6 @@ void gpio_led_cycle(FAR void *arg) priv->counter = 0; /* repeat cycle at 5 Hz*/ - work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); + if (gpio_led_started) + work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); } |