move initial sensor priority to parameters and purge ORB_PRIORITY

- CAL_ACCx_EN -> CAL_ACCx_PRIO
 - CAL_GYROx_EN -> CAL_GYROx_PRIO
 - CAL_MAGx_EN -> CAL_MAGx_PRIO

src/lib/battery/battery.cpp

@@ -186,7 +186,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 void
 Battery::publish()
 {
-	orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT);
+	orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance);
 }
 
 void

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -63,10 +63,10 @@ static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit,
 	return clip_count;
 }
 
-PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
+PX4Accelerometer::PX4Accelerometer(uint32_t device_id, enum Rotation rotation) :
 	ModuleParams(nullptr),
-	_sensor_pub{ORB_ID(sensor_accel), priority},
-	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
+	_sensor_pub{ORB_ID(sensor_accel)},
+	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo)},
 	_device_id{device_id},
 	_rotation{rotation}
 {

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -44,7 +44,7 @@
 class PX4Accelerometer : public ModuleParams
 {
 public:
-	PX4Accelerometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
+	PX4Accelerometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
 	~PX4Accelerometer() override;
 
 	uint32_t get_device_id() const { return _device_id; }

src/lib/drivers/barometer/PX4Barometer.cpp

@@ -36,9 +36,9 @@
 
 #include <lib/drivers/device/Device.hpp>
 
-PX4Barometer::PX4Barometer(uint32_t device_id, ORB_PRIO priority) :
+PX4Barometer::PX4Barometer(uint32_t device_id) :
 	CDev(nullptr),
-	_sensor_baro_pub{ORB_ID(sensor_baro), priority}
+	_sensor_baro_pub{ORB_ID(sensor_baro)}
 {
 	_class_device_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
 	_sensor_baro_pub.advertise();

src/lib/drivers/barometer/PX4Barometer.hpp

@@ -45,7 +45,7 @@ class PX4Barometer : public cdev::CDev
 {
 
 public:
-	PX4Barometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT);
+	PX4Barometer(uint32_t device_id);
 	~PX4Barometer() override;
 
 	const sensor_baro_s &get() { return _sensor_baro_pub.get(); }

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -50,10 +50,10 @@ static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
 	return sum;
 }
 
-PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
+PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) :
 	ModuleParams(nullptr),
-	_sensor_pub{ORB_ID(sensor_gyro), priority},
-	_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
+	_sensor_pub{ORB_ID(sensor_gyro)},
+	_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)},
 	_device_id{device_id},
 	_rotation{rotation}
 {

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -43,7 +43,7 @@
 class PX4Gyroscope : public ModuleParams
 {
 public:
-	PX4Gyroscope(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
+	PX4Gyroscope(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
 	~PX4Gyroscope() override;
 
 	uint32_t get_device_id() const { return _device_id; }

src/lib/drivers/magnetometer/PX4Magnetometer.cpp

@@ -36,9 +36,9 @@
 
 #include <lib/drivers/device/Device.hpp>
 
-PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
+PX4Magnetometer::PX4Magnetometer(uint32_t device_id, enum Rotation rotation) :
 	CDev(nullptr),
-	_sensor_pub{ORB_ID(sensor_mag), priority},
+	_sensor_pub{ORB_ID(sensor_mag)},
 	_device_id{device_id},
 	_rotation{rotation}
 {

src/lib/drivers/magnetometer/PX4Magnetometer.hpp

@@ -43,7 +43,7 @@
 class PX4Magnetometer : public cdev::CDev
 {
 public:
-	PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
+	PX4Magnetometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
 	~PX4Magnetometer() override;
 
 	bool external() { return _external; }

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -35,8 +35,8 @@
 
 #include <lib/drivers/device/Device.hpp>
 
-PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const ORB_PRIO priority, const uint8_t device_orientation) :
-	_distance_sensor_pub{ORB_ID(distance_sensor), priority}
+PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_orientation) :
+	_distance_sensor_pub{ORB_ID(distance_sensor)}
 {
 	_distance_sensor_pub.advertise();
 

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -44,7 +44,6 @@ class PX4Rangefinder
 
 public:
 	PX4Rangefinder(const uint32_t device_id,
-		       const ORB_PRIO priority = ORB_PRIO_DEFAULT,
 		       const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	~PX4Rangefinder();
 

src/lib/mixer_module/mixer_module.hpp

@@ -238,8 +238,8 @@ private:
 	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
 	uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
-	uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs), ORB_PRIO_DEFAULT};
-	uORB::PublicationMulti<multirotor_motor_limits_s> _to_mixer_status{ORB_ID(multirotor_motor_limits), ORB_PRIO_DEFAULT}; 	///< mixer status flags
+	uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};
+	uORB::PublicationMulti<multirotor_motor_limits_s> _to_mixer_status{ORB_ID(multirotor_motor_limits)}; 	///< mixer status flags
 
 	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
 	actuator_armed_s _armed{};

src/lib/parameters/param_translation.cpp

@@ -60,6 +60,44 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2020-06-29 (v1.11 beta): translate CAL_ACCx_EN/CAL_GYROx_EN/CAL_MAGx_EN -> CAL_ACCx_PRIO/CAL_GYROx_PRIO/CAL_MAGx_PRIO
+	if (node->type == BSON_INT32) {
+
+		const char *cal_sensor_en_params[] = {
+			"CAL_ACC0_EN",
+			"CAL_ACC1_EN",
+			"CAL_ACC2_EN",
+			"CAL_GYRO0_EN",
+			"CAL_GYRO1_EN",
+			"CAL_GYRO2_EN",
+			"CAL_MAG0_EN",
+			"CAL_MAG1_EN",
+			"CAL_MAG2_EN",
+			"CAL_MAG3_EN",
+		};
+
+		for (int i = 0; i < sizeof(cal_sensor_en_params) / sizeof(cal_sensor_en_params[0]); ++i) {
+			if (strcmp(cal_sensor_en_params[i], node->name) == 0) {
+
+				char new_parameter_name[17] {};
+				strcpy(new_parameter_name, cal_sensor_en_params[i]);
+
+				char *str_replace = strstr(new_parameter_name, "_EN");
+
+				if (str_replace != nullptr) {
+					strcpy(str_replace, "_PRIO");
+					PX4_INFO("%s -> %s", cal_sensor_en_params[i], new_parameter_name);
+					strcpy(node->name, new_parameter_name);
+				}
+
+				// if sensor wasn't disabled, reset to -1 so that it can be set to an appropriate default
+				if (node->i != 0) {
+					node->i = -1; // special value to process later
+				}
+			}
+		}
+	}
+
 
 	// translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10)
 
@@ -87,10 +125,7 @@ bool param_modify_on_import(bson_node_t node)
 		"CAL_MAG2_ID",
 		"TC_A2_ID",
 		"TC_B2_ID",
-		"TC_G2_ID",
-		"CAL_ACC_PRIME",
-		"CAL_GYRO_PRIME",
-		"CAL_MAG_PRIME",
+		"TC_G2_ID"
 	};
 	bool found = false;
 

src/lib/sensor_calibration/Accelerometer.cpp

@@ -118,9 +118,15 @@ void Accelerometer::ParametersUpdate()
 			_rotation.setIdentity();
 		}
 
-		// CAL_ACCx_EN
-		int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index);
-		_enabled = (enabled == 1);
+		// CAL_ACCx_PRIO
+		_priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index);
+
+		if (_priority < 0 || _priority > 100) {
+			// reset to default
+			PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, DEFAULT_PRIORITY);
+			SetCalibrationParam(SensorString(), "PRIO", _calibration_index, DEFAULT_PRIORITY);
+			_priority = DEFAULT_PRIORITY;
+		}
 
 		// CAL_ACCx_OFF{X,Y,Z}
 		_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
