L1 position controller: use double precision floating point for all lat/lon

src/lib/l1/ECL_L1_Pos_Controller.cpp

@@ -46,6 +46,7 @@
 
 #include <float.h>
 
+using matrix::Vector2d;
 using matrix::Vector2f;
 using matrix::wrap_pi;
 
@@ -72,17 +73,15 @@ float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
 }
 
 void
-ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
+ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector2d &vector_B,
-		const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
+		const Vector2d &vector_curr_position, const Vector2f &ground_speed_vector)
 {
 	/* this follows the logic presented in [1] */
 	float eta = 0.0f;
-	float xtrack_vel = 0.0f;
-	float ltrack_vel = 0.0f;
 
 	/* get the direction between the last (visited) and next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1),
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0),
-			  (double)vector_B(0), (double)vector_B(1));
+			  vector_B(1));
 
 	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@@ -132,11 +131,14 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 
 		/* unit vector from waypoint A to current position */
 		Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+
 		/* velocity across / orthogonal to line */
-		xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
+		float xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
+
 		/* velocity along line */
-		ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
+		float ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
+
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
 
@@ -158,33 +160,38 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 		/* calculate eta to fly to waypoint B */
 
 		/* velocity across / orthogonal to line */
-		xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
+		float xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
+
 		/* velocity along line */
-		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
+		float ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
+
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0));
 
 	} else {
-
 		/* calculate eta to fly along the line between A and B */
 
 		/* velocity across / orthogonal to line */
-		xtrack_vel = ground_speed_vector % vector_AB;
+		float xtrack_vel = ground_speed_vector % vector_AB;
+
 		/* velocity along line */
-		ltrack_vel = ground_speed_vector * vector_AB;
+		float ltrack_vel = ground_speed_vector * vector_AB;
+
 		/* calculate eta2 (angle of velocity vector relative to line) */
 		float eta2 = atan2f(xtrack_vel, ltrack_vel);
+
 		/* calculate eta1 (angle to L1 point) */
 		float xtrackErr = vector_A_to_airplane % vector_AB;
 		float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f);
+
 		/* limit output to 45 degrees */
 		sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
 		float eta1 = asinf(sine_eta1);
 		eta = eta1 + eta2;
+
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
-
 	}
 
 	/* limit angle to +-90 degrees */
@@ -201,7 +208,7 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 }
 
 void
-ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
+ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d &vector_curr_position, float radius,
 				       int8_t loiter_direction, const Vector2f &ground_speed_vector)
 {
 	/* the complete guidance logic in this section was proposed by [2] */
@@ -212,8 +219,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 	float K_velocity = 2.0f * _L1_damping * omega;
 
 	/* update bearing to next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1),
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0),
-			  (double)vector_A(0), (double)vector_A(1));
+			  vector_A(1));
 
 	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@@ -356,13 +363,17 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
 	update_roll_setpoint();
 }
 
-Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const
+Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2d &origin, const Vector2d &target) const
 {
 	/* this is an approximation for small angles, proposed by [2] */
-	Vector2f out(math::radians((target(0) - origin(0))),
+	const double x_angle = math::radians(target(0) - origin(0));
-		     math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
+	const double y_angle = math::radians(target(1) - origin(1));
+	const double x_origin_cos = cos(math::radians(origin(0)));
 
-	return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
+	return Vector2f{
+		static_cast<float>(x_angle * CONSTANTS_RADIUS_OF_EARTH),
+		static_cast<float>(y_angle *x_origin_cos * CONSTANTS_RADIUS_OF_EARTH),
+	};
 }
 
 void ECL_L1_Pos_Controller::set_l1_period(float period)

src/lib/l1/ECL_L1_Pos_Controller.hpp

@@ -143,8 +143,8 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
+	void navigate_waypoints(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_B,
-				const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed);
+				const matrix::Vector2d &vector_curr_position, const matrix::Vector2f &ground_speed);
 
 	/**
 	 * Navigate on an orbit around a loiter waypoint.
@@ -154,7 +154,7 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
+	void navigate_loiter(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_curr_position, float radius,
 			     int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
 
 	/**
@@ -235,7 +235,7 @@ private:
 	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
 	 * @return The vector in meters pointing from the reference position to the coordinates
 	 */
