FW L1 controller: also handle negative loiter direction case

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
FW L1 controller: when loitering, set the yaw setpoint to be tangentially to loiter circle
2021-10-20 09:45:22 +02:00 · 2021-10-19 15:40:56 +02:00 · 2021-10-19 15:40:39 +02:00
3 changed files with 8 additions and 6 deletions
--- a/src/lib/l1/ECL_L1_Pos_Controller.cpp
+++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp
@ -306,8 +306,10 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
 		_lateral_accel = lateral_accel_sp_circle;
 		_circle_mode = true;
 		_bearing_error = 0.0f;
-		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
+		// once loitering, the bearing is tangentially to the loiter,
+		// so subtract/add PI/2 from the bearing from current position to loiter center depending on loiter direction
+		const float center_to_tangent = M_PI_2_F * loiter_direction * -1.f;
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)) + center_to_tangent;
 	}

 	update_roll_setpoint();
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@ -155,7 +155,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 					_att_sp.pitch_body = constrain(_att_sp.pitch_body,
 								       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));

-					_att_sp.yaw_body = 0.0f;
+					_att_sp.yaw_body = NAN;
 					_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);

 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@ -693,7 +693,7 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
 			/* reset setpoints from other modes (auto) otherwise we won't
 			 * level out without new manual input */
 			_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
-			_att_sp.yaw_body = 0;
+			_att_sp.yaw_body = NAN;
 		}

 		_control_mode_current = FW_POSCTRL_MODE_POSITION;
@ -1678,7 +1678,7 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2
 				   _manual_height_rate_setpoint_m_s);

 	_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
-	_att_sp.yaw_body = 0;
+	_att_sp.yaw_body = NAN;

 	/* Copy thrust and pitch values from tecs */
 	if (_landed) {
@ -1797,7 +1797,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		}

 		_att_sp.roll_body = roll_sp_new;
-		_att_sp.yaw_body = 0;
+		_att_sp.yaw_body = NAN;
 	}

 	/* Copy thrust and pitch values from tecs */
Author	SHA1	Message	Date
Silvan Fuhrer	3c03d48876	FW L1 controller: also handle negative loiter direction case Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-20 09:45:22 +02:00
Silvan Fuhrer	f61491995b	FW L1 controller: when loitering, set the yaw setpoint to be tangentially to loiter circle Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-19 15:40:56 +02:00
Silvan Fuhrer	4f708d7a6f	FW: set attitude_setpoint.yaw to NAN if yaw is not controlled (instead of 0) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-10-19 15:40:39 +02:00