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

ROMFS/px4fmu_common/init.d/4009_qav250

@@ -0,0 +1,34 @@
+#!nsh
+#
+# @name Lumenier QAV250
+#
+# @type Quadrotor x
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Mark Whitehorn <kd0aij@gmail.com>
+#
+
+sh /etc/init.d/4001_quad_x
+
+if [ $AUTOCNF == yes ]
+then
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.08
+	param set MC_ROLLRATE_I 0.05
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.08
+	param set MC_PITCHRATE_I 0.1
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
+	param set PWM_MIN 1075
+	param set PWM_THR_MIN 0.06
+	param set PWM_MANTHR_MIN 0.06
+fi
+

src/drivers/device/vdev.cpp

@@ -67,6 +67,7 @@ private:
 
 #define PX4_MAX_DEV 500
 static px4_dev_t *devmap[PX4_MAX_DEV];
+pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER;
 
 /*
  * The standard NuttX operation dispatch table can't call C++ member functions
@@ -142,8 +143,12 @@ VDev::register_driver(const char *name, void *data)
 
 	// Make sure the device does not already exist
 	// FIXME - convert this to a map for efficiency
+
+	pthread_mutex_lock(&devmutex);
+
 	for (int i = 0; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) {
+			pthread_mutex_unlock(&devmutex);
 			return -EEXIST;
 		}
 	}
@@ -157,6 +162,8 @@ VDev::register_driver(const char *name, void *data)
 		}
 	}
 
+	pthread_mutex_unlock(&devmutex);
+
 	if (ret != PX4_OK) {
 		PX4_ERR("No free devmap entries - increase PX4_MAX_DEV");
 	}
@@ -174,6 +181,8 @@ VDev::unregister_driver(const char *name)
 		return -EINVAL;
 	}
 
+	pthread_mutex_lock(&devmutex);
+
 	for (int i = 0; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) {
 			delete devmap[i];
@@ -184,6 +193,8 @@ VDev::unregister_driver(const char *name)
 		}
 	}
 
+	pthread_mutex_unlock(&devmutex);
+
 	return ret;
 }
 
@@ -194,15 +205,20 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc
 	char name[32];
 	snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
 
+	pthread_mutex_lock(&devmutex);
+
 	for (int i = 0; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strcmp(devmap[i]->name, name) == 0) {
 			delete devmap[i];
 			PX4_DEBUG("Unregistered class DEV %s", name);
 			devmap[i] = NULL;
+			pthread_mutex_unlock(&devmutex);
 			return PX4_OK;
 		}
 	}
 
+	pthread_mutex_unlock(&devmutex);
+
 	return -EINVAL;
 }
 
@@ -497,15 +513,20 @@ VDev *VDev::getDev(const char *path)
 	PX4_DEBUG("VDev::getDev");
 	int i = 0;
 
+	pthread_mutex_lock(&devmutex);
+
 	for (; i < PX4_MAX_DEV; ++i) {
 		//if (devmap[i]) {
 		//	printf("%s %s\n", devmap[i]->name, path);
 		//}
 		if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) {
+			pthread_mutex_unlock(&devmutex);
 			return (VDev *)(devmap[i]->cdev);
 		}
 	}
 
+	pthread_mutex_unlock(&devmutex);
+
 	return NULL;
 }
 
@@ -514,11 +535,15 @@ void VDev::showDevices()
 	int i = 0;
 	PX4_INFO("Devices:");
 
+	pthread_mutex_lock(&devmutex);
+
 	for (; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
 			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
+
+	pthread_mutex_unlock(&devmutex);
 }
 
 void VDev::showTopics()
@@ -526,11 +551,15 @@ void VDev::showTopics()
 	int i = 0;
 	PX4_INFO("Devices:");
 
+	pthread_mutex_lock(&devmutex);
+
 	for (; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
 			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
+
+	pthread_mutex_unlock(&devmutex);
 }
 
 void VDev::showFiles()
@@ -538,12 +567,16 @@ void VDev::showFiles()
 	int i = 0;
 	PX4_INFO("Files:");
 
+	pthread_mutex_lock(&devmutex);
+
 	for (; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 &&
 		    strncmp(devmap[i]->name, "/dev/", 5) != 0) {
 			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
+
+	pthread_mutex_unlock(&devmutex);
 }
 
 const char *VDev::topicList(unsigned int *next)

src/drivers/px4io/px4io.cpp

@@ -2090,12 +2090,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 
 		} while (buflen > 0);
 
-		/* ensure a closing newline */
+		int ret;
+
+		/* send the closing newline */
 		msg->text[0] = '\n';
 		msg->text[1] = '\0';
 
