Merge remote-tracking branch 'upstream/master' into dev_ros

src/modules/mavlink/mavlink_main.cpp

@@ -1405,7 +1405,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW", 5.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:

src/modules/mavlink/mavlink_messages.cpp

@@ -1834,33 +1834,32 @@ protected:
 	}
 };
 
-
-class MavlinkStreamOpticalFlow : public MavlinkStream
+class MavlinkStreamOpticalFlowRad : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamOpticalFlow::get_name_static();
+		return MavlinkStreamOpticalFlowRad::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "OPTICAL_FLOW";
+		return "OPTICAL_FLOW_RAD";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+		return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamOpticalFlow(mavlink);
+		return new MavlinkStreamOpticalFlowRad(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
@@ -1868,11 +1867,11 @@ private:
 	uint64_t _flow_time;
 
 	/* do not allow top copying this class */
-	MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
-	MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
+	MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
+	MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
 
 protected:
-	explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+	explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
 		_flow_time(0)
 	{}
@@ -1882,18 +1881,23 @@ protected:
 		struct optical_flow_s flow;
 
 		if (_flow_sub->update(&_flow_time, &flow)) {
-			mavlink_optical_flow_t msg;
+			mavlink_optical_flow_rad_t msg;
 
 			msg.time_usec = flow.timestamp;
 			msg.sensor_id = flow.sensor_id;
-			msg.flow_x = flow.flow_raw_x;
-			msg.flow_y = flow.flow_raw_y;
-			msg.flow_comp_m_x = flow.flow_comp_x_m;
-			msg.flow_comp_m_y = flow.flow_comp_y_m;
+			msg.integrated_x = flow.pixel_flow_x_integral;
+			msg.integrated_y = flow.pixel_flow_y_integral;
+			msg.integrated_xgyro = flow.gyro_x_rate_integral;
+			msg.integrated_ygyro = flow.gyro_y_rate_integral;
+			msg.integrated_zgyro = flow.gyro_z_rate_integral;
+			msg.distance = flow.ground_distance_m;
 			msg.quality = flow.quality;
-			msg.ground_distance = flow.ground_distance_m;
+			msg.integration_time_us = flow.integration_timespan;
+			msg.sensor_id = flow.sensor_id;
+			msg.time_delta_distance_us = flow.time_since_last_sonar_update;
+			msg.temperature = flow.gyro_temperature;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
 		}
 	}
 };
@@ -2199,7 +2203,7 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
 	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
 	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
-	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+	new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
 	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
 	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),

src/modules/mavlink/mavlink_receiver.cpp

@@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_command_int(msg);
 		break;
 
-	case MAVLINK_MSG_ID_OPTICAL_FLOW:
-		handle_message_optical_flow(msg);
+	case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
+		handle_message_optical_flow_rad(msg);
 		break;
 
 	case MAVLINK_MSG_ID_SET_MODE:
@@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 {
 	/* optical flow */
-	mavlink_optical_flow_t flow;
-	mavlink_msg_optical_flow_decode(msg, &flow);
+	mavlink_optical_flow_rad_t flow;
+	mavlink_msg_optical_flow_rad_decode(msg, &flow);
 
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
-	f.timestamp = hrt_absolute_time();
-	f.flow_timestamp = flow.time_usec;
-	f.flow_raw_x = flow.flow_x;
-	f.flow_raw_y = flow.flow_y;
-	f.flow_comp_x_m = flow.flow_comp_m_x;
-	f.flow_comp_y_m = flow.flow_comp_m_y;
-	f.ground_distance_m = flow.ground_distance;
+	f.timestamp = flow.time_usec;
+	f.integration_timespan = flow.integration_time_us;
+	f.pixel_flow_x_integral = flow.integrated_x;
+	f.pixel_flow_y_integral = flow.integrated_y;
+	f.gyro_x_rate_integral = flow.integrated_xgyro;
+	f.gyro_y_rate_integral = flow.integrated_ygyro;
+	f.gyro_z_rate_integral = flow.integrated_zgyro;
+	f.time_since_last_sonar_update = flow.time_delta_distance_us;
+	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
+	f.gyro_temperature = flow.temperature;
 
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -389,13 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
-	f.timestamp = hrt_absolute_time();
-	f.flow_timestamp = flow.time_usec;
-	f.flow_raw_x = flow.integrated_x;
-	f.flow_raw_y = flow.integrated_y;
+	f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
+	f.integration_timespan = flow.integration_time_us;
+	f.pixel_flow_x_integral = flow.integrated_x;
+	f.pixel_flow_y_integral = flow.integrated_y;
+	f.gyro_x_rate_integral = flow.integrated_xgyro;
+	f.gyro_y_rate_integral = flow.integrated_ygyro;
+	f.gyro_z_rate_integral = flow.integrated_zgyro;
+	f.time_since_last_sonar_update = flow.time_delta_distance_us;
 	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
+	f.gyro_temperature = flow.temperature;
 
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);

