Merge branch 'master' into mpc_local_pos

src/modules/mavlink/mavlink.c

@@ -45,31 +45,6 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <systemlib/param/param.h>
-// #include <drivers/drv_hrt.h>
-// #include <time.h>
-// #include <float.h>
-// #include <unistd.h>
-// #include <nuttx/sched.h>
-// #include <sys/prctl.h>
-// #include <termios.h>
-// #include <errno.h>
-// #include <stdlib.h>
-// #include <poll.h>
-
-//
-// #include <systemlib/systemlib.h>
-// #include <systemlib/err.h>
-// #include <mavlink/mavlink_log.h>
-// #include <commander/px4_custom_mode.h>
-
-// #include "waypoints.h"
-// #include "orb_topics.h"
-// #include "mavlink_hil.h"
-// #include "util.h"
-// #include "waypoints.h"
-// #include "mavlink_parameters.h"
-
-// #include <uORB/topics/mission_result.h>
 
 /* define MAVLink specific parameters */
 /**

src/modules/mavlink/mavlink_main.cpp

@@ -151,8 +151,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	}
 
 	/* no valid instance, bail */
-	if (!instance)
+	if (!instance) {
 		return;
+	}
 
 	int uart = instance->get_uart_fd();
 
@@ -165,12 +166,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	int buf_free = 0;
 
 	if (instance->get_flow_control_enabled()
-		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+	    && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
 
 		if (buf_free == 0) {
 
 			if (last_write_times[(unsigned)channel] != 0 &&
-			hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) {
+			    hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
 
 				warnx("DISABLING HARDWARE FLOW CONTROL");
 				instance->enable_flow_control(false);
@@ -225,36 +226,42 @@ Mavlink::Mavlink() :
 	case 0:
 		_channel = MAVLINK_COMM_0;
 		break;
+
 	case 1:
 		_channel = MAVLINK_COMM_1;
 		break;
+
 	case 2:
 		_channel = MAVLINK_COMM_2;
 		break;
+
 	case 3:
 		_channel = MAVLINK_COMM_3;
 		break;
 #ifdef MAVLINK_COMM_4
+
 	case 4:
 		_channel = MAVLINK_COMM_4;
 		break;
 #endif
 #ifdef MAVLINK_COMM_5
-		case 5:
+
+	case 5:
 		_channel = MAVLINK_COMM_5;
 		break;
 #endif
 #ifdef MAVLINK_COMM_6
+
 	case 6:
 		_channel = MAVLINK_COMM_6;
 		break;
 #endif
+
 	default:
 		errx(1, "instance ID is out of range");
 		break;
 	}
 
-	LL_APPEND(_mavlink_instances, this);
 }
 
 Mavlink::~Mavlink()
@@ -280,6 +287,7 @@ Mavlink::~Mavlink()
 			}
 		} while (_task_running);
 	}
+
 	LL_DELETE(_mavlink_instances, this);
 }
 
@@ -589,7 +597,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 
 		/* setup output flow control */
 		if (enable_flow_control(true)) {
-			warnx("ERR FLOW CTRL EN");
+			warnx("hardware flow control not supported");
 		}
 	}
 
@@ -605,12 +613,16 @@ Mavlink::enable_flow_control(bool enabled)
 	}
 
 	struct termios uart_config;
+
 	int ret = tcgetattr(_uart_fd, &uart_config);
+
 	if (enabled) {
 		uart_config.c_cflag |= CRTSCTS;
+
 	} else {
 		uart_config.c_cflag &= ~CRTSCTS;
 	}
