fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 22:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mission: integrate dataman cache

Browse Source
This commit is contained in:
Igor Mišić
2023-07-24 13:10:31 +02:00
committed by Beat Küng
parent 6453a1c311
commit 80409672b1
4 changed files with 62 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+48 -39
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. */