dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw_pos_ctrl_l1: enable loiter type offboard position setpoint for Fixedwing vehicles 

* Use NAV_LOITER_RAD when provided offboard position setpoints
This commit is contained in:
JaeyoungLim 2019-10-28 22:42:28 +01:00 committed by Daniel Agar
parent 216556e7f6
commit 71fbe58e20
2 changed files with 15 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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.01f || pos_sp_curr.loiter_radius > -0.01f) {
loiter_radius = _parameters.loiter_radius;
loiter_direction = (loiter_radius > 0) ? 1 : -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();

2
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -295,6 +295,7 @@ private:
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 @@ private:
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 */