sensors : decouple maximum sensor count and allow flexible maximums

src/modules/sensors/common.h

@@ -43,6 +43,14 @@
 namespace sensors
 {
 
-static const int SENSOR_COUNT_MAX = 3;
+constexpr uint8_t MAG_COUNT_MAX = 4;
+constexpr uint8_t GYRO_COUNT_MAX = 3;
+constexpr uint8_t ACCEL_COUNT_MAX = 3;
+constexpr uint8_t BARO_COUNT_MAX = 3;
+
+constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX,
+				     math::max(GYRO_COUNT_MAX,
+						     math::max(ACCEL_COUNT_MAX,
+								     BARO_COUNT_MAX)));
 
 } /* namespace sensors */

src/modules/sensors/temperature_compensation.cpp

@@ -54,7 +54,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 	/* rate gyro calibration parameters */
 	parameter_handles.gyro_tc_enable = param_find("TC_G_ENABLE");
 
-	for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
+	for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) {
 		sprintf(nbuf, "TC_G%d_ID", j);
 		parameter_handles.gyro_cal_handles[j].ID = param_find(nbuf);
 
@@ -82,7 +82,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 	/* accelerometer calibration parameters */
 	parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE");
 
-	for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
+	for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
 		sprintf(nbuf, "TC_A%d_ID", j);
 		parameter_handles.accel_cal_handles[j].ID = param_find(nbuf);
 
@@ -110,7 +110,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 	/* barometer calibration parameters */
 	parameter_handles.baro_tc_enable = param_find("TC_B_ENABLE");
 
-	for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
+	for (unsigned j = 0; j < BARO_COUNT_MAX; j++) {
 		sprintf(nbuf, "TC_B%d_ID", j);
 		parameter_handles.baro_cal_handles[j].ID = param_find(nbuf);
 		sprintf(nbuf, "TC_B%d_X5", j);
@@ -152,7 +152,7 @@ int TemperatureCompensation::parameters_update()
 	/* rate gyro calibration parameters */
 	param_get(parameter_handles.gyro_tc_enable, &(_parameters.gyro_tc_enable));
 
-	for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
+	for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) {
 		if (param_get(parameter_handles.gyro_cal_handles[j].ID, &(_parameters.gyro_cal_data[j].ID)) == PX4_OK) {
 			param_get(parameter_handles.gyro_cal_handles[j].ref_temp, &(_parameters.gyro_cal_data[j].ref_temp));
 			param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(_parameters.gyro_cal_data[j].min_temp));
@@ -183,7 +183,7 @@ int TemperatureCompensation::parameters_update()
 	/* accelerometer calibration parameters */
 	param_get(parameter_handles.accel_tc_enable, &(_parameters.accel_tc_enable));
 
-	for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
+	for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
 		if (param_get(parameter_handles.accel_cal_handles[j].ID, &(_parameters.accel_cal_data[j].ID)) == PX4_OK) {
 			param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(_parameters.accel_cal_data[j].ref_temp));
 			param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp));
@@ -214,7 +214,7 @@ int TemperatureCompensation::parameters_update()
 	/* barometer calibration parameters */
 	param_get(parameter_handles.baro_tc_enable, &(_parameters.baro_tc_enable));
 
-	for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
+	for (unsigned j = 0; j < BARO_COUNT_MAX; j++) {
 		if (param_get(parameter_handles.baro_cal_handles[j].ID, &(_parameters.baro_cal_data[j].ID)) == PX4_OK) {
 			param_get(parameter_handles.baro_cal_handles[j].ref_temp, &(_parameters.baro_cal_data[j].ref_temp));
 			param_get(parameter_handles.baro_cal_handles[j].min_temp, &(_parameters.baro_cal_data[j].min_temp));
@@ -323,7 +323,7 @@ int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_in
 		return 0;
 	}
 
-	return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data);
+	return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data, GYRO_COUNT_MAX);
 }
 
 int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance)
@@ -332,7 +332,7 @@ int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_i
 		return 0;
 	}
 
-	return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data);
+	return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data, ACCEL_COUNT_MAX);
 }
 
 int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance)
@@ -341,14 +341,14 @@ int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_in
 		return 0;
 	}
 
-	return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data);
+	return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data, BARO_COUNT_MAX);
 }
 
 template<typename T>
 int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data,
