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

mission: integrate dataman cache

Browse Source
This commit is contained in:
Igor Mišić 2023-05-10 13:19:55 +02:00 committed by Beat Küng
parent 6453a1c311
commit 80409672b1
4 changed files with 62 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
src/modules/navigator/mission.cpp
View File

@ -70,27 +70,48 @@ Mission::Mission(Navigator *navigator) :
mission_init();
}
void
Mission::run()
{
if ((_mission.count > 0) && (_current_mission_index != _load_mission_index)) {
uint32_t start_index = _current_mission_index;
uint32_t end_index = start_index + DATAMAN_CACHE_SIZE;
end_index = math::min(end_index, static_cast<uint32_t>(_mission.count));
for (uint32_t index = start_index; index < end_index; ++index) {
_dataman_cache.load(static_cast<dm_item_t>(_mission.dataman_id), index);
}
_load_mission_index = _current_mission_index;
}
_dataman_cache.update();
}
void Mission::mission_init()
{
// init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start
mission_s mission{};
if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s))) {
if ((mission.timestamp != 0)
&& (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) {
if (mission.count > 0) {
PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", mission.dataman_id, mission.count);
if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&_mission), sizeof(mission_s))) {
if ((_mission.timestamp != 0)
&& (_mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) {
if (_mission.count > 0) {
PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", _mission.dataman_id, _mission.count);
}
} else {
PX4_ERR("reading mission state failed");
// initialize mission state in dataman
mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
mission.timestamp = hrt_absolute_time();
_dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s));
_mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
_mission.timestamp = hrt_absolute_time();
_dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&_mission), sizeof(mission_s));
}
}
_current_mission_index = _mission.current_seq;
}
void
@ -464,7 +485,7 @@ Mission::find_mission_land_start()
missionitem_prev = missionitem; // store the last mission item before reading a new one
bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast<uint8_t *>(&missionitem),
sizeof(mission_item_s));
sizeof(mission_item_s), 500_ms);
if (!success) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@ -563,6 +584,9 @@ Mission::update_mission()
bool failed = true;
_dataman_cache.invalidate();
_load_mission_index = -1;
/* Reset vehicle_roi
* Missions that do not explicitly configure ROI would not override
* an existing ROI setting from previous missions */
@ -668,8 +692,8 @@ Mission::advance_mission()
for (int32_t i = _current_mission_index - 1; i >= 0; i--) {
struct mission_item_s missionitem = {};
bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast<uint8_t *>(&missionitem),
sizeof(mission_item_s));
bool success = _dataman_cache.loadWait(dm_current, i, reinterpret_cast<uint8_t *>(&missionitem),
sizeof(mission_item_s), 100_ms);
if (!success) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@ -1619,7 +1643,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
int index_to_read = current_index + offset;
int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read;
const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;
const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id;
/* do not work on empty missions */
if (_mission.count == 0) {
@ -1646,8 +1670,8 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
struct mission_item_s mission_item_tmp;
/* read mission item from datamanager */
bool success = _dataman_client.readSync(dm_item, *mission_index_ptr, reinterpret_cast<uint8_t *>(&mission_item_tmp),
sizeof(mission_item_s));
bool success = _dataman_cache.loadWait(dataman_id, *mission_index_ptr, reinterpret_cast<uint8_t *>(&mission_item_tmp),
sizeof(mission_item_s), 500_ms);
if (!success) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@ -1670,7 +1694,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
success = _dataman_client.writeSync(dm_item, *mission_index_ptr, reinterpret_cast<uint8_t *>(&mission_item_tmp),
success = _dataman_client.writeSync(dataman_id, *mission_index_ptr, reinterpret_cast<uint8_t *>(&mission_item_tmp),
sizeof(struct mission_item_s));
if (!success) {
@ -1722,13 +1746,6 @@ Mission::save_mission_state()
{
mission_s mission_state = {};
/* lock MISSION_STATE item */
bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE);
if (!success_lock) {
PX4_ERR("DM_KEY_MISSION_STATE lock failed");
}
/* read current state */
bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
sizeof(mission_s));
@ -1773,11 +1790,6 @@ Mission::save_mission_state()
PX4_ERR("Can't save mission state");
}
}
/* unlock MISSION_STATE item */
if (success_lock) {
_dataman_client.unlockSync(DM_KEY_MISSION_STATE);
}
}
void
@ -1815,7 +1827,7 @@ Mission::check_mission_valid(bool force)
{
if ((!_home_inited && _navigator->home_global_position_valid()) || force) {
MissionFeasibilityChecker _missionFeasibilityChecker(_navigator);
MissionFeasibilityChecker _missionFeasibilityChecker(_navigator, _dataman_client);
_navigator->get_mission_result()->valid =
_missionFeasibilityChecker.checkMissionFeasible(_mission);
@ -1833,8 +1845,6 @@ Mission::check_mission_valid(bool force)
void
Mission::reset_mission(struct mission_s &mission)
{
_dataman_client.lockSync(DM_KEY_MISSION_STATE);
if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s))) {
if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) {
/* set current item to 0 */
@ -1842,13 +1852,14 @@ Mission::reset_mission(struct mission_s &mission)
/* reset jump counters */
if (mission.count > 0) {
const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
const dm_item_t dataman_id = (dm_item_t)mission.dataman_id;
for (unsigned index = 0; index < mission.count; index++) {
struct mission_item_s item;
const ssize_t len = sizeof(struct mission_item_s);
bool success = _dataman_client.readSync(dm_current, index, reinterpret_cast<uint8_t *>(&item), sizeof(mission_item_s));
bool success = _dataman_client.readSync(dataman_id, index, reinterpret_cast<uint8_t *>(&item), sizeof(mission_item_s),
500_ms);
if (!success) {
PX4_WARN("could not read mission item during reset");
@ -1858,7 +1869,7 @@ Mission::reset_mission(struct mission_s &mission)
if (item.nav_cmd == NAV_CMD_DO_JUMP) {
item.do_jump_current_count = 0;
success = _dataman_client.writeSync(dm_current, index, reinterpret_cast<uint8_t *>(&item), len);
success = _dataman_client.writeSync(dataman_id, index, reinterpret_cast<uint8_t *>(&item), len);
if (!success) {
PX4_WARN("could not save mission item during reset");
@ -1881,8 +1892,6 @@ Mission::reset_mission(struct mission_s &mission)
_dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s));
}
_dataman_client.unlockSync(DM_KEY_MISSION_STATE);
}
bool
@ -1910,7 +1919,7 @@ Mission::index_closest_mission_item()
struct mission_item_s missionitem = {};
bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast<uint8_t *>(&missionitem),
sizeof(mission_item_s));
sizeof(mission_item_s), 500_ms);
if (!success) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */

