Merge branch 'master' of github.com:PX4/Firmware into gps_logging_dual

18 files changed, 34 insertions, 42 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index b88f61ce8..e5bb772b3 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -119,7 +119,7 @@ int ardrone_interface_main(int argc, char *argv[])
 		ardrone_interface_task = task_spawn_cmd("ardrone_interface",
 						    SCHED_DEFAULT,
 						    SCHED_PRIORITY_MAX - 15,
-						    2048,
+						    1100,
 						    ardrone_interface_thread_main,
 						    (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk
index 058bd1397..d8e6c76c6 100644
--- a/src/drivers/ardrone_interface/module.mk
+++ b/src/drivers/ardrone_interface/module.mk
@@ -38,3 +38,4 @@
 MODULE_COMMAND	 = ardrone_interface
 SRCS		 = ardrone_interface.c \
 			   ardrone_motor_control.c
+MODULE_STACKSIZE = 1200
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index ac75682c4..321fdd173 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1380,7 +1380,6 @@ MPU6000_gyro::init()
 
 	_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
-out:
 	return ret;
 }
 
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 3fe1b0abc..1ce93aeea 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -753,8 +753,8 @@ MS5611::print_info()
 	printf("TEMP:           %d\n", _TEMP);
 	printf("SENS:           %lld\n", _SENS);
 	printf("OFF:            %lld\n", _OFF);
-	printf("P:              %.3f\n", _P);
-	printf("T:              %.3f\n", _T);
+	printf("P:              %.3f\n", (double)_P);
+	printf("T:              %.3f\n", (double)_T);
 	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
 
 	printf("factory_setup             %u\n", _prom.factory_setup);
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index 43318ca84..3b210ac59 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt()
 		if (_rx_dma_status == _dma_status_waiting) {
 
 			/* verify that the received packet is complete */
-			unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
+			int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
 			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
 				perf_count(_pc_badidle);
 
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index a0cf98340..9109af14f 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -254,9 +254,6 @@ SF0X::~SF0X()
 int
 SF0X::init()
 {
-	int ret = ERROR;
-	unsigned i = 0;
-
 	/* do regular cdev init */
 	if (CDev::init() != OK) {
 		goto out;
@@ -594,7 +591,7 @@ SF0X::collect()
 		valid = false;
 
 		/* wipe out partially read content from last cycle(s), check for dot */
-		for (int i = 0; i < (lend - 2); i++) {
+		for (unsigned i = 0; i < (lend - 2); i++) {
 			if (_linebuf[i] == '\n') {
 				char buf[sizeof(_linebuf)];
 				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
@@ -795,7 +792,7 @@ const int ERROR = -1;
 
 SF0X	*g_dev;
 
-void	start();
+void	start(const char *port);
 void	stop();
 void	test();
 void	reset();
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index de13b8969..aa0dca60c 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -145,7 +145,7 @@ private:
 
 ADC::ADC(uint32_t channels) :
 	CDev("adc", ADC_DEVICE_PATH),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
+	_sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")),
 	_channel_count(0),
 	_samples(nullptr),
 	_to_system_power(0)
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 2ec889efe..66a949af1 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -266,7 +266,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 	int loopcounter = 0;
-	int printcounter = 0;
 
 	thread_running = true;
 
@@ -274,9 +273,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	// orb_advert_t pub_dbg = -1;
 
-	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
-	// XXX write this out to perf regs
-
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
@@ -287,11 +283,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
 
-	uint64_t start_time = hrt_absolute_time();
 	bool initialized = false;
 
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
-	unsigned offset_count = 0;
 
 	/* rotation matrix for magnetic declination */
 	math::Matrix<3, 3> R_decl;
@@ -382,7 +376,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* Fill in gyro measurements */
 					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
+						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
@@ -393,7 +387,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* update accelerometer measurements */
 					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
+						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
 						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
@@ -445,7 +439,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* update magnetometer measurements */
 					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
+						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
 						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
@@ -498,8 +492,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					uint64_t timing_start = hrt_absolute_time();
-
 					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
 							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
 
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 7a71894ed..0a6d3fa8f 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1067,7 +1067,7 @@ FixedwingEstimator::task_main()
 					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
 						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
-					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset);
+					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
 					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
 
 					_gps_initialized = true;
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 1f3e9f098..6943239e5 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -304,8 +304,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 /* publications */
 	_rate_sp_pub(-1),
-	_actuators_0_pub(-1),
 	_attitude_sp_pub(-1),
+	_actuators_0_pub(-1),
 	_actuators_1_pub(-1),
 
 /* performance counters */
@@ -773,7 +773,7 @@ FixedwingAttitudeControl::task_main()
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
 					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
 					if (!isfinite(roll_u)) {
-						warnx("roll_u %.4f", roll_u);
+						warnx("roll_u %.4f", (double)roll_u);
 					}
 
 					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 9cbc26efe..5b877cd77 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -418,8 +418,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	launch_detected(false),
-	last_manual(false),
 	usePreTakeoffThrust(false),
+	last_manual(false),
 	flare_curve_alt_rel_last(0.0f),
 	launchDetector(),
 	_airspeed_error(0.0f),
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6c97bfca7..28dd97fca 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -149,10 +149,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		instance = Mavlink::get_instance(6);
 		break;
 #endif
-	}
-
-	/* no valid instance, bail */
-	if (!instance) {
+		default:
 		return;
 	}
 
@@ -211,9 +208,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 static void usage(void);
 
 Mavlink::Mavlink() :
-	next(nullptr),
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
+	next(nullptr),
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
@@ -234,7 +231,6 @@ Mavlink::Mavlink() :
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
 	_message_buffer({}),
-
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
 {
@@ -2030,14 +2026,14 @@ Mavlink::task_main(int argc, char *argv[])
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
 				if (_subscribe_to_stream_rate > 0.0f) {
-					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate);
+					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
 
 				} else {
 					warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
 				}
 
 			} else {
-				warnx("stream %s not found", _subscribe_to_stream, _device_name);
+				warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
 			}
 
 			delete _subscribe_to_stream;
@@ -2243,7 +2239,6 @@ Mavlink::stream(int argc, char *argv[])
 	const char *device_name = DEFAULT_DEVICE_NAME;
 	float rate = -1.0f;
 	const char *stream_name = nullptr;
-	int ch;
 
 	argc -= 2;
 	argv += 2;
@@ -2280,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[])
 		i++;
 	}
 
-	if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
+	if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
 		Mavlink *inst = get_instance_for_device(device_name);
 
 		if (inst != nullptr) {
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index c7a7d32f8..25c0da820 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -221,8 +221,6 @@ private:
 	int		_mavlink_fd;
 	bool		_task_running;
 
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
 	/* states */
 	bool		_hil_enabled;		/**< Hardware In the Loop mode */
 	bool		_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
@@ -282,7 +280,7 @@ private:
 
 	pthread_mutex_t _message_buffer_mutex;
 
-
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	/**
 	 * Send one parameter.
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index d432edd2b..21d5219d3 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -63,7 +63,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription()
 	free(_data);
 }
 
-const orb_id_t
+orb_id_t
 MavlinkOrbSubscription::get_topic()
 {
 	return _topic;
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 5c6543e81..8c09772c8 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -63,7 +63,7 @@ public:
 	 */
 	bool is_published();
 	void *get_data();
-	const orb_id_t get_topic();
+	orb_id_t get_topic();
 
 private:
 	const orb_id_t _topic;		/*< topic metadata */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 72b9ee83a..53769e0cf 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -932,6 +932,8 @@ void *MavlinkReceiver::start_helper(void *context)
 	rcv->receive_thread(NULL);
 
 	delete rcv;
+
+	return nullptr;
 }
 
 pthread_t
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index bb19d7e33..5ec30bd33 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -81,5 +81,9 @@ MavlinkStream::update(const hrt_abstime t)
 		/* interval expired, send message */
 		send(t);
 		_last_sent = (t / _interval) * _interval;
+
+		return 0;
 	}
+
+	return -1;
 }
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index def40d9ad..2979d20de 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -63,9 +63,13 @@ public:
 	MavlinkStream *next;
 
 	MavlinkStream();
-	~MavlinkStream();
+	virtual ~MavlinkStream();
 	void set_interval(const unsigned int interval);
 	void set_channel(mavlink_channel_t channel);
+
+	/**
+	 * @return 0 if updated / sent, -1 if unchanged
+	 */
 	int update(const hrt_abstime t);
 	virtual MavlinkStream *new_instance() = 0;
 	virtual void subscribe(Mavlink *mavlink) = 0;