-		const T *sensor_cal_data)
+		const T *sensor_cal_data, uint8_t sensor_count_max)
 {
-	for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
+	for (int i = 0; i < sensor_count_max; ++i) {
 		if (device_id == sensor_cal_data[i].ID) {
 			sensor_data.device_mapping[topic_instance] = i;
 			return i;
@@ -449,7 +449,7 @@ void TemperatureCompensation::print_status()
 	PX4_INFO(" gyro: enabled: %i", _parameters.gyro_tc_enable);
 
 	if (_parameters.gyro_tc_enable == 1) {
-		for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
+		for (int i = 0; i < GYRO_COUNT_MAX; ++i) {
 			uint8_t mapping = _gyro_data.device_mapping[i];
 
 			if (_gyro_data.device_mapping[i] != 255) {
@@ -461,7 +461,7 @@ void TemperatureCompensation::print_status()
 	PX4_INFO(" accel: enabled: %i", _parameters.accel_tc_enable);
 
 	if (_parameters.accel_tc_enable == 1) {
-		for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
+		for (int i = 0; i < ACCEL_COUNT_MAX; ++i) {
 			uint8_t mapping = _accel_data.device_mapping[i];
 
 			if (_accel_data.device_mapping[i] != 255) {
@@ -473,7 +473,7 @@ void TemperatureCompensation::print_status()
 	PX4_INFO(" baro: enabled: %i", _parameters.baro_tc_enable);
 
 	if (_parameters.baro_tc_enable == 1) {
-		for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
+		for (int i = 0; i < BARO_COUNT_MAX; ++i) {
 			uint8_t mapping = _baro_data.device_mapping[i];
 
 			if (_baro_data.device_mapping[i] != 255) {

src/modules/sensors/temperature_compensation.h

@@ -49,8 +49,12 @@
 namespace sensors
 {
 
-static_assert(SENSOR_COUNT_MAX == 3,
+static_assert(GYRO_COUNT_MAX == 3,
-	      "SENSOR_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
+	      "GYRO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
+static_assert(ACCEL_COUNT_MAX == 3,
+	      "ACCEL_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
+static_assert(BARO_COUNT_MAX == 3,
+	      "BARO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
 
 /**
  ** class TemperatureCompensation
@@ -182,21 +186,21 @@ private:
 	// create a struct containing all thermal calibration parameters
 	struct Parameters {
 		int gyro_tc_enable;
-		SensorCalData3D gyro_cal_data[SENSOR_COUNT_MAX];
+		SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX];
 		int accel_tc_enable;
-		SensorCalData3D accel_cal_data[SENSOR_COUNT_MAX];
+		SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX];
 		int baro_tc_enable;
-		SensorCalData1D baro_cal_data[SENSOR_COUNT_MAX];
+		SensorCalData1D baro_cal_data[BARO_COUNT_MAX];
 	};
 
 	// create a struct containing the handles required to access all calibration parameters
 	struct ParameterHandles {
 		param_t gyro_tc_enable;
-		SensorCalHandles3D gyro_cal_handles[SENSOR_COUNT_MAX];
+		SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX];
 		param_t accel_tc_enable;
-		SensorCalHandles3D accel_cal_handles[SENSOR_COUNT_MAX];
+		SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX];
 		param_t baro_tc_enable;
-		SensorCalHandles1D baro_cal_handles[SENSOR_COUNT_MAX];
+		SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX];
 	};
 
 
@@ -268,7 +272,7 @@ private:
 
 	template<typename T>
 	static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data,
-					const T *sensor_cal_data);
+					const T *sensor_cal_data, uint8_t sensor_count_max);
 };
 
 }

src/modules/sensors/voted_sensors_update.cpp

@@ -105,10 +105,10 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
 
 void VotedSensorsUpdate::initialize_sensors()
 {
-	init_sensor_class(ORB_ID(sensor_gyro), _gyro);
+	init_sensor_class(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX);
-	init_sensor_class(ORB_ID(sensor_mag), _mag);
+	init_sensor_class(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
-	init_sensor_class(ORB_ID(sensor_accel), _accel);
+	init_sensor_class(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX);
-	init_sensor_class(ORB_ID(sensor_baro), _baro);
+	init_sensor_class(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX);
 }
 
 void VotedSensorsUpdate::deinit()
@@ -148,9 +148,9 @@ void VotedSensorsUpdate::parameters_update()
 	/* temperature compensation */
 	_temperature_compensation.parameters_update();
 
-	for (unsigned topic_instance = 0; topic_instance < SENSOR_COUNT_MAX; ++topic_instance) {
+	/* gyro */
+	for (unsigned topic_instance = 0; topic_instance < GYRO_COUNT_MAX; ++topic_instance) {
 
-		/* gyro */
 		if (topic_instance < _gyro.subscription_count) {
 			// valid subscription, so get the driver id by getting the published sensor data
 			struct gyro_report report;
@@ -169,8 +169,12 @@ void VotedSensorsUpdate::parameters_update()
 				}
 			}
 		}
+	}
+
+
+	/* accel */
+	for (unsigned topic_instance = 0; topic_instance < ACCEL_COUNT_MAX; ++topic_instance) {
 
-		/* accel */
 		if (topic_instance < _accel.subscription_count) {
 			// valid subscription, so get the driver id by getting the published sensor data
 			struct accel_report report;
@@ -189,8 +193,11 @@ void VotedSensorsUpdate::parameters_update()
 				}
 			}
 		}
+	}
+
+	/* baro */
+	for (unsigned topic_instance = 0; topic_instance < BARO_COUNT_MAX; ++topic_instance) {
 
-		/* baro */
 		if (topic_instance < _baro.subscription_count) {
 			// valid subscription, so get the driver id by getting the published sensor data
 			struct baro_report report;
@@ -222,7 +229,7 @@ void VotedSensorsUpdate::parameters_update()
 	unsigned accel_cal_found_count = 0;
 
 	/* run through all gyro sensors */
-	for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
+	for (unsigned s = 0; s < GYRO_COUNT_MAX; s++) {
 
 		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 
@@ -237,7 +244,7 @@ void VotedSensorsUpdate::parameters_update()
 		bool config_ok = false;
 
 		/* run through all stored calibrations that are applied at the driver level*/
-		for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+		for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) {
 			/* initially status is ok per config */
 			failed = false;
 
@@ -296,7 +303,7 @@ void VotedSensorsUpdate::parameters_update()
 	if (gyro_count < gyro_cal_found_count) {
 
 		// run through all stored calibrations and reset them
-		for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+		for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) {
 
 			int device_id = 0;
 			(void)sprintf(str, "CAL_GYRO%u_ID", i);
@@ -305,7 +312,7 @@ void VotedSensorsUpdate::parameters_update()
 	}
 
 	/* run through all accel sensors */
-	for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
+	for (unsigned s = 0; s < ACCEL_COUNT_MAX; s++) {
 
 		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 
@@ -320,7 +327,7 @@ void VotedSensorsUpdate::parameters_update()
 		bool config_ok = false;
 
 		/* run through all stored calibrations */
-		for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+		for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) {
 			/* initially status is ok per config */
 			failed = false;
 
@@ -379,7 +386,7 @@ void VotedSensorsUpdate::parameters_update()
 	if (accel_count < accel_cal_found_count) {
 
 		// run through all stored calibrations and reset them
-		for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+		for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) {
 
 			int device_id = 0;
 			(void)sprintf(str, "CAL_ACC%u_ID", i);
@@ -388,7 +395,7 @@ void VotedSensorsUpdate::parameters_update()
 	}
 
 	/* run through all mag sensors */
-	for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
+	for (unsigned s = 0; s < MAG_COUNT_MAX; s++) {
 
 		/* set a valid default rotation (same as board).
 		 * if the mag is configured, this might be replaced
@@ -410,7 +417,7 @@ void VotedSensorsUpdate::parameters_update()
 		bool config_ok = false;
 
 		/* run through all stored calibrations */
-		for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+		for (unsigned i = 0; i < MAG_COUNT_MAX; i++) {
 			/* initially status is ok per config */
 			failed = false;
 
@@ -936,12 +943,14 @@ bool VotedSensorsUpdate::check_vibration()
 	return ret;
 }
 
-void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data)
+void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data,
+		uint8_t sensor_count_max)
 {
 	unsigned group_count = orb_group_count(meta);
 
-	if (group_count > SENSOR_COUNT_MAX) {
+	if (group_count > sensor_count_max) {
-		group_count = SENSOR_COUNT_MAX;
+		PX4_WARN("Detected %u %s sensors, but will only use %u", group_count, meta->o_name, sensor_count_max);
+		group_count = sensor_count_max;
 	}
 
 	for (unsigned i = 0; i < group_count; i++) {

src/modules/sensors/voted_sensors_update.h

@@ -166,7 +166,7 @@ private:
 		unsigned int last_failover_count;
 	};
 
-	void	init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data);
+	void	init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max);
 
 	/**
 	 * Poll the accelerometer for updated data.
@@ -237,7 +237,6 @@ private:
 	 */
 	bool apply_mag_calibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id);
 
-
 	SensorData _gyro;
 	SensorData _accel;
 	SensorData _mag;
@@ -245,18 +244,18 @@ private:
 
 	orb_advert_t	_mavlink_log_pub = nullptr;
 
-	float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */
+	float _last_baro_pressure[BARO_COUNT_MAX]; /**< pressure from last baro sensors */
 	float _last_best_baro_pressure = 0.0f; /**< pressure from last best baro */
 	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */
-	uint64_t _last_accel_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
+	uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX]; /**< latest full timestamp */
-	uint64_t _last_mag_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
+	uint64_t _last_mag_timestamp[MAG_COUNT_MAX]; /**< latest full timestamp */
-	uint64_t _last_baro_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
+	uint64_t _last_baro_timestamp[BARO_COUNT_MAX]; /**< latest full timestamp */
 
 	hrt_abstime _vibration_warning_timestamp = 0;
 	bool _vibration_warning = false;
 
 	math::Matrix<3, 3>	_board_rotation = {};	/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix<3, 3>	_mag_rotation[SENSOR_COUNT_MAX] = {};	/**< rotation matrix for the orientation that the external mag0 is mounted */
+	math::Matrix<3, 3>	_mag_rotation[MAG_COUNT_MAX] = {};	/**< rotation matrix for the orientation that the external mag0 is mounted */
 
 	const Parameters &_parameters;
 	const bool _hil_enabled; /**< is hardware-in-the-loop mode enabled? */