Merge master into cmake-2

ROMFS/px4fmu_common/init.d/rcS

@@ -365,11 +365,6 @@ then
 	then
 	fi
 
-	#
-	# UAVCAN
-	#
-	sh /etc/init.d/rc.uavcan
-
 	#
 	# Sensors System (start before Commander so Preflight checks are properly run)
 	#
@@ -576,6 +571,11 @@ then
 		fi
 	fi
 
+	#
+	# UAVCAN
+	#
+	sh /etc/init.d/rc.uavcan
+
 	#
 	# Logging
 	#

posix-configs/SITL/init/rcS

@@ -53,5 +53,6 @@ mavlink stream -r 80 -s ATTITUDE -u 14556
 mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 mavlink boot_complete
 sdlog2 start -r 100 -e -t -a

src/lib/ecl/validation/data_validator.cpp

@@ -44,9 +44,10 @@
 
 DataValidator::DataValidator(DataValidator *prev_sibling) :
 	_time_last(0),
-	_timeout_interval(70000),
+	_timeout_interval(20000),
 	_event_count(0),
 	_error_count(0),
+	_error_density(0),
 	_priority(0),
 	_mean{0.0f},
 	_lp{0.0f},
@@ -68,6 +69,13 @@ void
 DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
 {
 	_event_count++;
+
+	if (error_count_in > _error_count) {
+		_error_density += (error_count_in - _error_count);
+	} else if (_error_density > 0) {
+		_error_density--;
+	}
+
 	_error_count = error_count_in;
 	_priority = priority_in;
 
@@ -123,7 +131,13 @@ DataValidator::confidence(uint64_t timestamp)
 		return 0.0f;
 	}
 
-	return 1.0f;
+	/* cap error density counter at window size */
+	if (_error_density > ERROR_DENSITY_WINDOW) {
+		_error_density = ERROR_DENSITY_WINDOW;
+	}
+
+	/* return local error density for last N measurements */
+	return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
 }
 
 int
@@ -136,12 +150,13 @@ void
 DataValidator::print()
 {
 	if (_time_last == 0) {
-		ECL_INFO("\tno data\n");
+		ECL_INFO("\tno data");
 		return;
 	}
 
 	for (unsigned i = 0; i < _dimensions; i++) {
-		ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
+		ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f",
-			(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
+			(double) _value[i], (double)_lp[i], (double)_mean[i],
+			(double)_rms[i], (double)confidence(hrt_absolute_time()));
 	}
 }

src/lib/ecl/validation/data_validator.h

@@ -81,6 +81,12 @@ public:
 	 */
 	float*			value() { return _value; }
 
+	/**
+	 * Get the used status of this validator
+	 * @return		true if this validator ever saw data
+	 */
+	bool			used() { return (_time_last > 0); }
+
 	/**
 	 * Get the priority of this validator
 	 * @return		the stored priority
@@ -112,6 +118,7 @@ private:
 	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
 	uint64_t _event_count;			/**< total data counter */
 	uint64_t _error_count;			/**< error count */
+	int _error_density;			/**< ratio between successful reads and errors */
 	int _priority;				/**< sensor nominal priority */
 	float _mean[_dimensions];		/**< mean of value */
 	float _lp[3];				/**< low pass value */
@@ -120,7 +127,8 @@ private:
 	float _value[3];			/**< last value */
 	float _value_equal_count;		/**< equal values in a row */
 	DataValidator *_sibling;		/**< sibling in the group */
-	const unsigned NORETURN_ERRCOUNT = 100;	/**< if the error count reaches this value, return sensor as invalid */
+	const unsigned NORETURN_ERRCOUNT = 10000;	/**< if the error count reaches this value, return sensor as invalid */
+	const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */
 	const unsigned VALUE_EQUAL_COUNT_MAX = 100;	/**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
 
 	/* we don't want this class to be copied */

src/lib/ecl/validation/data_validator_group.cpp

@@ -97,24 +97,35 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 
 	// XXX This should eventually also include voting
 	int pre_check_best = _curr_best;
+	float pre_check_confidence = 1.0f;
+	int pre_check_prio = -1;
 	float max_confidence = -1.0f;
 	int max_priority = -1000;
 	int max_index = -1;
-	uint64_t min_error_count = 30000;
 	DataValidator *best = nullptr;
 
 	unsigned i = 0;
 
 	while (next != nullptr) {
 		float confidence = next->confidence(timestamp);
-		if (confidence > max_confidence ||
+
-			(fabsf(confidence - max_confidence) < 0.01f &&
+		if (i == pre_check_best) {
-				((next->error_count() < min_error_count) &&
+			pre_check_prio = next->priority();
-				(next->priority() >= max_priority)))) {
+			pre_check_confidence = confidence;
+		}
+
+		/*
+		 * Switch if:
+		 * 1) the confidence is higher and priority is equal or higher
+		 * 2) the confidence is no less than 1% different and the priority is higher
+		 */
+		if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
+			(confidence > max_confidence && (next->priority() >= max_priority)) ||
+			(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))
+			) {
 			max_index = i;
 			max_confidence = confidence;
 			max_priority = next->priority();
-			min_error_count = next->error_count();
 			best = next;
 		}
 
