Major refactor of HoTT drivers and finished sensor read implementation.

7 files changed, 151 insertions, 108 deletions

diff --git a/src/drivers/hott/comms.c b/src/drivers/hott/comms.cpp
index a2de87407..a2de87407 100644
--- a/src/drivers/hott/comms.c
+++ b/src/drivers/hott/comms.cpp
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.c b/src/drivers/hott/hott_sensors/hott_sensors.cpp
index 41ca0c92f..ad7e74e62 100644
--- a/src/drivers/hott/hott_sensors/hott_sensors.c
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -55,6 +55,14 @@
 #include "../comms.h"
 #include "../messages.h"
 
+#define DEFAULT_UART "/dev/ttyS2";		/**< USART5 */
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
 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 */
@@ -64,7 +72,7 @@ static const char commandline_usage[] = "usage: hott_sensors start|status|stop [
 /**
  * Deamon management function.
  */
-__EXPORT int hott_sensors_main(int argc, char *argv[]);
+extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]);
 
 /**
  * Mainloop of daemon.
@@ -96,11 +104,13 @@ int
 recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
 {
 	static const int timeout_ms = 1000;
-	struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
-
+	
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
 	
 	// XXX should this poll be inside the while loop???
-	if (poll(fds, 1, timeout_ms) > 0) {
+	if (poll(&fds, 1, timeout_ms) > 0) {
 		int i = 0;
 		bool stop_byte_read = false;
 		while (true)  {
@@ -129,7 +139,7 @@ hott_sensors_thread_main(int argc, char *argv[])
 
 	thread_running = true;
 
-	const char *device = "/dev/ttyS2";		/**< Default telemetry port: USART5 */
+	const char *device = DEFAULT_UART;
 
 	/* read commandline arguments */
 	for (int i = 0; i < argc && argv[i]; i++) {
@@ -151,20 +161,20 @@ hott_sensors_thread_main(int argc, char *argv[])
 		thread_running = false;
 	}
 
-	pub_messages_init();
+	init_pub_messages();
 
-	uint8_t buffer[MESSAGE_BUFFER_SIZE];
+	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
 	size_t size = 0;
 	uint8_t id = 0;
 	while (!thread_should_exit) {
 		// Currently we only support a General Air Module sensor.
-		build_gam_request(&buffer, &size);
+		build_gam_request(&buffer[0], &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);
+		recv_data(uart, &buffer[0], &size, &id);
 
 		// Determine which moduel sent it and process accordingly.
 		if (id == GAM_SENSOR_ID) {
@@ -199,12 +209,12 @@ hott_sensors_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn(daemon_name,
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
-					 2048,
-					 hott_sensors_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn_cmd(daemon_name,
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 40,
+					     2048,
+					     hott_sensors_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk
index ca65d3de2..b5f5762ba 100644
--- a/src/drivers/hott/hott_sensors/module.mk
+++ b/src/drivers/hott/hott_sensors/module.mk
@@ -37,6 +37,6 @@
 
 MODULE_COMMAND	= hott_sensors
 
-SRCS		= hott_sensors.c \
-		  ../messages.c \
-		  ../comms.c
+SRCS		= hott_sensors.cpp \
+		  ../messages.cpp \
+		  ../comms.cpp
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.c b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index a88f357f6..1c68e06b1 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.c
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -57,6 +57,14 @@
 #include "../comms.h"
 #include "../messages.h"
 
+#define DEFAULT_UART "/dev/ttyS1";		/**< USART2 */
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
 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 */
@@ -66,7 +74,7 @@ static const char commandline_usage[] = "usage: hott_telemetry start|status|stop
 /**
  * Deamon management function.
  */
-__EXPORT int hott_telemetry_main(int argc, char *argv[]);
+extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]);
 
 /**
  * Mainloop of daemon.
@@ -80,11 +88,14 @@ int
 recv_req_id(int uart, uint8_t *id)
 {
 	static const int timeout_ms = 1000;  // TODO make it a define
-	struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
 
 	uint8_t mode;
+	
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
 
-	if (poll(fds, 1, timeout_ms) > 0) {
+	if (poll(&fds, 1, timeout_ms) > 0) {
 		/* Get the mode: binary or text  */
 		read(uart, &mode, sizeof(mode));
 
@@ -109,11 +120,13 @@ 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 } };
 
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
 	
 	// XXX should this poll be inside the while loop???
