fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 23:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

NPFG: Add fallback for corner cases

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+31 -10
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
src/modules/fw_pos_control/FixedwingPositionControl.hpp
+8
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.
*