@@ -140,7 +146,7 @@ void Accelerometer::Reset()
 	_scale = Vector3f{1.f, 1.f, 1.f};
 	_thermal_offset.zero();
 
-	_enabled = true;
+	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 
 	_calibration_index = -1;
 }
@@ -150,10 +156,17 @@ bool Accelerometer::ParametersSave()
 	if (_calibration_index >= 0) {
 		// save calibration
 		SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
-		SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0);
+		SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
 		SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
 		SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale);
 
+		// if (_external) {
+		// 	SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
+
+		// } else {
+		// 	SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
+		// }
+
 		return true;
 	}
 

src/lib/sensor_calibration/Accelerometer.hpp

@@ -47,6 +47,9 @@ class Accelerometer
 public:
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
+
 	static constexpr const char *SensorString() { return "ACC"; }
 
 	Accelerometer();
@@ -63,8 +66,9 @@ public:
 	void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
 
 	uint32_t device_id() const { return _device_id; }
-	bool enabled() const { return _enabled; }
+	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }
+	const int32_t &priority() const { return _priority; }
 	const matrix::Dcmf &rotation() const { return _rotation; }
 
 	// apply offsets and scale
@@ -91,8 +95,8 @@ private:
 
 	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
+	int32_t _priority{DEFAULT_PRIORITY};
 
-	bool _enabled{true};
 	bool _external{false};
 };
 } // namespace calibration

src/lib/sensor_calibration/Gyroscope.cpp

@@ -118,9 +118,15 @@ void Gyroscope::ParametersUpdate()
 			_rotation.setIdentity();
 		}
 
