Merge branch 'master' of github.com:PX4/Firmware into pca8574

1 files changed, 17 insertions, 15 deletions

diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 90a744015..1e779e8dc 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -51,6 +51,8 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
+#include <drivers/drv_hrt.h>
+
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5
 
@@ -62,7 +64,6 @@ static int _airspeed_sub = -1;
 static int _esc_sub = -1;
 
 static orb_advert_t _esc_pub;
-struct esc_status_s _esc;
 
 static bool _home_position_set = false;
 static double _home_lat = 0.0d;
@@ -82,8 +83,6 @@ init_sub_messages(void)
 void 
 init_pub_messages(void)
 {
-	memset(&_esc, 0, sizeof(_esc));
-	_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
 }
 
 void
@@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer)
 	size_t size = sizeof(msg);
 	memset(&msg, 0, size);
 	memcpy(&msg, buffer, size);
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+
+	// Publish it.
+	esc.timestamp = hrt_absolute_time();
+	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));
 
 	/* announce the esc if needed, just publish else */
 	if (_esc_pub > 0) {
-		orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
+		orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
 	} else {
-		_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
+		_esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
 	}
-
-	// Publish it.
-	_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));
 }
 
 void