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

src/modules/mavlink/mavlink_commands.cpp

@@ -40,9 +40,12 @@
 
 #include "mavlink_commands.h"
 
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) :
+	_cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
+	_cmd{},
+	_channel(channel),
+	_cmd_time(0)
 {
-	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
 }
 
 void

src/modules/mavlink/mavlink_ftp.cpp

@@ -50,16 +50,20 @@ MavlinkFTP::getServer()
 	return _server;
 }
 
-MavlinkFTP::MavlinkFTP()
+MavlinkFTP::MavlinkFTP() :
+	_session_fds{},
+	_workBufs{},
+	_workFree{},
+	_lock{}
 {
 	// initialise the request freelist
 	dq_init(&_workFree);
 	sem_init(&_lock, 0, 1);
-    
-    // initialize session list
-    for (size_t i=0; i<kMaxSession; i++) {
-        _session_fds[i] = -1;
-    }
+
+	// initialize session list
+	for (size_t i=0; i<kMaxSession; i++) {
+		_session_fds[i] = -1;
+	}
 
 	// drop work entries onto the free list
 	for (unsigned i = 0; i < kRequestQueueSize; i++) {

src/modules/mavlink/mavlink_main.cpp

@@ -216,6 +216,7 @@ Mavlink::Mavlink() :
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
 	next(nullptr),
+	_instance_id(0),
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
@@ -231,17 +232,24 @@ Mavlink::Mavlink() :
 	_mission_pub(-1),
 	_mission_result_sub(-1),
 	_mode(MAVLINK_MODE_NORMAL),
+	_channel(MAVLINK_COMM_0),
+	_logbuffer{},
 	_total_counter(0),
+	_receive_thread{},
 	_verbose(false),
 	_forwarding_on(false),
 	_passing_on(false),
 	_ftp_on(false),
 	_uart_fd(-1),
+	_baudrate(57600),
+	_datarate(10000),
 	_mavlink_param_queue_index(0),
+	mavlink_link_termination_allowed(false),
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
-	_message_buffer({}),
+	_message_buffer{},
+	_message_buffer_mutex{},
 	_param_initialized(false),
 	_param_system_id(0),
 	_param_component_id(0),

src/modules/mavlink/mavlink_main.h

@@ -376,4 +376,8 @@ private:
 	 * Main mavlink task.
 	 */
 	int		task_main(int argc, char *argv[]);
+
+	/* do not allow copying this class */
+	Mavlink(const Mavlink&);
+	Mavlink operator=(const Mavlink&);
 };

src/modules/mavlink/mavlink_messages.cpp

@@ -258,7 +258,16 @@ private:
 	MavlinkOrbSubscription *status_sub;
 	MavlinkOrbSubscription *pos_sp_triplet_sub;
 
+	/* do not allow top copying this class */
+	MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &);
+	MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &);
+
 protected:
+	explicit MavlinkStreamHeartbeat() : MavlinkStream(),
+		status_sub(nullptr),
+		pos_sp_triplet_sub(nullptr)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
@@ -322,7 +331,15 @@ public:
 private:
 	MavlinkOrbSubscription *status_sub;
 
+	/* do not allow top copying this class */
+	MavlinkStreamSysStatus(MavlinkStreamSysStatus &);
+	MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &);
+
 protected:
+	explicit MavlinkStreamSysStatus() : MavlinkStream(),
+		status_sub(nullptr)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
@@ -384,8 +401,13 @@ private:
 	uint64_t mag_timestamp;
 	uint64_t baro_timestamp;
 
+	/* do not allow top copying this class */
+	MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &);
+	MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &);
+
 protected:
 	explicit MavlinkStreamHighresIMU() : MavlinkStream(),
+		sensor_sub(nullptr),
 		sensor_time(0),
 		accel_timestamp(0),
 		gyro_timestamp(0),
@@ -469,8 +491,14 @@ private:
 	MavlinkOrbSubscription *att_sub;
 	uint64_t att_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamAttitude(MavlinkStreamAttitude &);
+	MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &);
+
+
 protected:
 	explicit MavlinkStreamAttitude() : MavlinkStream(),
+		att_sub(nullptr),
 		att_time(0)
 	{}
 
@@ -520,8 +548,13 @@ private:
 	MavlinkOrbSubscription *att_sub;
 	uint64_t att_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &);
+	MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &);
+
 protected:
 	explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
+		att_sub(nullptr),
 		att_time(0)
 	{}
 
