Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

FlightTaskAuto: remove generateSetpoints function, replace by implementation

Browse Source
This commit is contained in:
Matthias Grob
2021-11-17 09:21:56 +01:00
committed by Daniel Agar
parent ece8fdddec
commit 9eac0edbc0
2 changed files with 35 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+35 -45
View File
@@ -175,7 +175,41 @@ bool FlightTaskAuto::update()
_yawspeed_setpoint);
}
_generateSetpoints();
_checkEmergencyBraking();
Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp};
if (isTargetModified()) {
// In case object avoidance has injected a new setpoint, we take this as the next waypoints
waypoints[2] = _position_setpoint;
}
const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned;
_updateTrajConstraints();
PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
_position_smoothing.generateSetpoints(
_position,
waypoints,
_velocity_setpoint,
_deltatime,
should_wait_for_yaw_align,
smoothed_setpoints
);
_jerk_setpoint = smoothed_setpoints.jerk;
_acceleration_setpoint = smoothed_setpoints.acceleration;
_velocity_setpoint = smoothed_setpoints.velocity;
_position_setpoint = smoothed_setpoints.position;
_unsmoothed_velocity_setpoint = smoothed_setpoints.unsmoothed_velocity;
_want_takeoff = smoothed_setpoints.unsmoothed_velocity(2) < -0.3f;
if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
// no valid heading -> generate heading in this flight task
// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
if (!_generateHeadingAlongTraj()) {
_yaw_setpoint = PX4_ISFINITE(_yaw_sp_prev) ? _yaw_sp_prev : _yaw;
}
}
// update previous type
_type_previous = _type;
@@ -695,50 +729,6 @@ void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi)
_yaw_sp_prev += delta_psi;
}
void FlightTaskAuto::_generateSetpoints()
{
_checkEmergencyBraking();
Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp};
if (isTargetModified()) {
// In case object avoidance has injected a new setpoint, we take this as the next waypoints
waypoints[2] = _position_setpoint;
}
const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned;
_updateTrajConstraints();
PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
_position_smoothing.generateSetpoints(
_position,
waypoints,
_velocity_setpoint,
_deltatime,
should_wait_for_yaw_align,
smoothed_setpoints
);
_jerk_setpoint = smoothed_setpoints.jerk;
_acceleration_setpoint = smoothed_setpoints.acceleration;
_velocity_setpoint = smoothed_setpoints.velocity;
_position_setpoint = smoothed_setpoints.position;
_unsmoothed_velocity_setpoint = smoothed_setpoints.unsmoothed_velocity;
_want_takeoff = smoothed_setpoints.unsmoothed_velocity(2) < -0.3f;
if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
// no valid heading -> generate heading in this flight task
_generateHeading();
}
}
void FlightTaskAuto::_generateHeading()
{
// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
if (!_generateHeadingAlongTraj()) {
_yaw_setpoint = PX4_ISFINITE(_yaw_sp_prev) ? _yaw_sp_prev : _yaw;
}
}
void FlightTaskAuto::_checkEmergencyBraking()
{
if (!_is_emergency_braking_active) {
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
-2
View File
@@ -110,8 +110,6 @@ protected:
void _ekfResetHandlerVelocityZ(float delta_vz) override;
void _ekfResetHandlerHeading(float delta_psi) override;
void _generateSetpoints(); /**< Generate setpoints along line. */
void _generateHeading();
void _checkEmergencyBraking();
bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
bool isTargetModified() const;