Update ORB topic 'esc_status'

5 files changed, 33 insertions, 34 deletions

diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 086132573..f1b12b067 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer)
 
 	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));
+	esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1) - 20.0F;
+	esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;
+	esc.esc[0].esc_current = static_cast<float>((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F;
 
 	/* announce the esc if needed, just publish else */
 	if (_esc_pub > 0) {
@@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size)
 	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.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F);
 	msg.temperature2 = 20;  // 0 deg. C.
 
-	uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
+	const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F);
 	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);
+	const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F);
 	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);
+	const 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;
 
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 3996b76a6..1055487cb 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -600,8 +600,8 @@ MK::task_main()
 				esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
 				esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
 				esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
-				esc.esc[i].esc_voltage = (uint16_t) 0;
-				esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
+				esc.esc[i].esc_voltage = 0.0F;
+				esc.esc[i].esc_current = static_cast<float>(Motor[i].Current) * 0.1F;
 				esc.esc[i].esc_rpm = (uint16_t) 0;
 				esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
 
@@ -614,7 +614,7 @@ MK::task_main()
 					esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
 				}
 
-				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
+				esc.esc[i].esc_temperature = static_cast<float>(Motor[i].Temperature);
 				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
 				esc.esc[i].esc_errorcount = (uint16_t) 0;
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index d2845eb75..aeff4fd30 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -241,15 +241,15 @@ struct log_GPSP_s {
 #define LOG_ESC_MSG 18
 struct log_ESC_s {
 	uint16_t counter;
-	uint8_t esc_count;
-	uint8_t esc_connectiontype;
-	uint8_t esc_num;
+	uint8_t  esc_count;
+	uint8_t  esc_connectiontype;
+	uint8_t  esc_num;
 	uint16_t esc_address;
 	uint16_t esc_version;
-	uint16_t esc_voltage;
-	uint16_t esc_current;
+	float    esc_voltage;
+	float    esc_current;
 	uint16_t esc_rpm;
-	uint16_t esc_temperature;
+	float    esc_temperature;
 	float    esc_setpoint;
 	uint16_t esc_setpoint_raw;
 };
@@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
-	LOG_FORMAT(ESC, "HBBBHHHHHHfH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(ESC, "HBBBHHffHffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB",			"Bottom,BottomRate,Flags"),
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index 628824efa..b82796908 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -78,6 +78,7 @@ enum ESC_CONNECTION_TYPE {
 
 /**
  * Electronic speed controller status.
+ * Unsupported float fields should be assigned NaN.
  */
 struct esc_status_s {
 	/* use of a counter and timestamp recommended (but not necessary) */
@@ -89,17 +90,17 @@ struct esc_status_s {
 	enum ESC_CONNECTION_TYPE esc_connectiontype;	/**< how ESCs connected to the system */
 
 	struct {
-		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
 		enum ESC_VENDOR esc_vendor;		/**< Vendor of current ESC */
-		uint16_t esc_version;			/**< Version of current ESC - if supported */
-		uint16_t esc_voltage;			/**< Voltage measured from current ESC - if supported */
-		uint16_t esc_current;			/**< Current measured from current ESC (100mA steps) - if supported */
-		uint16_t esc_rpm;				/**< RPM measured from current ESC - if supported */
-		uint16_t esc_temperature;		/**< Temperature measured from current ESC - if supported */
-		float    esc_setpoint;			/**< setpoint of current ESC */
+		uint32_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+		float esc_voltage;			/**< Voltage measured from current ESC [V] - if supported */
+		float esc_current;			/**< Current measured from current ESC [A] - if supported */
+		float esc_temperature;			/**< Temperature measured from current ESC [degC] - if supported */
+		float esc_setpoint;			/**< setpoint of current ESC */
 		uint16_t esc_setpoint_raw;		/**< setpoint of current ESC (Value sent to ESC) */
-		uint16_t esc_state;				/**< State of ESC - depend on Vendor */
-		uint16_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
+		uint16_t esc_version;			/**< Version of current ESC - if supported */
+		uint16_t esc_rpm;			/**< RPM measured from current ESC [RPM] - if supported */
+		uint16_t esc_state;			/**< State of ESC - depend on Vendor */
 	} esc[CONNECTED_ESC_MAX];
 
 };
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 0601d9fa2..249ffba07 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -134,14 +134,12 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
 
 		ref.esc_address = msg.getSrcNodeID().get();
 
-		// >0 checks allow to weed out NaNs and negative values that aren't supported.
-		ref.esc_voltage     = (msg.voltage > 0)     ? msg.voltage * 10.0F     : 0;
-		ref.esc_current     = (msg.current > 0)     ? msg.current * 10.0F     : 0;
-		ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0;
-
-		ref.esc_setpoint   = msg.power_rating_pct;
-		ref.esc_rpm        = abs(msg.rpm);
-		ref.esc_errorcount = msg.error_count;
+		ref.esc_voltage     = msg.voltage;
+		ref.esc_current     = msg.current;
+		ref.esc_temperature = msg.temperature;
+		ref.esc_setpoint    = msg.power_rating_pct;
+		ref.esc_rpm         = abs(msg.rpm);
+		ref.esc_errorcount  = msg.error_count;
 	}
 }