From 86adaeb3e8f28c92005a38b7c71e12111efe8694 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 6 Jul 2013 15:02:34 +0200 Subject: More cleanups --- src/drivers/hott/comms.c | 9 ++++---- src/drivers/hott/comms.h | 4 +--- src/drivers/hott/hott_sensors/hott_sensors.c | 26 ++++++++++------------- src/drivers/hott/hott_telemetry/hott_telemetry.c | 7 ++---- src/drivers/hott/messages.c | 27 ++++++++++++++++++++++-- src/drivers/hott/messages.h | 5 +++-- 6 files changed, 47 insertions(+), 31 deletions(-) (limited to 'src/drivers/hott') diff --git a/src/drivers/hott/comms.c b/src/drivers/hott/comms.c index 4a7d3c845..a2de87407 100644 --- a/src/drivers/hott/comms.c +++ b/src/drivers/hott/comms.c @@ -41,17 +41,17 @@ #include #include -#include #include #include #include #include -#include #include #include +#include +#include int -open_uart(const char *device, struct termios *uart_config_original) +open_uart(const char *device) { /* baud rate */ static const speed_t speed = B19200; @@ -65,7 +65,8 @@ open_uart(const char *device, struct termios *uart_config_original) /* Back up the original uart configuration to restore it after exit */ int termios_state; - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + struct termios uart_config_original; + if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); } diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h index a1173631d..4954a309e 100644 --- a/src/drivers/hott/comms.h +++ b/src/drivers/hott/comms.h @@ -41,9 +41,7 @@ #ifndef COMMS_H_ #define COMMS_H -#include - -int open_uart(const char *device, struct termios *uart_config_original); +int open_uart(const char *device); #endif /* COMMS_H_ */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.c b/src/drivers/hott/hott_sensors/hott_sensors.c index dc10685b4..41ca0c92f 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.c +++ b/src/drivers/hott/hott_sensors/hott_sensors.c @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include @@ -60,7 +59,7 @@ static int thread_should_exit = false; /**< Deamon exit flag */ static int thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static const char daemon_name[] = "hott_sensors"; -static const char commandline_usage[] = "usage: hott_sensor start|status|stop [-d ]"; +static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d ]"; /** * Deamon management function. @@ -96,8 +95,6 @@ send_poll(int uart, uint8_t *buffer, size_t size) int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) { - usleep(5000); - static const int timeout_ms = 1000; struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; @@ -108,7 +105,6 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) bool stop_byte_read = false; while (true) { read(uart, &buffer[i], sizeof(buffer[i])); - //printf("[%d]: %d\n", i, buffer[i]); if (stop_byte_read) { // XXX process checksum @@ -149,37 +145,37 @@ hott_sensors_thread_main(int argc, char *argv[]) } /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - struct termios uart_config_original; - const int uart = open_uart(device, &uart_config_original); - + const int uart = open_uart(device); if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; } + pub_messages_init(); + uint8_t buffer[MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; - bool connected = true; while (!thread_should_exit) { // Currently we only support a General Air Module sensor. build_gam_request(&buffer, &size); send_poll(uart, buffer, size); + + // The sensor will need a little time before it starts sending. + usleep(5000); + recv_data(uart, &buffer, &size, &id); // Determine which moduel sent it and process accordingly. if (id == GAM_SENSOR_ID) { - //warnx("extract"); - extract_gam_message(buffer); + publish_gam_message(buffer); } else { - warnx("Unknown sensor ID received: %d", id); - } + warnx("Unknown sensor ID: %d", id); + } } warnx("exiting"); - close(uart); - thread_running = false; return 0; diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.c b/src/drivers/hott/hott_telemetry/hott_telemetry.c index fc80ac4d2..c87810b42 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.c +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.c @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -189,15 +188,13 @@ hott_telemetry_thread_main(int argc, char *argv[]) } /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - struct termios uart_config_original; - const int uart = open_uart(device, &uart_config_original); - + const int uart = open_uart(device); if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; } - messages_init(); + sub_messages_init(); uint8_t buffer[MESSAGE_BUFFER_SIZE]; size_t size = 0; diff --git a/src/drivers/hott/messages.c b/src/drivers/hott/messages.c index 1a29b7e73..ba2f6212d 100644 --- a/src/drivers/hott/messages.c +++ b/src/drivers/hott/messages.c @@ -58,19 +58,31 @@ static int gps_sub = -1; static int home_sub = -1; static int sensor_sub = -1; static int airspeed_sub = -1; +static int esc_sub = -1; + +//orb_advert_t _esc_pub; +//struct esc_s _esc; + static bool home_position_set = false; static double home_lat = 0.0d; static double home_lon = 0.0d; void -messages_init(void) +sub_messages_init(void) { battery_sub = orb_subscribe(ORB_ID(battery_status)); gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); home_sub = orb_subscribe(ORB_ID(home_position)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + //esc_sub = orb_subscribe(ORB_ID(esc)); +} + +void +pub_messages_init(void) +{ + //esc_pub = orb_subscribe(ORB_ID(esc)); } void @@ -87,17 +99,28 @@ build_gam_request(uint8_t *buffer, size_t *size) } void -extract_gam_message(const uint8_t *buffer) +publish_gam_message(const uint8_t *buffer) { struct gam_module_msg msg; size_t size = sizeof(msg); memset(&msg, 0, size); memcpy(&msg, buffer, size); + /* announce the esc if needed, just publish else */ +// if (esc_pub > 0) { +// orb_publish(ORB_ID(airspeed), _esc_pub, &_esc); +// +// } else { +// _esc_pub = orb_advertise(ORB_ID(esc), &_esc); +// } + // Publish it. uint16_t rpm = ((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + //_esc.rpm = rpm; uint8_t temp = msg.temperature2 + 20; + //_esc.temperature = temp; float current = ((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1f; + //_esc.current = current; printf("RPM: %d TEMP: %d A: %2.1f\n", rpm, temp, current); } diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index ecfad8569..dce90f273 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -256,9 +256,10 @@ struct gps_module_msg { #define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE -void messages_init(void); +void sub_messages_init(void); +void pub_messages_init(void); void build_gam_request(uint8_t *buffer, size_t *size); -void extract_gam_message(const uint8_t *buffer); +void publish_gam_message(const uint8_t *buffer); void build_eam_response(uint8_t *buffer, size_t *size); void build_gps_response(uint8_t *buffer, size_t *size); float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -- cgit v1.2.3