fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 07:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixedwing Pos Control: Handle vehicle transition waypoints outside controllers (#18503)

Browse Source 
* Handle VTOL transition waypoints outside FW auto control modes
This commit is contained in:
JaeyoungLim
2021-11-08 11:21:08 +01:00
committed by GitHub
parent 66ab7e50df
commit 031f7f831b
1 changed files with 56 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+56 -94
View File
@@ -745,9 +745,34 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
publishOrbitStatus(pos_sp_curr);
}
_position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr);
position_setpoint_s current_sp = pos_sp_curr;
switch (_position_sp_type) {
if (_vehicle_status.in_transition_to_fw) {
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
// during the transition
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
&lon_transition);
_transition_waypoint(0) = lat_transition;
_transition_waypoint(1) = lon_transition;
}
current_sp.lat = _transition_waypoint(0);
current_sp.lon = _transition_waypoint(1);
} else {
/* reset transition waypoint, will be set upon entering front transition */
_transition_waypoint(0) = static_cast<double>(NAN);
_transition_waypoint(1) = static_cast<double>(NAN);
}
const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
switch (position_sp_type) {
case position_setpoint_s::SETPOINT_TYPE_IDLE:
_att_sp.thrust_body[0] = 0.0f;
_att_sp.roll_body = 0.0f;
@@ -755,19 +780,19 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
break;
case position_setpoint_s::SETPOINT_TYPE_POSITION:
control_auto_position(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
control_auto_position(now, curr_pos, ground_speed, pos_sp_prev, current_sp);
break;
case position_setpoint_s::SETPOINT_TYPE_LOITER:
control_auto_loiter(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr, pos_sp_next);
control_auto_loiter(now, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
break;
case position_setpoint_s::SETPOINT_TYPE_LAND:
control_auto_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
control_auto_landing(now, curr_pos, ground_speed, pos_sp_prev, current_sp);
break;
case position_setpoint_s::SETPOINT_TYPE_TAKEOFF:
control_auto_takeoff(now, dt, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
control_auto_takeoff(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
break;
}
@@ -898,28 +923,9 @@ uint8_t
FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr)
{
Vector2d curr_wp{0, 0};
Vector2d prev_wp{0, 0};
if (_vehicle_status.in_transition_to_fw) {
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
// during the transition
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
&lon_transition);
_transition_waypoint(0) = lat_transition;
_transition_waypoint(1) = lon_transition;
}
curr_wp = prev_wp = _transition_waypoint;
} else {
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
}
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
const float acc_rad = _l1_control.switch_distance(500.0f);
@@ -979,45 +985,24 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
Vector2d curr_wp{0, 0};
Vector2d prev_wp{0, 0};
if (_vehicle_status.in_transition_to_fw) {
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
// during the transition
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
&lon_transition);
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
_transition_waypoint(0) = lat_transition;
_transition_waypoint(1) = lon_transition;
}
curr_wp = prev_wp = _transition_waypoint;
if (pos_sp_prev.valid) {
prev_wp(0) = pos_sp_prev.lat;
prev_wp(1) = pos_sp_prev.lon;
} else {
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
if (pos_sp_prev.valid) {
prev_wp(0) = pos_sp_prev.lat;
prev_wp(1) = pos_sp_prev.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
}
/* reset transition waypoint, will be set upon entering front transition */
_transition_waypoint(0) = static_cast<double>(NAN);
_transition_waypoint(1) = static_cast<double>(NAN);
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
}
float mission_airspeed = _param_fw_airspd_trim.get();
if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
@@ -1109,43 +1094,20 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
Vector2d curr_wp{0, 0};
Vector2d prev_wp{0, 0};
if (_vehicle_status.in_transition_to_fw) {
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
// during the transition
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
&lon_transition);
_transition_waypoint(0) = lat_transition;
_transition_waypoint(1) = lon_transition;
}
curr_wp = prev_wp = _transition_waypoint;
if (pos_sp_prev.valid) {
prev_wp(0) = pos_sp_prev.lat;
prev_wp(1) = pos_sp_prev.lon;
} else {
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
if (pos_sp_prev.valid) {
prev_wp(0) = pos_sp_prev.lat;
prev_wp(1) = pos_sp_prev.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
}
/* reset transition waypoint, will be set upon entering front transition */
_transition_waypoint(0) = static_cast<double>(NAN);
_transition_waypoint(1) = static_cast<double>(NAN);
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
}
float mission_airspeed = _param_fw_airspd_trim.get();