src/modules/mavlink/mavlink_receiver.h

@@ -112,7 +112,7 @@ private:
 	void handle_message(mavlink_message_t *msg);
 	void handle_message_command_long(mavlink_message_t *msg);
 	void handle_message_command_int(mavlink_message_t *msg);
-	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_optical_flow_rad(mavlink_message_t *msg);
 	void handle_message_hil_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -296,7 +296,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float w_flow = 0.0f;
 
 	float sonar_prev = 0.0f;
-	hrt_abstime flow_prev = 0;			// time of last flow measurement
+	//hrt_abstime flow_prev = 0;			// time of last flow measurement
 	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
 	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)
 
@@ -489,8 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
 
 				/* calculate time from previous update */
-				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
-				flow_prev = flow.flow_timestamp;
+//				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
+//				flow_prev = flow.flow_timestamp;
 
 				if ((flow.ground_distance_m > 0.31f) &&
 					(flow.ground_distance_m < 4.0f) &&
@@ -548,8 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* convert raw flow to angular flow (rad/s) */
 					float flow_ang[2];
-					flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
-					flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
+					//todo check direction of x und y axis
+					flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
+					flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
 					/* flow measurements vector */
 					float flow_m[3];
 					flow_m[0] = -flow_ang[0] * flow_dist;

src/modules/px4iofirmware/registers.c

@@ -412,7 +412,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		 * text handling function.
 		 */
 		return mixer_handle_text(values, num_values * sizeof(*values));
-		break;
 
 	default:
 		/* avoid offset wrap */
@@ -584,10 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_REBOOT_BL:
-			// do not reboot if FMU is armed and IO's safety is off
-			// this state defines an active system.
-			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
-			    (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 				// don't allow reboot while armed
 				break;
 			}
@@ -633,12 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 	case PX4IO_PAGE_RC_CONFIG: {
 
 		/**
-		 * do not allow a RC config change while outputs armed
-		 * = FMU is armed and IO's safety is off
-		 * this state defines an active system.
+		 * do not allow a RC config change while safety is off
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
-			    (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+		if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 			break;
 		}
 

src/modules/sdlog2/sdlog2.c

@@ -1518,11 +1518,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- FLOW --- */
 		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
 			log_msg.msg_type = LOG_FLOW_MSG;
-			log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
-			log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
-			log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
-			log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
-			log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
+			log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
+			log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
+			log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral;
+			log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral;
+			log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral;
+			log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan;
+			log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral;
+			log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral;
 			log_msg.body.log_FLOW.quality = buf.flow.quality;
 			log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
 			LOGBUFFER_WRITE_AND_COUNT(FLOW);

src/modules/sdlog2/sdlog2_messages.h

@@ -200,13 +200,19 @@ struct log_ARSP_s {
 /* --- FLOW - OPTICAL FLOW --- */
 #define LOG_FLOW_MSG 15
 struct log_FLOW_s {
-	int16_t flow_raw_x;
-	int16_t flow_raw_y;
-	float flow_comp_x;
-	float flow_comp_y;
-	float distance;
-	uint8_t	quality;
+	uint64_t timestamp;
 	uint8_t sensor_id;
+	float pixel_flow_x_integral;
+	float pixel_flow_y_integral;
+	float gyro_x_rate_integral;
+	float gyro_y_rate_integral;
+	float gyro_z_rate_integral;
+	float ground_distance_m;
+	uint32_t integration_timespan;
+	uint32_t time_since_last_sonar_update;
+	uint16_t frame_count_since_last_readout;
+	int16_t gyro_temperature;
+	uint8_t	quality;
 };
 
 /* --- GPOS - GLOBAL POSITION ESTIMATE --- */

src/modules/uORB/Publication.cpp

@@ -46,6 +46,7 @@
 #include "topics/vehicle_attitude_setpoint.h"
 #include "topics/vehicle_rates_setpoint.h"
 #include "topics/actuator_outputs.h"
+#include "topics/actuator_direct.h"
 #include "topics/encoders.h"
 #include "topics/tecs_status.h"
 #include "topics/rc_channels.h"
@@ -77,6 +78,7 @@ template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
 template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
 template class __EXPORT Publication<vehicle_rates_setpoint_s>;
 template class __EXPORT Publication<actuator_outputs_s>;
+template class __EXPORT Publication<actuator_direct_s>;
 template class __EXPORT Publication<encoders_s>;
 template class __EXPORT Publication<tecs_status_s>;
 template class __EXPORT Publication<rc_channels_s>;

src/modules/uORB/objects_common.cpp

@@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
+#include "topics/actuator_direct.h"
+ORB_DEFINE(actuator_direct, struct actuator_direct_s);
+
 #include "topics/multirotor_motor_limits.h"
 ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
 

src/modules/uORB/topics/actuator_direct.h

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_direct.h
+ *
+ * Actuator direct values.
+ *
+ * Values published to this topic are the direct actuator values which
+ * should be passed to actuators, bypassing mixing
+ */
+
+#ifndef TOPIC_ACTUATOR_DIRECT_H
+#define TOPIC_ACTUATOR_DIRECT_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATORS_DIRECT		16
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct actuator_direct_s {
+	uint64_t timestamp;				/**< timestamp in us since system boot */
+	float values[NUM_ACTUATORS_DIRECT];		/**< actuator values, from -1 to 1 */
+	unsigned nvalues;				/**< number of valid values */
+};
+
+/**
+ * @}
+ */
+
+/* actuator direct ORB */
+ORB_DECLARE(actuator_direct);
+
+#endif // TOPIC_ACTUATOR_DIRECT_H

src/modules/uORB/topics/optical_flow.h

@@ -55,16 +55,22 @@
  */
 struct optical_flow_s {
 
-	uint64_t timestamp;		/**< in microseconds since system start          */
-
-	uint64_t flow_timestamp;		/**< timestamp from flow sensor */
-	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
-	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
-	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */
-	float flow_comp_y_m;		/**< speed over ground in meters, rotation-compensated */
-	float ground_distance_m;	/**< Altitude / distance to ground in meters */
-	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
+	uint64_t timestamp;		/**< in microseconds since system start  */
 	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
+	float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */
+	float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */
+	float gyro_x_rate_integral;	 /**< accumulated gyro value in radians around x axis */
+	float gyro_y_rate_integral;  /**< accumulated gyro value in radians around y axis */
+	float gyro_z_rate_integral;   /**< accumulated gyro value in radians around z axis */
+	float ground_distance_m;	 /**< Altitude / distance to ground in meters */
+	uint32_t integration_timespan; /**<accumulation timespan in microseconds     */
+	uint32_t time_since_last_sonar_update;/**< time since last sonar update in microseconds */
+	uint16_t frame_count_since_last_readout;/**< number of accumulated frames in timespan */
+	int16_t gyro_temperature;/**< 	Temperature * 100 in centi-degrees Celsius */
+	uint8_t	quality;		/**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
+
+
+
 
 };
 

src/modules/uavcan/actuators/esc.cpp

@@ -76,7 +76,9 @@ int UavcanEscController::init()
 
 void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 {
-	if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
+	if ((outputs == nullptr) || 
+            (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
+            (num_outputs > CONNECTED_ESC_MAX)) {
 		perf_count(_perfcnt_invalid_input);
 		return;
 	}
@@ -101,10 +103,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	for (unsigned i = 0; i < num_outputs; i++) {
 		if (_armed_mask & MOTOR_BIT(i)) {
 			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
-			}
-
+                        // trim negative values back to 0. Previously
+                        // we set this to 0.1, which meant motors kept
+                        // spinning when armed, but that should be a
+                        // policy decision for a specific vehicle
+                        // type, as it is not appropriate for all
+                        // types of vehicles (eg. fixed wing).
+			if (scaled < 0.0F) {
+				scaled = 0.0F;
+                        }
 			if (scaled > cmd_max) {
 				scaled = cmd_max;
 				perf_count(_perfcnt_scaling_error);

src/modules/uavcan/uavcan_main.cpp

@@ -269,6 +269,24 @@ void UavcanNode::node_spin_once()
 	}
 }
 
+/*
+  add a fd to the list of polled events. This assumes you want
+  POLLIN for now.
+ */
+int UavcanNode::add_poll_fd(int fd)
+{
+	int ret = _poll_fds_num;
+	if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
+		errx(1, "uavcan: too many poll fds, exiting");
+	}
+	_poll_fds[_poll_fds_num] = ::pollfd();
+	_poll_fds[_poll_fds_num].fd = fd;
+	_poll_fds[_poll_fds_num].events = POLLIN;
+	_poll_fds_num += 1;
+	return ret;
+}
+
+
 int UavcanNode::run()
 {
 	(void)pthread_mutex_lock(&_node_mutex);
@@ -280,9 +298,9 @@ int UavcanNode::run()
 
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
+	_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
 
-	actuator_outputs_s outputs;
-	memset(&outputs, 0, sizeof(outputs));
+	memset(&_outputs, 0, sizeof(_outputs));
 
 	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
 	if (busevent_fd < 0)
@@ -304,11 +322,15 @@ int UavcanNode::run()
 	 * the value returned from poll() to detect whether actuator control has timed out or not.
 	 * Instead, all ORB events need to be checked individually (see below).
 	 */
-	_poll_fds_num = 0;
-	_poll_fds[_poll_fds_num] = ::pollfd();
-	_poll_fds[_poll_fds_num].fd = busevent_fd;
-	_poll_fds[_poll_fds_num].events = POLLIN;
-	_poll_fds_num += 1;
+	add_poll_fd(busevent_fd);
+
+	/*
+	 * setup poll to look for actuator direct input if we are
+	 * subscribed to the topic
+	 */
+	if (_actuator_direct_sub != -1) {
+		_actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub);
+	}
 
 	while (!_task_should_exit) {
 		// update actuator controls subscriptions if needed
@@ -326,6 +348,8 @@ int UavcanNode::run()
 
 		node_spin_once();  // Non-blocking
 
+		bool new_output = false;
+
 		// this would be bad...
 		if (poll_ret < 0) {
 			log("poll error %d", errno);
@@ -333,24 +357,39 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			unsigned poll_id = 1;
 			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
-					if (_poll_fds[poll_id].revents & POLLIN) {
+					if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
 						controls_updated = true;
 						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
 					}
-					poll_id++;
 				}
 			}
 
+			/*
+			  see if we have any direct actuator updates
+			 */
+			if (_actuator_direct_sub != -1 && 
+			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
+			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
+			    !_test_in_progress) {
+				if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) {
+					_actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS;
+				}
+				memcpy(&_outputs.output[0], &_actuator_direct.values[0], 
+				       _actuator_direct.nvalues*sizeof(float));
+				_outputs.noutputs = _actuator_direct.nvalues;
+				new_output = true;
+			}
+
 			// can we mix?
 			if (_test_in_progress) {
-				float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
-				test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
-
-				// Output to the bus
-				_esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS);
+				memset(&_outputs, 0, sizeof(_outputs));
+				if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) {
+					_outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+					_outputs.noutputs = _test_motor.motor_number+1;
+				}
+				new_output = true;
 			} else if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
