Use NAV_LOITER_RAD when provided offboard position setpoints

PX4 · Oct 15, 2019 · 094b708 · 094b708
1 parent bfab544
commit 094b708
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 2 deletions.
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -98,6 +98,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
 	_parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF");
 	_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
+	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
 
 	// if vehicle is vtol these handles will be set when we get the vehicle status
 	_parameter_handles.airspeed_trans = PARAM_INVALID;
@@ -165,6 +166,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_early_config_change, &(_parameters.land_early_config_change));
 	param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale));
 	param_get(_parameter_handles.land_throtTC_scale, &(_parameters.land_throtTC_scale));
+	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
 
 	// VTOL parameter VTOL_TYPE
 	if (_parameter_handles.vtol_type != PARAM_INVALID) {
@@ -868,8 +870,17 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
 			/* waypoint is a loiter waypoint */
-			_l1_control.navigate_loiter(curr_wp, curr_pos, pos_sp_curr.loiter_radius,
-						    pos_sp_curr.loiter_direction, nav_speed_2d);
+			float loiter_radius = pos_sp_curr.loiter_radius;
+			uint8_t loiter_direction = pos_sp_curr.loiter_direction;
+
+			if (pos_sp_curr.loiter_radius <= 0) { //TODO: Get loiter radius from parameter
+				loiter_radius = _parameters.loiter_radius;
+				loiter_direction = 1;
+
+			}
+
+			_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, nav_speed_2d);
+
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 

diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -295,6 +295,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 		float pitchsp_offset_rad;
 
 		float throttle_land_max;
+		float loiter_radius;
 
 		float land_heading_hold_horizontal_distance;
 		float land_flare_pitch_min_deg;
@@ -368,6 +369,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 		param_t land_early_config_change;
 		param_t land_airspeed_scale;
 		param_t land_throtTC_scale;
+		param_t loiter_radius;
 
 		param_t vtol_type;
 	} _parameter_handles {};				///< handles for interesting parameters */