-	matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const;
+	matrix::Vector2f get_local_planar_vector(const matrix::Vector2d &origin, const matrix::Vector2d &target) const;
 
 	/**
 	 * Update roll angle setpoint. This will also apply slew rate limits if set.

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -35,6 +35,19 @@
 
 #include <vtol_att_control/vtol_type.h>
 
+using math::constrain;
+using math::max;
+using math::min;
+using math::radians;
+
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector2d;
+using matrix::Vector3f;
+using matrix::wrap_pi;
+
 FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
@@ -587,7 +600,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim
 }
 
 bool
-FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2f &curr_pos,
+FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2d &curr_pos,
 		const Vector2f &ground_speed,
 		const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
 {
@@ -662,8 +675,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 		_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
 
-		Vector2f curr_wp{0.0f, 0.0f};
+		Vector2d curr_wp{0, 0};
-		Vector2f prev_wp{0.0f, 0.0f};
+		Vector2d prev_wp{0, 0};
 
 		if (_vehicle_status.in_transition_to_fw) {
 
@@ -674,8 +687,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 				waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
 								   &lon_transition);
 
-				_transition_waypoint(0) = (float)lat_transition;
+				_transition_waypoint(0) = lat_transition;
-				_transition_waypoint(1) = (float)lon_transition;
+				_transition_waypoint(1) = lon_transition;
 			}
 
 
@@ -683,24 +696,25 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 
 		} else {
 			/* current waypoint (the one currently heading for) */
-			curr_wp = Vector2f((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
+			curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
 
 			if (pos_sp_prev.valid) {
-				prev_wp(0) = (float)pos_sp_prev.lat;
+				prev_wp(0) = pos_sp_prev.lat;
-				prev_wp(1) = (float)pos_sp_prev.lon;
+				prev_wp(1) = pos_sp_prev.lon;
 
 			} else {
 				/*
 				 * No valid previous waypoint, go for the current wp.
 				 * This is automatically handled by the L1 library.
 				 */
-				prev_wp(0) = (float)pos_sp_curr.lat;
+				prev_wp(0) = pos_sp_curr.lat;
-				prev_wp(1) = (float)pos_sp_curr.lon;
+				prev_wp(1) = pos_sp_curr.lon;
 			}
 
 
 			/* reset transition waypoint, will be set upon entering front transition */
-			_transition_waypoint.setNaN();
+			_transition_waypoint(0) = static_cast<double>(NAN);
+			_transition_waypoint(1) = static_cast<double>(NAN);
 		}
 
 		/* Initialize attitude controller integrator reset flags to 0 */
@@ -1007,8 +1021,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 					get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
 				}
 
-				Vector2f prev_wp{(float)_hdg_hold_prev_wp.lat, (float)_hdg_hold_prev_wp.lon};
+				Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
-				Vector2f curr_wp{(float)_hdg_hold_curr_wp.lat, (float)_hdg_hold_curr_wp.lon};
+				Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};
 
 				/* populate l1 control setpoint */
 				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
@@ -1157,24 +1171,24 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 }
 
 void
-FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos,
+FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	/* current waypoint (the one currently heading for) */
-	Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
+	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
-	Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */
+	Vector2d prev_wp{0, 0}; /* previous waypoint */
 
 	if (pos_sp_prev.valid) {
-		prev_wp(0) = (float)pos_sp_prev.lat;
+		prev_wp(0) = pos_sp_prev.lat;
-		prev_wp(1) = (float)pos_sp_prev.lon;
+		prev_wp(1) = pos_sp_prev.lon;
 
 	} else {
 		/*
 		 * No valid previous waypoint, go for the current wp.
 		 * This is automatically handled by the L1 library.
 		 */
-		prev_wp(0) = (float)pos_sp_curr.lat;
+		prev_wp(0) = pos_sp_curr.lat;
-		prev_wp(1) = (float)pos_sp_curr.lon;
+		prev_wp(1) = pos_sp_curr.lon;
 	}
 
 	// apply flaps for takeoff according to the corresponding scale factor set