-		// CAL_GYROx_EN
-		int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index);
-		_enabled = (enabled == 1);
+		// CAL_GYROx_PRIO
+		_priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index);
+
+		if (_priority < 0 || _priority > 100) {
+			// reset to default
+			PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, DEFAULT_PRIORITY);
+			SetCalibrationParam(SensorString(), "PRIO", _calibration_index, DEFAULT_PRIORITY);
+			_priority = DEFAULT_PRIORITY;
+		}
 
 		// CAL_GYROx_OFF{X,Y,Z}
 		_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
@@ -136,7 +142,7 @@ void Gyroscope::Reset()
 	_offset.zero();
 	_thermal_offset.zero();
 
-	_enabled = true;
+	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 
 	_calibration_index = -1;
 }
@@ -146,9 +152,16 @@ bool Gyroscope::ParametersSave()
 	if (_calibration_index >= 0) {
 		// save calibration
 		SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
-		SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0);
+		SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
 		SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
 
+		// if (_external) {
+		// 	SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
+
+		// } else {
+		// 	SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
+		// }
+
 		return true;
 	}
 

src/lib/sensor_calibration/Gyroscope.hpp

@@ -47,6 +47,9 @@ class Gyroscope
 public:
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
+
 	static constexpr const char *SensorString() { return "GYRO"; }
 
 	Gyroscope();
@@ -62,8 +65,9 @@ public:
 	void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
 
 	uint32_t device_id() const { return _device_id; }
-	bool enabled() const { return _enabled; }
+	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }
+	const int32_t &priority() const { return _priority; }
 	const matrix::Dcmf &rotation() const { return _rotation; }
 
 	// apply offsets and scale
@@ -89,8 +93,8 @@ private:
 
 	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
+	int32_t _priority{DEFAULT_PRIORITY};
 
-	bool _enabled{true};
 	bool _external{false};
 };
 } // namespace calibration

src/lib/sensor_calibration/Magnetometer.cpp

@@ -64,6 +64,21 @@ void Magnetometer::set_device_id(uint32_t device_id)
 
 void Magnetometer::set_external(bool external)
 {
+	// update priority default appropriately if not set
+	if (_calibration_index < 0) {
+		if ((_priority < 0) || (_priority > 100)) {
+			_priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+
+		} else if (!_external && external && (_priority == DEFAULT_PRIORITY)) {
+			// internal -> external
+			_priority = DEFAULT_EXTERNAL_PRIORITY;
+
+		} else if (_external && !external && (_priority == DEFAULT_EXTERNAL_PRIORITY)) {
+			// external -> internal
+			_priority = DEFAULT_PRIORITY;
+		}
+	}
+
 	_external = external;
 }
 
@@ -113,9 +128,16 @@ void Magnetometer::ParametersUpdate()
 			_rotation = get_rot_matrix((enum Rotation)rotation);
 		}
 
-		// CAL_MAGx_EN
-		int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index);
-		_enabled = (enabled == 1);
+		// CAL_MAGx_PRIO
+		_priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index);
+
+		if ((_priority < 0) || (_priority > 100)) {
+			// reset to default
+			int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+			PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, new_priority);
+			SetCalibrationParam(SensorString(), "PRIO", _calibration_index, new_priority);
+			_priority = new_priority;
+		}
 
 		// CAL_MAGx_OFF{X,Y,Z}
 		_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
@@ -151,7 +173,7 @@ void Magnetometer::Reset()
 	_power_compensation.zero();
 	_power = 0.f;
 
-	_enabled = true;
+	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 
 	_calibration_index = -1;
 }
@@ -161,7 +183,7 @@ bool Magnetometer::ParametersSave()
 	if (_calibration_index >= 0) {
 		// save calibration
 		SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
-		SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0);
+		SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
 		SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
 
 		Vector3f scale{_scale.diag()};

src/lib/sensor_calibration/Magnetometer.hpp

@@ -48,6 +48,9 @@ class Magnetometer
 public:
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
+
 	static constexpr const char *SensorString() { return "MAG"; }
 
 	Magnetometer();
@@ -66,9 +69,10 @@ public:
 	void set_rotation(Rotation rotation);
 
 	uint32_t device_id() const { return _device_id; }
-	bool enabled() const { return _enabled; }
+	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }
 	const matrix::Vector3f &offset() const { return _offset; }
+	const int32_t &priority() const { return _priority; }
 	const matrix::Dcmf &rotation() const { return _rotation; }
 	const Rotation &rotation_enum() const { return _rotation_enum; }
 	const matrix::Matrix3f &scale() const { return _scale; }
@@ -98,8 +102,8 @@ private:
 
 	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
+	int32_t _priority{-1};
 
-	bool _enabled{true};
 	bool _external{false};
 };
 } // namespace calibration