@@ -589,12 +622,21 @@ private:
 	MavlinkOrbSubscription *airspeed_sub;
 	uint64_t airspeed_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
+	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
+
 protected:
 	explicit MavlinkStreamVFRHUD() : MavlinkStream(),
+		att_sub(nullptr),
 		att_time(0),
+		pos_sub(nullptr),
 		pos_time(0),
+		armed_sub(nullptr),
 		armed_time(0),
+		act_sub(nullptr),
 		act_time(0),
+		airspeed_sub(nullptr),
 		airspeed_time(0)
 	{}
 
@@ -665,8 +707,13 @@ private:
 	MavlinkOrbSubscription *gps_sub;
 	uint64_t gps_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &);
+	MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &);
+
 protected:
 	explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
+		gps_sub(nullptr),
 		gps_time(0)
 	{}
 
@@ -726,9 +773,15 @@ private:
 	MavlinkOrbSubscription *home_sub;
 	uint64_t home_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &);
+	MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &);
+
 protected:
 	explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
+		pos_sub(nullptr),
 		pos_time(0),
+		home_sub(nullptr),
 		home_time(0)
 	{}
 
@@ -789,8 +842,13 @@ private:
 	MavlinkOrbSubscription *pos_sub;
 	uint64_t pos_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &);
+	MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &);
+
 protected:
 	explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
+		pos_sub(nullptr),
 		pos_time(0)
 	{}
 
@@ -845,8 +903,13 @@ private:
 	MavlinkOrbSubscription *pos_sub;
 	uint64_t pos_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &);
+	MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &);
+
 protected:
 	explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
+		pos_sub(nullptr),
 		pos_time(0)
 	{}
 
@@ -899,7 +962,15 @@ public:
 private:
 	MavlinkOrbSubscription *home_sub;
 
+	/* do not allow top copying this class */
+	MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &);
+	MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &);
+
 protected:
+	explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(),
+		home_sub(nullptr)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
@@ -962,8 +1033,13 @@ private:
 	MavlinkOrbSubscription *act_sub;
 	uint64_t act_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &);
+	MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &);
+
 protected:
 	explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
+		act_sub(nullptr),
 		act_time(0)
 	{}
 
@@ -1033,10 +1109,17 @@ private:
 	MavlinkOrbSubscription *act_sub;
 	uint64_t act_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamHILControls(MavlinkStreamHILControls &);
+	MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &);
+
 protected:
 	explicit MavlinkStreamHILControls() : MavlinkStream(),
+		status_sub(nullptr),
 		status_time(0),
+		pos_sp_triplet_sub(nullptr),
 		pos_sp_triplet_time(0),
+		act_sub(nullptr),
 		act_time(0)
 	{}
 
@@ -1159,7 +1242,15 @@ public:
 private:
 	MavlinkOrbSubscription *pos_sp_triplet_sub;
 
+	/* do not allow top copying this class */
+	MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
+	MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
+
 protected:
+	explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(),
+		pos_sp_triplet_sub(nullptr)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
@@ -1208,8 +1299,13 @@ private:
 	MavlinkOrbSubscription *pos_sp_sub;
 	uint64_t pos_sp_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &);
+	MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &);
+
 protected:
 	explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
+		pos_sp_sub(nullptr),
 		pos_sp_time(0)
 	{}
 
@@ -1261,8 +1357,13 @@ private:
 	MavlinkOrbSubscription *att_sp_sub;
 	uint64_t att_sp_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &);
+	MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
+
 protected:
 	explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
+		att_sp_sub(nullptr),
 		att_sp_time(0)
 	{}
 
@@ -1314,8 +1415,13 @@ private:
 	MavlinkOrbSubscription *att_rates_sp_sub;
 	uint64_t att_rates_sp_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &);
+	MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &);
+
 protected:
 	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
+		att_rates_sp_sub(nullptr),
 		att_rates_sp_time(0)
 	{}
 
@@ -1367,8 +1473,13 @@ private:
 	MavlinkOrbSubscription *rc_sub;
 	uint64_t rc_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &);
+	MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &);
+
 protected:
 	explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
+		rc_sub(nullptr),
 		rc_time(0)
 	{}
 
@@ -1456,8 +1567,13 @@ private:
 	MavlinkOrbSubscription *manual_sub;
 	uint64_t manual_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamManualControl(MavlinkStreamManualControl &);
+	MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &);
+
 protected:
 	explicit MavlinkStreamManualControl() : MavlinkStream(),
+		manual_sub(nullptr),
 		manual_time(0)
 	{}
 
