vmount: fix calculation of GPS location to angles

src/drivers/vmount/common.h

@@ -74,7 +74,7 @@ struct ControlData {
 			double lon;              /**< longitude in [deg] */
 			double lat;              /**< latitude in [deg] */
 			float altitude;          /**< altitude in [m] */
-			float roll_angle;        /**< roll is set to a fixed angle. Set to 0 for level horizon */
+			float roll_angle;        /**< roll is set to a fixed angle. Set to 0 for level horizon [rad] */
 			float pitch_fixed_angle; /**< ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude */
 		} lonlat;
 	} type_data;

src/drivers/vmount/output.cpp

@@ -47,8 +47,6 @@
 #include <geo/geo.h>
 #include <math.h>
 
-#define LATLON_TO_M  0.01113195
-
 namespace vmount
 {
 
@@ -85,13 +83,18 @@ int OutputBase::initialize()
 float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
 				   const vehicle_global_position_s &global_position)
 {
-	double scale = cos(M_DEG_TO_RAD * ((global_position.lat + lat) * 0.00000005));
-	float x = (float)((lon - global_position.lon) * scale * LATLON_TO_M);
-	float y = (float)((lat - global_position.lat) * LATLON_TO_M);
-	float z = altitude - global_position.alt;
-	float target_distance = sqrtf(x * x + y * y);
+	if (!map_projection_initialized(&_projection_reference)) {
+		map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
+	}
 
-	return atan2f(z, target_distance) * (float)M_RAD_TO_DEG;
+	float x1, y1, x2, y2;
+	map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
+	map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
+	float dx = x1 - x2, dy = y1 - y2;
+	float target_distance = sqrtf(dx * dx + dy * dy);
+	float z = altitude - global_position.alt;
+
+	return atan2f(z, target_distance);
 }
 
 void OutputBase::_set_angle_setpoints(const ControlData *control_data)
@@ -160,8 +163,7 @@ void OutputBase::_handle_position_update(bool force_update)
 	}
 
 	float roll = _cur_control_data->type_data.lonlat.roll_angle;
-	float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon,
-			lat, lon) * (float)M_RAD_TO_DEG;
+	float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, lat, lon);
 
 	_angle_setpoints[0] = roll;
 	_angle_setpoints[1] = pitch;

src/drivers/vmount/output.h

@@ -41,6 +41,7 @@
 
 #include "common.h"
 #include <drivers/drv_hrt.h>
+#include <geo/geo.h>
 #include <uORB/topics/vehicle_global_position.h>
 
 
@@ -79,8 +80,10 @@ public:
 	virtual void print_status() = 0;
 
 protected:
-	static float _calculate_pitch(double lon, double lat, float altitude,
-				      const vehicle_global_position_s &global_position);
+	float _calculate_pitch(double lon, double lat, float altitude,
+			       const vehicle_global_position_s &global_position);
+
+	map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
 
 	const OutputConfig &_config;
 
@@ -91,14 +94,14 @@ protected:
 	void _handle_position_update(bool force_update = false);
 
 	const ControlData *_cur_control_data = nullptr;
-	float _angle_setpoints[3] = { 0.f, 0.f, 0.f };
+	float _angle_setpoints[3] = { 0.f, 0.f, 0.f }; ///< [rad]
 	float _angle_speeds[3] = { 0.f, 0.f, 0.f };
 	bool _stabilize[3] = { false, false, false };
 
 	/** calculate the _angle_outputs (with speed) and stabilize if needed */
 	void _calculate_output_angles(const hrt_abstime &t);
 
-	float _angle_outputs[3] = { 0.f, 0.f, 0.f };
+	float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad]
 	hrt_abstime _last_update;
 
 	int _get_vehicle_attitude_sub() const { return _vehicle_attitude_sub; }