Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-10-06 09:02:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(navigator): reduce flash on non-VTOL targets

Browse Source
This commit is contained in:
jonas
2026-07-27 08:28:45 +02:00
committed by Beat Küng
parent 69b39a9cf7
commit 0e9d76a655
5 changed files with 37 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_route_cache.cpp
+12 -44
View File
@@ -45,8 +45,6 @@
#include "mission_route_types.h"
#include <inttypes.h>
#include <px4_platform_common/log.h>
void MissionRouteCache::update(const mission_s &mission)
@@ -196,7 +194,7 @@ void MissionRouteCache::updateMissionLandItemCache(const mission_s &mission)
_dataman_cache_land_item.invalidate();
if (valid_land_index && !valid_dataman_id) {
PX4_ERR("Mission land cache rejected invalid dataman id: %" PRIu8, mission.mission_dataman_id);
PX4_ERR("Mission land cache: invalid dataman id");
}
if (state.index >= 0) {
@@ -226,8 +224,7 @@ void MissionRouteCache::updateMissionLandItemCache(const mission_s &mission)
state.retry.clear();
} else {
PX4_WARN("Mission land cache invalid or incomplete, retrying mission_id=%" PRIu32 ", index=%" PRIi32,
state.mission_id, state.index);
PX4_WARN("Mission land cache retry");
_dataman_cache_land_item.invalidate();
state.retry.scheduleRetry(now);
}
@@ -258,7 +255,7 @@ bool MissionRouteCache::queueMissionLandItem()
return true;
}
PX4_WARN("Mission land cache queue failed, retrying! item=%" PRIu8 ", index=%" PRIi32, state.dataman_id, state.index);
PX4_WARN("Mission land cache retry");
return false;
}
@@ -290,7 +287,6 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission)
reinterpret_cast<uint8_t *>(&state.read_stats), sizeof(mission_stats_entry_s));
if (!success) {
state.error_state = SafePointDatamanState::kRead;
state.dataman_state = SafePointDatamanState::kError;
}
@@ -304,38 +300,17 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission)
}
if (!success) {
state.error_state = SafePointDatamanState::kReadWait;
state.dataman_state = SafePointDatamanState::kError;
break;
}
if (state.read_stats.num_items > DM_KEY_SAFE_POINTS_MAX) {
PX4_ERR("Safe points update failed! invalid count: %" PRIu16, state.read_stats.num_items);
state.error_state = SafePointDatamanState::kReadWait;
state.dataman_state = SafePointDatamanState::kError;
break;
}
if (state.read_stats.opaque_id != state.source_id) {
PX4_WARN("Safe point state id mismatch, expected %" PRIu32 ", got %" PRIu32,
state.source_id, state.read_stats.opaque_id);
state.error_state = SafePointDatamanState::kReadWait;
state.dataman_state = SafePointDatamanState::kError;
break;
}
if (state.read_stats.num_items > 0 && state.read_stats.dataman_id != state.source_dataman_id) {
PX4_WARN("Safe point dataman id mismatch, expected %" PRIu8 ", got %" PRIu8,
state.source_dataman_id, state.read_stats.dataman_id);
state.error_state = SafePointDatamanState::kReadWait;
state.dataman_state = SafePointDatamanState::kError;
break;
}
if (state.read_stats.num_items > 0 && state.read_stats.dataman_id != DM_KEY_SAFE_POINTS_0
&& state.read_stats.dataman_id != DM_KEY_SAFE_POINTS_1) {
PX4_ERR("Safe points update failed! invalid dataman id: %" PRIu8, state.read_stats.dataman_id);
state.error_state = SafePointDatamanState::kReadWait;
if (!(state.read_stats.num_items <= DM_KEY_SAFE_POINTS_MAX
&& state.read_stats.opaque_id == state.source_id
&& (state.read_stats.num_items == 0
|| (state.read_stats.dataman_id == state.source_dataman_id
&& (state.read_stats.dataman_id == DM_KEY_SAFE_POINTS_0
|| state.read_stats.dataman_id == DM_KEY_SAFE_POINTS_1))))) {
PX4_ERR("Safe point cache metadata invalid");
state.dataman_state = SafePointDatamanState::kError;
break;
}
@@ -358,18 +333,12 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission)
}
if (_dataman_cache_safepoint.size() != state.read_stats.num_items) {
PX4_WARN("Safe point cache resize failed, retrying! requested: %" PRIu16 ", actual: %d",
state.read_stats.num_items, _dataman_cache_safepoint.size());
state.error_state = SafePointDatamanState::kReadWait;
state.dataman_state = SafePointDatamanState::kError;
break;
}
for (int index = 0; index < state.read_stats.num_items; ++index) {
if (!_dataman_cache_safepoint.load(static_cast<dm_item_t>(state.read_stats.dataman_id), index)) {
PX4_WARN("Safe point cache queue failed, retrying! item=%" PRIu8 ", index=%d",
state.read_stats.dataman_id, index);
state.error_state = SafePointDatamanState::kReadWait;
state.dataman_state = SafePointDatamanState::kError;
break;
}
@@ -406,7 +375,6 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission)
state.retry.clear();
} else {
state.error_state = SafePointDatamanState::kLoad;
state.dataman_state = SafePointDatamanState::kError;
}
@@ -418,10 +386,10 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission)
const RetryBackoff retry = state.retry;
if (state.dataman_state == SafePointDatamanState::kError) {
PX4_WARN("Safe points update failed, retrying! error state: %" PRIu8, static_cast<uint8_t>(state.error_state));
PX4_WARN("Safe point cache retry");
} else {
PX4_ERR("Safe points update failed! invalid dataman state: %" PRIu8, static_cast<uint8_t>(state.dataman_state));
PX4_ERR("Safe point cache state invalid");
}
resetSafePointCacheState(false);
src/modules/navigator/mission_route_cache.h
-1
View File
@@ -155,7 +155,6 @@ private:
struct SafePointState {
SafePointDatamanState dataman_state{SafePointDatamanState::kUpdateRequestWait};
SafePointDatamanState error_state{SafePointDatamanState::kUpdateRequestWait};
mission_stats_entry_s read_stats{}; ///< Async DM_KEY_SAFE_POINTS_STATE read buffer.
mission_stats_entry_s cached_stats{}; ///< Identity of the safe-point set currently in the RAM cache.
uint32_t source_id{0}; ///< Active mission.safe_points_id being tracked.
src/modules/navigator/mission_route_provider.h
+4 -4
View File
@@ -70,11 +70,11 @@ public:
* items that follow it. The next rally point starts a new block.
* Invalid rally points are skipped so a later nearby valid rally point can still be considered.
*/
virtual land_approaches_s getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
land_approaches_s getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
float home_altitude_amsl) const;
virtual bool hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
float home_altitude_amsl) const;
virtual bool hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const;
bool hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
float home_altitude_amsl) const;
bool hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const;
protected:
/**
src/modules/navigator/mission_route_types.cpp
+1 -1
View File
@@ -91,7 +91,7 @@ bool extractSafePointPosition(const mission_item_s &safe_point_item, float home_
break;
default:
PX4_WARN("RTL safe point frame %u unsupported", static_cast<unsigned>(safe_point_item.frame));
PX4_WARN("RTL: unsupported rally frame");
return false;
}
src/modules/navigator/rtl.cpp
+20 -6
View File
@@ -303,7 +303,9 @@ void RTL::setRtlTypeAndDestination()
}
if (new_rtl_type == RtlType::RTL_DIRECT) {
#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL
landing_loiter = selectLandingApproach(destination);
#endif
if (!landing_loiter.isValid()) {
landing_loiter.lat = destination.lat;
@@ -369,8 +371,10 @@ void RTL::setRtlTypeAndDestination()
PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_point_index)
{
#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL
const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
#endif
const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache();
PositionYawSetpoint closest_safe_point{static_cast<double>(NAN), static_cast<double>(NAN), NAN, NAN};
@@ -414,14 +418,19 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
candidate_setpoint.lat, candidate_setpoint.lon)};
const bool current_safe_point_has_approaches{
#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL
const bool current_safe_point_has_approaches {
mission_route_cache.hasVtolLandApproachesAtSafePointIndex(current_seq, _home_pos_sub.get().alt)
};
_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
const bool approach_requirement_satisfied = !vtol_in_fw_mode || (_param_rtl_appr_force.get() == 0)
|| current_safe_point_has_approaches;
#else
constexpr bool approach_requirement_satisfied = true;
#endif
if (((dist + MIN_DIST_THRESHOLD) < min_dist)
&& (!vtol_in_fw_mode || (_param_rtl_appr_force.get() == 0) || current_safe_point_has_approaches)) {
if (((dist + MIN_DIST_THRESHOLD) < min_dist) && approach_requirement_satisfied) {
min_dist = dist;
closest_safe_point = candidate_setpoint;
safe_point_index = static_cast<uint8_t>(current_seq);
@@ -437,17 +446,22 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache();
float min_dist = FLT_MAX;
if (_param_rtl_type.get() != RTL_TYPE_SAFE_POINT_DIRECT) {
#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL
const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
_home_has_land_approach = mission_route_cache.hasVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
#endif
const bool prioritize_safe_points_over_home = ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode);
#if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL
const bool required_approach_missing_for_home = (vtol_in_fw_mode && (_param_rtl_appr_force.get() == 1) && !_home_has_land_approach);
#else
constexpr bool required_approach_missing_for_home = false;
#endif
// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we force approach landing for vtol in fw and it is not defined for home.
const bool deprioritize_home = prioritize_safe_points_over_home || required_approach_missing_for_home;