mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 04:03:33 +08:00
465 lines
16 KiB
C++
465 lines
16 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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 and a land item
|
|
* @return false otherwise
|
|
*/
|
|
bool hasMissionLandStart() const { return _mission.land_start_index > 0 && _mission.land_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 Do need move to item
|
|
*
|
|
* @return true if the item is horizontally further away than the mission item
|
|
* @return false otherwise
|
|
*/
|
|
bool do_need_move_to_item();
|
|
|
|
/**
|
|
* @brief Handle landing
|
|
*
|
|
* @param new_work_item_type new work item type state machine to be set
|
|
* @param next_mission_items the next mission items after the current mission item
|
|
* @param num_found_items number of found next mission items
|
|
*/
|
|
void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
|
|
size_t &num_found_items);
|
|
/**
|
|
* @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 */
|
|
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.
|
|
|
|
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*/
|
|
|
|
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};
|
|
};
|