@@ -1326,24 +1340,24 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f
 }
 
 void
-FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f &curr_pos,
+FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	/* current waypoint (the one currently heading for) */
-	Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
+	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
-	Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */
+	Vector2d prev_wp{0, 0}; /* previous waypoint */
 
 	if (pos_sp_prev.valid) {
-		prev_wp(0) = (float)pos_sp_prev.lat;
+		prev_wp(0) = pos_sp_prev.lat;
-		prev_wp(1) = (float)pos_sp_prev.lon;
+		prev_wp(1) = pos_sp_prev.lon;
 
 	} else {
 		/*
 		 * No valid previous waypoint, go for the current wp.
 		 * This is automatically handled by the L1 library.
 		 */
-		prev_wp(0) = (float)pos_sp_curr.lat;
+		prev_wp(0) = pos_sp_curr.lat;
-		prev_wp(1) = (float)pos_sp_curr.lon;
+		prev_wp(1) = pos_sp_curr.lon;
 	}
 
 	// apply full flaps for landings. this flag will also trigger the use of flaperons
@@ -1392,8 +1406,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f
 		create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon,
 						   pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon);
 
-		curr_wp(0) = (float)lat;
+		curr_wp(0) = lat;
-		curr_wp(1) = (float)lon;
+		curr_wp(1) = lon;
 	}
 
 	// we want the plane to keep tracking the desired flight path until we start flaring
@@ -1693,7 +1707,7 @@ FixedwingPositionControl::Run()
 
 		_vehicle_status_sub.update(&_vehicle_status);
 
-		Vector2f curr_pos((float)_current_latitude, (float)_current_longitude);
+		Vector2d curr_pos(_current_latitude, _current_longitude);
 		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
 
 		//Convert Local setpoints to global setpoints

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -89,22 +89,13 @@
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
 
-using math::constrain;
-using math::max;
-using math::min;
-using math::radians;
-
-using matrix::Dcmf;
-using matrix::Eulerf;
-using matrix::Quatf;
-using matrix::Vector2f;
-using matrix::Vector3f;
-using matrix::wrap_pi;
-
 using namespace launchdetection;
 using namespace runwaytakeoff;
 using namespace time_literals;
 
+using matrix::Vector2d;
+using matrix::Vector2f;
+
 static constexpr float HDG_HOLD_DIST_NEXT =
 	3000.0f; // initial distance of waypoint in front of plane in heading hold mode
 static constexpr float HDG_HOLD_REACHED_DIST =
@@ -248,7 +239,7 @@ private:
 
 	bool _vtol_tailsitter{false};
 
-	Vector2f _transition_waypoint{NAN, NAN};
+	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
 
 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
@@ -324,13 +315,13 @@ private:
 	 */
 	void		update_desired_altitude(float dt);
 
-	bool		control_position(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
+	bool		control_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
 					 const position_setpoint_s &pos_sp_prev,
 					 const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
-	void		control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
+	void		control_takeoff(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
 					const position_setpoint_s &pos_sp_prev,
 					const position_setpoint_s &pos_sp_curr);
-	void		control_landing(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
+	void		control_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
 					const position_setpoint_s &pos_sp_prev,
 					const position_setpoint_s &pos_sp_curr);
 

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp

@@ -46,7 +46,6 @@
 #include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
 
-using matrix::Vector2f;
 using namespace time_literals;
 
 namespace runwaytakeoff
@@ -69,8 +68,8 @@ void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat,
 	_state = RunwayTakeoffState::THROTTLE_RAMP;
 	_initialized_time = now;
 	_climbout = true; // this is true until climbout is finished
-	_start_wp(0) = (float)current_lat;
+	_start_wp(0) = current_lat;
-	_start_wp(1) = (float)current_lon;
+	_start_wp(1) = current_lon;
 }
 
 void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl,