-	if (poll(fds, 1, timeout_ms) > 0) {
+	if (poll(&fds, 1, timeout_ms) > 0) {
 		int i = 0;
 		bool stop_byte_read = false;
 		while (true)  {
@@ -172,7 +185,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
 
 	thread_running = true;
 
-	const char *device = "/dev/ttyS1";		/**< Default telemetry port: USART2 */
+	const char *device = DEFAULT_UART;
 
 	/* read commandline arguments */
 	for (int i = 0; i < argc && argv[i]; i++) {
@@ -194,9 +207,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
 		thread_running = false;
 	}
 
-	sub_messages_init();
+	init_sub_messages();
 
-	uint8_t buffer[MESSAGE_BUFFER_SIZE];
+	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
 	size_t size = 0;
 	uint8_t id = 0;
 	bool connected = true;
@@ -212,7 +225,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
 			case EAM_SENSOR_ID:
 				build_eam_response(buffer, &size);
 				break;
-
+			case GAM_SENSOR_ID:
+				build_gam_response(buffer, &size);
+				break;
 			case GPS_SENSOR_ID:
 				build_gps_response(buffer, &size);
 				break;
diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk
index 7673d7e77..b19cbd14c 100644
--- a/src/drivers/hott/hott_telemetry/module.mk
+++ b/src/drivers/hott/hott_telemetry/module.mk
@@ -37,6 +37,6 @@
 
 MODULE_COMMAND	= hott_telemetry
 
-SRCS		= hott_telemetry.c \
-		  ../messages.c \
-		  ../comms.c
+SRCS		= hott_telemetry.cpp \
+		  ../messages.cpp \
+		  ../comms.cpp
diff --git a/src/drivers/hott/messages.c b/src/drivers/hott/messages.cpp
index 36d5fe942..004322a2d 100644
--- a/src/drivers/hott/messages.c
+++ b/src/drivers/hott/messages.cpp
@@ -46,6 +46,7 @@
 #include <unistd.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -53,36 +54,37 @@
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5
 
-static int battery_sub = -1;
-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;
+static int _battery_sub = -1;
+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;
+orb_advert_t _esc_pub;
+struct esc_status_s _esc;
 
 
-static bool home_position_set = false;
-static double home_lat = 0.0d;
-static double home_lon = 0.0d;
+static bool _home_position_set = false;
+static double _home_lat = 0.0d;
+static double _home_lon = 0.0d;
 
 void 
-sub_messages_init(void)
+init_sub_messages(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));
+	_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_status));
 }
 
 void 
-pub_messages_init(void)
+init_pub_messages(void)
 {
-	//esc_pub = orb_subscribe(ORB_ID(esc));
+	memset(&_esc, 0, sizeof(_esc));
+	_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
 }
 
 void
@@ -107,21 +109,22 @@ publish_gam_message(const uint8_t *buffer)
 	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);
-//	}
+	if (_esc_pub > 0) {
+		orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
+	} else {
+		_esc_pub = orb_advertise(ORB_ID(esc_status), &_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);
+	_esc.esc_count = 1;
+	_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
+
+	_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
+	_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
+	_esc.esc[0].esc_temperature = msg.temperature1 - 20; 
+	_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
+	_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
+	//printf("T: %d\n", _esc.esc[0].esc_temperature);
 }
 
 void 
@@ -130,12 +133,12 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	/* get a local copy of the current sensor values */
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+	orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
 
 	/* get a local copy of the battery data */
 	struct battery_status_s battery;
 	memset(&battery, 0, sizeof(battery));
-	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+	orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
 
 	struct eam_module_msg msg;
 	*size = sizeof(msg);
@@ -145,7 +148,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	msg.eam_sensor_id = EAM_SENSOR_ID;
 	msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
 	
-	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius - 20);
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
 	msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
 
 	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
@@ -157,7 +160,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	/* get a local copy of the airspeed data */
 	struct airspeed_s airspeed;
 	memset(&airspeed, 0, sizeof(airspeed));
-	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
+	orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
 
 	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
 	msg.speed_L = (uint8_t)speed & 0xff;
@@ -168,19 +171,54 @@ build_eam_response(uint8_t *buffer, size_t *size)
 }
 
 void 
