Rebase changes on upstream master

This brings in many of the changes from the PX4 fork on ATLFLight.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -400,6 +400,35 @@ void AttitudeEstimatorQ::task_main()
 
 			if (!_failsafe) {
 				uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
+#ifdef __PX4_POSIX
+				perf_end(_perf_accel);
+				perf_end(_perf_mpu);
+				perf_end(_perf_mag);
+#endif
+
+				if (_voter_gyro.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_gyro.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
+									  _voter_gyro.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
+
+				if (_voter_accel.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_accel.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
+									  _voter_accel.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
 
 #ifdef __PX4_POSIX
 				perf_end(_perf_accel);
@@ -607,8 +636,11 @@ void AttitudeEstimatorQ::task_main()
 
 		/* attitude quaternions for control state */
 		ctrl_state.q[0] = _q(0);
+
 		ctrl_state.q[1] = _q(1);
+
 		ctrl_state.q[2] = _q(2);
+
 		ctrl_state.q[3] = _q(3);
 
 		/* attitude rates for control state */

src/modules/commander/commander.cpp

@@ -1610,6 +1610,11 @@ int commander_thread_main(int argc, char *argv[])
 					_usb_telemetry_active = true;
 				}
 
+				/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
+				if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
+					_usb_telemetry_active = true;
+				}
+
 				if (telemetry.heartbeat_time > 0) {
 					telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
 				}
@@ -2977,7 +2982,7 @@ set_control_mode()
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
 	control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
-	control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
+	//control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
 	switch (status.nav_state) {

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -993,6 +993,10 @@ void MulticopterPositionControl::control_auto(float dt)
 	}
 
 	if (current_setpoint_valid) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
 		/* scaled space: 1 == position error resulting max allowed speed */
 		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
 
@@ -1187,6 +1191,13 @@ MulticopterPositionControl::task_main()
 
 		poll_subscriptions();
 
+		/* get current rotation matrix and euler angles from control state quaternions */
+		math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+		_R = q_att.to_dcm();
+		math::Vector<3> euler_angles;
+		euler_angles = _R.to_euler();
+		_yaw     = euler_angles(2);
+
 		parameters_update(false);
 
 		hrt_abstime t = hrt_absolute_time();

src/modules/muorb/adsp/module.mk

@@ -42,6 +42,10 @@ SRCS		= \
 		  px4muorb.cpp \
 		  uORBFastRpcChannel.cpp
 endif
+=======
+SRCS		=  param/param.c
+
+SRCS		+= perf_counter.c \
 
 INCLUDE_DIRS	+= \
 		  ${PX4_BASE}/src/modules/uORB

src/modules/muorb/krait/CMakeLists.txt

@@ -46,4 +46,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/modules/navigator/mission_block.cpp

@@ -417,6 +417,32 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 	}
 }
 