+
 	ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
 
 	if (!ret) {
@@ -1653,6 +1665,9 @@ Mavlink::task_main(int argc, char *argv[])
 		case 'm':
 			if (strcmp(optarg, "custom") == 0) {
 				_mode = MAVLINK_MODE_CUSTOM;
+
+			} else if (strcmp(optarg, "camera") == 0) {
+				_mode = MAVLINK_MODE_CAMERA;
 			}
 
 			break;
@@ -1696,6 +1711,10 @@ Mavlink::task_main(int argc, char *argv[])
 		warnx("mode: CUSTOM");
 		break;
 
+	case MAVLINK_MODE_CAMERA:
+		warnx("mode: CAMERA");
+		break;
+
 	default:
 		warnx("ERROR: Unknown mode");
 		break;
@@ -1703,7 +1722,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 
-	warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
+	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1763,6 +1782,14 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
 		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
 		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
+		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
+		break;
+
+	case MAVLINK_MODE_CAMERA:
+		configure_stream("SYS_STATUS", 1.0f);
+		configure_stream("ATTITUDE", 15.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
+		configure_stream("CAMERA_CAPTURE", 1.0f);
 		break;
 
 	default:
@@ -1773,11 +1800,14 @@ Mavlink::task_main(int argc, char *argv[])
 	_mavlink_param_queue_index = param_count();
 
 	MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult);
-	MavlinkRateLimiter fast_rate_limiter(20000.0f / rate_mult);
+	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
 
 	/* set main loop delay depending on data rate to minimize CPU overhead */
 	_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
 
+	/* now the instance is fully initialized and we can bump the instance count */
+	LL_APPEND(_mavlink_instances, this);
+
 	while (!_task_should_exit) {
 		/* main loop */
 		usleep(_main_loop_delay);
@@ -1924,10 +1954,18 @@ int Mavlink::start_helper(int argc, char *argv[])
 int
 Mavlink::start(int argc, char *argv[])
 {
+	// Wait for the instance count to go up one
+	// before returning to the shell
+	int ic = Mavlink::instance_count();
+
 	// Instantiate thread
 	char buf[24];
-	sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
+	sprintf(buf, "mavlink_if%d", ic);
 
+	// This is where the control flow splits
+	// between the starting task and the spawned
+	// task - start_helper() only returns
+	// when the started task exits.
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
@@ -1935,6 +1973,26 @@ Mavlink::start(int argc, char *argv[])
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 
+	// Ensure that this shell command
+	// does not return before the instance
+	// is fully initialized. As this is also
+	// the only path to create a new instance,
+	// this is effectively a lock on concurrent
+	// instance starting. XXX do a real lock.
+
+	// Sleep 500 us between each attempt
+	const unsigned sleeptime = 500;
+
+	// Wait 100 ms max for the startup.
+	const unsigned limit = 100 * 1000 / sleeptime;
+
+	unsigned count = 0;
+
+	while (ic == Mavlink::instance_count() && count < limit) {
+		::usleep(sleeptime);
+		count++;
+	}
+
 	return OK;
 }
 
@@ -1960,20 +2018,26 @@ Mavlink::stream(int argc, char *argv[])
 	bool err_flag = false;
 
 	int i = 0;
+
 	while (i < argc) {
 
-		if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) {
-			rate = strtod(argv[i+1], nullptr);
+		if (0 == strcmp(argv[i], "-r") && i < argc - 1) {
+			rate = strtod(argv[i + 1], nullptr);
+
 			if (rate < 0.0f) {
 				err_flag = true;
 			}
+
 			i++;
-		} else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) {
-			device_name = argv[i+1];
+
+		} else if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
+			device_name = argv[i + 1];
 			i++;
-		} else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) {
-			stream_name = argv[i+1];
+
+		} else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
+			stream_name = argv[i + 1];
 			i++;
+
 		} else {
 			err_flag = true;
 		}
