diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f6bcc9260b..1332abe232 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -53,6 +53,7 @@ add_subdirectory(l1 EXCLUDE_FROM_ALL) add_subdirectory(led EXCLUDE_FROM_ALL) add_subdirectory(matrix EXCLUDE_FROM_ALL) add_subdirectory(mathlib EXCLUDE_FROM_ALL) +add_subdirectory(mission EXCLUDE_FROM_ALL) add_subdirectory(mixer_module EXCLUDE_FROM_ALL) add_subdirectory(motion_planning EXCLUDE_FROM_ALL) add_subdirectory(npfg EXCLUDE_FROM_ALL) diff --git a/src/lib/mission/CMakeLists.txt b/src/lib/mission/CMakeLists.txt new file mode 100644 index 0000000000..5a7bec967d --- /dev/null +++ b/src/lib/mission/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(planned_mission_interface + planned_mission_interface.cpp +) diff --git a/src/lib/mission/planned_mission_interface.cpp b/src/lib/mission/planned_mission_interface.cpp new file mode 100644 index 0000000000..bfd445bcf7 --- /dev/null +++ b/src/lib/mission/planned_mission_interface.cpp @@ -0,0 +1,595 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file planned_mission_interface.cpp + * + */ + +#include "planned_mission_interface.h" + +#include + +#include "dataman/dataman.h" +#include "lib/geo/geo.h" +#include "modules/navigator/mission_block.h" + +void PlannedMissionInterface::update() +{ + _home_pos_sub.update(); + + if (!_is_land_start_item_searched && _home_pos_sub.get().valid_alt) { + findLandStartItem(); + _is_land_start_item_searched = true; + } + + if (_mission_sub.updated()) { + mission_s new_mission; + _mission_sub.update(&new_mission); + + if (isMissionValid(new_mission)) { + /* Check if it was updated externally*/ + if (new_mission.timestamp > _mission.timestamp) { + bool mission_waypoints_changed{checkMissionWaypointsChanged(_mission, new_mission)}; + _mission = new_mission; + + if (goToItem(new_mission.current_seq, true) == EXIT_SUCCESS) { + if (_home_pos_sub.get().valid_alt) { + findLandStartItem(); + + } else { + _is_land_start_item_searched = false; + } + + onMissionUpdate(mission_waypoints_changed); + } + } + } + } +} + +void PlannedMissionInterface::getPreviousPositionItems(int32_t start_index, struct mission_item_s items[], + size_t &num_found_items, uint8_t max_num_items) const +{ + num_found_items = 0u; + + int32_t next_mission_index{start_index}; + + for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { + if (next_mission_index < 0) { + break; + } + + mission_item_s next_mission_item; + bool found_next_item{false}; + + do { + found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == EXIT_SUCCESS; + next_mission_index--; + } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); + + if (found_next_item) { + items[item_idx] = next_mission_item; + num_found_items = item_idx + 1; + + } else { + break; + } + } +} + +void PlannedMissionInterface::getNextPositionItems(int32_t start_index, struct mission_item_s items[], + size_t &num_found_items, uint8_t max_num_items) const +{ + // Make sure vector does not contain any preexisting elements. + num_found_items = 0u; + + int32_t next_mission_index{start_index}; + + for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { + if (next_mission_index >= _mission.count) { + break; + } + + mission_item_s next_mission_item; + bool found_next_item{false}; + + do { + found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == EXIT_SUCCESS; + next_mission_index++; + } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); + + if (found_next_item) { + items[item_idx] = next_mission_item; + num_found_items = item_idx + 1; + + } else { + break; + } + } +} + +int PlannedMissionInterface::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, + bool write_jumps, bool mission_direction_backward) const +{ + if (mission_index >= _mission.count || mission_index < 0) { + return EXIT_FAILURE; + } + + int32_t new_mission_index{mission_index}; + mission_item_s new_mission; + + for (uint16_t jump_count = 0u; jump_count < _max_jump_iteraion; jump_count++) { + if (readMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) { + PX4_ERR("Could not read next item."); + return EXIT_FAILURE; + } + + if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) { + if (new_mission.do_jump_mission_index >= _mission.count || new_mission.do_jump_mission_index < 0) { + PX4_ERR("Do Jump mission index is out of bounds."); + return EXIT_FAILURE; + } + + if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) { + if (write_jumps) { + new_mission.do_jump_current_count++; + + if (writeMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) { + PX4_ERR("Could not update jump count on mission item."); + // Still continue searching for next non jump item. + } + } + + new_mission_index = new_mission.do_jump_mission_index; + + } else { + if (mission_direction_backward) { + new_mission_index--; + + } else { + new_mission_index++; + } + } + + } else { + break; + } + } + + mission_index = new_mission_index; + mission = new_mission; + + return EXIT_SUCCESS; +} + +bool PlannedMissionInterface::hasMissionLandStart() const +{ + return (_land_start_index != _invalid_index) && (_land_start_index < _mission.count); +} + +int PlannedMissionInterface::goToNextItem(bool execute_jump) +{ + if (_mission.current_seq + 1 >= (_mission.count)) { + return EXIT_FAILURE; + } + + return goToItem(_mission.current_seq + 1, execute_jump); +} + +int PlannedMissionInterface::goToPreviousItem(bool execute_jump) +{ + if (_mission.current_seq <= 0) { + return EXIT_FAILURE; + } + + return goToItem(_mission.current_seq - 1, execute_jump, true); +} + +int PlannedMissionInterface::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward) +{ + mission_item_s mission_item; + + if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == EXIT_SUCCESS) { + if (setMissionIndex(index) == EXIT_SUCCESS) { + _current_planned_mission_item = mission_item; + + } else { + return EXIT_FAILURE; + } + + } else { + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} + +int PlannedMissionInterface::goToPreviousPositionItem(bool execute_jump) +{ + do { + if (goToPreviousItem(execute_jump) != EXIT_SUCCESS) { + return EXIT_FAILURE; + } + } while (!MissionBlock::item_contains_position(_current_planned_mission_item)); + + return EXIT_SUCCESS; +} + +int PlannedMissionInterface::goToNextPositionItem(bool execute_jump) +{ + do { + if (goToNextItem(execute_jump) != EXIT_SUCCESS) { + return EXIT_FAILURE; + } + } while (!MissionBlock::item_contains_position(_current_planned_mission_item)); + + return EXIT_SUCCESS; +} + +int PlannedMissionInterface::setMissionIndex(int32_t index) +{ + // Nothing to do if it is already at the current index. + if (index == _mission.current_seq) { + return EXIT_SUCCESS; + } + + mission_s mission{_mission}; + mission.current_seq = index; + mission.timestamp = hrt_absolute_time(); + + if (writeMission(mission) == EXIT_SUCCESS) { + _mission = mission; + return EXIT_SUCCESS; + + } else { + return EXIT_FAILURE; + } +} + +int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, float alt, float home_alt, + const vehicle_status_s &vehicle_status) +{ + int32_t min_dist_index(-1); + float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); + + for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) { + mission_item_s mission; + + if (readMissionItem(mission, mission_item_index) != EXIT_SUCCESS) { + PX4_ERR("Could not set mission closest to position."); + return EXIT_FAILURE; + } + + if (MissionBlock::item_contains_position(mission)) { + // do not consider land waypoints for a fw + if (!((mission.nav_cmd == NAV_CMD_LAND) && + (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + (!vehicle_status.is_vtol))) { + float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon, + MissionBlock::get_absolute_altitude_for_item(mission, home_alt), + lat, + lon, + alt, + &dist_xy, &dist_z); + + if (dist < min_dist) { + min_dist = dist; + min_dist_index = mission_item_index; + } + } + } + } + + return goToItem(min_dist_index, false); +} + +int PlannedMissionInterface::goToMissionLandStart() +{ + if (!hasMissionLandStart()) { + return EXIT_FAILURE; + } + + return goToItem(_land_start_index, false); +} + +int PlannedMissionInterface::initMission() +{ + mission_s mission; + bool ret_val{EXIT_FAILURE}; + + if (readMission(mission) == EXIT_SUCCESS) { + _mission = mission; + + if (goToItem(mission.current_seq, true) == EXIT_SUCCESS) { + findLandStartItem(); + ret_val = EXIT_SUCCESS; + } + + } else { + resetMission(); + } + + _mission_pub.advertise(); + + return ret_val; +} + +void PlannedMissionInterface::resetMission() +{ + /* we do not need to reset mission if there is none.*/ + if (_mission.count == 0u) { + return; + } + + /* Set a new mission*/ + mission_s new_mission{.timestamp = hrt_absolute_time(), + .current_seq = 0, + .count = 0u, + .dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0}; + + if (writeMission(new_mission) == EXIT_SUCCESS) { + _mission = new_mission; + + } else { + PX4_ERR("Mission Initialization failed."); + } +} + +void PlannedMissionInterface::resetMissionJumpCounter() +{ + for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) { + mission_item_s mission_item; + + if (readMissionItem(mission_item, mission_index) != EXIT_SUCCESS) { + PX4_ERR("Could not read mission item for jump count reset."); + break; + } + + if (mission_item.nav_cmd == NAV_CMD_DO_JUMP) { + mission_item.do_jump_current_count = 0u; + + if (writeMissionItem(mission_item, mission_index) != EXIT_SUCCESS) { + PX4_ERR("Could not write mission item for jump count reset."); + break; + } + } + } +} + +int PlannedMissionInterface::writeMission(mission_s &mission) +{ + int ret_val{EXIT_SUCCESS}; + + if (!isMissionValid(mission)) { + return EXIT_FAILURE; + } + + /* lock MISSION_STATE item */ + int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + + if (dm_lock_ret != 0) { + PX4_ERR("DM_KEY_MISSION_STATE lock failed"); + return EXIT_FAILURE; + } + + if (dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) { + PX4_ERR("Can't save mission state"); + ret_val = EXIT_FAILURE; + + } else { + _mission_pub.publish(mission); + } + + dm_unlock(DM_KEY_MISSION_STATE); + + return ret_val; +} + +int PlannedMissionInterface::readMission(mission_s &read_mission) const +{ + int ret_val{EXIT_SUCCESS}; + + /* lock MISSION_STATE item */ + int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + + if (dm_lock_ret != 0) { + PX4_ERR("DM lock failed."); + return EXIT_FAILURE; + } + + mission_s mission; + + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) { + PX4_ERR("Can't read mission state."); + ret_val = EXIT_FAILURE; + + } else { + if (isMissionValid(mission)) { + read_mission = mission; + + } else { + ret_val = EXIT_FAILURE; + } + } + + dm_unlock(DM_KEY_MISSION_STATE); + + return ret_val; +} + +int PlannedMissionInterface::readMissionItem(mission_item_s &read_mission_item, size_t index) const +{ + int ret_val{EXIT_SUCCESS}; + + if (index >= _mission.count) { + return EXIT_FAILURE; + } + + dm_item_t current_dm_item{static_cast(_mission.dataman_id)}; + + /* lock current mission item */ + /*int dm_lock_ret = dm_lock(current_dm_item); + + if (dm_lock_ret != 0) { + PX4_ERR("DM lock failed."); + return EXIT_FAILURE; + }*/ + + mission_item_s mission_item; + + if (dm_read(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) { + PX4_ERR("Can't read mission item from DM."); + ret_val = EXIT_FAILURE; + + } else { + read_mission_item = mission_item; + } + + //dm_unlock(current_dm_item); + + return ret_val; +} + +int PlannedMissionInterface::writeMissionItem(const mission_item_s &mission_item, size_t index) const +{ + int ret_val{EXIT_SUCCESS}; + + if (index >= _mission.count) { + return EXIT_FAILURE; + } + + dm_item_t current_dm_item{static_cast(_mission.dataman_id)}; + + /* lock current mission item */ + /*int dm_lock_ret = dm_lock(current_dm_item); + + if (dm_lock_ret != 0) { + PX4_ERR("DM lock failed."); + return EXIT_FAILURE; + }*/ + + if (dm_write(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) { + PX4_ERR("Can't write mission item to DM."); + ret_val = EXIT_FAILURE; + } + + //dm_unlock(current_dm_item); + + return ret_val; +} + +bool PlannedMissionInterface::isMissionValid(mission_s &mission) const +{ + bool ret_val{false}; + + if ((mission.current_seq < mission.count) && + (mission.current_seq >= 0) && + (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) && + (mission.timestamp != 0u)) { + ret_val = true; + + } + + return ret_val; +} + +void PlannedMissionInterface::findLandStartItem() +{ + _land_start_index = _invalid_index; + _land_index = _invalid_index; + + for (size_t mission_item_index = 1u; mission_item_index < _mission.count; mission_item_index++) { + mission_item_s mission; + + if (readMissionItem(mission, mission_item_index) == EXIT_SUCCESS) { + if (mission.nav_cmd == NAV_CMD_DO_LAND_START) { + _land_start_index = mission_item_index; + } + + if ((mission.nav_cmd == NAV_CMD_VTOL_LAND) || (mission.nav_cmd == NAV_CMD_LAND)) { + _land_index = mission_item_index; + break; + } + + } else { + break; + } + } + + if (_land_start_index != _invalid_index) { + mission_item_s mission; + size_t num_found_item{0u}; + getNextPositionItems(_land_start_index, &mission, num_found_item, 1u); + + if (num_found_item == 1u) { + _land_start_pos.lat = mission.lat; + _land_start_pos.lon = mission.lon; + _land_start_pos.alt = mission.altitude_is_relative ? mission.altitude + + _home_pos_sub.get().alt : mission.altitude; + + } else { + PX4_ERR("Could not read land start coordinates."); + _land_start_pos.lat = 0.0; + _land_start_pos.lon = 0.0; + } + + _land_pos.lat = _land_start_pos.lat; + _land_pos.lon = _land_start_pos.lon; + _land_pos.alt = _land_start_pos.alt; + } + + if (_land_index != _invalid_index) { + mission_item_s mission; + + if (readMissionItem(mission, _land_index) == EXIT_SUCCESS) { + _land_pos.lat = mission.lat; + _land_pos.lon = mission.lon; + _land_pos.alt = mission.altitude_is_relative ? mission.altitude + + _home_pos_sub.get().alt : mission.altitude; + } + + if (_land_start_index == _invalid_index) { + _land_start_index = _land_index; + _land_start_pos.lat = _land_pos.lat; + _land_start_pos.lon = _land_pos.lon; + _land_start_pos.alt = _land_pos.alt; + } + } +} + +bool PlannedMissionInterface::checkMissionWaypointsChanged(const mission_s &old_mission, + const mission_s &new_mission) const +{ + return (new_mission.count != old_mission.count) || (new_mission.dataman_id != old_mission.dataman_id); +} diff --git a/src/lib/mission/planned_mission_interface.h b/src/lib/mission/planned_mission_interface.h new file mode 100644 index 0000000000..3358adb76a --- /dev/null +++ b/src/lib/mission/planned_mission_interface.h @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file planned_mission_interface.h + * + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include "navigator/navigation.h" + +class PlannedMissionInterface +{ +public: + void update(); + void getPreviousPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items, + uint8_t max_num_items) const; + void getNextPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items, + uint8_t max_num_items) const; + bool hasMissionLandStart() const; + int goToNextItem(bool execute_jump); + int goToPreviousItem(bool execute_jump); + int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false); + int goToPreviousPositionItem(bool execute_jump); + int goToNextPositionItem(bool execute_jump); + int goToMissionLandStart(); + int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status); + virtual void onMissionUpdate(bool has_mission_items_changed) = 0; + + int initMission(); + void resetMission(); + void resetMissionJumpCounter(); + +private: + int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps, + bool mission_direction_backward = false) const; + int setMissionIndex(int32_t index); + int writeMission(mission_s &mission); + int readMission(mission_s &read_mission) const; + int readMissionItem(mission_item_s &read_mission_item, size_t index) const; + int writeMissionItem(const mission_item_s &mission_item, size_t index) const ; + bool isMissionValid(mission_s &mission) const; + void findLandStartItem(); + bool checkMissionWaypointsChanged(const mission_s &old_mission, const mission_s &new_mission) const; +public: + static const uint16_t _invalid_index{UINT16_MAX}; +private: + static const uint16_t _max_jump_iteraion{10u}; +protected: + mission_s _mission; + mission_item_s _current_planned_mission_item; + uint16_t _land_start_index; + uint16_t _land_index; + struct { + double lat; + double lon; + float alt; + } _land_start_pos{.lat = 0.0, + .lon = 0.0, + .alt = 0.0f}, + _land_pos{.lat = 0.0, + .lon = 0.0, + .alt = 0.0f}; + +private: + bool _is_land_start_item_searched{false}; + + uORB::Subscription _mission_sub{ORB_ID(mission)}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ + uORB::Publication _mission_pub{ORB_ID(mission)}; +}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index aee4b1a1cc..2b6482c698 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -924,9 +924,14 @@ MissionBlock::mission_apply_limitation(mission_item_s &item) float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const +{ + return get_absolute_altitude_for_item(mission_item, _navigator->get_home_position()->alt); +} + +float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt) { if (mission_item.altitude_is_relative) { - return mission_item.altitude + _navigator->get_home_position()->alt; + return mission_item.altitude + home_alt; } else { return mission_item.altitude; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index c2c05eb17d..2a1c8f199b 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -99,6 +99,8 @@ public: */ static bool item_contains_marker(const mission_item_s &item); + static float get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt); + /** * @brief Set the payload deployment successful flag object *