Address review comments

src/lib/l1/ECL_L1_Pos_Controller.cpp

@@ -82,9 +82,9 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 	float eta = 0.0f;
 
 	/* get the direction between the last (visited) and next waypoint */
-	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));
+	Vector2f vector_P_to_B = vector_B - vector_curr_position;
+	Vector2f vector_P_to_B_unit = vector_P_to_B.normalized();
+	_target_bearing = atan2f(vector_P_to_B_unit(1), vector_P_to_B_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);
@@ -120,13 +120,13 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	Vector2f vector_P_to_B = vector_curr_position - vector_B;
-	Vector2f vector_P_to_B_unit = vector_P_to_B.normalized();
+	Vector2f vector_B_to_P = vector_curr_position - vector_B;
+	Vector2f vector_B_to_P_unit = vector_B_to_P.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_P_to_B_unit % vector_AB, vector_P_to_B_unit * vector_AB);
+	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_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) {
@@ -245,7 +245,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 	}
 
 	/* update bearing to next waypoint */
-	_target_bearing = atan2f(vector_A_to_airplane_unit(1), vector_A_to_airplane_unit(0));
+	_target_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
 
 	/* calculate eta angle towards the loiter center */
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1429,6 +1429,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 	/* current waypoint (the one currently heading for) */
 	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
 	Vector2d prev_wp{0, 0}; /* previous waypoint */
+	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));
 
 	if (pos_sp_prev.valid) {
 		prev_wp(0) = pos_sp_prev.lat;
@@ -1480,8 +1482,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
 		 * If we use the navigator heading or not is decided later.
 		 */
-		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));
 
@@ -1555,10 +1555,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
 						dt);
 
-			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));
+			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
 
 			if (_param_fw_use_npfg.get()) {
 				_npfg.setAirspeedNom(target_airspeed * _eas2tas);