-		int ret;
-
 		for (int i = 0; i < 30; i++) {
 			/* failed, but give it a 2nd shot */
 			ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
@@ -2108,27 +2108,24 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 			}
 		}
 
-		if (ret) {
+		if (ret == 0) {
-			return ret;
+			/* success, exit */
+			break;
 		}
 
 		retries--;
 
-		DEVICE_LOG("mixer sent");
+	} while (retries > 0);
 
-	} while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
+	if (retries == 0) {
+		mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail");
+		/* load must have failed for some reason */
+		return -EINVAL;
 
-	/* check for the mixer-OK flag */
+	} else {
-	if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
+		/* all went well, set the mixer ok flag */
-		mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
+		return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK);
-		return 0;
 	}
-
-	DEVICE_LOG("mixer rejected by IO");
-	mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
-
-	/* load must have failed for some reason */
-	return -EINVAL;
 }
 
 void

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -56,6 +56,8 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vision_position_estimate.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -117,22 +119,27 @@ private:
 
 	int		_sensors_sub = -1;
 	int		_params_sub = -1;
+	int		_vision_sub = -1;
+	int		_mocap_sub = -1;
 	int		_global_pos_sub = -1;
 	orb_advert_t	_att_pub = nullptr;
 
 	struct {
 		param_t	w_acc;
 		param_t	w_mag;
+		param_t	w_ext_hdg;
 		param_t	w_gyro_bias;
 		param_t	mag_decl;
 		param_t	mag_decl_auto;
 		param_t	acc_comp;
 		param_t	bias_max;
 		param_t vibe_thresh;
+		param_t	ext_hdg_mode;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	float		_w_accel = 0.0f;
 	float		_w_mag = 0.0f;
+	float		_w_ext_hdg = 0.0f;
 	float		_w_gyro_bias = 0.0f;
 	float		_mag_decl = 0.0f;
 	bool		_mag_decl_auto = false;
@@ -140,11 +147,18 @@ private:
 	float		_bias_max = 0.0f;
 	float		_vibration_warning_threshold = 1.0f;
 	hrt_abstime	_vibration_warning_timestamp = 0;
+	int		_ext_hdg_mode = 0;
 
 	Vector<3>	_gyro;
 	Vector<3>	_accel;
 	Vector<3>	_mag;
 
+	vision_position_estimate_s _vision = {};
+	Vector<3>	_vision_hdg;
+
+	att_pos_mocap_s _mocap = {};
+	Vector<3>	_mocap_hdg;
+
 	Quaternion	_q;
 	Vector<3>	_rates;
 	Vector<3>	_gyro_bias;
@@ -168,6 +182,7 @@ private:
 	bool		_data_good = false;
 	bool		_failsafe = false;
 	bool		_vibration_warning = false;
+	bool		_ext_hdg_good = false;
 
 	int		_mavlink_fd = -1;
 
@@ -196,12 +211,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 
 	_params_handles.w_acc		= param_find("ATT_W_ACC");
 	_params_handles.w_mag		= param_find("ATT_W_MAG");
+	_params_handles.w_ext_hdg	= param_find("ATT_W_EXT_HDG");
 	_params_handles.w_gyro_bias	= param_find("ATT_W_GYRO_BIAS");
 	_params_handles.mag_decl	= param_find("ATT_MAG_DECL");
 	_params_handles.mag_decl_auto	= param_find("ATT_MAG_DECL_A");
 	_params_handles.acc_comp	= param_find("ATT_ACC_COMP");
 	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
 	_params_handles.vibe_thresh	= param_find("ATT_VIBE_THRESH");
+	_params_handles.ext_hdg_mode	= param_find("ATT_EXT_HDG_M");
 }
 
 /**
@@ -269,6 +286,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
 void AttitudeEstimatorQ::task_main()
 {
 	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
+
+	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
+	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
+
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
@@ -374,6 +395,47 @@ void AttitudeEstimatorQ::task_main()
 			}
 		}
 
+		// Update vision and motion capture heading
+		bool vision_updated = false;
+		orb_check(_vision_sub, &vision_updated);
+
+		bool mocap_updated = false;
+		orb_check(_mocap_sub, &mocap_updated);
+
+		if (vision_updated) {
+			orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
+			math::Quaternion q(_vision.q);
+
+			math::Matrix<3, 3> Rvis = q.to_dcm();
+			math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+			// Rvis is Rwr (robot respect to world) while v is respect to world.
+			// Hence Rvis must be transposed having (Rwr)' * Vw
+			// Rrw * Vw = vn. This way we have consistency
+			_vision_hdg = Rvis.transposed() * v;
+		}
+
+		if (mocap_updated) {
+			orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
+			math::Quaternion q(_mocap.q);
+			math::Matrix<3, 3> Rmoc = q.to_dcm();
+
+			math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+			// Rmoc is Rwr (robot respect to world) while v is respect to world.
+			// Hence Rmoc must be transposed having (Rwr)' * Vw
+			// Rrw * Vw = vn. This way we have consistency
+			_mocap_hdg = Rmoc.transposed() * v;
+		}
+
+		// Check for timeouts on data
+		if (_ext_hdg_mode == 1) {
+			_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
+
+		} else if (_ext_hdg_mode == 2) {
+			_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
+		}
+
 		bool gpos_updated;
 		orb_check(_global_pos_sub, &gpos_updated);
 
@@ -478,6 +540,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 
 		param_get(_params_handles.w_acc, &_w_accel);
 		param_get(_params_handles.w_mag, &_w_mag);
+		param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
 		param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
 		float mag_decl_deg = 0.0f;
 		param_get(_params_handles.mag_decl, &mag_decl_deg);
@@ -490,6 +553,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 		_acc_comp = acc_comp_int != 0;
 		param_get(_params_handles.bias_max, &_bias_max);
 		param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
+		param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
 	}
 }
 
@@ -545,12 +609,34 @@ bool AttitudeEstimatorQ::update(float dt)
 	// Angular rate of correction
 	Vector<3> corr;
 
-	// Magnetometer correction
+	if (_ext_hdg_mode > 0 && _ext_hdg_good) {
-	// Project mag field vector to global frame and extract XY component
+		if (_ext_hdg_mode == 1) {
-	Vector<3> mag_earth = _q.conjugate(_mag);
+			// Vision heading correction
-	float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
+			// Project heading to global frame and extract XY component
-	// Project magnetometer correction to body frame
+			Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
-	corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
+			float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
+			// Project correction to body frame
+			corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
+		}
+
+		if (_ext_hdg_mode == 2) {
+			// Mocap heading correction
+			// Project heading to global frame and extract XY component
+			Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
+			float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
+			// Project correction to body frame
+			corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
+		}
+	}
+
+	if (_ext_hdg_mode == 0  || !_ext_hdg_good) {
+		// Magnetometer correction
+		// Project mag field vector to global frame and extract XY component
+		Vector<3> mag_earth = _q.conjugate(_mag);
+		float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
+		// Project magnetometer correction to body frame
+		corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
+	}
 
 	// Accelerometer correction
 	// Project 'k' unit vector of earth frame to body frame

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
  */
 PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
 