@@ -1988,7 +2052,10 @@ Mavlink::stream(int argc, char *argv[])
 			inst->configure_stream_threadsafe(stream_name, rate);
 
 		} else {
-			errx(1, "mavlink for device %s is not running", device_name);
+
+			// If the link is not running we should complain, but not fall over
+			// because this is so easy to get wrong and not fatal. Warning is sufficient.
+			errx(0, "mavlink for device %s is not running", device_name);
 		}
 
 	} else {

src/modules/mavlink/mavlink_main.h

@@ -146,7 +146,8 @@ public:
 
 	enum MAVLINK_MODE {
 		MAVLINK_MODE_NORMAL = 0,
-		MAVLINK_MODE_CUSTOM
+		MAVLINK_MODE_CUSTOM,
+		MAVLINK_MODE_CAMERA
 	};
 
 	void		set_mode(enum MAVLINK_MODE);

src/modules/mavlink/mavlink_messages.cpp

@@ -262,7 +262,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (status_sub->update(t)) {
-
 			mavlink_msg_sys_status_send(_channel,
 						    status->onboard_control_sensors_present,
 						    status->onboard_control_sensors_enabled,
@@ -318,7 +317,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (sensor_sub->update(t)) {
-
 			uint16_t fields_updated = 0;
 
 			if (accel_timestamp != sensor->accelerometer_timestamp) {
@@ -385,7 +383,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (att_sub->update(t)) {
-
 			mavlink_msg_attitude_send(_channel,
 						  att->timestamp / 1000,
 						  att->roll, att->pitch, att->yaw,
@@ -422,7 +419,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (att_sub->update(t)) {
-
 			mavlink_msg_attitude_quaternion_send(_channel,
 							     att->timestamp / 1000,
 							     att->q[0],
@@ -494,7 +490,6 @@ protected:
 		updated |= airspeed_sub->update(t);
 
 		if (updated) {
-
 			float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
 			uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
 			float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
@@ -538,7 +533,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (gps_sub->update(t)) {
-
 			mavlink_msg_gps_raw_int_send(_channel,
 						     gps->timestamp_position,
 						     gps->fix_type,
@@ -592,15 +586,15 @@ protected:
 
 		if (updated) {
 			mavlink_msg_global_position_int_send(_channel,
-						     pos->timestamp / 1000,
-						     pos->lat * 1e7,
-						     pos->lon * 1e7,
-						     pos->alt * 1000.0f,
-						     (pos->alt - home->alt) * 1000.0f,
-						     pos->vel_n * 100.0f,
-						     pos->vel_e * 100.0f,
-						     pos->vel_d * 100.0f,
-						     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+							     pos->timestamp / 1000,
+							     pos->lat * 1e7,
+							     pos->lon * 1e7,
+							     pos->alt * 1000.0f,
+							     (pos->alt - home->alt) * 1000.0f,
+							     pos->vel_n * 100.0f,
+							     pos->vel_e * 100.0f,
+							     pos->vel_d * 100.0f,
+							     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
 		}
 	}
 };
@@ -633,7 +627,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (pos_sub->update(t)) {
-
 			mavlink_msg_local_position_ned_send(_channel,
 							    pos->timestamp / 1000,
 							    pos->x,
@@ -730,7 +723,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (act_sub->update(t)) {
-
 			mavlink_msg_servo_output_raw_send(_channel,
 							  act->timestamp / 1000,
 							  _n,
@@ -790,7 +782,6 @@ protected:
 		updated |= act_sub->update(t);
 
 		if (updated) {
-
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
@@ -870,7 +861,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (pos_sp_triplet_sub->update(t)) {
-
 			mavlink_msg_global_position_setpoint_int_send(_channel,
 					MAV_FRAME_GLOBAL,
 					(int32_t)(pos_sp_triplet->current.lat * 1e7),
@@ -908,14 +898,14 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sp_sub->update(t);
-
-		mavlink_msg_local_position_setpoint_send(_channel,
-				MAV_FRAME_LOCAL_NED,
-				pos_sp->x,
-				pos_sp->y,
-				pos_sp->z,
-				pos_sp->yaw);
+		if (pos_sp_sub->update(t)) {
+			mavlink_msg_local_position_setpoint_send(_channel,
+					MAV_FRAME_LOCAL_NED,
+					pos_sp->x,
+					pos_sp->y,
+					pos_sp->z,
+					pos_sp->yaw);
+		}
 	}
 };
 
@@ -947,7 +937,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (att_sp_sub->update(t)) {
-
 			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
 					att_sp->timestamp / 1000,
 					att_sp->roll_body,
@@ -986,7 +975,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (att_rates_sp_sub->update(t)) {
-
 			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
 					att_rates_sp->timestamp / 1000,
 					att_rates_sp->roll,
@@ -1025,7 +1013,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (rc_sub->update(t)) {
-
 			const unsigned port_width = 8;
 
 			for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
@@ -1075,7 +1062,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (manual_sub->update(t)) {
-
 			mavlink_msg_manual_control_send(_channel,
 							mavlink_system.sysid,
 							manual->roll * 1000,
@@ -1115,7 +1101,6 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		if (flow_sub->update(t)) {
-
 			mavlink_msg_optical_flow_send(_channel,
 						      flow->timestamp,
 						      flow->sensor_id,
@@ -1127,6 +1112,134 @@ protected:
 	}
 };
 
+class MavlinkStreamAttitudeControls : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "ATTITUDE_CONTROLS";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitudeControls();
+	}
+
+private:
+	MavlinkOrbSubscription *att_ctrl_sub;
+	struct actuator_controls_s *att_ctrl;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+		att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (att_ctrl_sub->update(t)) {
+			/* send, add spaces so that string buffer is at least 10 chars long */
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl->timestamp / 1000,
+							   "rll ctrl    ",
+							   att_ctrl->control[0]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl->timestamp / 1000,
+							   "ptch ctrl    ",
+							   att_ctrl->control[1]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl->timestamp / 1000,
+							   "yaw ctrl     ",
+							   att_ctrl->control[2]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl->timestamp / 1000,
+							   "thr ctrl     ",
+							   att_ctrl->control[3]);
+		}
+	}
+};
+
+class MavlinkStreamNamedValueFloat : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "NAMED_VALUE_FLOAT";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamNamedValueFloat();
+	}
+
+private:
+	MavlinkOrbSubscription *debug_sub;
+	struct debug_key_value_s *debug;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
+		debug = (struct debug_key_value_s *)debug_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (debug_sub->update(t)) {
+			/* enforce null termination */
+			debug->key[sizeof(debug->key) - 1] = '\0';
+
+			mavlink_msg_named_value_float_send(_channel,
+							   debug->timestamp_ms,
+							   debug->key,
+							   debug->value);
+		}
+	}
+};
+
+class MavlinkStreamCameraCapture : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "CAMERA_CAPTURE";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamCameraCapture();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+	struct vehicle_status_s *status;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+		status = (struct vehicle_status_s *)status_sub->get_data();
+
+
+	}
+
+	void send(const hrt_abstime t)
+	{
+		(void)status_sub->update(t);
+
+		if (status->arming_state == ARMING_STATE_ARMED
+		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+
+			/* send camera capture on */
+			mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+
+		} else {
+			/* send camera capture off */
+			mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+		}
+	}
+};
 
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
@@ -1151,72 +1264,8 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamRCChannelsRaw(),
 	new MavlinkStreamManualControl(),
 	new MavlinkStreamOpticalFlow(),
+	new MavlinkStreamAttitudeControls(),
+	new MavlinkStreamNamedValueFloat(),
+	new MavlinkStreamCameraCapture(),
 	nullptr
 };
-
-
-
-
-
-
-
-//
-//
-//
-//
-//
-//
-//void
-//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
-//{
-//	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
-//
-//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
-//		/* send, add spaces so that string buffer is at least 10 chars long */
-//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//						   l->listener->last_sensor_timestamp / 1000,
-//						   "ctrl0    ",
-//						   l->listener->actuators_0.control[0]);
-//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//						   l->listener->last_sensor_timestamp / 1000,
-//						   "ctrl1    ",
-//						   l->listener->actuators_0.control[1]);
-//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//						   l->listener->last_sensor_timestamp / 1000,
-//						   "ctrl2     ",
-//						   l->listener->actuators_0.control[2]);
-//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//						   l->listener->last_sensor_timestamp / 1000,
-//						   "ctrl3     ",
-//						   l->listener->actuators_0.control[3]);
-//	}
-//}
-//
-//void
-//MavlinkOrbListener::l_debug_key_value(const struct listener *l)
-//{
-//	struct debug_key_value_s debug;
-//
-//	orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug);
-//
-//	/* Enforce null termination */
-//	debug.key[sizeof(debug.key) - 1] = '\0';
-//
-//	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//					   l->listener->last_sensor_timestamp / 1000,
-//					   debug.key,
-//					   debug.value);
-//}
-//
-//void
-//MavlinkOrbListener::l_nav_cap(const struct listener *l)
-//{
-//
-//	orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
-//
-//	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//				   hrt_absolute_time() / 1000,
-//				   "turn dist",
-//				   l->listener->nav_cap.turn_distance);
-//
-//}