@@ -103,8 +102,8 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl
 			 * The navigator will take this as starting point to navigate towards the takeoff WP.
 			 */
 			if (_param_rwto_hdg.get() == 0) {
-				_start_wp(0) = (float)current_lat;
+				_start_wp(0) = current_lat;
-				_start_wp(1) = (float)current_lon;
+				_start_wp(1) = current_lon;
 			}
 
 			mavlink_log_info(mavlink_log_pub, "#Climbout");
@@ -248,14 +247,6 @@ float RunwayTakeoff::getMaxPitch(float max)
 	}
 }
 
-/*
- * Returns the "previous" (start) WP for navigation.
- */
-Vector2f RunwayTakeoff::getStartWP()
-{
-	return _start_wp;
-}
-
 void RunwayTakeoff::reset()
 {
 	_initialized = false;

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@@ -88,7 +88,7 @@ public:
 	bool resetIntegrators();
 	float getMinPitch(float climbout_min, float min);
 	float getMaxPitch(float max);
-	matrix::Vector2f getStartWP();
+	const matrix::Vector2d &getStartWP() const { return _start_wp; };
 
 	void reset();
 
@@ -99,7 +99,7 @@ private:
 	hrt_abstime _initialized_time{0};
 	float _init_yaw{0.f};
 	bool _climbout{false};
-	matrix::Vector2f _start_wp;
+	matrix::Vector2d _start_wp;
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -149,7 +149,7 @@ RoverPositionControl::vehicle_attitude_poll()
 }
 
 bool
-RoverPositionControl::control_position(const matrix::Vector2f &current_position,
+RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 				       const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
 {
 	float dt = 0.01; // Using non zero value to a avoid division by zero
@@ -172,14 +172,14 @@ RoverPositionControl::control_position(const matrix::Vector2f &current_position,
 		//bool was_circle_mode = _gnd_control.circle_mode();
 
 		/* current waypoint (the one currently heading for) */
-		matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
+		matrix::Vector2d curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
 
 		/* previous waypoint */
-		matrix::Vector2f prev_wp = curr_wp;
+		matrix::Vector2d prev_wp = curr_wp;
 
 		if (pos_sp_triplet.previous.valid) {
-			prev_wp(0) = (float)pos_sp_triplet.previous.lat;
+			prev_wp(0) = pos_sp_triplet.previous.lat;
-			prev_wp(1) = (float)pos_sp_triplet.previous.lon;
+			prev_wp(1) = pos_sp_triplet.previous.lon;
 		}
 
 		matrix::Vector2f ground_speed_2d(ground_speed);
@@ -433,7 +433,7 @@ RoverPositionControl::run()
 			_pos_reset_counter = _global_pos.lat_lon_reset_counter;
 
 			matrix::Vector3f ground_speed(_local_pos.vx, _local_pos.vy,  _local_pos.vz);
-			matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon);
+			matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon);
 			matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);
 
 			if (!manual_mode && _control_mode.flag_control_position_enabled) {

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -150,7 +150,7 @@ private:
 	} _pos_ctrl_state {STOPPING};			/// Position control state machine
 
 	/* previous waypoint */
-	matrix::Vector2f _prev_wp{0.0f, 0.0f};
+	matrix::Vector2d _prev_wp{0, 0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,
@@ -190,7 +190,7 @@ private:
 	/**
 	 * Control position.
 	 */
-	bool		control_position(const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed,
+	bool		control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed,
 					 const position_setpoint_triplet_s &_pos_sp_triplet);
 	void		control_velocity(const matrix::Vector3f &current_velocity, const position_setpoint_triplet_s &pos_sp_triplet);
 	void		control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);