+void
+MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
+{
+	item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+
+	/* use current position and use return altitude as clearance */
+	item->lat = _navigator->get_global_position()->lat;
+	item->lon = _navigator->get_global_position()->lon;
+	item->altitude = _navigator->get_global_position()->alt;
+
+	if (min_clearance > 0.0f) {
+		item->altitude += min_clearance;
+	}
+
+	item->altitude_is_relative = false;
+	item->yaw = NAN;
+	item->loiter_radius = _navigator->get_loiter_radius();
+	item->loiter_direction = 1;
+	item->acceptance_radius = (_navigator->get_acceptance_radius() > min_clearance / 2.0f) ?
+					(min_clearance / 2) : _navigator->get_acceptance_radius();
+	item->time_inside = 0.0f;
+	item->pitch_min = min_pitch;
+	item->autocontinue = false;
+	item->origin = ORIGIN_ONBOARD;
+}
+
 void
 MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
 {

src/modules/simulator/simulator_mavlink.cpp

@@ -530,8 +530,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
 	pthread_attr_destroy(&sender_thread_attr);
 
-	mavlink_status_t udp_status = {};
-	mavlink_status_t serial_status = {};
+
+	// set the threads name
+#ifdef __PX4_DARWIN
+	pthread_setname_np("sim_rcv");
+#else
+	pthread_setname_np(pthread_self(), "sim_rcv");
+#endif
 
 	bool sim_delay = false;
 
@@ -575,9 +580,10 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 			if (len > 0) {
 				mavlink_message_t msg;
+				mavlink_status_t status;
 
 				for (int i = 0; i < len; i++) {
-					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) {
+					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) {
 						// have a message, handle it
 						handle_message(&msg, publish);
 					}
@@ -591,9 +597,10 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 			if (len > 0) {
 				mavlink_message_t msg;
+				mavlink_status_t status;
 
 				for (int i = 0; i < len; ++i) {
-					if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
+					if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) {
 						// have a message, handle it
 						handle_message(&msg, true);
 					}

src/modules/systemlib/circuit_breaker_params.c

@@ -42,6 +42,8 @@
  * parameter needs to set to the key (magic).
  */
 
+#ifdef __PX4_QURT
+
 /**
  * Circuit breaker for power supply check
  *

src/modules/systemlib/param/param_shmem.c

@@ -72,7 +72,11 @@
 
 #include "shmem.h"
 
-#define debug(fmt, args...)		do { } while(0)
+#if 0
+# define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
+#else
+# define debug(fmt, args...)		do { } while(0)
+#endif
 
 #define PARAM_OPEN	open
 #define PARAM_CLOSE	close
@@ -85,7 +89,7 @@ extern struct param_info_s	param_array[];
 extern struct param_info_s	*param_info_base;
 extern struct param_info_s	*param_info_limit;
 #else
-// TODO: start and end are reversed
+// FIXME - start and end are reversed
 static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters;
 #endif
 
@@ -110,7 +114,7 @@ const int bits_per_allocation_unit  = (sizeof(*param_changed_storage) * 8);
 extern int get_shmem_lock(void);
 extern void release_shmem_lock(void);
 
-struct param_wbuf_s *param_find_changed(param_t param);
+struct param_wbuf_s * param_find_changed(param_t param);
 
 void init_params(void);
 extern void init_shared_memory(void);
@@ -123,10 +127,9 @@ uint64_t sync_other_prev_time = 0, sync_other_current_time = 0;
 extern void update_to_shmem(param_t param, union param_value_u value);
 extern int update_from_shmem(param_t param, union param_value_u *value);
 static int param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes);
-unsigned char set_called_from_get = 0;
+unsigned char set_called_from_get=0;
 
-static int param_import_done =
-	0; /*at startup, params are loaded from file, if present. we dont want to send notifications that time since muorb is not ready*/
+static int param_import_done=0; /*at startup, params are loaded from file, if present. we dont want to send notifications that time since muorb is not ready*/
 
 static int param_load_default_no_notify(void);
 
@@ -236,6 +239,11 @@ param_find_changed(param_t param)
 	param_assert_locked();
 
 	if (param_values != NULL) {
+#if 0	/* utarray_find requires bsearch, not available */
+		struct param_wbuf_s key;
+		key.param = param;
+		s = utarray_find(param_values, &key, param_compare_values);
+#else
 		while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
 			if (s->param == param) {
 				break;
@@ -501,10 +509,11 @@ param_get(param_t param, void *val)
 
 	union param_value_u value;
 
-	if (update_from_shmem(param, &value)) {
-		set_called_from_get = 1;
+	if(update_from_shmem(param, &value))
+	{
+		set_called_from_get=1;
 		param_set_internal(param, &value, true, false);
-		set_called_from_get = 0;
+		set_called_from_get=0;
 	}
 
 

src/modules/systemlib/system_params.c

@@ -37,6 +37,8 @@
  * System wide parameters
  */
 
+#ifdef __PX4_QURT
+
 /**
  * Auto-start script index.
  *
@@ -111,3 +113,5 @@ PARAM_DEFINE_INT32(SYS_COMPANION, 157600);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_PARAM_VER, 1);
+
+#endif

src/modules/uORB/uORBDevices_posix.cpp

@@ -406,7 +406,6 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 	 * don't match then we might have a visible update.
 	 */
 	while (sd->generation != _generation) {
-
 		/*
 		 * Handle non-rate-limited subscribers.
 		 */