More cleanups

6 files changed, 47 insertions, 31 deletions

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 <fcntl.h>
 #include <nuttx/config.h>
-#include <poll.h>
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
 #include <sys/ioctl.h>
-#include <unistd.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
+#include <termios.h>
+#include <unistd.h>
 
 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 <termios.h>
-
-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 <stdlib.h>
 #include <stdio.h>
 #include <string.h>
-#include <termios.h>
 #include <sys/ioctl.h>
 #include <unistd.h>
 #include <systemlib/err.h>
@@ -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 <device>]";
+static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]";
 
 /**
  * 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 <stdlib.h>
 #include <stdio.h>
 #include <string.h>
-#include <termios.h>
 #include <sys/ioctl.h>
 #include <unistd.h>
 #include <systemlib/err.h>
@@ -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);