@@ -125,17 +136,29 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 	/* the current best sensor is not matching the previous best sensor */
 	if (max_index != _curr_best) {
 
+		bool true_failsafe = true;
+
+		/* check wether the switch was a failsafe or preferring a higher priority sensor */
+		if (pre_check_prio != -1 && pre_check_prio < max_priority &&
+			fabsf(pre_check_confidence - max_confidence) < 0.1f) {
+			/* this is not a failover */
+			true_failsafe = false;
+		}
+
 		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
 		if (_curr_best < 0) {
 			_prev_best = max_index;
 		} else {
 			/* we were initialized before, this is a real failsafe */
 			_prev_best = pre_check_best;
-			_toggle_count++;
 
-			/* if this is the first time, log when we failed */
+			if (true_failsafe) {
-			if (_first_failover_time == 0) {
+				_toggle_count++;
-				_first_failover_time = timestamp;
+
+				/* if this is the first time, log when we failed */
+				if (_first_failover_time == 0) {
+					_first_failover_time = timestamp;
+				}
 			}
 		}
 
@@ -185,8 +208,10 @@ DataValidatorGroup::print()
 	unsigned i = 0;
 
 	while (next != nullptr) {
-		ECL_INFO("sensor #%u:\n", i);
+		if (next->used()) {
-		next->print();
+			ECL_INFO("sensor #%u, prio: %d", i, next->priority());
+			next->print();
+		}
 		next = next->sibling();
 		i++;
 	}

src/lib/ecl/validation/data_validator_group.h

@@ -100,6 +100,7 @@ private:
 	int _prev_best;		/**< the previous best index */
 	uint64_t _first_failover_time;	/**< timestamp where the first failover occured or zero if none occured */
 	unsigned _toggle_count;		/**< number of back and forth switches between two sensors */
+	static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f;
 
 	/* we don't want this class to be copied */
 	DataValidatorGroup(const DataValidatorGroup&);

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -116,4 +116,4 @@ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
  * @min 0.001
  * @max 100
  */
-PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.1f);
+PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f);

src/modules/mavlink/mavlink_main.cpp

@@ -1666,6 +1666,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("GLOBAL_POSITION_INT", 3.0f);
 		configure_stream("LOCAL_POSITION_NED", 3.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 2.0f);
 		configure_stream("RC_CHANNELS", 1.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
@@ -1682,6 +1683,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GPS_RAW_INT", 5.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
 		configure_stream("LOCAL_POSITION_NED", 30.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("ATTITUDE_TARGET", 10.0f);
@@ -1723,7 +1725,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ATTITUDE_TARGET", 8.0f);
 		configure_stream("PARAM_VALUE", 300.0f);
 		configure_stream("MISSION_ITEM", 50.0f);
-		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("DISTANCE_SENSOR", 10.0f);
 		configure_stream("VFR_HUD", 20.0f);

src/modules/simulator/simulator.h

@@ -51,6 +51,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/optical_flow.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
 namespace simulator
@@ -259,11 +260,13 @@ private:
 	orb_advert_t _baro_pub;
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
+	orb_advert_t _flow_pub;
 
 	bool _initialized;
 
 	// class methods
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
+	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
 
 #ifndef __PX4_QURT
 	// uORB publisher handlers

src/modules/simulator/simulator_mavlink.cpp

@@ -245,6 +245,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		update_sensors(&imu);
 		break;
 
+	case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
+		mavlink_hil_optical_flow_t flow;
+		mavlink_msg_hil_optical_flow_decode(msg, &flow);
+		publish_flow_topic(&flow);
+		break;
+
 	case MAVLINK_MSG_ID_HIL_GPS:
 		mavlink_hil_gps_t gps_sim;
 		mavlink_msg_hil_gps_decode(msg, &gps_sim);
@@ -762,3 +768,38 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
 
 	return OK;
 }
+
+int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink)
+{
+	uint64_t timestamp = hrt_absolute_time();
+
+	/* flow */
+	{
+		struct optical_flow_s flow;
+		memset(&flow, 0, sizeof(flow));
+
+		flow.sensor_id = flow_mavlink->sensor_id;
+		flow.timestamp = timestamp;
+		flow.time_since_last_sonar_update = 0;
+		flow.frame_count_since_last_readout = 0; // ?
+		flow.integration_timespan = flow_mavlink->integration_time_us;
+
+		flow.ground_distance_m = flow_mavlink->distance;
+		flow.gyro_temperature = flow_mavlink->temperature;
+		flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
+		flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
+		flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
+		flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
+		flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
+		flow.quality = flow_mavlink->quality;
+
+		if (_flow_pub == nullptr) {
+		_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
+
+		} else {
+		orb_publish(ORB_ID(optical_flow), _flow_pub, &flow);
+		}
+	}
+
+	return OK;
+}