+/**
+ * Complimentary filter external heading weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
+
 /**
  * Complimentary filter gyroscope bias weight
  *
@@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
  */
 PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
 
+/**
+ * External heading usage mode (from Motion capture/Vision)
+ * Set to 1 to use heading estimate from vision.
+ * Set to 2 to use heading from motion capture.
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 2
+ */
+PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
+
 /**
  * Enable acceleration compensation based on GPS
  * velocity.

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -855,18 +855,45 @@ void MulticopterPositionControl::control_auto(float dt)
 		}
 	}
 
+	bool current_setpoint_valid = false;
+	bool previous_setpoint_valid = false;
+
+	math::Vector<3> prev_sp;
+	math::Vector<3> curr_sp;
+
 	if (_pos_sp_triplet.current.valid) {
-		/* in case of interrupted mission don't go to waypoint but stay at current position */
-		_reset_pos_sp = true;
-		_reset_alt_sp = true;
 
 		/* project setpoint to local frame */
-		math::Vector<3> curr_sp;
 		map_projection_project(&_ref_pos,
 				       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
 				       &curr_sp.data[0], &curr_sp.data[1]);
 		curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
 
+		if (PX4_ISFINITE(curr_sp(0)) &&
+			PX4_ISFINITE(curr_sp(1)) &&
+			PX4_ISFINITE(curr_sp(2))) {
+			current_setpoint_valid = true;
+		}
+	}
+
+	if (_pos_sp_triplet.previous.valid) {
+		map_projection_project(&_ref_pos,
+					   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
+					   &prev_sp.data[0], &prev_sp.data[1]);
+		prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
+
+		if (PX4_ISFINITE(prev_sp(0)) &&
+			PX4_ISFINITE(prev_sp(1)) &&
+			PX4_ISFINITE(prev_sp(2))) {
+			previous_setpoint_valid = true;
+		}
+	}
+
+	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
 
