fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 02:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Positon control: do not use position_setpoint.valid to validate current position_setpoint

Browse Source 
Instead of checking the .valid flag of position_setpoint, check for ISFINITE() of lat, lon, alt
    when pulling the position_setpoint triplet. This fixes problems where the .valid flag didn't
    reflect the proper state of the setpoint (e.g. .valid was true, .lat though NAN)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-10-21 09:51:38 +02:00
parent d8e483ae20
commit 67b2c835e0
2 changed files with 27 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+22 -11
View File
@@ -231,7 +231,7 @@ FixedwingPositionControl::vehicle_command_poll()
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
// only abort landing before point of no return (horizontal and vertical)
if (_control_mode.flag_control_auto_enabled &&
_pos_sp_triplet.current.valid &&
_position_setpoint_current_valid &&
(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
updateLandingAbortStatus(position_controller_landing_status_s::ABORTED_BY_OPERATOR);
@@ -683,11 +683,9 @@ FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_
waypoint_prev = temp_prev;
waypoint_prev.alt = _current_altitude;
waypoint_prev.valid = true;
waypoint_next = temp_next;
waypoint_next.alt = _current_altitude;
waypoint_next.valid = true;
}
float
@@ -743,7 +741,7 @@ FixedwingPositionControl::updateManualTakeoffStatus()
}
void
FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid)
FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
{
/* only run position controller in fixed-wing mode and during transitions for VTOL */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
@@ -756,7 +754,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
_skipping_takeoff_detection = false;
if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
_control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
_control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) {
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
@@ -1089,7 +1087,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
if (pos_sp_prev.valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
prev_wp(0) = pos_sp_prev.lat;
prev_wp(1) = pos_sp_prev.lon;
@@ -1118,7 +1116,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
float position_sp_alt = pos_sp_curr.alt;
// Altitude first order hold (FOH)
if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
if (_position_setpoint_previous_valid &&
((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
) {
@@ -1268,7 +1266,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
if (pos_sp_prev.valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
prev_wp(0) = pos_sp_prev.lat;
prev_wp(1) = pos_sp_prev.lon;
@@ -1310,7 +1308,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid
&& in_circle_mode && _param_fw_lnd_earlycfg.get()) {
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter altitude control) already such that we don't
@@ -2183,6 +2181,19 @@ FixedwingPositionControl::Run()
} else {
if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
_position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat)
&& PX4_ISFINITE(_pos_sp_triplet.previous.lon)
&& PX4_ISFINITE(_pos_sp_triplet.previous.alt);
_position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat)
&& PX4_ISFINITE(_pos_sp_triplet.current.lon)
&& PX4_ISFINITE(_pos_sp_triplet.current.alt);
_position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat)
&& PX4_ISFINITE(_pos_sp_triplet.next.lon)
&& PX4_ISFINITE(_pos_sp_triplet.next.alt);
// reset the altitude foh (first order hold) logic
_min_current_sp_distance_xy = FLT_MAX;
}
@@ -2214,7 +2225,7 @@ FixedwingPositionControl::Run()
Vector2d curr_pos(_current_latitude, _current_longitude);
Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
set_control_mode_current(_local_pos.timestamp);
update_in_air_states(_local_pos.timestamp);
@@ -2563,7 +2574,7 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
// set the landing approach entrance location when we have just started the landing and store it
// NOTE: the landing approach vector is relative to the land point. ekf resets may cause a local frame
// jump, so we reference to the land point, which is globally referenced and will update
if (pos_sp_prev.valid) {
if (_position_setpoint_previous_valid) {
height_above_land_point = pos_sp_prev.alt - pos_sp_curr.alt;
local_approach_entrance = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon);
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+5 -2
View File
@@ -222,6 +222,10 @@ private:
vehicle_local_position_s _local_pos{};
vehicle_status_s _vehicle_status{};
bool _position_setpoint_previous_valid{false};
bool _position_setpoint_current_valid{false};
bool _position_setpoint_next_valid{false};
perf_counter_t _loop_perf; // loop performance counter
// [us] Last absolute time position control has been called
@@ -664,9 +668,8 @@ private:
* May also change the position setpoint type depending on the desired behavior.
*
* @param now Current system time [us]
* @param pos_sp_curr_valid True if the current position setpoint is valid
*/
void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
void set_control_mode_current(const hrt_abstime &now);
/**
* @brief Compensate trim throttle for air density and vehicle weight.