10
src/modules/navigator/mission.h
View File

@ -74,6 +74,11 @@ public:
Mission(Navigator *navigator);
~Mission() override = default;
/**
* @brief function to call regularly to do background work
*/
void run();
void on_inactive() override;
void on_inactivation() override;
void on_activation() override;
@ -323,8 +328,11 @@ private:
uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */
mission_s _mission {};
DatamanClient _dataman_client{};
static constexpr uint32_t DATAMAN_CACHE_SIZE = 10;
DatamanCache _dataman_cache{"mission_dm_cache_miss", DATAMAN_CACHE_SIZE};
DatamanClient &_dataman_client = _dataman_cache.client();
int32_t _load_mission_index{-1};
int32_t _current_mission_index{-1};
// track location of planned mission landing

6
src/modules/navigator/mission_feasibility_checker.h
View File

@ -54,14 +54,16 @@ class MissionFeasibilityChecker: public ModuleParams
{
private:
Navigator *_navigator{nullptr};
DatamanClient &_dataman_client;
FeasibilityChecker _feasibility_checker;
bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid);
public:
MissionFeasibilityChecker(Navigator *navigator) :
MissionFeasibilityChecker(Navigator *navigator, DatamanClient &dataman_client) :
ModuleParams(nullptr),
_navigator(navigator),
_dataman_client(dataman_client),
_feasibility_checker()
{
@ -75,6 +77,4 @@ public:
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(const mission_s &mission);
DatamanClient _dataman_client{};
};

2
src/modules/navigator/navigator_main.cpp
View File

@ -917,6 +917,8 @@ void Navigator::run()
publish_mission_result();
}
_mission.run();
perf_end(_loop_perf);
}
}