Convert L1 controller to local coordinates

src/lib/l1/ECL_L1_Pos_Controller.cpp

@@ -73,8 +73,8 @@ float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
 }
 
 void
-ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector2d &vector_B,
-		const Vector2d &vector_curr_position, const Vector2f &ground_speed_vector)
+ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
+		const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
 {
 	_has_guidance_updated = true;
 
@@ -82,8 +82,9 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector
 	float eta = 0.0f;
 
 	/* get the direction between the last (visited) and next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0),
-			  vector_B(1));
+	Vector2f vector_B_to_P = vector_B - vector_curr_position;
+	Vector2f vector_B_to_P_unit = vector_B_to_P.normalized();
+	_target_bearing = atan2f(vector_B_to_P_unit(1), vector_B_to_P_unit(0));
 
 	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@@ -92,20 +93,20 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector
 	_L1_distance = _L1_ratio * ground_speed;
 
 	/* calculate vector from A to B */
-	Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+	Vector2f vector_AB = vector_B - vector_A;
 
 	/*
 	 * check if waypoints are on top of each other. If yes,
 	 * skip A and directly continue to B
 	 */
 	if (vector_AB.length() < 1.0e-6f) {
-		vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
+		vector_AB = vector_B - vector_curr_position;
 	}
 
 	vector_AB.normalize();
 
 	/* calculate the vector from waypoint A to the aircraft */
-	Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+	Vector2f vector_A_to_airplane = vector_curr_position - vector_A;
 
 	/* calculate crosstrack error (output only) */
 	_crosstrack_error = vector_AB % vector_A_to_airplane;
@@ -119,12 +120,13 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	Vector2f vector_P_to_B = vector_curr_position - vector_B;
+	Vector2f vector_P_to_B_unit = vector_P_to_B.normalized();
 
 	/* calculate angle of airplane position vector relative to line) */
 
 	// XXX this could probably also be based solely on the dot product
-	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
+	float AB_to_BP_bearing = atan2f(vector_P_to_B_unit % vector_AB, vector_P_to_B_unit * vector_AB);
 
 	/* extension from [2], fly directly to A */
 	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) {
@@ -210,7 +212,7 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector
 }
 
 void
-ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d &vector_curr_position, float radius,
+ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
 				       int8_t loiter_direction, const Vector2f &ground_speed_vector)
 {
 	_has_guidance_updated = true;
@@ -222,10 +224,6 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
 	float K_crosstrack = omega * omega;
 	float K_velocity = 2.0f * _L1_damping * omega;
 
-	/* update bearing to next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0),
-			  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);
 
@@ -233,7 +231,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
 	_L1_distance = _L1_ratio * ground_speed;
 
 	/* calculate the vector from waypoint A to current position */
-	Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+	Vector2f vector_A_to_airplane = vector_curr_position - vector_A;
 
 	Vector2f vector_A_to_airplane_unit;
 
@@ -246,6 +244,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
 		vector_A_to_airplane_unit = vector_A_to_airplane;
 	}
 
+	/* update bearing to next waypoint */
+	_target_bearing = atan2f(vector_A_to_airplane_unit(1), vector_A_to_airplane_unit(0));
+
 	/* calculate eta angle towards the loiter center */
 
 	/* velocity across / orthogonal to line from waypoint to current position */

src/lib/l1/ECL_L1_Pos_Controller.hpp

@@ -143,9 +143,8 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_waypoints(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_B,
-				const matrix::Vector2d &vector_curr_position, const matrix::Vector2f &ground_speed);
-
+	void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
+				const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed);
 	/**
 	 * Navigate on an orbit around a loiter waypoint.
 	 *
@@ -154,7 +153,7 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_loiter(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_curr_position, float radius,
+	void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
 			     int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
 
 	/**

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1208,7 +1208,10 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 	} else {
-		_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed));
+		Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+		Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+		Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
+		_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed));
 		_att_sp.roll_body = _l1_control.get_roll_setpoint();
 	}
 
@@ -1377,7 +1380,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 	} else {
-		_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed));
+		Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+		Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+		_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction,
+					    get_nav_speed_2d(ground_speed));
 		_att_sp.roll_body = _l1_control.get_roll_setpoint();
 	}
 
@@ -1480,7 +1486,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 		} else {
-			_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed);
+			Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+			Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+			Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
+						 _runway_takeoff.getStartWP()(1));
+			_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
 			_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
 		}
 
@@ -1550,7 +1560,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 			} else {
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+				Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+				Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+				Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
+							 _runway_takeoff.getStartWP()(1));
+				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
 				_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			}
 
@@ -1807,7 +1821,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 
 			} else {
 				// normal navigation
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+				Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+				Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+				Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
+							 prev_wp(1));
+				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
 			}
 
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
@@ -1922,7 +1940,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 
 			} else {
 				// normal navigation
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+				Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+				Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+				Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
+							 prev_wp(1));
+				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
 			}
 
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
@@ -2086,7 +2108,11 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 
 			} else {
 				/* populate l1 control setpoint */
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+				Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+				Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+				Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
+							 prev_wp(1));
+				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
 				_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			}
 
@@ -2220,7 +2246,8 @@ FixedwingPositionControl::Run()
 
 		// Convert Local setpoints to global setpoints
 		if (!_global_local_proj_ref.isInitialized()
-		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
+		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
+		    || (_local_pos.vxy_reset_counter != _pos_reset_counter)) {
 
 			_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
 							     _local_pos.ref_timestamp);

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -281,7 +281,11 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 					_pos_ctrl_state = STOPPING;  // We are closer than loiter radius to waypoint, stop.
 
 				} else {
-					_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+					Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+					Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+					Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
+								 prev_wp(1));
+					_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
 
 					_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;