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

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};
-		Vector2f prev_wp{0.0f, 0.0f};
+		Vector2d curr_wp{0, 0};
+		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(1) = (float)lon_transition;
+				_transition_waypoint(0) = lat_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(1) = (float)pos_sp_prev.lon;
+				prev_wp(0) = pos_sp_prev.lat;
+				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(1) = (float)pos_sp_curr.lon;
+				prev_wp(0) = pos_sp_curr.lat;
+				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};
-				Vector2f curr_wp{(float)_hdg_hold_curr_wp.lat, (float)_hdg_hold_curr_wp.lon};
+				Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_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);
-	Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */
+	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
+	Vector2d prev_wp{0, 0}; /* previous waypoint */
 
 	if (pos_sp_prev.valid) {
-		prev_wp(0) = (float)pos_sp_prev.lat;
-		prev_wp(1) = (float)pos_sp_prev.lon;
+		prev_wp(0) = pos_sp_prev.lat;
+		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(1) = (float)pos_sp_curr.lon;
+		prev_wp(0) = pos_sp_curr.lat;
+		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);
-	Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */
+	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
+	Vector2d prev_wp{0, 0}; /* previous waypoint */
 
 	if (pos_sp_prev.valid) {
-		prev_wp(0) = (float)pos_sp_prev.lat;
-		prev_wp(1) = (float)pos_sp_prev.lon;
+		prev_wp(0) = pos_sp_prev.lat;
+		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(1) = (float)pos_sp_curr.lon;
+		prev_wp(0) = pos_sp_curr.lat;
+		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(1) = (float)lon;
+		curr_wp(0) = lat;
+		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(1) = (float)current_lon;
+	_start_wp(0) = current_lat;
+	_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(1) = (float)current_lon;
+				_start_wp(0) = current_lat;
+				_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(1) = (float)pos_sp_triplet.previous.lon;
+			prev_wp(0) = pos_sp_triplet.previous.lat;
+			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);