Attempt to calculate mag yangle individually

PetervdPerk-NXP · Oct 30, 2023 · 57c98d2 · 57c98d2
1 parent 26fd4c8
commit 57c98d2
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 3 deletions.
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
@@ -693,7 +693,7 @@ int Sensors::print_status()
 
 	if (_vehicle_magnetometer) {
 		PX4_INFO_RAW("\n");
-		_vehicle_magnetometer->PrintStatus();
+		_vehicle_magnetometer->PrintStatus(_vehicle_acceleration.getLastAccel());
 	}
 
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER

diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
@@ -207,6 +207,11 @@ void VehicleAcceleration::ParametersUpdate(bool force)
 	}
 }
 
+matrix::Vector3f VehicleAcceleration::getLastAccel()
+{
+	return _acceleration_prev;
+}
+
 void VehicleAcceleration::Run()
 {
 	// backup schedule

diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
@@ -65,6 +65,8 @@ class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem
 	bool Start();
 	void Stop();
 
+	matrix::Vector3f getLastAccel();
+
 	void PrintStatus();
 
 private:

diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
@@ -745,7 +745,7 @@ void VehicleMagnetometer::UpdateStatus()
 	}
 }
 
-void VehicleMagnetometer::PrintStatus()
+void VehicleMagnetometer::PrintStatus(matrix::Vector3f accel)
 {
 	if (_selected_sensor_sub_index >= 0) {
 		PX4_INFO_RAW("[vehicle_magnetometer] selected %s: %" PRIu32 " (%" PRId8 ")\n",
@@ -755,6 +755,20 @@ void VehicleMagnetometer::PrintStatus()
 
 	_voter.print();
 
+	// Yaw angle from this formula https://ahrs.readthedocs.io/en/latest/filters/tilt.html
+	float roll = atan2(accel(1), -accel(2)); // Note negata Az
+	float pitch = atan2(-accel(0), sqrt(accel(1) * accel(1) + accel(2) * accel(2)));
+
+	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+		if (_calibration[sensor_index].device_id() != 0) {
+			float angle = atan2((double)_last_data[sensor_index](2) * sin(pitch) - (double)_last_data[sensor_index](1) * cos(pitch),
+					    (double)_last_data[sensor_index](0) * cos(roll) +
+					    sin(roll) * ((double)_last_data[sensor_index](1) * sin(pitch) + (double)_last_data[sensor_index](2) * cos(pitch)));
+
+			PX4_INFO_RAW("Sensor %i angle: %05.3f\n", sensor_index, (double)angle * 180 / M_PI);
+		}
+	}
+
 	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
 		if (_advertised[i] && (_priority[i] > 0)) {
 			_calibration[i].PrintStatus();

diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
@@ -75,7 +75,7 @@ class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem
 	bool Start();
 	void Stop();
 
-	void PrintStatus();
+	void PrintStatus(matrix::Vector3f accel);
 
 private:
 	void Run() override;