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

Conflicts:
	src/platforms/px4_middleware.h

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -133,7 +133,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      7200,
+					      7700,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);

src/modules/commander/commander.cpp

@@ -739,9 +739,6 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
 	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 
-	/* welcome user */
-	warnx("starting");
-
 	const char *main_states_str[MAIN_STATE_MAX];
 	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
 	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
@@ -1246,6 +1243,7 @@ int commander_thread_main(int argc, char *argv[])
 		orb_check(safety_sub, &updated);
 
 		if (updated) {
+			bool previous_safety_off = safety.safety_off;
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
 			/* disarm if safety is now on and still armed */
@@ -1259,6 +1257,19 @@ int commander_thread_main(int argc, char *argv[])
 					arming_state_changed = true;
 				}
 			}
+
+			//Notify the user if the status of the safety switch changes
+			if(safety.safety_switch_available && previous_safety_off != safety.safety_off) {
+
+				if(safety.safety_off) {
+					set_tune(TONE_NOTIFY_POSITIVE_TUNE);
+				}
+				else {
+					tune_neutral(true);
+				}
+
+				status_changed = true;
+			}
 		}
 
 		/* update vtol vehicle status*/
@@ -1952,6 +1963,12 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* reset arm_tune_played when disarmed */
 		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
+
+			//Notify the user that it is safe to approach the vehicle
+			if(arm_tune_played) {
+				tune_neutral(true);
+			}
+
 			arm_tune_played = false;
 		}
 

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -280,6 +280,8 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         status.arming_state = test->current_state.arming_state;
         status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
         status.hil_state = test->hil_state;
+        // The power status of the test unit is not relevant for the unit test
+        status.circuit_breaker_engaged_power_check = true;
         safety.safety_switch_available = test->safety_switch_available;
         safety.safety_off = test->safety_off;
         armed.armed = test->current_state.armed;

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -1061,7 +1061,9 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators.timestamp_sample = _att.timestamp;
 			_actuators_airframe.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp_sample = _att.timestamp;
 
 			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {

src/modules/mavlink/mavlink_main.cpp

@@ -1286,30 +1286,11 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	if (Mavlink::instance_exists(_device_name, this)) {
-		warnx("mavlink instance for %s already running", _device_name);
+		warnx("%s already running", _device_name);
 		return ERROR;
 	}
 
-	/* inform about mode */
-	switch (_mode) {
-	case MAVLINK_MODE_NORMAL:
-		warnx("mode: NORMAL");
-		break;
-
-	case MAVLINK_MODE_CUSTOM:
-		warnx("mode: CUSTOM");
-		break;
-
-	case MAVLINK_MODE_ONBOARD:
-		warnx("mode: ONBOARD");
-		break;
-
-	default:
-		warnx("ERROR: Unknown mode");
-		break;
-	}
-
-	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
+	warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1337,7 +1318,7 @@ Mavlink::task_main(int argc, char *argv[])
 		 * marker ring buffer approach.
 		 */
 		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
-			errx(1, "can't allocate message buffer, exiting");
+			errx(1, "msg buf:");
 		}
 
 		/* initialize message buffer mutex */
@@ -1571,8 +1552,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_subscriptions = nullptr;
 
-	warnx("waiting for UART receive thread");
-
 	/* wait for threads to complete */
 	pthread_join(_receive_thread, NULL);
 

src/modules/mavlink/mavlink_mission.cpp

@@ -120,13 +120,11 @@ MavlinkMissionManager::init_offboard_mission()
 		_count = mission_state.count;
 		_current_seq = mission_state.current_seq;
 
-		warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq);
-
 	} else {
 		_dataman_id = 0;
 		_count = 0;
 		_current_seq = 0;
-		warnx("offboard mission init: ERROR, reading mission state failed");
+		warnx("offboard mission init: ERROR");
 	}
 }
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -141,6 +141,7 @@ private:
 	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_controller_latency_perf;
 
 	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
 	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
@@ -289,7 +290,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_actuators_0_circuit_breaker_enabled(false),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
+	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
 
 {
 	memset(&_v_att, 0, sizeof(_v_att));
@@ -886,10 +888,12 @@ MulticopterAttitudeControl::task_main()
 				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
 				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 				_actuators.timestamp = hrt_absolute_time();
+				_actuators.timestamp_sample = _v_att.timestamp;
 
 				if (!_actuators_0_circuit_breaker_enabled) {
 					if (_actuators_0_pub > 0) {
 						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
+						perf_end(_controller_latency_perf);
 
 					} else if (_actuators_id) {
 						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);

src/modules/sdlog2/sdlog2.c

@@ -199,6 +199,8 @@ static bool space_warning_sent = false;
 static pthread_t logwriter_pthread = 0;
 static pthread_attr_t logwriter_attr;
 
+static perf_counter_t perf_write;
+
 /**
  * Log buffer writing thread. Open and close file here.
  */
@@ -451,10 +453,10 @@ int open_log_file()
 	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
 
 	if (fd < 0) {
-		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
 
 	} else {
-		mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
+		mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name);
 	}
 
 	return fd;
@@ -515,8 +517,6 @@ static void *logwriter_thread(void *arg)
 	/* set name */
 	prctl(PR_SET_NAME, "sdlog2_writer", 0);
 
-	perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write");
-
 	int log_fd = open_log_file();
 
 	if (log_fd < 0) {
@@ -620,16 +620,11 @@ static void *logwriter_thread(void *arg)
 	fsync(log_fd);
 	close(log_fd);
 
-	/* free performance counter */
-	perf_free(perf_write);
-
 	return NULL;
 }
 
 void sdlog2_start_log()
 {
-	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging");
-
 	/* create log dir if needed */
 	if (create_log_dir() != 0) {
 		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
@@ -655,6 +650,9 @@ void sdlog2_start_log()
 
 	logwriter_should_exit = false;
 
+	/* allocate write performance counter */
+	perf_write = perf_alloc(PC_ELAPSED, "sd write");
+
 	/* start log buffer emptying thread */
 	if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
 		errx(1, "error creating logwriter thread");
@@ -674,8 +672,6 @@ void sdlog2_start_log()
 
 void sdlog2_stop_log()
 {
-	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging");
-
 	logging_enabled = false;
 
 	/* wake up write thread one last time */
@@ -701,6 +697,11 @@ void sdlog2_stop_log()
 	perf_print_all(perf_fd);
 	close(perf_fd);
 
+	/* free log writer performance counter */
+	perf_free(perf_write);
+
+	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped");
+
 	sdlog2_status();
 }
 

src/modules/systemlib/perf_counter.c

@@ -39,6 +39,7 @@
 
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
 #include <sys/queue.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
@@ -67,10 +68,13 @@ struct perf_ctr_count {
 struct perf_ctr_elapsed {
 	struct perf_ctr_header	hdr;
 	uint64_t		event_count;
+	uint64_t		event_overruns;
 	uint64_t		time_start;
 	uint64_t		time_total;
 	uint64_t		time_least;
 	uint64_t		time_most;
+	float			mean;
+	float			M2;
 };
 
 /**
@@ -126,6 +130,28 @@ perf_alloc(enum perf_counter_type type, const char *name)
 	return ctr;
 }
 
+perf_counter_t
+perf_alloc_once(enum perf_counter_type type, const char *name)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		if (!strcmp(handle->name, name)) {
+			if (type == handle->type) {
+				/* they are the same counter */
+				return handle;
+			} else {
+				/* same name but different type, assuming this is an error and not intended */
+				return NULL;
+			}
+		}
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	/* if the execution reaches here, no existing counter of that name was found */
+	return perf_alloc(type, name);
+}
+
 void
 perf_free(perf_counter_t handle)
 {
@@ -213,17 +239,72 @@ perf_end(perf_counter_t handle)
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
 			if (pce->time_start != 0) {
-				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
+				int64_t elapsed = hrt_absolute_time() - pce->time_start;
+
+				if (elapsed < 0) {
+					pce->event_overruns++;
+				} else {
+
+					pce->event_count++;
+					pce->time_total += elapsed;
+
+					if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0))
+						pce->time_least = elapsed;
+
+					if (pce->time_most < (uint64_t)elapsed)
+						pce->time_most = elapsed;
+
+					// maintain mean and variance of the elapsed time in seconds
+					// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+					float dt = elapsed / 1e6f;
+					float delta_intvl = dt - pce->mean;
+					pce->mean += delta_intvl / pce->event_count;
+					pce->M2 += delta_intvl * (dt - pce->mean);
+
+					pce->time_start = 0;
+				}
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+#include <systemlib/err.h>
+
+void
+perf_set(perf_counter_t handle, int64_t elapsed)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			if (elapsed < 0) {
+				pce->event_overruns++;
+			} else {
 
 				pce->event_count++;
 				pce->time_total += elapsed;
 
-				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+				if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0))
 					pce->time_least = elapsed;
 
