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

NPFG: Add fallback for corner cases

This commit is contained in:
Konrad 2023-10-26 08:45:02 +02:00 committed by Silvan Fuhrer
parent 1c25d65a1e
commit 1d07697a9e
5 changed files with 83 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/NpfgStatus.msg
View File

@ -14,3 +14,4 @@ float32 min_ground_speed_ref # minimum forward ground speed reference [m/s]
float32 adapted_period # adapted period (if auto-tuning enabled) [s]
float32 p_gain # controller proportional gain [rad/s]
float32 time_const # controller time constant [s]
float32 can_run_factor # estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]

26
src/lib/npfg/npfg.cpp
View File

@ -47,6 +47,32 @@
using matrix::Vector2d;
using matrix::Vector2f;
float NPFG::canRun(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const
{
if (is_wind_valid) {
// If we have a valid wind estimate, npfg is able to handle all degenerated cases
return 1.f;
}
// NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed
// Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle
const Vector2f ground_vel(local_pos.vx, local_pos.vy);
const float ground_speed(ground_vel.norm());
const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID *
local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh),
0.f, 1.f));
// Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction.
const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f}));
const Vector2f ground_vel_norm(ground_vel.normalized());
const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) -
COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK -
COS_HEADING_TRACK_ANGLE_PUSHED_BACK)),
0.f, 1.f));
return flying_forward_factor * low_ground_speed_factor;
}
void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel,
const Vector2f &unit_path_tangent,
const Vector2f &position_on_path, const float path_curvature)

17
src/lib/npfg/npfg.hpp
View File

@ -63,6 +63,8 @@
#include <matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
#include <uORB/topics/vehicle_local_position.h>
/*
* NPFG
* Lateral-directional nonlinear path following guidance logic with excess wind handling
@ -71,6 +73,17 @@ class NPFG
{
public:
/**
* @brief Can run
*
* Evaluation if all the necessary information are available such that npfg can produce meaningful results.
*
* @param[in] local_pos is the current vehicle local position uorb message
* @param[in] is_wind_valid flag if the wind estimation is valid
* @return estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]. Can be used to define proper mitigation actions.
*/
float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const;
/*
* Computes the lateral acceleration and airspeed references necessary to track
* a path in wind (including excess wind conditions).
@ -270,6 +283,10 @@ public:
float getRollSetpoint() { return roll_setpoint_; }
private:
static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough
static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe
static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°)
static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind
static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon
static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m]

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

@ -475,6 +475,7 @@ FixedwingPositionControl::status_publish()
npfg_status.adapted_period = _npfg.getAdaptedPeriod();
npfg_status.p_gain = _npfg.getPGain();
npfg_status.time_const = _npfg.getTimeConst();
npfg_status.can_run_factor = _npfg.canRun(_local_pos, _wind_valid);
npfg_status.timestamp = hrt_absolute_time();
_npfg_status_pub.publish(npfg_status);
@ -504,6 +505,25 @@ FixedwingPositionControl::landing_status_publish()
_pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status);
}
float FixedwingPositionControl::getCorrectedNpfgRollSetpoint()
{
// Scale the npfg output to zero if npfg is not certain for correct output
float new_roll_setpoint(_npfg.getRollSetpoint());
const float can_run_factor(_npfg.canRun(_local_pos, _wind_valid));
if ((1.f - can_run_factor) < FLT_EPSILON) {
_need_report_npfg_uncertain_condition = true;
}
if (((1.f - can_run_factor) > FLT_EPSILON) && _need_report_npfg_uncertain_condition) {
_need_report_npfg_uncertain_condition = false;
events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning,
"Roll command reduced due to uncertain velocity/wind estimates!");
}
return can_run_factor * (new_roll_setpoint);
}
void
FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_status)
{
@ -1067,7 +1087,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
}
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
@ -1116,7 +1136,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.yaw_body = _yaw;
@ -1211,7 +1231,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
ground_speed,
_wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
@ -1397,7 +1417,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
_att_sp.roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint());
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@ -1495,7 +1515,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@ -1644,7 +1664,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
// use npfg's bearing to commanded course, controlled via yaw angle while on runway
_att_sp.yaw_body = _npfg.getBearing();
@ -1723,7 +1743,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
/* longitudinal guidance */
@ -1849,7 +1869,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
pos_sp_curr.loiter_direction_counter_clockwise,
ground_speed, _wind_vel);
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
@ -1926,7 +1946,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
pos_sp_curr.loiter_direction_counter_clockwise,
ground_speed, _wind_vel);
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
/* longitudinal guidance */
@ -2075,7 +2095,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
@ -2444,6 +2464,7 @@ FixedwingPositionControl::Run()
}
}
}
// if there's any change in landing gear setpoint publish it

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

@ -415,6 +415,7 @@ private:
// nonlinear path following guidance - lateral-directional position control
NPFG _npfg;
bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed
PerformanceModel _performance_model;
@ -468,6 +469,13 @@ private:
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
float getLoadFactor();
/**
* @brief Get the NPFG roll setpoint with mitigation strategy if npfg is not certain about its output
*
* @return roll setpoint
*/
float getCorrectedNpfgRollSetpoint();
/**
* @brief Sets the landing abort status and publishes landing status.
*