Merge branch 'ext_mag_param' into logging

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -6,28 +6,45 @@
 ms5611 start
 adc start
 
-# Mag might be external
-if hmc5883 start
-then
-	echo "[init] Using HMC5883"
-fi
-
 if mpu6000 start
 then
-	echo "[init] Using MPU6000"
+	echo "Internal MPU6000"
 fi
 
 if l3gd20 start
 then
-	echo "[init] Using L3GD20(H)"
+	echo "Internal L3GD20(H)"
+fi
+
+# MAG selection
+if param compare SENS_EXT_MAG 2
+then
+	if hmc5883 -I start
+	then
+		echo "Internal HMC5883"
+	fi
+else
+	# Use only external as primary
+	if param compare SENS_EXT_MAG 1
+	then
+		if hmc5883 -X start
+		then
+			echo "External HMC5883"
+		fi
+	else
+	# auto-detect the primary, prefer external
+		if hmc5883 start
+		then
+			echo "Default HMC5883"
+		fi
+	fi
 fi
 
 if ver hwcmp PX4FMU_V2
 then
-	# IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST
 	if lsm303d start
 	then
-		echo "[init] Using LSM303D"
+		echo "Internal LSM303D"
 	fi
 fi
 

src/drivers/device/device.h

@@ -240,6 +240,7 @@ private:
 	 * @param context	Pointer to the interrupted context.
 	 */
 	static void	dev_interrupt(int irq, void *context);
+
 };
 
 /**
@@ -469,6 +470,10 @@ private:
 	 * @return		OK, or -errno on error.
 	 */
 	int		remove_poll_waiter(struct pollfd *fds);
+
+	/* do not allow copying this class */
+	CDev(const CDev&);
+	CDev operator=(const CDev&);
 };
 
 /**

src/drivers/hmc5883/hmc5883.cpp

@@ -1358,8 +1358,8 @@ namespace hmc5883
 #endif
 const int ERROR = -1;
 
-HMC5883	*g_dev_int;
-HMC5883	*g_dev_ext;
+HMC5883	*g_dev_int = nullptr;
+HMC5883	*g_dev_ext = nullptr;
 
 void	start(int bus, enum Rotation rotation);
 void	test(int bus);
@@ -1395,6 +1395,11 @@ start(int bus, enum Rotation rotation)
 			errx(0, "already started internal");
 		g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
 		if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+
+			/* tear down the failing onboard instance */
+			delete g_dev_int;
+			g_dev_int = nullptr;
+
 			if (bus == PX4_I2C_BUS_ONBOARD) {
 				goto fail;
 			}

src/drivers/px4io/px4io_uploader.cpp

@@ -204,12 +204,8 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 		if (bl_rev <= 2) {
 			ret = verify_rev2(fw_size);
-		} else if(bl_rev == 3) {
-			ret = verify_rev3(fw_size);
 		} else {
-			/* verify rev 4 and higher still uses the same approach and
-			 * every version *needs* to be verified.
-			 */
+			/* verify rev 3 and higher. Every version *needs* to be verified. */
 			ret = verify_rev3(fw_size);
 		}
 
@@ -276,14 +272,14 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 int
 PX4IO_Uploader::recv(uint8_t *p, unsigned count)
 {
+	int ret;
 	while (count--) {
-		int ret = recv(*p++, 5000);
+		ret = recv(*p++, 5000);
 
 		if (ret != OK)
-			return ret;
+			break;
 	}
-
-	return OK;
+	return ret;
 }
 
 void
@@ -314,21 +310,19 @@ PX4IO_Uploader::send(uint8_t c)
 #endif
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;
-
 	return OK;
 }
 
 int
 PX4IO_Uploader::send(uint8_t *p, unsigned count)
 {
+	int ret;
 	while (count--) {
-		int ret = send(*p++);
-
+		ret = send(*p++);
 		if (ret != OK)
-			return ret;
+			break;
 	}
-
-	return OK;
+	return ret;
 }
 
 int
@@ -419,12 +413,15 @@ PX4IO_Uploader::program(size_t fw_size)
 	int ret;
 	size_t sent = 0;
 
-	file_buf = (uint8_t *)malloc(PROG_MULTI_MAX);
+	file_buf = new uint8_t[PROG_MULTI_MAX];
 	if (!file_buf) {
 		log("Can't allocate program buffer");
 		return -ENOMEM;
 	}
 
+	ASSERT((fw_size & 3) == 0);
+	ASSERT((PROG_MULTI_MAX & 3) == 0);
+
 	log("programming %u bytes...", (unsigned)fw_size);
 
 	ret = lseek(_fw_fd, 0, SEEK_SET);
@@ -443,34 +440,26 @@ PX4IO_Uploader::program(size_t fw_size)
 			    (unsigned)sent,
 			    (int)count,
 			    (int)errno);
-		}
-
-		if (count == 0) {
-			free(file_buf);
-			return OK;
+			ret = -errno;
+			break;
 		}
 
 		sent += count;
 
-		if (count < 0)
-			return -errno;
-
-		ASSERT((count % 4) == 0);
-
 		send(PROTO_PROG_MULTI);
 		send(count);
-		send(&file_buf[0], count);
+		send(file_buf, count);
 		send(PROTO_EOC);
 
 		ret = get_sync(1000);
 
 		if (ret != OK) {
-			free(file_buf);
-			return ret;
+			break;
 		}
 	}
-	free(file_buf);
-	return OK;
+
+	delete [] file_buf;
+	return ret;
 }
 
 int

src/drivers/px4io/uploader.h

@@ -75,7 +75,6 @@ private:
 		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
 
 		PROG_MULTI_MAX		= 60,		/**< protocol max is 255, must be multiple of 4 */
-		READ_MULTI_MAX		= 60,		/**< protocol max is 255, something overflows with >= 64 */
 
 	};
 

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),
@@ -230,17 +231,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

@@ -372,4 +372,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_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),
@@ -111,15 +113,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();

src/modules/mavlink/mavlink_receiver.h

@@ -156,4 +156,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++

src/modules/sensors/sensor_params.c

@@ -292,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
  */
 PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
 
+/**
+* Set usage of external magnetometer
+*
+*  * Set to 0 (default) to auto-detect (will try to get the external as primary)
+*  * Set to 1 to force the external magnetometer as primary
+*  * Set to 2 to force the internal magnetometer as primary
+*
+* @min 0
+* @max 2
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_INT32(SENS_EXT_MAG, 0);
+
 
 /**
  * RC Channel 1 Minimum