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

FixedwingPositionControl: follow (infinite) lines instead of waypoints during takeoff and landing

This commit is contained in:
Thomas Stastny 2023-08-21 14:59:08 +02:00
parent 3ffb57bcce
commit 3047cad05d
2 changed files with 82 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -1353,17 +1353,20 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
// by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP
Vector2f takeoff_bearing_vector = {cosf(_launch_current_yaw), sinf(_launch_current_yaw)};
float takeoff_bearing = _launch_current_yaw;
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
// the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded
takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local);
const Vector2f takeoff_bearing_vector = takeoff_waypoint_local - start_pos_local;
if (takeoff_bearing_vector.norm() > FLT_EPSILON) {
takeoff_bearing = atan2f(takeoff_bearing_vector(1), takeoff_bearing_vector(0));
}
}
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
_wind_vel, 0.0f);
navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
@ -1440,11 +1443,15 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
// by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP
Vector2f takeoff_bearing_vector = {cosf(_launch_current_yaw), sinf(_launch_current_yaw)};
float takeoff_bearing = _launch_current_yaw;
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
// the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded
takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local);
const Vector2f takeoff_bearing_vector = takeoff_waypoint_local - launch_local_position;
if (takeoff_bearing_vector.norm() > FLT_EPSILON) {
takeoff_bearing = atan2f(takeoff_bearing_vector(1), takeoff_bearing_vector(0));
}
}
/* Set control values depending on the detection state */
@ -1454,8 +1461,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
0.0f);
navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@ -1603,7 +1609,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
@ -1682,7 +1688,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
@ -2616,27 +2622,6 @@ FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, con
return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike);
}
Vector2f
FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_position,
const Vector2f &takeoff_waypoint) const
{
Vector2f takeoff_bearing_vector = takeoff_waypoint - launch_position;
if (takeoff_bearing_vector.norm_squared() > FLT_EPSILON) {
takeoff_bearing_vector.normalize();
} else {
// TODO: a new bearing only based fixed-wing takeoff command / mission item will get rid of the need
// for this check
// takeoff in the direction of the airframe
takeoff_bearing_vector(0) = cosf(_yaw);
takeoff_bearing_vector(1) = sinf(_yaw);
}
return takeoff_bearing_vector;
}
void
FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point)
@ -2887,6 +2872,43 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint,
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
}
void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2,
const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const Vector2f line_segment = point_on_line_2 - point_on_line_1;
if (line_segment.norm() <= FLT_EPSILON) {
// degenerate case: line segment has zero length. maintain the last npfg command.
return;
}
const Vector2f unit_path_tangent = line_segment.normalized();
const Vector2f point_1_to_vehicle = vehicle_pos - point_on_line_1;
_closest_point_on_path = point_on_line_1 + point_1_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
const float path_curvature = 0.f;
_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature);
// for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined.
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
}
void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line, const float line_bearing,
const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const Vector2f unit_path_tangent{cosf(line_bearing), sinf(line_bearing)};
const Vector2f point_on_line_to_vehicle = vehicle_pos - point_on_line;
_closest_point_on_path = point_on_line + point_on_line_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
const float path_curvature = 0.f;
_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature);
// for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined.
_target_bearing = line_bearing;
}
void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
{

39
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -732,15 +732,6 @@ private:
*/
float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const;
/**
* @brief Calculates the unit takeoff bearing vector from the launch position to takeoff waypont.
*
* @param launch_position Vehicle launch position in local coordinates (NE) [m]
* @param takeoff_waypoint Takeoff waypoint position in local coordinates (NE) [m]
* @return Unit takeoff bearing vector
*/
Vector2f calculateTakeoffBearingVector(const Vector2f &launch_position, const Vector2f &takeoff_waypoint) const;
/**
* @brief Calculates the touchdown position for landing with optional manual lateral adjustments.
*
@ -804,13 +795,41 @@ private:
const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Line (infinite) following logic. Two points on the line are used to define the
* line in 2D space (first to second point determines the direction). Determines the
* relevant parameters for evaluating the NPFG guidance law, then updates control setpoints.
*
* @param[in] point_on_line_1 Line segment start position in local coordinates. (N,E) [m]
* @param[in] point_on_line_2 Line segment end position in local coordinates. (N,E) [m]
* @param[in] vehicle_pos Vehicle position in WGS84 coordinates (N,E) [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, const Vector2f &vehicle_pos,
const Vector2f &ground_vel, const Vector2f &wind_vel);
/*
* Line (infinite) following logic. One point on the line and a line bearing are used to define
* the line in 2D space. Determines the relevant parameters for evaluating the NPFG guidance law,
* then updates control setpoints.
*
* @param[in] point_on_line Arbitrary position on line in local coordinates. (N,E) [m]
* @param[in] line_bearing Line bearing [rad] (from north)
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateLine(const Vector2f &point_on_line, const float line_bearing, const Vector2f &vehicle_pos,
const Vector2f &ground_vel, const Vector2f &wind_vel);
/*
* Loitering (unlimited) logic. Takes loiter center, radius, and direction and
* determines the relevant parameters for evaluating the NPFG guidance law,
* then updates control setpoints.
*
* @param[in] loiter_center The position of the center of the loiter circle [m]
* @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
* @param[in] radius Loiter radius [m]
* @param[in] loiter_direction_counter_clockwise Specifies loiter direction
* @param[in] ground_vel Vehicle ground velocity vector [m/s]