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

FlightTaskAuto: refactor _commanded_speed_ts -> _time_last_cruise_speed_override

This commit is contained in:
Matthias Grob 2022-04-12 16:45:46 +02:00 committed by Roman Bapst
parent 68cf686892
commit 97b2947416
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -81,7 +81,7 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp
_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; _yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
_updateTrajConstraints(); _updateTrajConstraints();
_is_emergency_braking_active = false; _is_emergency_braking_active = false;
_commanded_speed_ts = 0; _time_last_cruise_speed_override = 0;
return ret; return ret;
} }
@ -226,7 +226,7 @@ bool FlightTaskAuto::update()
void FlightTaskAuto::overrideCruiseSpeed(const float cruise_speed_m_s) void FlightTaskAuto::overrideCruiseSpeed(const float cruise_speed_m_s)
{ {
_mc_cruise_speed = cruise_speed_m_s; _mc_cruise_speed = cruise_speed_m_s;
_commanded_speed_ts = hrt_absolute_time(); _time_last_cruise_speed_override = hrt_absolute_time();
} }
void FlightTaskAuto::_prepareLandSetpoints() void FlightTaskAuto::_prepareLandSetpoints()
@ -354,8 +354,9 @@ bool FlightTaskAuto::_evaluateTriplets()
// Prioritize cruise speed from the triplet when it's valid and more recent than the previously commanded cruise speed // Prioritize cruise speed from the triplet when it's valid and more recent than the previously commanded cruise speed
const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed; const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed;
if (PX4_ISFINITE(cruise_speed_from_triplet) && cruise_speed_from_triplet >= 0 if (PX4_ISFINITE(cruise_speed_from_triplet)
&& _sub_triplet_setpoint.get().current.timestamp > _commanded_speed_ts) { && (cruise_speed_from_triplet > 0.f)
&& (_sub_triplet_setpoint.get().current.timestamp > _time_last_cruise_speed_override)) {
_mc_cruise_speed = cruise_speed_from_triplet; _mc_cruise_speed = cruise_speed_from_triplet;
} }

2
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -202,7 +202,7 @@ private:
_triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/ _triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
matrix::Vector3f _closest_pt; /**< closest point to the vehicle position on the line previous - target */ matrix::Vector3f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
hrt_abstime _commanded_speed_ts{0}; hrt_abstime _time_last_cruise_speed_override{0}; ///< timestamp the cruise speed was last time overriden using DO_CHANGE_SPEED
MapProjection _reference_position{}; /**< Class used to project lat/lon setpoint into local frame. */ MapProjection _reference_position{}; /**< Class used to project lat/lon setpoint into local frame. */
float _reference_altitude{NAN}; /**< Altitude relative to ground. */ float _reference_altitude{NAN}; /**< Altitude relative to ground. */