+build_gam_response(uint8_t *buffer, size_t *size)
+{
+	/* get a local copy of the ESC Status values */
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+	orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
+
+	struct gam_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.gam_sensor_id = GAM_SENSOR_ID;
+	msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
+	
+	msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
+	msg.temperature2 = 20;  // 0 deg. C.
+
+	uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
+	msg.main_voltage_L = (uint8_t)voltage & 0xff;
+	msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
+
+	uint16_t current = (uint16_t)(esc.esc[0].esc_current);
+	msg.current_L = (uint8_t)current & 0xff;
+	msg.current_H = (uint8_t)(current >> 8) & 0xff;
+
+	uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
+	msg.rpm_L = (uint8_t)rpm & 0xff;
+	msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
+
+	msg.stop = STOP_BYTE;
+	memcpy(buffer, &msg, *size);
+}
+
+void 
 build_gps_response(uint8_t *buffer, size_t *size)
 {
 	/* get a local copy of the current sensor values */
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+	orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
 
  	/* get a local copy of the battery data */
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
-	orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
+	orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
 
-	struct gps_module_msg msg = { 0 };
+	struct gps_module_msg msg;
 	*size = sizeof(msg);
 	memset(&msg, 0, *size);
 
@@ -200,7 +238,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
 
 		/* GPS speed */
-		uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
+		uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
 		msg.gps_speed_L = (uint8_t)speed & 0xff;
 		msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
 		
@@ -246,33 +284,33 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
 		
 		/* Altitude */
-		uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f);
+		uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f);
 		msg.altitude_L = (uint8_t)alt & 0xff;
 		msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
-		/* Get any (and probably only ever one) home_sub postion report */
+		/* Get any (and probably only ever one) _home_sub postion report */
 		bool updated;
-		orb_check(home_sub, &updated);
+		orb_check(_home_sub, &updated);
 		if (updated) {
 			/* get a local copy of the home position data */
 			struct home_position_s home;
 			memset(&home, 0, sizeof(home));
-			orb_copy(ORB_ID(home_position), home_sub, &home);
+			orb_copy(ORB_ID(home_position), _home_sub, &home);
 
-			home_lat = ((double)(home.lat))*1e-7d;
-			home_lon = ((double)(home.lon))*1e-7d;
-			home_position_set = true;
+			_home_lat = ((double)(home.lat))*1e-7d;
+			_home_lon = ((double)(home.lon))*1e-7d;
+			_home_position_set = true;
 		}
 
 		/* Distance from home */
-		if (home_position_set) {
-			uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
+		if (_home_position_set) {
+			uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
 
 			msg.distance_L = (uint8_t)dist & 0xff;
 			msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
 
 			/* Direction back to home */
-			uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
+			uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
 			msg.home_direction = (uint8_t)bearing >> 1;
 		}
 	}
diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h
index dce90f273..451bee91c 100644
--- a/src/drivers/hott/messages.h
+++ b/src/drivers/hott/messages.h
@@ -119,23 +119,16 @@ struct eam_module_msg {
 	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. */
 };
 
-/** 
- * The maximum buffer size required to store an Electric Air Module message.
- */
-#define EAM_MESSAGE_BUFFER_SIZE sizeof(union { 	\
-    struct eam_module_msg eam; 			\
-})
-
 
 /* General Air Module (GAM) constants. */
 #define GAM_SENSOR_ID			0x8d
 #define GAM_SENSOR_TEXT_ID		0xd0
 
 struct gam_module_msg {
-        uint8_t start_byte;		// start byte constant value 0x7c
+        uint8_t start;		// start byte constant value 0x7c
         uint8_t gam_sensor_id;		// EAM sensort id. constat value 0x8d
         uint8_t warning_beeps;		// 1=A 2=B ... 0x1a=Z  0 = no alarm
-        uint8_t sensor_id;		// constant value 0xd0
+        uint8_t sensor_text_id;		// constant value 0xd0
         uint8_t alarm_invers1;		// alarm bitmask. Value is displayed inverted
         uint8_t alarm_invers2;		// alarm bitmask. Value is displayed inverted                                                                
         uint8_t cell1;			// cell 1 voltage lower value. 0.02V steps, 124=2.48V
@@ -180,14 +173,6 @@ struct gam_module_msg {
         uint8_t checksum;		// checksum
 };
 
-/** 
- * The maximum buffer size required to store a General Air Module message.
- */
-#define GAM_MESSAGE_BUFFER_SIZE sizeof(union { 	\
-    struct gam_module_msg gam; 			\
-})
-
-
 /* GPS sensor constants. */
 #define GPS_SENSOR_ID		0x8a
 #define GPS_SENSOR_TEXT_ID	0xa0
@@ -247,20 +232,15 @@ struct gps_module_msg {
 	uint8_t checksum;			/**< Byte 45: Parity Byte */
 };
 
-/** 
- * The maximum buffer size required to store a HoTT message.
- */
-#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { 	\
-    struct gps_module_msg gps; 			\
-})
-
-#define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE
+// The maximum size of a message.
+#define MAX_MESSAGE_BUFFER_SIZE 45
 
-void sub_messages_init(void);
-void pub_messages_init(void);
+void init_sub_messages(void);
+void init_pub_messages(void);
 void build_gam_request(uint8_t *buffer, size_t *size);
 void publish_gam_message(const uint8_t *buffer);
 void build_eam_response(uint8_t *buffer, size_t *size);
+void build_gam_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);
 void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);