mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-30 08:33:35 +08:00
Mission+RTL: Refactoring mission and RTL to keep them separate. RTL does all its mission related computation in its own class.
Dataman: Add write function to dataman cache. RTL and mission have a new common base class mission_base. Both inherit from them and mission, RTL mission, and rtl reverse mission inherit from them and implement their desired functionalities. This simplifies the logic in mission as well as make the logic in rtl mission reverse and mission more readable. Rtl mission reverse now functional again for VTOL flying back the mission and transitioning to MC at the home position. Dataman cache has new write functionality to write to dataman while updating write item in its cache if necessary. Dataman cache is now only updated when the respective module is active. Leads to a higher computation time once on activation, but decreases unnecessary cache updates when inactive.
This commit is contained in:
Konrad
@@ -0,0 +1,446 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 mission_base.h
|
||||
*
|
||||
* Mission base mode class that can be used for modes interacting with a mission.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <dataman_client/DatamanClient.hpp>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/navigator_mission_item.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
#include "mission_block.h"
|
||||
#include "navigation.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class Navigator;
|
||||
|
||||
class MissionBase : public MissionBlock, public ModuleParams
|
||||
{
|
||||
public:
|
||||
MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed);
|
||||
~MissionBase() override = default;
|
||||
|
||||
virtual void on_inactive() override;
|
||||
virtual void on_inactivation() override;
|
||||
virtual void on_activation() override;
|
||||
virtual void on_active() override;
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief Maximum time to wait for dataman loading
|
||||
*
|
||||
*/
|
||||
static constexpr hrt_abstime MAX_DATAMAN_LOAD_WAIT{500_ms};
|
||||
|
||||
// Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message
|
||||
enum class WorkItemType {
|
||||
WORK_ITEM_TYPE_DEFAULT, /**< default mission item */
|
||||
WORK_ITEM_TYPE_CLIMB, /**< takeoff before moving to waypoint */
|
||||
WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */
|
||||
WORK_ITEM_TYPE_ALIGN_HEADING, /**< align for next waypoint */
|
||||
WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF,
|
||||
WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
|
||||
WORK_ITEM_TYPE_PRECISION_LAND
|
||||
} _work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */
|
||||
|
||||
enum class MissionType {
|
||||
MISSION_TYPE_NONE,
|
||||
MISSION_TYPE_MISSION
|
||||
} _mission_type{MissionType::MISSION_TYPE_NONE};
|
||||
|
||||
/**
|
||||
* @brief Get the Previous Mission Position Items
|
||||
*
|
||||
* @param[in] start_index is the index from where to start searching the previous mission position items
|
||||
* @param[out] items_index is an array of indexes indicating the previous mission position items found
|
||||
* @param[out] num_found_items are the amount of previous position items found
|
||||
* @param[in] max_num_items are the maximum amount of previous position items to be searched
|
||||
*/
|
||||
void getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items,
|
||||
uint8_t max_num_items);
|
||||
/**
|
||||
* @brief Get the next mission item containing a position setpoint
|
||||
*
|
||||
* @param[in] start_index is the index from where to start searching (first possible return index)
|
||||
* @param[out] items_index is an array of indexes indicating the next mission position items found
|
||||
* @param[out] num_found_items are the amount of next position items found
|
||||
* @param[in] max_num_items are the maximum amount of next position items to be searched
|
||||
*/
|
||||
void getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items,
|
||||
uint8_t max_num_items);
|
||||
/**
|
||||
* @brief Has Mission a Land Start or Land Item
|
||||
*
|
||||
* @return true If mission has a land start of land item
|
||||
* @return false otherwise
|
||||
*/
|
||||
bool hasMissionLandStart() const { return _mission.land_start_index > 0;};
|
||||
/**
|
||||
* @brief Go to next Mission Item
|
||||
* Go to next non jump mission item
|
||||
*
|
||||
* @param[in] execute_jump Flag indicating if a jump should be executed or ignored
|
||||
* @return PX4_OK if next mission item exists, PX4_ERR otherwise
|
||||
*/
|
||||
int goToNextItem(bool execute_jump);
|
||||
/**
|
||||
* @brief Go to previous Mission Item
|
||||
* Go to previous non jump mission item
|
||||
* @param[in] execute_jump Flag indicating if a jump should be executed or ignored
|
||||
* @return PX4_OK if previous mission item exists, PX4_ERR otherwise
|
||||
*/
|
||||
int goToPreviousItem(bool execute_jump);
|
||||
/**
|
||||
* @brief Go to Mission Item
|
||||
*
|
||||
* @param[in] index Index of the mission item to go to
|
||||
* @param[in] execute_jump Flag indicating if a jump should be executed of ignored
|
||||
* @param[in] mission_direction_backward Flag indicating if a mission is flown backward
|
||||
* @return PX4_OK if the mission item exists, PX4_ERR otherwise
|
||||
*/
|
||||
int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false);
|
||||
/**
|
||||
* @brief Go To Previous Mission Position Item
|
||||
*
|
||||
* @param[in] execute_jump Flag indicating if a jump should be executed or ignored
|
||||
* @return PX4_OK if previous mission item exists, PX4_ERR otherwise
|
||||
*/
|
||||
int goToPreviousPositionItem(bool execute_jump);
|
||||
/**
|
||||
* @brief Go To Next Mission Position Item
|
||||
*
|
||||
* @param[in] execute_jump Flag indicating if a jump should be executed or ignored
|
||||
* @return PX4_OK if next mission item exists, PX4_ERR otherwise
|
||||
*/
|
||||
int goToNextPositionItem(bool execute_jump);
|
||||
/**
|
||||
* @brief Go to Mission Land Start Item
|
||||
*
|
||||
* @return PX4_OK if land start item exists and is loaded, PX4_ERR otherwise
|
||||
*/
|
||||
int goToMissionLandStart();
|
||||
/**
|
||||
* @brief Set the Mission to closest mission position item from current position
|
||||
*
|
||||
* @param[in] lat latitude of the current position
|
||||
* @param[in] lon longitude of the current position
|
||||
* @param[in] alt altitude of the current position
|
||||
* @param[in] home_alt altitude of the home position
|
||||
* @param[in] vehicle_status vehicle status struct
|
||||
* @return PX4_OK if closest item is found and loaded, PX4_ERR otherwise
|
||||
*/
|
||||
int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status);
|
||||
/**
|
||||
* @brief Initialize Mission
|
||||
*
|
||||
* @return PX4_OK if mission could be loaded, PX4_ERR otherwise
|
||||
*/
|
||||
int initMission();
|
||||
/**
|
||||
* @brief Reset Mission
|
||||
*
|
||||
*/
|
||||
void resetMission();
|
||||
/**
|
||||
* @brief Reset Mission Jump Counter of Mission Jump Items
|
||||
*
|
||||
*/
|
||||
void resetMissionJumpCounter();
|
||||
/**
|
||||
* @brief Get the Non Jump Mission Item
|
||||
*
|
||||
* @param[out] mission_index Index of the mission item
|
||||
* @param[out] mission The return mission item
|
||||
* @param execute_jump Flag indicating if a jump item should be executed or ignored
|
||||
* @param write_jumps Flag indicating if the jump counter should be updated
|
||||
* @param mission_direction_backward Flag indicating if the mission is flown backwards
|
||||
* @return PX4_OK if mission item could be loaded, PX4_ERR otherwise
|
||||
*/
|
||||
int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps,
|
||||
bool mission_direction_backward = false);
|
||||
/**
|
||||
* @brief Is Mission Parameters Valid
|
||||
*
|
||||
* @param mission Mission struct
|
||||
* @return true is mission parameters are valid
|
||||
* @return false otherwise
|
||||
*/
|
||||
bool isMissionValid(mission_s &mission) const;
|
||||
|
||||
/**
|
||||
* On mission update
|
||||
* Change behaviour after external mission update.
|
||||
* @param[in] has_mission_items_changed flag if the mission items have been changed.
|
||||
*/
|
||||
void onMissionUpdate(bool has_mission_items_changed);
|
||||
|
||||
/**
|
||||
* Update mission topic
|
||||
*/
|
||||
void update_mission();
|
||||
|
||||
/**
|
||||
* Move on to next mission item or switch to loiter
|
||||
*/
|
||||
void advance_mission();
|
||||
|
||||
/**
|
||||
* @brief Configures mission items in current setting
|
||||
*
|
||||
* Configure the mission items depending on current mission item index and settings such
|
||||
* as terrain following, etc.
|
||||
*/
|
||||
void set_mission_items();
|
||||
|
||||
/**
|
||||
* @brief Load current mission item
|
||||
*
|
||||
* Load current mission item from dataman cache.
|
||||
*/
|
||||
void loadCurrentMissionItem();
|
||||
|
||||
/**
|
||||
* Set the mission result
|
||||
*/
|
||||
void set_mission_result();
|
||||
|
||||
/**
|
||||
* @brief Reset the item cache
|
||||
*/
|
||||
void resetItemCache();
|
||||
|
||||
/**
|
||||
* @brief Set the actions to be performed on Active Mission Item
|
||||
*
|
||||
*/
|
||||
virtual void setActiveMissionItems() = 0;
|
||||
/**
|
||||
* @brief Set the Next Mission Item after old mission item has been completed
|
||||
*
|
||||
* @return true if the next mission item could be set
|
||||
* @return false otherwise
|
||||
*/
|
||||
virtual bool setNextMissionItem() = 0;
|
||||
/**
|
||||
* @brief Set action at the end of the mission
|
||||
*
|
||||
*/
|
||||
void setEndOfMissionItems();
|
||||
/**
|
||||
* @brief Publish navigator mission item
|
||||
*
|
||||
*/
|
||||
void publish_navigator_mission_item();
|
||||
/**
|
||||
* @brief I position setpoint equal
|
||||
*
|
||||
* @param p1 First position setpoint to compare
|
||||
* @param p2 Second position setpoint to compare
|
||||
* @return true if both setpoints are equal
|
||||
* @return false otherwise
|
||||
*/
|
||||
bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
|
||||
|
||||
bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/
|
||||
bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/
|
||||
bool _initialized_mission_checked{false}; /**< Flag indicating if the initialized mission has been checked by the mission validator*/
|
||||
bool _system_disarmed_while_inactive{false}; /**< Flag indicating if the system has been disarmed while mission is inactive*/
|
||||
mission_s _mission; /**< Currently active mission*/
|
||||
float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */
|
||||
|
||||
DatamanCache _dataman_cache{"mission_dm_cache_miss", 10}; /**< Dataman cache of mission items*/
|
||||
DatamanClient &_dataman_client = _dataman_cache.client(); /**< Dataman client*/
|
||||
|
||||
uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription*/
|
||||
uORB::SubscriptionData<vehicle_land_detected_s> _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
|
||||
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
|
||||
uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
|
||||
uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; /**< Navigator mission item publication*/
|
||||
uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)}; /**< Mission publication*/
|
||||
private:
|
||||
/**
|
||||
* @brief Maximum number of jump mission items iterations
|
||||
*
|
||||
*/
|
||||
static constexpr uint16_t MAX_JUMP_ITERATION{10u};
|
||||
/**
|
||||
* @brief Update Dataman cache
|
||||
*
|
||||
*/
|
||||
void updateDatamanCache();
|
||||
/**
|
||||
* @brief Update mission subscription
|
||||
*
|
||||
*/
|
||||
void updateMavlinkMission();
|
||||
|
||||
/**
|
||||
* Check whether a mission is ready to go
|
||||
*/
|
||||
void check_mission_valid();
|
||||
|
||||
/**
|
||||
* Reset mission
|
||||
*/
|
||||
void checkMissionRestart();
|
||||
|
||||
/**
|
||||
* Set a mission item as reached
|
||||
*/
|
||||
void set_mission_item_reached();
|
||||
|
||||
/**
|
||||
* Updates the heading of the vehicle. Rotary wings only.
|
||||
*/
|
||||
void heading_sp_update();
|
||||
|
||||
/**
|
||||
* Abort landing
|
||||
*/
|
||||
void do_abort_landing();
|
||||
|
||||
/**
|
||||
* Inform about a changed mission item after a DO_JUMP
|
||||
*/
|
||||
void report_do_jump_mission_changed(int index, int do_jumps_remaining);
|
||||
|
||||
/**
|
||||
* @brief Cache the mission items containing gimbal, camera mode and trigger commands
|
||||
*
|
||||
* @param mission_item The mission item to cache if applicable
|
||||
*/
|
||||
void cacheItem(const mission_item_s &mission_item);
|
||||
|
||||
/**
|
||||
* @brief Update the cached items up to the given index
|
||||
*
|
||||
* @param end_index The index to update up to
|
||||
*/
|
||||
void updateCachedItemsUpToIndex(int end_index);
|
||||
|
||||
/**
|
||||
* @brief Replay the cached gimbal and camera mode items
|
||||
*/
|
||||
void replayCachedGimbalCameraItems();
|
||||
|
||||
/**
|
||||
* @brief Replay the cached trigger items
|
||||
*
|
||||
*/
|
||||
void replayCachedTriggerItems();
|
||||
|
||||
/**
|
||||
* @brief Replay the cached speed change items and delete them afterwards
|
||||
*
|
||||
*/
|
||||
void replayCachedSpeedChangeItems();
|
||||
|
||||
/**
|
||||
* @brief Check if there are cached gimbal or camera mode items to be replayed
|
||||
*
|
||||
* @return true if there are cached items
|
||||
*/
|
||||
bool haveCachedGimbalOrCameraItems();
|
||||
|
||||
/**
|
||||
* @brief Check if the camera was triggering
|
||||
*
|
||||
* @return true if there was a camera trigger command in the cached items that didn't disable triggering
|
||||
*/
|
||||
bool cameraWasTriggering();
|
||||
|
||||
/**
|
||||
* @brief Set the Mission Index
|
||||
*
|
||||
* @param[in] index Index of the mission item
|
||||
*/
|
||||
void setMissionIndex(int32_t index);
|
||||
|
||||
/**
|
||||
* @brief Parameters update
|
||||
*
|
||||
* Check for parameter changes and update them if needed.
|
||||
*/
|
||||
void parameters_update();
|
||||
|
||||
/**
|
||||
* @brief Check if a climb is necessary to align with mission altitude prior to starting the mission
|
||||
*
|
||||
* @param mission_item_index The index of the mission item to check if a climb is necessary
|
||||
*/
|
||||
void checkClimbRequired(int32_t mission_item_index);
|
||||
|
||||
/**
|
||||
* @brief check if relevant data in the new mission have changed.
|
||||
* @param[in] new_mission new mission received over uorb
|
||||
* @return true if the relevant mission data has changed, false otherwise
|
||||
*/
|
||||
bool checkMissionDataChanged(mission_s new_mission);
|
||||
|
||||
int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/
|
||||
int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/
|
||||
|
||||
int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images.
|
||||
bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment.
|
||||
|
||||
mission_item_s _last_gimbal_configure_item {};
|
||||
mission_item_s _last_gimbal_control_item {};
|
||||
mission_item_s _last_camera_mode_item {};
|
||||
mission_item_s _last_camera_trigger_item {};
|
||||
mission_item_s _last_speed_change_item {};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
|
||||
(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
|
||||
)
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
};
|
||||
Reference in New Issue
Block a user