@@ -358,39 +397,41 @@ int UavcanNode::run()
 				unsigned num_outputs_max = 8;
 
 				// Do mixing
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
-				outputs.timestamp = hrt_absolute_time();
+				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
 
-				// iterate actuators
-				for (unsigned i = 0; i < outputs.noutputs; i++) {
-					// last resort: catch NaN, INF and out-of-band errors
-					if (!isfinite(outputs.output[i])) {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = -1.0f;
-					}
-
-					// limit outputs to valid range
-
-					// never go below min
-					if (outputs.output[i] < -1.0f) {
-						outputs.output[i] = -1.0f;
-					}
-
-					// never go below max
-					if (outputs.output[i] > 1.0f) {
-						outputs.output[i] = 1.0f;
-					}
-				}
-
-				// Output to the bus
-				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
+				new_output = true;
 			}
 		}
 
+		if (new_output) {
+			// iterate actuators, checking for valid values
+			for (uint8_t i = 0; i < _outputs.noutputs; i++) {
+				// last resort: catch NaN, INF and out-of-band errors
+				if (!isfinite(_outputs.output[i])) {
+					/*
+					 * Value is NaN, INF or out of band - set to the minimum value.
+					 * This will be clearly visible on the servo status and will limit the risk of accidentally
+					 * spinning motors. It would be deadly in flight.
+					 */
+					_outputs.output[i] = -1.0f;
+				}
+
+				// never go below min
+				if (_outputs.output[i] < -1.0f) {
+					_outputs.output[i] = -1.0f;
+				}
+
+				// never go above max
+				if (_outputs.output[i] > 1.0f) {
+					_outputs.output[i] = 1.0f;
+				}
+			}
+			// Output to the bus
+			_outputs.timestamp = hrt_absolute_time();
+			_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
+		}
+
+
 		// Check motor test state
 		bool updated = false;
 		orb_check(_test_motor_sub, &updated);
