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/PreflightCheck.cpp

@@ -539,6 +539,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		}
 	}
 
+
+#ifdef __PX4_QURT
+	// WARNING: Preflight checks are important and should be added back when
+	// all the sensors are supported
+	PX4_WARN("WARNING: Preflight checks PASS always.");
+	failed = false;
+#endif
+
 	/* Report status */
 	return !failed;
 }

src/modules/commander/commander.cpp

@@ -687,7 +687,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				}
 			}
 
-			if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
+			if ((hil_ret != TRANSITION_DENIED) && (arming_ret != TRANSITION_DENIED) && (main_ret != TRANSITION_DENIED)) {
 				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
@@ -769,7 +769,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
+				mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
 						     (double)cmd->param1,
 						     (double)cmd->param2,
 						     (double)cmd->param3,
@@ -1431,12 +1431,15 @@ int commander_thread_main(int argc, char *argv[])
 	pthread_attr_init(&commander_low_prio_attr);
 	pthread_attr_setstacksize(&commander_low_prio_attr, 2880);
 
+#ifndef __PX4_QURT
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
 
 	/* low priority */
 	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
 	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
+#endif
+
 	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
 	pthread_attr_destroy(&commander_low_prio_attr);
 
@@ -1607,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;
 				}
@@ -2188,8 +2196,13 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* RC input check */
+#ifndef __PX4_QURT
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
 		    (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
+#else
+		// HACK: remove old data check due to timestamp issue in QURT
+		if (!status.rc_input_blocked && sp_man.timestamp != 0) {
+#endif
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
@@ -2969,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

@@ -41,6 +41,11 @@ ifeq ($(PX4_TARGET_OS),qurt)
 SRCS		= \
 		  px4muorb.cpp \
 		  uORBFastRpcChannel.cpp
+SRCS		=  param/param.c 
+endif
+endif
+
+SRCS		+= perf_counter.c \
 
 INCLUDE_DIRS	+= \
 		  ${PX4_BASE}/src/modules/uORB

src/modules/muorb/krait/CMakeLists.txt

@@ -30,9 +30,9 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-if("${DSPAL_STUBS_ENABLE}" STREQUAL "1")
-	include_directories(../krait-stubs)
-endif()
+#if("${DSPAL_STUBS_ENABLE}" STREQUAL "1")
+#	include_directories(../krait-stubs)
+#endif()
 
 px4_add_module(
 	MODULE modules__muorb__krait

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/position_estimator_inav/position_estimator_inav_main.cpp

@@ -81,6 +81,7 @@
 #define MIN_VALID_W 0.00001f
 #define PUB_INTERVAL 10000	// limit publish rate to 100 Hz
 #define EST_BUF_SIZE 250000 / PUB_INTERVAL		// buffer size is 0.5s
+#define MAX_WAIT_FOR_BARO_SAMPLE 3000000 // wait 3 secs for the baro to respond
 
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
@@ -410,10 +411,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	/* wait for initial baro value */
 	bool wait_baro = true;
-
 	TerrainEstimator *terrain_estimator = new TerrainEstimator();
 
 	thread_running = true;
+	hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();
 
 	while (wait_baro && !thread_should_exit) {
 		int ret = px4_poll(&fds_init[0], 1, 1000);
@@ -421,14 +422,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (ret < 0) {
 			/* poll error */
 			mavlink_log_info(mavlink_fd, "[inav] poll error on init");
-			PX4_WARN("INAV poll error");
-
-		} else if (ret > 0) {
+		} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
+			wait_baro = false;
+			mavlink_log_info(mavlink_fd, "[inav] timed out waiting for a baro sample");
+		}
+		else if (ret > 0) {
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
 				if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
 					baro_timestamp = sensor.baro_timestamp[0];
+					baro_wait_for_sample_time = hrt_absolute_time();
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {

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/CMakeLists.txt

@@ -36,7 +36,6 @@ include_directories(${CMAKE_BINARY_DIR}/src/modules/param)
 
 set(SRCS
 	perf_counter.c
-	param/param.c
 	conversions.c
 	cpuload.c
 	pid/pid.c
@@ -57,8 +56,14 @@ if(${OS} STREQUAL "nuttx")
 		printload.c
 		up_cxxinitialize.c 
 		)
+elseif ("${CONFIG_SHMEM}" STREQUAL "1")
+	list(APPEND SRCS
+		param/param_shmem.c
+		print_load_posix.c
+		)
 else()
 	list(APPEND SRCS
+		param/param.c
 		print_load_posix.c
 		)	
 endif()

src/modules/systemlib/bson/tinybson.c

@@ -54,15 +54,9 @@
 #define CODER_CHECK(_c)		do { if (_c->dead) { debug("coder dead"); return -1; }} while(0)
 #define CODER_KILL(_c, _reason)	do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0)
 
-#ifdef __PX4_QURT
-#define BSON_READ  px4_read
-#define BSON_WRITE px4_write
-#define BSON_FSYNC px4_fsync
-#else
 #define BSON_READ  read
 #define BSON_WRITE write
-#define BSON_FSYNC fsync
-#endif
+#define BSON_FSYNC px4_fsync
 
 static int
 read_x(bson_decoder_t decoder, void *p, size_t s)

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/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

@@ -287,6 +287,7 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 	case ORBIOCSETINTERVAL:
 		sd->update_interval = arg;
+		sd->last_update = hrt_absolute_time();
 		return PX4_OK;
 
 	case ORBIOCGADVERTISER:
@@ -405,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.
 		 */
@@ -426,31 +426,17 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 			break;
 		}
 
-		/*
-		 * If the interval timer is still running, the topic should not
-		 * appear updated, even though at this point we know that it has.
-		 * We have previously been through here, so the subscriber
-		 * must have collected the update we reported, otherwise
-		 * update_reported would still be true.
-		 */
-		if (!hrt_called(&sd->update_call)) {
+		// If we have not yet reached the deadline, then assume that we can ignore any
+		// newly received data.
+		if (sd->last_update + sd->update_interval > hrt_absolute_time()) {
 			break;
 		}
 
-		/*
-		 * Make sure that we don't consider the topic to be updated again
-		 * until the interval has passed once more by restarting the interval
-		 * timer and thereby re-scheduling a poll notification at that time.
-		 */
-		hrt_call_after(&sd->update_call,
-			       sd->update_interval,
-			       &uORB::DeviceNode::update_deferred_trampoline,
-			       (void *)this);
-
 		/*
 		 * Remember that we have told the subscriber that there is data.
 		 */
 		sd->update_reported = true;
+		sd->last_update = hrt_absolute_time();
 		ret = true;
 
 		break;

src/modules/uORB/uORBDevices_posix.hpp

@@ -111,6 +111,7 @@ private:
 	struct SubscriberData {
 		unsigned  generation; /**< last generation the subscriber has seen */
 		unsigned  update_interval; /**< if nonzero minimum interval between updates */
+		uint64_t last_update; /**< time at which the last update was provided, used when update_interval is nonzero */
 		struct hrt_call update_call;  /**< deferred wakeup call if update_period is nonzero */
 		void    *poll_priv; /**< saved copy of fds->f_priv while poll is active */
 		bool    update_reported; /**< true if we have reported the update via poll/check */