@@ -1510,8 +1626,13 @@ private:
 	MavlinkOrbSubscription *flow_sub;
 	uint64_t flow_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
+	MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
+
 protected:
 	explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
+		flow_sub(nullptr),
 		flow_time(0)
 	{}
 
@@ -1563,8 +1684,13 @@ private:
 	MavlinkOrbSubscription *att_ctrl_sub;
 	uint64_t att_ctrl_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &);
+	MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &);
+
 protected:
 	explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
+		att_ctrl_sub(nullptr),
 		att_ctrl_time(0)
 	{}
 
@@ -1626,8 +1752,13 @@ private:
 	MavlinkOrbSubscription *debug_sub;
 	uint64_t debug_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &);
+	MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &);
+
 protected:
 	explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
+		debug_sub(nullptr),
 		debug_time(0)
 	{}
 
@@ -1678,7 +1809,15 @@ public:
 private:
 	MavlinkOrbSubscription *status_sub;
 
+	/* do not allow top copying this class */
+	MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &);
+	MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &);
+
 protected:
+	explicit MavlinkStreamCameraCapture() : MavlinkStream(),
+		status_sub(nullptr)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
@@ -1729,8 +1868,13 @@ private:
 	MavlinkOrbSubscription *range_sub;
 	uint64_t range_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &);
+	MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &);
+
 protected:
 	explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
+		range_sub(nullptr),
 		range_time(0)
 	{}
 

src/modules/mavlink/mavlink_mission.h

@@ -174,4 +174,8 @@ private:
 
 	mavlink_channel_t	_channel;
 	uint8_t			_comp_id;
+
+	/* do not allow copying this class */
+	MavlinkMissionManager(const MavlinkMissionManager&);
+	MavlinkMissionManager operator=(const MavlinkMissionManager&);
 };

src/modules/mavlink/mavlink_orb_subscription.h

@@ -82,6 +82,10 @@ private:
 	const orb_id_t _topic;		///< topic metadata
 	int _fd;			///< subscription handle
 	bool _published;		///< topic was ever published
+
+	/* do not allow copying this class */
+	MavlinkOrbSubscription(const MavlinkOrbSubscription&);
+	MavlinkOrbSubscription operator=(const MavlinkOrbSubscription&);
 };
 
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -86,7 +86,9 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
 
 MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink(parent),
-
+	status{},
+	hil_local_pos{},
+	_control_mode{},
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
 	_attitude_pub(-1),
@@ -112,15 +114,13 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_manual_pub(-1),
 	_telemetry_heartbeat_time(0),
 	_radio_status_available(false),
-	_control_mode_sub(-1),
+	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
-	_hil_local_alt0(0.0)
+	_hil_local_alt0(0.0f),
+	_hil_local_proj_ref{}
 {
-	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
-	memset(&_control_mode, 0, sizeof(_control_mode));
 
 	// make sure the FTP server is started
 	(void)MavlinkFTP::getServer();
@@ -740,10 +740,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		gyro.temperature = imu.temperature;
 
 		if (_gyro_pub < 0) {
-			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
+			orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
 		}
 	}
 
@@ -762,10 +762,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		accel.temperature = imu.temperature;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
 		}
 	}
 
@@ -783,10 +783,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		mag.z = imu.zmag;
 
 		if (_mag_pub < 0) {
-			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
 
 		} else {
-			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
+			orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
 		}
 	}
 
@@ -801,10 +801,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		baro.temperature = imu.temperature;
 
 		if (_baro_pub < 0) {
-			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
+			orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
 		}
 	}
 
@@ -1078,10 +1078,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		accel.temperature = 25.0f;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
 		}
 	}
 

src/modules/mavlink/mavlink_receiver.h

@@ -160,4 +160,8 @@ private:
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
+
+	/* do not allow copying this class */
+	MavlinkReceiver(const MavlinkReceiver&);
+	MavlinkReceiver operator=(const MavlinkReceiver&);
 };

src/modules/mavlink/mavlink_stream.h

@@ -77,6 +77,10 @@ protected:
 
 private:
 	hrt_abstime _last_sent;
+
+	/* do not allow top copying this class */
+	MavlinkStream(const MavlinkStream&);
+	MavlinkStream& operator=(const MavlinkStream&);
 };
 
 

src/modules/mavlink/module.mk

@@ -52,3 +52,5 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 MAXOPTIMIZATION	 = -Os
 
 MODULE_STACKSIZE = 1024
+
+EXTRACXXFLAGS	= -Weffc++