@@ -459,7 +500,6 @@ UavcanNode::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	// the first fd used by CAN
-	_poll_fds_num = 1;
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
@@ -472,9 +512,7 @@ UavcanNode::subscribe()
 		}
 
 		if (_control_subs[i] > 0) {
-			_poll_fds[_poll_fds_num].fd = _control_subs[i];
-			_poll_fds[_poll_fds_num].events = POLLIN;
-			_poll_fds_num++;
+			_poll_ids[i] = add_poll_fd(_control_subs[i]);
 		}
 	}
 }
@@ -572,6 +610,14 @@ UavcanNode::print_info()
 	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
 	printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
 
+	if (_outputs.noutputs != 0) {
+		printf("ESC output: ");
+		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+			printf("%d ", (int)(_outputs.output[i]*1000));
+		}
+		printf("\n");
+	}
+
 	// Sensor bridges
 	auto br = _sensor_bridges.getHead();
 	while (br != nullptr) {
@@ -590,7 +636,7 @@ UavcanNode::print_info()
 static void print_usage()
 {
 	warnx("usage: \n"
-	      "\tuavcan {start|status|stop}");
+	      "\tuavcan {start|status|stop|arm|disarm}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -637,6 +683,16 @@ int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}
 
+	if (!std::strcmp(argv[1], "arm")) {
+		inst->arm_actuators(true);
+		::exit(0);
+	}
+
+	if (!std::strcmp(argv[1], "disarm")) {
+		inst->arm_actuators(false);
+		::exit(0);
+	}
+
 	if (!std::strcmp(argv[1], "stop")) {
 		delete inst;
 		::exit(0);

src/modules/uavcan/uavcan_main.hpp

@@ -42,6 +42,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/test_motor.h>
+#include <uORB/topics/actuator_direct.h>
 
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
@@ -57,6 +58,9 @@
 #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
 #define UAVCAN_DEVICE_PATH	"/dev/uavcan/esc"
 
+// we add two to allow for actuator_direct and busevent
+#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
+
 /**
  * A UAVCAN node.
  */
@@ -97,6 +101,8 @@ private:
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
 	int		run();
+	int		add_poll_fd(int fd);			///< add a fd to poll list, returning index into _poll_fds[]
+
 
 	int			_task = -1;			///< handle to the OS task
 	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
@@ -125,6 +131,15 @@ private:
 	int			_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
 	actuator_controls_s 	_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
 	orb_id_t		_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
-	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {};	///< +1 for /dev/uavcan/busevent
+	pollfd			_poll_fds[UAVCAN_NUM_POLL_FDS] = {};
 	unsigned		_poll_fds_num = 0;
+
+	int			_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
+	uint8_t			_actuator_direct_poll_fd_num;
+	actuator_direct_s	_actuator_direct;
+
+	actuator_outputs_s	_outputs;
+
+	// index into _poll_fds for each _control_subs handle
+	uint8_t			_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
 };