Merge branch 'vfr_fix' into follower2

17 files changed, 201 insertions, 71 deletions

diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py
index fcc40b09e..aef1cc7a3 100644
--- a/Tools/px_romfs_pruner.py
+++ b/Tools/px_romfs_pruner.py
@@ -57,7 +57,7 @@ def main():
         for (root, dirs, files) in os.walk(args.folder):
                 for file in files:
                         # only prune text files
-                        if ".zip" in file or ".bin" in file or ".swp" in file:
+                        if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file:
                                 continue
 
                         file_path = os.path.join(root, file)
diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c
index 4e3ba2d7e..1ce235da8 100644
--- a/src/drivers/boards/aerocore/aerocore_init.c
+++ b/src/drivers/boards/aerocore/aerocore_init.c
@@ -93,6 +93,19 @@
 #  endif
 #endif
 
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h
index 70142a314..776a2071e 100644
--- a/src/drivers/boards/aerocore/board_config.h
+++ b/src/drivers/boards/aerocore/board_config.h
@@ -171,6 +171,25 @@ __BEGIN_DECLS
 
 extern void stm32_spiinitialize(void);
 
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index a70a6240c..188885be2 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -209,6 +209,27 @@ __BEGIN_DECLS
 
 extern void stm32_spiinitialize(void);
 
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 0190a5b5b..ee365e47c 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -229,6 +229,27 @@ __BEGIN_DECLS
 
 extern void stm32_spiinitialize(void);
 
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index bf41bb1fe..9b25c574a 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -94,6 +94,19 @@
 #  endif
 #endif
 
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index bc586f395..f66ec7c95 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -165,7 +165,7 @@ ORB_DECLARE(output_pwm);
 
 #define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
 #define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
-#define DSMX8_BIND_PULSES 10 	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
+#define DSMX8_BIND_PULSES 9 	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
 
 /** power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp
index 1b706b5b2..a2e292de2 100644
--- a/src/drivers/gps/ashtech.cpp
+++ b/src/drivers/gps/ashtech.cpp
@@ -178,11 +178,20 @@ int ASHTECH::handle_message(int len)
 		_gps_position->alt = alt * 1000;
 		_rate_count_lat_lon++;
 
-		if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) {
+		if (fix_quality <= 0) {
 			_gps_position->fix_type = 0;
 
 		} else {
-			_gps_position->fix_type = 3 + fix_quality;
+			/*
+			 * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality,
+			 * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type
+			 */
+			if (fix_quality == 5) { fix_quality = 3; }
+
+			/*
+			 * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed.
+			 */
+			_gps_position->fix_type = 3 + fix_quality - 1;
 		}
 
 		_gps_position->timestamp_position = hrt_absolute_time();
@@ -229,6 +238,11 @@ int ASHTECH::handle_message(int len)
 		      *cc Checksum
 		    */
 		bufptr = (char *)(_rx_buffer + 10);
+
+		/*
+		 * Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet
+		 */
+		int coordinatesFound = 0;
 		double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
 		int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
 		double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0;
@@ -241,15 +255,31 @@ int ASHTECH::handle_message(int len)
 
 		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
 
-		if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
+		if (bufptr && *(++bufptr) != ',') {
+			/*
+			 * if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row)
+			 * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
+			 */
+			lat = strtod(bufptr, &endp);
+			if (bufptr != endp) {coordinatesFound++;}
+			bufptr = endp;
+		}
 
 		if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
 
-		if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
+		if (bufptr && *(++bufptr) != ',') {
+			lon = strtod(bufptr, &endp);
+			if (bufptr != endp) {coordinatesFound++;}
+			bufptr = endp;
+		}
 
 		if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
 
-		if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
+		if (bufptr && *(++bufptr) != ',') {
+			alt = strtod(bufptr, &endp);
+			if (bufptr != endp) {coordinatesFound++;}
+			bufptr = endp;
+		}
 
 		if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; }
 
