diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b353ad9437..6531210bdd 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 7e54b0cb3b..eb9afdb7f8 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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.