@@ -876,13 +903,8 @@ void MulticopterPositionControl::control_auto(float dt)
 		/* by default use current setpoint as is */
 		math::Vector<3> pos_sp_s = curr_sp_s;
 
-		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
+		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) {
 			/* follow "previous - current" line */
-			math::Vector<3> prev_sp;
-			map_projection_project(&_ref_pos,
-						   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
-						   &prev_sp.data[0], &prev_sp.data[1]);
-			prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
 
 			if ((curr_sp - prev_sp).length() > MIN_DIST) {
 

src/modules/px4iofirmware/mixer.cpp

@@ -88,9 +88,6 @@ static int	mixer_callback(uintptr_t handle,
 
 static MixerGroup mixer_group(mixer_callback, 0);
 
-/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
-static void mixer_set_failsafe();
-
 void
 mixer_tick(void)
 {
@@ -479,15 +476,6 @@ mixer_handle_text(const void *buffer, size_t length)
 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
 
-			/* only set mixer ok if no residual is left over */
-			if (resid == 0) {
-				r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
-
-			} else {
-				/* not yet reached the end of the mixer, set as not ok */
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-			}
-
 			isr_debug(2, "used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
@@ -496,9 +484,6 @@ mixer_handle_text(const void *buffer, size_t length)
 			}
 
 			mixer_text_length = resid;
-
-			/* update failsafe values */
-			mixer_set_failsafe();
 		}
 
 		break;
@@ -507,7 +492,7 @@ mixer_handle_text(const void *buffer, size_t length)
 	return 0;
 }
 
-static void
+void
 mixer_set_failsafe()
 {
 	/*

src/modules/px4iofirmware/px4io.h

@@ -190,6 +190,8 @@ extern pwm_limit_t pwm_limit;
  */
 extern void	mixer_tick(void);
 extern int	mixer_handle_text(const void *buffer, size_t length);
+/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
+extern void	mixer_set_failsafe(void);
 
 /**
  * Safety switch/LED.

src/modules/px4iofirmware/registers.c

@@ -469,8 +469,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * Allow FMU override of arming state (to allow in-air restores),
 			 * but only if the arming state is not in sync on the IO side.
 			 */
-			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+
+			if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
+			} else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 				r_status_flags = value;
+
+			}
+
+			if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
+
+				/* update failsafe values, now that the mixer is set to ok */
+				mixer_set_failsafe();
 			}
 
 			break;