@@ -280,7 +310,7 @@ int ASHTECH::handle_message(int len)
 		_gps_position->alt = alt * 1000;
 		_rate_count_lat_lon++;
 
-		if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) {
+		if (coordinatesFound < 3) {
 			_gps_position->fix_type = 0;
 
 		} else {
@@ -461,12 +491,12 @@ int ASHTECH::receive(unsigned timeout)
 		uint64_t time_started = hrt_absolute_time();
 
 		int j = 0;
-		ssize_t count = 0;
+		ssize_t bytes_count = 0;
 
 		while (true) {
 
 			/* pass received bytes to the packet decoder */
-			while (j < count) {
+			while (j < bytes_count) {
 				int l = 0;
 
 				if ((l = parse_char(buf[j])) > 0) {
@@ -486,7 +516,7 @@ int ASHTECH::receive(unsigned timeout)
 			}
 
 			/* everything is read */
-			j = count = 0;
+			j = bytes_count = 0;
 
 			/* then poll for new data */
 			int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2);
@@ -507,7 +537,7 @@ int ASHTECH::receive(unsigned timeout)
 					 * won't block even on a blocking device.  If more bytes are
 					 * available, we'll go back to poll() again...
 					 */
-					count = ::read(_fd, buf, sizeof(buf));
+					bytes_count = ::read(_fd, buf, sizeof(buf));
 				}
 			}
 		}
@@ -600,7 +630,7 @@ int ASHTECH::configure(unsigned &baudrate)
 	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
 
 
-	for (int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
+	for (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
 		baudrate = baudrates_to_try[baud_i];
 		set_baudrate(_fd, baudrate);
 		write(_fd, (uint8_t *)comm, sizeof(comm));
diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h
index 6e65d0b2b..6ba522b9c 100644
--- a/src/drivers/gps/ashtech.h
+++ b/src/drivers/gps/ashtech.h
@@ -68,7 +68,7 @@ class ASHTECH : public GPS_Helper
 	char                 *_parse_pos; 		/** parse position */
 
 	bool	_gsv_in_progress;			/**< Indicates that gsv data parsing is in progress */
-	/* int     _satellites_count; 			/**< Number of satellites info parsed. */
+	/* int     _satellites_count; 			**< Number of satellites info parsed. */
 	uint8_t count;					/**< Number of satellites in satellite info */
 	uint8_t svid[SAT_INFO_MAX_SATELLITES]; 		/**< Space vehicle ID [1..255], see scheme below  */
 	uint8_t used[SAT_INFO_MAX_SATELLITES];		/**< 0: Satellite not used, 1: used for navigation */
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/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 5dfa5e6d2..90090653b 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -785,9 +785,6 @@ private:
 	MavlinkOrbSubscription *_airspeed_sub;
 	uint64_t _airspeed_time;
 
-	MavlinkOrbSubscription *_sensor_combined_sub;
-	uint64_t _sensor_combined_time;
-
 	/* do not allow top copying this class */
 	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
 	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -803,9 +800,7 @@ protected:
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
 		_act_time(0),
 		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
-		_airspeed_time(0),
-		_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
-		_sensor_combined_time(0)
+		_airspeed_time(0)
 	{}
 
 	void send(const hrt_abstime t)
@@ -815,14 +810,12 @@ protected:
 		struct actuator_armed_s armed;
 		struct actuator_controls_s act;
 		struct airspeed_s airspeed;
-		struct sensor_combined_s sensor_combined;
 
 		bool updated = _att_sub->update(&_att_time, &att);
 		updated |= _pos_sub->update(&_pos_time, &pos);
 		updated |= _armed_sub->update(&_armed_time, &armed);
 		updated |= _act_sub->update(&_act_time, &act);
 		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
-		updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
 
 		if (updated) {
 			mavlink_vfr_hud_t msg;
@@ -831,7 +824,7 @@ protected:
 			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
 			msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-			msg.alt = sensor_combined.baro_alt_meter;
+			msg.alt = pos.alt;
 			msg.climb = -pos.vel_d;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 54b8dc09a..a4b089d46 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;
-	uint16_t esc_rpm;
-	uint16_t esc_temperature;
+	float    esc_voltage;
+	float    esc_current;
+	int32_t  esc_rpm;
+	float    esc_temperature;
 	float    esc_setpoint;
 	uint16_t esc_setpoint_raw;
 };
@@ -464,7 +464,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, "HBBBHHffiffH",		"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..b45c49788 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 */
+                int32_t esc_rpm;                        /**< Motor RPM, negative for reverse rotation [RPM] - 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_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 223d94731..1990651ef 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -73,7 +73,7 @@ int UavcanEscController::init()
 
 void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 {
-	if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
+	if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
 		perf_count(_perfcnt_invalid_input);
 		return;
 	}
@@ -93,25 +93,24 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	 */
 	uavcan::equipment::esc::RawCommand msg;
 
+	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
+
 	if (_armed) {
 		for (unsigned i = 0; i < num_outputs; i++) {
 
-			float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
+			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
 			if (scaled < 1.0F) {
 				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
 			}
 
-			if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
-				scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
-				perf_count(_perfcnt_scaling_error);
-			} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
-				scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
+			if (scaled > cmd_max) {
+				scaled = cmd_max;
 				perf_count(_perfcnt_scaling_error);
-			} else {
-				; // Correct value
 			}
 
-			msg.cmd.push_back(static_cast<unsigned>(scaled));
+			msg.cmd.push_back(static_cast<int>(scaled));
+
+			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
 		}
 	}
 
@@ -129,10 +128,31 @@ void UavcanEscController::arm_esc(bool arm)
 
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
 {
-	// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
+	if (msg.esc_index < CONNECTED_ESC_MAX) {
+		_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
+		_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
+
+		auto &ref = _esc_status.esc[msg.esc_index];
+
+		ref.esc_address = msg.getSrcNodeID().get();
+
+		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         = msg.rpm;
+		ref.esc_errorcount  = msg.error_count;
+	}
 }
 
 void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
 {
-	// TODO publish to ORB
+	_esc_status.counter += 1;
+	_esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN;
+
+	if (_esc_status_pub > 0) {
+		(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
+	} else {
+		_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
+	}
 }
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index cf0988210..f4a3877e6 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -48,6 +48,8 @@
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
 #include <systemlib/perf_counter.h>
+#include <uORB/topics/esc_status.h>
+
 
 class UavcanEscController
 {
@@ -73,9 +75,8 @@ private:
 	void orb_pub_timer_cb(const uavcan::TimerEvent &event);
 
 
-	static constexpr unsigned MAX_RATE_HZ = 100;			///< XXX make this configurable
-	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
-	static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
+	static constexpr unsigned MAX_RATE_HZ = 200;			///< XXX make this configurable
+	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
 
 	typedef uavcan::MethodBinder<UavcanEscController*,
 		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
@@ -84,6 +85,10 @@ private:
 	typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
 		TimerCbBinder;
 
+	bool		_armed = false;
+	esc_status_s	_esc_status = {};
+	orb_advert_t	_esc_status_pub = -1;
+
 	/*
 	 * libuavcan related things
 	 */
@@ -94,12 +99,6 @@ private:
 	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
 
 	/*
-	 * ESC states
-	 */
-	bool 				_armed = false;
-	uavcan::equipment::esc::Status	_states[MAX_ESCS];
-
-	/*
 	 * Perf counters
 	 */
 	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
diff --git a/uavcan b/uavcan
-Subproject 01d5bb242a6197e17e0ed466ed86e7ba42bd7d0
+Subproject 4de0338824972de4d3a8e29697ea1a2d95a926c