-				if (pce->time_most < elapsed)
+				if (pce->time_most < (uint64_t)elapsed)
 					pce->time_most = elapsed;
 
+				// maintain mean and variance of the elapsed time in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = elapsed / 1e6f;
+				float delta_intvl = dt - pce->mean;
+				pce->mean += delta_intvl / pce->event_count;
+				pce->M2 += delta_intvl * (dt - pce->mean);
+
 				pce->time_start = 0;
 			}
 		}
@@ -310,14 +391,17 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+		float rms = sqrtf(pce->M2 / (pce->event_count-1));
 
-		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
-		       handle->name,
-		       pce->event_count,
-		       pce->time_total,
-		       pce->time_total / pce->event_count,
-		       pce->time_least,
-		       pce->time_most);
+		dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
+			handle->name,
+			pce->event_count,
+			pce->event_overruns,
+			pce->time_total,
+			pce->time_total / pce->event_count,
+			pce->time_least,
+			pce->time_most,
+			(double)(1e6f * rms));
 		break;
 	}
 
@@ -325,14 +409,13 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 		float rms = sqrtf(pci->M2 / (pci->event_count-1));
 
-		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n",
-		       handle->name,
-		       pci->event_count,
-		       (pci->time_last - pci->time_first) / pci->event_count,
-		       pci->time_least,
-		       pci->time_most,
-			   (double)(1000 * pci->mean),
-			   (double)(1000 * rms));
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
+			handle->name,
+			pci->event_count,
+			(pci->time_last - pci->time_first) / pci->event_count,
+			pci->time_least,
+			pci->time_most,
+			(double)(1e6f * rms));
 		break;
 	}
 

src/modules/systemlib/perf_counter.h

@@ -57,7 +57,7 @@ typedef struct perf_ctr_header	*perf_counter_t;
 __BEGIN_DECLS
 
 /**
- * Create a new counter.
+ * Create a new local counter.
  *
  * @param type			The type of the new counter.
  * @param name			The counter name.
@@ -66,6 +66,16 @@ __BEGIN_DECLS
  */
 __EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name);
 
+/**
+ * Get the reference to an existing counter or create a new one if it does not exist.
+ *
+ * @param type			The type of the counter.
+ * @param name			The counter name.
+ * @return			Handle for the counter, or NULL if a counter
+ *				could not be allocated.
+ */
+__EXPORT extern perf_counter_t	perf_alloc_once(enum perf_counter_type type, const char *name);
+
 /**
  * Free a counter.
  *
@@ -102,6 +112,18 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  */
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
+/**
+ * Register a measurement
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresponding perf_begin call. It sets the
+ * value provided as argument as a new measurement.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ * @param elapsed		The time elapsed. Negative values lead to incrementing the overrun counter.
+ */
+__EXPORT extern void		perf_set(perf_counter_t handle, int64_t elapsed);
+
 /**
  * Cancel a performance event.
  *