support mag power compensation with battery_status instance 1

src/lib/mag_compensation/MagCompensation.cpp

@@ -48,13 +48,6 @@ MagCompensator::MagCompensator(ModuleParams *parent):
 void MagCompensator::calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect)
 {
 	if (_armed) {
-		int type = _param_mag_compensation_type.get();
-
-		if (type == CompensationType::THROTTLE) {
-			mag = mag + param_vect * _throttle; //for param [gauss]
-
-		} else if (type == CompensationType::CURRENT) {
-			mag = mag + param_vect * _battery_current * 0.001; //for param [gauss/kA]
-		}
+		mag = mag + param_vect * _power;
 	}
 }

src/lib/mag_compensation/MagCompensation.hpp

@@ -53,26 +53,11 @@ public:
 
 	void update_armed_flag(bool armed) { _armed = armed; }
 
-	void update_throttle(float throttle) { _throttle = throttle; }
-
-	void update_current(float battery_current) { _battery_current = battery_current; }
+	void update_power(float power) { _power = power; }
 
 	void calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect);
 
 private:
-
-	enum CompensationType {
-		DISABLED = 0,
-		THROTTLE,
-		CURRENT
-	};
-
-	float _throttle{0};
-	float _battery_current{0};
+	float _power{0};
 	bool _armed{false};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_compensation_type
-	)
-
 };

src/lib/mag_compensation/mag_compensation_params.c

@@ -35,7 +35,8 @@
  *
  * @value 0 Disabled
  * @value 1 Throttle-based compensation
- * @value 2 Current-based compensation
+ * @value 2 Current-based compensation (battery_status instance 0)
+ * @value 3 Current-based compensation (battery_status instance 1)
  *
  * @category system
  * @group Sensor Calibration

src/modules/sensors/parameters.cpp

@@ -59,6 +59,8 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
 	parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
 	/* mag compensation */
+	parameter_handles.mag_comp_type = param_find("CAL_MAG_COMP_TYP");
+
 	parameter_handles.mag_comp_paramX[0] = param_find("CAL_MAG0_XCOMP");
 	parameter_handles.mag_comp_paramX[1] = param_find("CAL_MAG1_XCOMP");
 	parameter_handles.mag_comp_paramX[2] = param_find("CAL_MAG2_XCOMP");
@@ -120,6 +122,8 @@ void update_parameters(const ParameterHandles &parameter_handles, Parameters &pa
 	param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1]));
 	param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2]));
 
+	param_get(parameter_handles.mag_comp_type, &(parameters.mag_comp_type));
+
 	param_get(parameter_handles.mag_comp_paramX[0], &(parameters.mag_comp_paramX[0]));
 	param_get(parameter_handles.mag_comp_paramX[1], &(parameters.mag_comp_paramX[1]));
 	param_get(parameter_handles.mag_comp_paramX[2], &(parameters.mag_comp_paramX[2]));

src/modules/sensors/parameters.h

@@ -57,6 +57,7 @@ struct Parameters {
 	float board_offset[3];
 
 	//parameters for current/throttle-based mag compensation
+	int32_t mag_comp_type;
 	float mag_comp_paramX[4];
 	float mag_comp_paramY[4];
 	float mag_comp_paramZ[4];
@@ -76,6 +77,7 @@ struct ParameterHandles {
 
 	param_t board_offset[3];
 
+	param_t mag_comp_type;
 	param_t mag_comp_paramX[4];
 	param_t mag_comp_paramY[4];
 	param_t mag_comp_paramZ[4];

src/modules/sensors/sensors.cpp

@@ -125,7 +125,7 @@ private:
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};				/**< notification of parameter updates */
 	uORB::Subscription	_vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
 	uORB::Subscription	_vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
-	uORB::Subscription	_battery_status_sub{ORB_ID(battery_status)};		/**< vehicle control mode subscription */
+	uORB::Subscription	_battery_status_sub{ORB_ID(battery_status), 0};		/**< battery_status instance 0 subscription */
 
 	uORB::Publication<airspeed_s>			_airspeed_pub{ORB_ID(airspeed)};			/**< airspeed */
 	uORB::Publication<sensor_combined_s>		_sensor_pub{ORB_ID(sensor_combined)};			/**< combined sensor data topic */
@@ -138,6 +138,15 @@ private:
 		{this, ORB_ID(sensor_gyro_integrated), 2}
 	};
 
+	enum class MagCompensationType {
+		Disabled = 0,
+		Throttle,
+		Current_inst0,
+		Current_inst1
+	};
+
+	MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
+
 	uint32_t _selected_sensor_device_id{0};
 	uint8_t _selected_sensor_sub_index{0};
 
@@ -468,16 +477,34 @@ void Sensors::Run()
 			_voted_sensors_update.update_mag_comp_armed(_armed);
 		}
 
-		if (_actuator_ctrl_0_sub.updated()) {
+		//check mag power compensation type (change battery current subscription instance if necessary)
+		if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst0
+		    && _mag_comp_type != MagCompensationType::Current_inst0) {
+			_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0};
+		}
+
+		if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst1
+		    && _mag_comp_type != MagCompensationType::Current_inst1) {
+			_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1};
+		}
+
+		_mag_comp_type = (MagCompensationType)_parameters.mag_comp_type;
+
+		//update power signal for mag compensation
+		if (_mag_comp_type == MagCompensationType::Throttle) {
 			actuator_controls_s controls {};
-			_actuator_ctrl_0_sub.copy(&controls);
-			_voted_sensors_update.update_mag_comp_throttle(controls.control[actuator_controls_s::INDEX_THROTTLE]);
+
+			if (_actuator_ctrl_0_sub.update(&controls)) {
+				_voted_sensors_update.update_mag_comp_power(controls.control[actuator_controls_s::INDEX_THROTTLE]);
 			}
 
-		if (_battery_status_sub.updated()) {
+		} else if (_mag_comp_type == MagCompensationType::Current_inst0
+			   || _mag_comp_type == MagCompensationType::Current_inst1) {
 			battery_status_s bat_stat {};
-			_battery_status_sub.copy(&bat_stat);
-			_voted_sensors_update.update_mag_comp_current(bat_stat.current_a);
+
+			if (_battery_status_sub.update(&bat_stat)) {
+				_voted_sensors_update.update_mag_comp_power(bat_stat.current_a * 0.001f); //current in [kA]
+			}
 		}
 	}
 

src/modules/sensors/voted_sensors_update.cpp

@@ -954,12 +954,7 @@ void VotedSensorsUpdate::update_mag_comp_armed(bool armed)
 	_mag_compensator.update_armed_flag(armed);
 }
 
-void VotedSensorsUpdate::update_mag_comp_throttle(float throttle)
+void VotedSensorsUpdate::update_mag_comp_power(float power)
 {
-	_mag_compensator.update_throttle(throttle);
-}
-
-void VotedSensorsUpdate::update_mag_comp_current(float current)
-{
-	_mag_compensator.update_current(current);
+	_mag_compensator.update_power(power);
 }

src/modules/sensors/voted_sensors_update.h

@@ -142,14 +142,10 @@ public:
 	void update_mag_comp_armed(bool armed);
 
 	/**
-	 * Update throttle for mag compensation.
+	 * Update power signal for mag compensation.
+	 * power: either throttle value [0,1] or current measurement in [kA]
 	 */
-	void update_mag_comp_throttle(float throttle);
-
-	/**
-	 * Update current for mag compensation.
-	 */
-	void update_mag_comp_current(float current);
+	void update_mag_comp_power(float power);
 
 private: