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+RTL: Refactoring mission and RTL to keep them separate. RTL does all its mission related computation in its own class.

Browse Source 
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 2023-06-27 15:42:49 +02:00 committed by Silvan Fuhrer
parent 1a7e438099
commit 007ed11bbe
24 changed files with 4561 additions and 3237 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/Mission.msg
View File

@ -4,6 +4,9 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam
uint16 count # count of the missions stored in the dataman
int32 current_seq # default -1, start at the one changed latest
int16 mission_update_counter # indicates updates to the mission, reload from dataman if increased
int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased
int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased
int32 land_start_index # Index of the land start marker, if unavailable index of the land item, -1 otherwise
int32 land_index # Index of the land item, -1 otherwise
uint16 mission_update_counter # indicates updates to the mission, reload from dataman if increased
uint16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased
uint16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased

26
src/lib/dataman_client/DatamanClient.cpp
View File

@ -556,6 +556,32 @@ bool DatamanCache::loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uin
return success;
}
bool DatamanCache::writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout)
{
if (length > g_per_item_size[item]) {
PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast<uint8_t>(item));
return false;
}
bool success = _client.writeSync(item, index, buffer, length, timeout);
if (success && _items) {
for (uint32_t i = 0; i < _num_items; ++i) {
if ((_items[i].response.item == item) &&
(_items[i].response.index == index) &&
((_items[i].cache_state == State::ResponseReceived) ||
(_items[i].cache_state == State::RequestPrepared))) {
memcpy(_items[i].response.data, buffer, length);
_items[i].cache_state = State::ResponseReceived;
break;
}
}
}
return success;
}
void DatamanCache::update()
{
if (_item_counter > 0) {

13
src/lib/dataman_client/DatamanClient.hpp
View File

@ -235,6 +235,19 @@ public:
*/
bool loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 0);
/**
* @brief Write data back and update it in the cache if stored.
*
* @param[in] item The data item type to write.
* @param[in] index The index of the data item.
* @param[in] buffer The buffer that contains the data to write.
* @param[in] length The length of the data to write.
* @param[in] timeout The maximum time in microseconds to wait for the response.
*
* @return True if the write operation succeeded, false otherwise.
*/
bool writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms);
/**
* @brief Updates the dataman cache by checking for responses from the DatamanClient and processing them.
*

5
src/modules/navigator/CMakeLists.txt
View File

@ -40,10 +40,15 @@ px4_add_module(
SRCS
navigator_main.cpp
navigator_mode.cpp
mission_base.cpp
mission_block.cpp
mission.cpp
loiter.cpp
rtl.cpp
rtl_direct.cpp
rtl_direct_mission_land.cpp
rtl_mission_fast.cpp
rtl_mission_fast_reverse.cpp
takeoff.cpp
land.cpp
precland.cpp

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

File diff suppressed because it is too large Load Diff

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

@ -46,92 +46,32 @@
#pragma once
#include "mission_block.h"
#include "mission_feasibility_checker.h"
#include "navigator_mode.h"
#include <cstdint>
#include <float.h>
#include <dataman_client/DatamanClient.hpp>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/navigator_mission_item.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/uORB.h>
#include "mission_base.h"
#include "navigation.h"
class Navigator;
class Mission : public MissionBlock, public ModuleParams
class Mission : public MissionBase
{
public:
Mission(Navigator *navigator);
~Mission() override = default;
~Mission() = default;
/**
* @brief function to call regularly to do background work
*/
void run();
void on_inactive() override;
void on_inactivation() override;
void on_activation() override;
void on_active() override;
virtual void on_inactive() override;
virtual void on_activation() override;
bool set_current_mission_index(uint16_t index);
bool land_start();
bool landing();
uint16_t get_land_start_index() const { return _mission.land_start_index; }
bool get_land_start_available() const { return hasMissionLandStart(); }
uint16_t get_land_start_index() const { return _land_start_index; }
bool get_land_start_available() const { return _land_start_available; }
bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; }
bool get_mission_changed() const { return _mission_changed ; }
bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; }
double get_landing_start_lat() { return _landing_start_lat; }
double get_landing_start_lon() { return _landing_start_lon; }
float get_landing_start_alt() { return _landing_start_alt; }
bool isLanding();
double get_landing_lat() { return _landing_lat; }
double get_landing_lon() { return _landing_lon; }
float get_landing_alt() { return _landing_alt; }
float get_landing_loiter_rad() { return _landing_loiter_radius; }
void set_closest_item_as_current();
/**
* Set a new mission mode and handle the switching between the different modes
*
* For a list of the different modes refer to mission_result.msg
*/
void set_execution_mode(const uint8_t mode);
private:
void mission_init();
/**
* 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();
bool setNextMissionItem() override;
/**
* Returns true if we need to move to waypoint location before starting descent
@ -144,232 +84,23 @@ private:
bool do_need_move_to_takeoff();
/**
* Copies position from setpoint if valid, otherwise copies current position
* Calculate takeoff height for mission item considering ground clearance
*/
void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint);
/**
* Create mission item to align towards next waypoint
*/
void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next);
/**
* Updates the heading of the vehicle. Rotary wings only.
*/
void heading_sp_update();
/**
* Abort landing
*/
void do_abort_landing();
/**
* Read the current and the next mission item. The next mission item read is the
* next mission item that contains a position.
*
* @return true if current mission item available
*/
bool prepare_mission_items(mission_item_s *mission_item,
mission_item_s *next_position_mission_item, bool *has_next_position_item,
mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr);
/**
* Read current (offset == 0) or a specific (offset > 0) mission item
* from the dataman and watch out for DO_JUMPS
*
* @return true if successful
*/
bool read_mission_item(int offset, struct mission_item_s *mission_item);
float calculate_takeoff_altitude(struct mission_item_s *mission_item);
/**
* Save current mission state to dataman
*/
void save_mission_state();
/**
* Inform about a changed mission item after a DO_JUMP
*/
void report_do_jump_mission_changed(int index, int do_jumps_remaining);
void setActiveMissionItems() override;
/**
* Set a mission item as reached
*/
void set_mission_item_reached();
void handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items);
/**
* Set the current mission item
*/
void set_current_mission_item();
void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items);
/**
* Check whether a mission is ready to go
*/
void check_mission_valid(bool force);
void handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
size_t &num_found_items);
/**
* Reset mission
*/
void reset_mission(struct mission_s &mission);
/**
* Returns true if we need to reset the mission (call this only when inactive)
*/
bool need_to_reset_mission();
/**
* Find and store the index of the landing sequence (DO_LAND_START)
*/
bool find_mission_land_start();
/**
* Return the index of the closest mission item to the current global position.
*/
int32_t index_closest_mission_item();
bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
void publish_navigator_mission_item();
/**
* @brief Get the index associated with the last item that contains a position
* @param mission The mission to search
* @param start_index The index to start searching from
* @param prev_pos_index The index of the previous position item containing a position
* @return true if a previous position item was found
*/
bool getPreviousPositionItemIndex(const mission_s &mission, int start_index, uint16_t &prev_pos_index) const;
/**
* @brief Get the next item after start_index that contains a position
*
* @param mission The mission to search
* @param start_index The index to start searching from
* @param mission_item The mission item to populate
* @return true if successful
*/
bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item);
/**
* @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 Reset the item cache
*/
void resetItemCache();
/**
* @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 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(uint16_t mission_item_index);
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::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item};
uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */
mission_s _mission {};
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
bool _land_start_available{false};
uint16_t _land_start_index{UINT16_MAX}; /**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */
double _landing_start_lat{0.0};
double _landing_start_lon{0.0};
float _landing_start_alt{0.0f};
double _landing_lat{0.0};
double _landing_lon{0.0};
float _landing_alt{0.0f};
float _landing_loiter_radius{0.f};
float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_MISSION
} _mission_type{MISSION_TYPE_NONE};
bool _inited{false};
bool _home_inited{false};
bool _need_mission_reset{false};
bool _need_mission_save{false};
bool _mission_waypoints_changed{false};
bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */
// Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message
enum work_item_type {
WORK_ITEM_TYPE_DEFAULT, /**< default mission item */
WORK_ITEM_TYPE_CLIMB, /**< climb at current position before moving to waypoint */
WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */
WORK_ITEM_TYPE_ALIGN_HEADING, /**< align heading 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{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */
uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
bool _execution_mode_changed{false};
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 {};
};

1235
src/modules/navigator/mission_base.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

446
src/modules/navigator/mission_base.h Normal file
View File

@ -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};
};

41
src/modules/navigator/mission_block.cpp
View File

@ -901,15 +901,54 @@ 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;
}
}
void
MissionBlock::copy_position_if_valid(struct mission_item_s *const mission_item,
const struct position_setpoint_s *const setpoint) const
{
if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
mission_item->lat = setpoint->lat;
mission_item->lon = setpoint->lon;
mission_item->altitude = setpoint->alt;
} else {
mission_item->lat = _navigator->get_global_position()->lat;
mission_item->lon = _navigator->get_global_position()->lon;
mission_item->altitude = _navigator->get_global_position()->alt;
}
mission_item->altitude_is_relative = false;
}
void
MissionBlock::set_align_mission_item(struct mission_item_s *const mission_item,
const struct mission_item_s *const mission_item_next) const
{
mission_item->nav_cmd = NAV_CMD_WAYPOINT;
copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current));
mission_item->altitude_is_relative = false;
mission_item->autocontinue = true;
mission_item->time_inside = 0.0f;
mission_item->yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
mission_item_next->lat, mission_item_next->lon);
mission_item->force_heading = true;
}
void
MissionBlock::initialize()
{

21
src/modules/navigator/mission_block.h
View File

@ -91,6 +91,15 @@ public:
*/
static bool item_contains_gate(const mission_item_s &item);
/**
* Get the absolute altitude for mission item
*
* @param mission_item the mission item of interest
* @param home_alt the home altitude in [m AMSL].
* @return Mission item altitude in [m AMSL]
*/
static float get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt);
/**
* Check if the mission item contains a marker
*
@ -124,6 +133,18 @@ public:
_payload_deploy_timeout_s = timeout_s;
}
/**
* Copies position from setpoint if valid, otherwise copies current position
*/
void copy_position_if_valid(struct mission_item_s *const mission_item,
const struct position_setpoint_s *const setpoint) const;
/**
* Create mission item to align towards next waypoint
*/
void set_align_mission_item(struct mission_item_s *const mission_item,
const struct mission_item_s *const mission_item_next) const;
protected:
/**
* Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items)

25
src/modules/navigator/navigator.h
View File

@ -77,6 +77,7 @@
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/mode_completed.h>
#include <uORB/uORB.h>
@ -246,32 +247,12 @@ public:
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
void increment_mission_instance_count() { _mission_result.instance_count++; }
int mission_instance_count() const { return _mission_result.instance_count; }
void set_mission_failure_heading_timeout();
bool is_planned_mission() const { return _navigation_mode == &_mission; }
bool on_mission_landing() { return (_mission.landing() && _navigation_mode == &_mission); }
bool start_mission_landing() { return _mission.land_start(); }
bool get_mission_start_land_available() { return _mission.get_land_start_available(); }
int get_mission_landing_index() { return _mission.get_land_start_index(); }
double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); }
double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); }
float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); }
double get_mission_landing_lat() { return _mission.get_landing_lat(); }
double get_mission_landing_lon() { return _mission.get_landing_lon(); }
float get_mission_landing_alt() { return _mission.get_landing_alt(); }
float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
// RTL
bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
@ -339,8 +320,6 @@ private:
vehicle_local_position_s _local_pos{}; /**< local vehicle position */
vehicle_status_s _vstatus{}; /**< vehicle status */
bool _rtl_activated{false};
// Publications
geofence_result_s _geofence_result{};
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */
@ -359,8 +338,6 @@ private:
bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */
bool _mission_result_updated{false}; /**< flags if mission result has seen an update */
bool _shouldEngageMissionForLanding{false};
Mission _mission; /**< class that handles the missions */
Loiter _loiter; /**< class that handles loiter */
Takeoff _takeoff; /**< class for handling takeoff commands */

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

@ -168,8 +168,8 @@ void Navigator::run()
fds[2].fd = _mission_sub;
fds[2].events = POLLIN;
int16_t geofence_update_counter{0};
int16_t safe_points_update_counter{0};
uint16_t geofence_update_counter{0};
uint16_t safe_points_update_counter{0};
/* rate-limit position subscription to 20 Hz / 50 ms */
orb_set_interval(_local_pos_sub, 50);
@ -589,8 +589,9 @@ void Navigator::run()
// find NAV_CMD_DO_LAND_START in the mission and
// use MAV_CMD_MISSION_START to start the mission from the next item containing a position setpoint
uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED};
if (_mission.land_start()) {
if (_mission.get_land_start_available()) {
vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
vcmd.param1 = _mission.get_land_start_index();
@ -598,9 +599,10 @@ void Navigator::run()
} else {
PX4_WARN("planned mission landing not available");
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED;
}
publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
publish_vehicle_command_ack(cmd, result);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) {
@ -694,7 +696,6 @@ void Navigator::run()
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false;
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL);
navigation_mode_new = &_mission;
break;
@ -704,122 +705,19 @@ void Navigator::run()
navigation_mode_new = &_loiter;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
// If we are already in mission landing, do not switch.
if (_navigation_mode == &_mission && _mission.isLanding()) {
navigation_mode_new = &_mission;
} else {
_pos_sp_triplet_published_invalid_once = false;
const bool rtl_activated_now = !_rtl_activated;
switch (_rtl.get_rtl_type()) {
case RTL::RTL_TYPE_MISSION_LANDING:
case RTL::RTL_TYPE_CLOSEST: {
// If a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode.
// In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission.
if (rtl_activated_now) {
_shouldEngageMissionForLanding = _rtl.getRTLDestinationTypeMission()
&& _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
}
if (_shouldEngageMissionForLanding && (on_mission_landing() || _rtl.getRTLState() > RTL::RTL_STATE_CLIMB)) {
// already in a mission landing, we just need to inform the user and stay in mission
if (rtl_activated_now) {
mavlink_log_info(get_mavlink_log_pub(), "RTL to Mission landing, continue landing\t");
events::send(events::ID("rtl_land_at_mission_continue_landing"), events::Log::Info,
"RTL to Mission landing, continue landing");
}
if (_navigation_mode != &_mission) {
// the first time we're here start the mission landig
start_mission_landing();
}
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
navigation_mode_new = &_mission;
} else {
navigation_mode_new = &_rtl;
}
break;
}
case RTL::RTL_TYPE_MISSION_LANDING_REVERSED:
if (_mission.get_land_start_available() && !get_land_detected()->landed) {
// the mission contains a landing spot
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
if (_navigation_mode != &_mission) {
if (_navigation_mode == nullptr) {
// switching from an manual mode, go to landing if not already landing
if (!on_mission_landing()) {
start_mission_landing();
}
} else {
// switching from an auto mode, continue the mission from the closest item
_mission.set_closest_item_as_current();
}
}
if (rtl_activated_now) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t");
events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info,
"RTL Mission activated, continue mission");
}
navigation_mode_new = &_mission;
} else {
// fly the mission in reverse if switching from a non-manual mode
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE);
if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) &&
(! _mission.get_mission_finished()) &&
(!get_land_detected()->landed)) {
// determine the closest mission item if switching from a non-mission mode, and we are either not already
// mission mode or the mission waypoints changed.
// The seconds condition is required so that when no mission was uploaded and one is available the closest
// mission item is determined and also that if the user changes the active mission index while rtl is active
// always that waypoint is tracked first.
if ((_navigation_mode != &_mission) && (rtl_activated_now || _mission.get_mission_waypoints_changed())) {
_mission.set_closest_item_as_current();
}
if (rtl_activated_now) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t");
events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info,
"RTL Mission activated, fly mission in reverse");
}
navigation_mode_new = &_mission;
} else {
if (rtl_activated_now) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t");
events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info,
"RTL Mission activated, fly to home");
}
navigation_mode_new = &_rtl;
}
}
break;
default:
if (rtl_activated_now) {
mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t");
events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated");
}
navigation_mode_new = &_rtl;
break;
}
_rtl_activated = true;
break;
navigation_mode_new = &_rtl;
}
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_takeoff;
@ -854,11 +752,6 @@ void Navigator::run()
break;
}
if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
_rtl_activated = false;
_rtl.resetRtlState();
}
// Do not execute any state machine while we are disarmed
if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
navigation_mode_new = nullptr;
@ -928,9 +821,7 @@ void Navigator::run()
publish_mission_result();
}
_mission.run();
_geofence.run();
_rtl.run();
perf_end(_loop_perf);
}

1
src/modules/navigator/navigator_mode.cpp
View File

@ -55,7 +55,6 @@ NavigatorMode::run(bool active)
{
if (active) {
if (!_active) {
_navigator->set_mission_result_updated();
on_activation();
} else {

2
src/modules/navigator/navigator_mode.h
View File

@ -54,6 +54,8 @@ public:
void run(bool active);
bool isActive() {return _active;};
/**
* This function is called while the mode is inactive
*/

1060
src/modules/navigator/rtl.cpp
View File

File diff suppressed because it is too large Load Diff

186
src/modules/navigator/rtl.h
View File

@ -44,106 +44,85 @@
#include <px4_platform_common/module_params.h>
#include "navigator_mode.h"
#include "mission_block.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/rtl_time_estimate.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind.h>
#include <matrix/math.hpp>
#include <lib/geo/geo.h>
#include <dataman_client/DatamanClient.hpp>
#include "rtl_direct.h"
#include "rtl_direct_mission_land.h"
#include "rtl_mission_fast.h"
#include "rtl_mission_fast_reverse.h"
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rtl_time_estimate.h>
class Navigator;
class RTL : public MissionBlock, public ModuleParams
class RTL : public NavigatorMode, public ModuleParams
{
public:
RTL(Navigator *navigator);
~RTL() = default;
enum RTLType {
RTL_TYPE_HOME_OR_RALLY = 0,
RTL_TYPE_MISSION_LANDING,
RTL_TYPE_MISSION_LANDING_REVERSED,
RTL_TYPE_CLOSEST,
enum class RtlType {
RTL_DIRECT,
RTL_DIRECT_MISSION_LAND,
RTL_MISSION_FAST,
RTL_MISSION_FAST_REVERSE,
};
enum RTLDestinationType {
RTL_DESTINATION_HOME = 0,
RTL_DESTINATION_MISSION_LANDING,
RTL_DESTINATION_SAFE_POINT,
};
enum RTLHeadingMode {
RTL_NAVIGATION_HEADING = 0,
RTL_DESTINATION_HEADING,
RTL_CURRENT_HEADING,
};
enum RTLState {
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LOITER,
RTL_STATE_TRANSITION_TO_MC,
RTL_MOVE_TO_LAND_HOVER_VTOL,
RTL_STATE_LAND,
RTL_STATE_LANDED,
RTL_STATE_HEAD_TO_CENTER,
};
/**
* @brief function to call regularly to do background work
*/
void run();
void on_inactivation() override;
void on_inactive() override;
void on_activation() override;
void on_active() override;
void find_RTL_destination();
void initialize() override {};
void set_return_alt_min(bool min) { _rtl_alt_min = min; }
int get_rtl_type() const { return _param_rtl_type.get(); }
void get_rtl_xy_z_speed(float &xy, float &z);
matrix::Vector2f get_wind();
RTLState getRTLState() { return _rtl_state; }
bool getRTLDestinationTypeMission() { return _destination.type == RTLDestinationType::RTL_DESTINATION_MISSION_LANDING; }
void resetRtlState() { _rtl_state = RTL_STATE_NONE; }
void set_return_alt_min(bool min) { _enforce_rtl_alt = min; }
void updateSafePoints() { _initiate_safe_points_updated = true; }
private:
bool hasMissionLandStart();
void set_rtl_item();
/**
* @brief function to call regularly to do background work
*/
void updateDatamanCache();
void advance_rtl();
void setRtlTypeAndDestination();
float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
void calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate);
/**
* @brief Find RTL destination.
*
*/
void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt);
float getCruiseGroundSpeed();
/**
* @brief Set the position of the land start marker in the planned mission as destination.
*
*/
void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item);
float getClimbRate();
/**
* @brief Set the safepoint as destination.
*
* @param mission_safe_point is the mission safe point/rally point to set as destination.
*/
void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point);
float getDescendRate();
/**
* @brief
*
* @param cone_half_angle_deg
* @return float
*/
float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg);
float getCruiseSpeed();
float getHoverLandSpeed();
RTLState _rtl_state{RTL_STATE_NONE};
void parameters_update();
enum class DatamanState {
UpdateRequestWait,
@ -153,71 +132,46 @@ private:
Error
};
struct RTLPosition {
double lat;
double lon;
float alt;
float yaw;
uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points)
RTLDestinationType type{RTL_DESTINATION_HOME};
hrt_abstime _destination_check_time{0};
void set(const home_position_s &home_position)
{
lat = home_position.lat;
lon = home_position.lon;
alt = home_position.alt;
yaw = home_position.yaw;
safe_point_index = 0;
type = RTL_DESTINATION_HOME;
}
};
RtlType _rtl_type{RtlType::RTL_DIRECT};
DatamanState _dataman_state{DatamanState::UpdateRequestWait};
DatamanState _error_state{DatamanState::UpdateRequestWait};
uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated
bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache
DatamanCache _dataman_cache{"rtl_dm_cache_miss", 4};
DatamanClient &_dataman_client = _dataman_cache.client();
DatamanCache _dataman_cache_geofence{"rtl_dm_cache_miss_geo", 4};
DatamanClient &_dataman_client_geofence = _dataman_cache_geofence.client();
bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed
DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
int16_t _mission_counter = -1;
mission_stats_entry_s _stats;
RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point)
RtlDirect _rtl_direct;
hrt_abstime _destination_check_time{0};
RtlDirectMissionLand _rtl_direct_mission_land;
float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position
RtlMissionFast _rtl_mission;
bool _rtl_alt_min{false};
RtlMissionFastReverse _rtl_mission_reverse;
bool _enforce_rtl_alt{false};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,
(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay,
(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
(ParamInt<px4::params::RTL_TYPE>) _param_rtl_type,
(ParamInt<px4::params::RTL_CONE_ANG>) _param_rtl_cone_half_angle_deg,
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md,
(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad,
(ParamInt<px4::params::RTL_HDG_MD>) _param_rtl_hdg_md,
(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
(ParamInt<px4::params::RTL_TIME_MARGIN>) _param_rtl_time_margin
(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,
(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
)
param_t _param_mpc_z_v_auto_up{PARAM_INVALID};
param_t _param_mpc_z_v_auto_dn{PARAM_INVALID};
param_t _param_mpc_land_speed{PARAM_INVALID};
param_t _param_fw_climb_rate{PARAM_INVALID};
param_t _param_fw_sink_rate{PARAM_INVALID};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
param_t _param_fw_airspeed_trim{PARAM_INVALID};
param_t _param_mpc_xy_cruise{PARAM_INVALID};
param_t _param_rover_cruise_speed{PARAM_INVALID};
uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::SubscriptionData<mission_s> _mission_sub{ORB_ID(mission)};
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};
uORB::SubscriptionData<wind_s> _wind_sub{ORB_ID(wind)};
uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
};
float time_to_home(const matrix::Vector3f &to_home_vec,
const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s,
float vehicle_descent_speed_m_s);

723
src/modules/navigator/rtl_direct.cpp Normal file
View File

@ -0,0 +1,723 @@
/****************************************************************************
*
* Copyright (c) 2013-2020 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 rtl.cpp
*
* Helper class to access RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Kent <julian@auterion.com>
*/
#include "rtl_direct.h"
#include "navigator.h"
#include <dataman/dataman.h>
#include <px4_platform_common/events.h>
#include <lib/geo/geo.h>
static constexpr float DELAY_SIGMA = 0.01f;
using namespace time_literals;
using namespace math;
RtlDirect::RtlDirect(Navigator *navigator) :
MissionBlock(navigator),
ModuleParams(navigator)
{
_param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP");
_param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN");
_param_mpc_land_speed = param_find("MPC_LAND_SPEED");
_param_fw_climb_rate = param_find("FW_T_CLMB_R_SP");
_param_fw_sink_rate = param_find("FW_T_SINK_R_SP");
_param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM");
_param_mpc_xy_cruise = param_find("MPC_XY_CRUISE");
_param_rover_cruise_speed = param_find("GND_SPEED_THR_SC");
}
void RtlDirect::on_inactivation()
{
if (_navigator->get_precland()->is_activated()) {
_navigator->get_precland()->on_inactivation();
}
}
void RtlDirect::on_activation(bool enforce_rtl_alt)
{
_global_pos_sub.update();
_local_pos_sub.update();
_land_detected_sub.update();
_vehicle_status_sub.update();
parameters_update();
if (_land_detected_sub.get().landed) {
// For safety reasons don't go into RTL if landed.
_rtl_state = RTL_STATE_LANDED;
} else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || enforce_rtl_alt) {
// If lower than return altitude, climb up first.
// If rtl_alt_min is true then forcing altitude change even if above.
_rtl_state = RTL_STATE_CLIMB;
} else {
// Otherwise go start with climb
_rtl_state = RTL_STATE_RETURN;
}
// reset cruising speed and throttle to default for RTL
_navigator->reset_cruising_speed();
_navigator->set_cruising_throttle();
set_rtl_item();
}
void RtlDirect::on_active()
{
_global_pos_sub.update();
_local_pos_sub.update();
_land_detected_sub.update();
_vehicle_status_sub.update();
parameters_update();
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) {
advance_rtl();
set_rtl_item();
}
if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) {
// Need to update the position and type on the current setpoint triplet.
_navigator->get_precland()->on_active();
} else if (_navigator->get_precland()->is_activated()) {
_navigator->get_precland()->on_inactivation();
}
}
void RtlDirect::set_rtl_item()
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon,
_global_pos_sub.get().lat, _global_pos_sub.get().lon);
const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
const RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
_mission_item.lat = _global_pos_sub.get().lat;
_mission_item.lon = _global_pos_sub.get().lon;
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _local_pos_sub.get().heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t",
(int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt));
events::send<int32_t, int32_t>(events::ID("rtl_climb_to"), events::Log::Info,
"RTL: climb to {1m_v} ({2m_v} above destination)",
(int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt));
break;
}
case RTL_STATE_RETURN: {
// For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status
// can be displayed on groundstation and the WP is accepted once within loiter radius
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
} else {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
}
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = _rtl_alt; // Don't change altitude
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING &&
destination_dist > _param_rtl_min_dist.get()) {
_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat,
_destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING ||
destination_dist < _param_rtl_min_dist.get()) {
// Use destination yaw if close to _destination.
_mission_item.yaw = _destination.yaw;
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _local_pos_sub.get().heading;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t",
(int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
events::send<int32_t, int32_t>(events::ID("rtl_return_at"), events::Log::Info,
"RTL: return at {1m_v} ({2m_v} above destination)",
(int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt));
break;
}
case RTL_STATE_DESCEND: {
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
// Except for vtol which might be still off here and should point towards this location.
const float d_current = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
_mission_item.lat, _mission_item.lon);
if (_vehicle_status_sub.get().is_vtol && (d_current > _navigator->get_acceptance_radius())) {
_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
_mission_item.lat, _mission_item.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _local_pos_sub.get().heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
// Disable previous setpoint to prevent drift.
pos_sp_triplet->previous.valid = false;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t",
(int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
events::send<int32_t, int32_t>(events::ID("rtl_descend_to"), events::Log::Info,
"RTL: descend to {1m_v} ({2m_v} above destination)",
(int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt));
break;
}
case RTL_STATE_LOITER: {
const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON);
if (autocontinue) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t",
(double)_param_rtl_land_delay.get());
events::send<float>(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get());
} else {
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t");
events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering");
}
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude; // Don't change altitude.
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _local_pos_sub.get().heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f);
_mission_item.autocontinue = autocontinue;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
break;
}
case RTL_STATE_HEAD_TO_CENTER: {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat,
_destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _destination.yaw;
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _local_pos_sub.get().heading;
}
_mission_item.vtol_back_transition = true;
// acceptance_radius will be overwritten since vtol_back_transition is set,
// set as a default value only
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
// Disable previous setpoint to prevent drift.
pos_sp_triplet->previous.valid = false;
break;
}
case RTL_STATE_TRANSITION_TO_MC: {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
break;
}
case RTL_MOVE_TO_LAND_HOVER_VTOL: {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
// have to reset here because these field were used in set_vtol_transition_item
_mission_item.time_inside = 0.f;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat,
_destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _destination.yaw;
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _local_pos_sub.get().heading;
}
_mission_item.origin = ORIGIN_ONBOARD;
break;
}
case RTL_STATE_LAND: {
// Land at destination.
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = _destination.alt;
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _local_pos_sub.get().heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.land_precision = _param_rtl_pld_md.get();
if (_mission_item.land_precision == 1) {
_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
_navigator->get_precland()->on_activation();
} else if (_mission_item.land_precision == 2) {
_navigator->get_precland()->set_mode(PrecLandMode::Required);
_navigator->get_precland()->on_activation();
}
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t");
events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination");
break;
}
case RTL_STATE_LANDED: {
set_idle_item(&_mission_item);
break;
}
default:
break;
}
reset_mission_item_reached();
// Execute command if set. This is required for commands like VTOL transition.
if (!MissionBlock::item_contains_position(_mission_item)) {
issue_command(_mission_item);
}
// Convert mission item to current position setpoint and make it valid.
mission_apply_limitation(_mission_item);
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
_navigator->set_position_setpoint_triplet_updated();
}
}
void RtlDirect::advance_rtl()
{
// determines if the vehicle should loiter above land
const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA;
// vehicle is a vtol and currently in fixed wing mode
const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
&& _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
switch (_rtl_state) {
case RTL_STATE_CLIMB:
_rtl_state = RTL_STATE_RETURN;
break;
case RTL_STATE_RETURN:
_rtl_state = RTL_STATE_DESCEND;
break;
case RTL_STATE_DESCEND:
if (descend_and_loiter) {
_rtl_state = RTL_STATE_LOITER;
} else if (vtol_in_fw_mode) {
_rtl_state = RTL_STATE_HEAD_TO_CENTER;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LOITER:
if (vtol_in_fw_mode) {
_rtl_state = RTL_STATE_HEAD_TO_CENTER;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_HEAD_TO_CENTER:
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
break;
case RTL_STATE_TRANSITION_TO_MC:
_rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL;
break;
case RTL_MOVE_TO_LAND_HOVER_VTOL:
_rtl_state = RTL_STATE_LAND;
break;
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_LANDED;
_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
break;
default:
break;
}
}
rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
{
_global_pos_sub.update();
rtl_time_estimate_s rtl_time_estimate{};
RTLState start_state_for_estimate{RTL_STATE_NONE};
if (isActive()) {
start_state_for_estimate = _rtl_state;
}
// Calculate RTL time estimate only when there is a valid home position
// TODO: Also check if vehicle position is valid
if (!_navigator->home_global_position_valid()) {
rtl_time_estimate.valid = false;
} else {
rtl_time_estimate.valid = true;
// Sum up time estimate for various segments of the landing procedure
switch (start_state_for_estimate) {
case RTL_STATE_NONE:
case RTL_STATE_CLIMB: {
// Climb segment is only relevant if the drone is below return altitude
const float climb_dist = _global_pos_sub.get().alt < _rtl_alt ? (_rtl_alt - _global_pos_sub.get().alt) : 0;
if (climb_dist > FLT_EPSILON) {
rtl_time_estimate.time_estimate += climb_dist / getClimbRate();
}
}
// FALLTHROUGH
case RTL_STATE_RETURN:
// Add cruise segment to home
rtl_time_estimate.time_estimate += get_distance_to_next_waypoint(
_destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed();
// FALLTHROUGH
case RTL_STATE_HEAD_TO_CENTER:
case RTL_STATE_TRANSITION_TO_MC:
case RTL_STATE_DESCEND: {
// when descending, the target altitude is stored in the current mission item
float initial_altitude = 0.f;
float loiter_altitude = 0.f;
if (start_state_for_estimate == RTL_STATE_DESCEND) {
// Take current vehicle altitude as the starting point for calculation
initial_altitude = _global_pos_sub.get().alt; // TODO: Check if this is in the right frame
loiter_altitude = _mission_item.altitude; // Next waypoint = loiter
} else {
// Take the return altitude as the starting point for the calculation
initial_altitude = _rtl_alt; // CLIMB and RETURN
loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
}
// Add descend segment (first landing phase: return alt to loiter alt)
rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate();
}
// FALLTHROUGH
case RTL_STATE_LOITER:
// Add land delay (the short pause for deploying landing gear)
// TODO: Check if landing gear is deployed or not
rtl_time_estimate.time_estimate += _param_rtl_land_delay.get();
// FALLTHROUGH
case RTL_MOVE_TO_LAND_HOVER_VTOL:
case RTL_STATE_LAND: {
float initial_altitude;
// Add land segment (second landing phase) which comes after LOITER
if (start_state_for_estimate == RTL_STATE_LAND) {
// If we are in this phase, use the current vehicle altitude instead
// of the altitude paramteter to get a continous time estimate
initial_altitude = _global_pos_sub.get().alt;
} else {
// If this phase is not active yet, simply use the loiter altitude,
// which is where the LAND phase will start
const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
initial_altitude = loiter_altitude;
}
// Prevent negative times when close to the ground
if (initial_altitude > _destination.alt) {
rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed();
}
}
break;
case RTL_STATE_LANDED:
// Remaining time is 0
break;
}
// Prevent negative durations as phyiscally they make no sense. These can
// occur during the last phase of landing when close to the ground.
rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate);
// Use actual time estimate to compute the safer time estimate with additional scale factor and a margin
rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate
+ _param_rtl_time_margin.get();
}
// return message
rtl_time_estimate.timestamp = hrt_absolute_time();
return rtl_time_estimate;
}
float RtlDirect::getCruiseSpeed()
{
float ret = 1e6f;
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) {
ret = 1e6f;
}
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) {
ret = 1e6f;
}
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) {
ret = 1e6f;
}
}
return ret;
}
float RtlDirect::getHoverLandSpeed()
{
float ret = 1e6f;
if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) {
ret = 1e6f;
}
return ret;
}
matrix::Vector2f RtlDirect::get_wind()
{
_wind_sub.update();
matrix::Vector2f wind;
if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) {
wind(0) = _wind_sub.get().windspeed_north;
wind(1) = _wind_sub.get().windspeed_east;
}
return wind;
}
float RtlDirect::getClimbRate()
{
float ret = 1e6f;
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) {
ret = 1e6f;
}
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) {
ret = 1e6f;
}
}
return ret;
}
float RtlDirect::getDescendRate()
{
float ret = 1e6f;
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) {
ret = 1e6f;
}
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) {
ret = 1e6f;
}
}
return ret;
}
float RtlDirect::getCruiseGroundSpeed()
{
float cruise_speed = getCruiseSpeed();
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
const vehicle_global_position_s &global_position = *_navigator->get_global_position();
matrix::Vector2f wind = get_wind();
matrix::Vector2f to_destination_vec;
get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon,
&to_destination_vec(0), &to_destination_vec(1));
const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero();
const float wind_towards_home = wind.dot(to_home_dir);
const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm();
// Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient
const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf(
0.f, wind_towards_home);
cruise_speed = ground_speed;
}
return cruise_speed;
}
void RtlDirect::parameters_update()
{
if (_parameter_update_sub.updated()) {
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
// If any parameter updated, call updateParams() to check if
// this class attributes need updating (and do so).
updateParams();
}
}

239
src/modules/navigator/rtl_direct.h Normal file
View File

@ -0,0 +1,239 @@
/***************************************************************************
*
* 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 rtl_direct.h
*
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
#include <px4_platform_common/module_params.h>
#include "mission_block.h"
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rtl_time_estimate.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind.h>
#include <matrix/Vector2.hpp>
using namespace time_literals;
class Navigator;
class RtlDirect : public MissionBlock, public ModuleParams
{
public:
/**
* @brief Return to launch position.
* Defines the position and landing yaw for the return to launch destination.
*
*/
struct RtlPosition {
double lat; /**< latitude in WGS84 [rad].*/
double lon; /**< longitude in WGS84 [rad].*/
float alt; /**< altitude in MSL [m].*/
float yaw; /**< final yaw when landed [rad].*/
};
RtlDirect(Navigator *navigator);
~RtlDirect() = default;
/**
* @brief on inactivation
*
*/
void on_inactivation() override;
/**
* @brief On activation.
* Initialize the return to launch calculations.
*
* @param[in] enforce_rtl_alt boolean if the minimal return to launch altitude should be enforced at the beginning of the return, even when the current vehicle altitude is above.
*/
void on_activation(bool enforce_rtl_alt);
/**
* @brief on active
* Update the return to launch calculation and set new setpoints for controller if necessary.
*
*/
void on_active() override;
/**
* @brief Calculate the estimated time needed to return to launch.
*
* @return estimated time to return to launch.
*/
rtl_time_estimate_s calc_rtl_time_estimate();
void setRtlAlt(float alt) {_rtl_alt = alt;};
void setRtlPosition(RtlPosition position) {_destination = position;};
private:
/**
* @brief Return to launch heading mode.
*
*/
enum RTLHeadingMode {
RTL_NAVIGATION_HEADING = 0,
RTL_DESTINATION_HEADING,
RTL_CURRENT_HEADING,
};
/**
* @brief Return to launch state machine.
*
*/
enum RTLState {
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LOITER,
RTL_STATE_TRANSITION_TO_MC,
RTL_MOVE_TO_LAND_HOVER_VTOL,
RTL_STATE_LAND,
RTL_STATE_LANDED,
RTL_STATE_HEAD_TO_CENTER,
};
private:
/**
* @brief Get the horizontal wind velocity
*
* @return horizontal wind velocity.
*/
matrix::Vector2f get_wind();
/**
* @brief Set the return to launch control setpoint.
*
*/
void set_rtl_item();
/**
* @brief Advance the return to launch state machine.
*
*/
void advance_rtl();
/**
* @brief Get the Cruise Ground Speed
*
* @return Ground speed in cruise mode [m/s].
*/
float getCruiseGroundSpeed();
/**
* @brief Get the climb rate
*
* @return Climb rate [m/s]
*/
float getClimbRate();
/**
* @brief Get the descend rate
*
* @return descend rate [m/s]
*/
float getDescendRate();
/**
* @brief Get the cruise speed
*
* @return cruise speed [m/s]
*/
float getCruiseSpeed();
/**
* @brief Get the Hover Land Speed
*
* @return Hover land speed [m/s]
*/
float getHoverLandSpeed();
/**
* Check for parameter changes and update them if needed.
*/
void parameters_update();
/** Current state in the state machine.*/
RTLState _rtl_state{RTL_STATE_NONE};
RtlPosition _destination{}; ///< the RTL position to fly to
float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,
(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay,
(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md,
(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad,
(ParamInt<px4::params::RTL_HDG_MD>) _param_rtl_hdg_md,
(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
(ParamInt<px4::params::RTL_TIME_MARGIN>) _param_rtl_time_margin
)
param_t _param_mpc_z_v_auto_up{PARAM_INVALID};
param_t _param_mpc_z_v_auto_dn{PARAM_INVALID};
param_t _param_mpc_land_speed{PARAM_INVALID};
param_t _param_fw_climb_rate{PARAM_INVALID};
param_t _param_fw_sink_rate{PARAM_INVALID};
param_t _param_fw_airspeed_trim{PARAM_INVALID};
param_t _param_mpc_xy_cruise{PARAM_INVALID};
param_t _param_rover_cruise_speed{PARAM_INVALID};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position 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_local_position_s> _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle status subscription */
uORB::SubscriptionData<wind_s> _wind_sub{ORB_ID(wind)};
};

281
src/modules/navigator/rtl_direct_mission_land.cpp Normal file
View File

@ -0,0 +1,281 @@
/***************************************************************************
*
* 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 rtl_direct_mission_land.cpp
*
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "rtl_direct_mission_land.h"
#include "navigator.h"
#include <drivers/drv_hrt.h>
static constexpr int32_t DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE = 5;
RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) :
MissionBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE)
{
}
void RtlDirectMissionLand::on_activation(bool enforce_rtl_alt)
{
_land_detected_sub.update();
_global_pos_sub.update();
_needs_climbing = false;
if (hasMissionLandStart()) {
_is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, false) == PX4_OK);
if ((_global_pos_sub.get().alt < _rtl_alt) || enforce_rtl_alt) {
// If lower than return altitude, climb up first.
// If enforce_rtl_alt is true then forcing altitude change even if above.
_needs_climbing = true;
}
} else {
_is_current_planned_mission_item_valid = false;
}
if (_land_detected_sub.get().landed) {
// already landed, no need to do anything, invalidad the position mission item.
_is_current_planned_mission_item_valid = false;
}
MissionBase::on_activation();
}
bool RtlDirectMissionLand::setNextMissionItem()
{
return (goToNextPositionItem(true) == PX4_OK);
}
void RtlDirectMissionLand::setActiveMissionItems()
{
WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// Climb to altitude
if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
// do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT,
// even if current climb altitude is below (e.g. RTL immediately after take off)
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
} else {
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
}
_mission_item.lat = _global_pos_sub.get().lat;
_mission_item.lon = _global_pos_sub.get().lon;
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL Mission land: climb to %d m\t",
(int)ceilf(_rtl_alt));
events::send<int32_t>(events::ID("rtl_mission_land_climb"), events::Log::Info,
"RTL Mission Land: climb to {1m_v}",
(int32_t)ceilf(_rtl_alt));
_needs_climbing = false;
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB;
} else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
_vehicle_status_sub.get().is_vtol &&
!_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
// Transition to fixed wing if necessary.
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
_mission_item.yaw = _navigator->get_local_position()->heading;
// keep current setpoints (FW position controller generates wp to track during transition)
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
} else if (item_contains_position(_mission_item)) {
if (_mission_item.nav_cmd == NAV_CMD_LAND ||
_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
handleLanding(new_work_item_type);
} else {
// convert mission item to a simple waypoint, keep loiter to alt
if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
}
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
pos_sp_triplet->previous = pos_sp_triplet->current;
}
int32_t next_mission_item_index;
size_t num_found_items = 0;
getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u);
if (num_found_items > 0) {
const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
mission_item_s next_mission_item;
bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index,
reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
if (success) {
mission_apply_limitation(next_mission_item);
mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
}
}
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
}
issue_command(_mission_item);
/* set current work item type */
_work_item_type = new_work_item_type;
reset_mission_item_reached();
if (_mission_type == MissionType::MISSION_TYPE_MISSION) {
set_mission_result();
}
publish_navigator_mission_item(); // for logging
_navigator->set_position_setpoint_triplet_updated();
}
void RtlDirectMissionLand::handleLanding(WorkItemType &new_work_item_type)
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
bool needs_to_land = !_land_detected_sub.get().landed &&
((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND)
|| (_mission_item.nav_cmd == NAV_CMD_LAND));
bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol &&
(_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
needs_to_land;
if (needs_vtol_landing) {
if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
float altitude = _global_pos_sub.get().alt;
if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
altitude = pos_sp_triplet->current.alt;
}
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
_mission_item.vtol_back_transition = true;
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
/* transition to MC */
if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
_mission_item.altitude = _global_pos_sub.get().alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
// make previous setpoint invalid, such that there will be no prev-current line following
// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
} else if (needs_to_land) {
/* move to landing waypoint before descent if necessary */
if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
do_need_move_to_land() &&
(_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT ||
_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
_mission_item.altitude = _global_pos_sub.get().alt;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
// make previous setpoint invalid, such that there will be no prev-current line following.
// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
}
}
bool RtlDirectMissionLand::do_need_move_to_land()
{
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_global_pos_sub.get().lat, _global_pos_sub.get().lon);
return d_current > _navigator->get_acceptance_radius();
}
rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate()
{
rtl_time_estimate_s time_estimate;
time_estimate.valid = false;
time_estimate.timestamp = hrt_absolute_time();
return time_estimate;
}

72
src/modules/navigator/rtl_direct_mission_land.h Normal file
View File

@ -0,0 +1,72 @@
/***************************************************************************
*
* 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 rtl_direct_mission_land.h
*
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
#include "mission.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/rtl_time_estimate.h>
class Navigator;
class RtlDirectMissionLand : public MissionBase
{
public:
RtlDirectMissionLand(Navigator *navigator);
~RtlDirectMissionLand() = default;
void on_activation(bool enforce_rtl_alt);
rtl_time_estimate_s calc_rtl_time_estimate();
void setRtlAlt(float alt) {_rtl_alt = alt;};
private:
bool setNextMissionItem() override;
void setActiveMissionItems() override;
void handleLanding(WorkItemType &new_work_item_type);
bool do_need_move_to_land();
bool _needs_climbing{false}; //< Flag if climbing is required at the start
float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the land position
};

241
src/modules/navigator/rtl_mission_fast.cpp Normal file
View File

@ -0,0 +1,241 @@
/***************************************************************************
*
* 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 rtl_mission_fast.cpp
*
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "rtl_mission_fast.h"
#include "navigator.h"
#include <drivers/drv_hrt.h>
static constexpr int32_t DEFAULT_MISSION_FAST_CACHE_SIZE = 5;
RtlMissionFast::RtlMissionFast(Navigator *navigator) :
MissionBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE)
{
}
void RtlMissionFast::on_activation()
{
_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
if (_land_detected_sub.get().landed) {
// already landed, no need to do anything, invalidad the position mission item.
_is_current_planned_mission_item_valid = false;
}
MissionBase::on_activation();
}
void RtlMissionFast::on_active()
{
_home_pos_sub.update();
MissionBase::on_active();
}
void RtlMissionFast::on_inactive()
{
_home_pos_sub.update();
MissionBase::on_inactive();
}
bool RtlMissionFast::setNextMissionItem()
{
return (goToNextPositionItem(true) == PX4_OK);
}
void RtlMissionFast::setActiveMissionItems()
{
WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// Transition to fixed wing if necessary.
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
_vehicle_status_sub.get().is_vtol &&
!_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
_mission_item.yaw = _navigator->get_local_position()->heading;
// keep current setpoints (FW position controller generates wp to track during transition)
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
} else if (item_contains_position(_mission_item)) {
if (_mission_item.nav_cmd == NAV_CMD_LAND ||
_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
handleLanding(new_work_item_type);
} else {
// convert mission item to a simple waypoint, keep loiter to alt
if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
}
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
pos_sp_triplet->previous = pos_sp_triplet->current;
}
int32_t next_mission_item_index;
size_t num_found_items = 0;
getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u);
if (num_found_items > 0) {
const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
mission_item_s next_mission_item;
bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index,
reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
if (success) {
mission_apply_limitation(next_mission_item);
mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
}
}
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
}
issue_command(_mission_item);
/* set current work item type */
_work_item_type = new_work_item_type;
reset_mission_item_reached();
if (_mission_type == MissionType::MISSION_TYPE_MISSION) {
set_mission_result();
}
publish_navigator_mission_item(); // for logging
_navigator->set_position_setpoint_triplet_updated();
}
void RtlMissionFast::handleLanding(WorkItemType &new_work_item_type)
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
bool needs_to_land = !_land_detected_sub.get().landed &&
((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND)
|| (_mission_item.nav_cmd == NAV_CMD_LAND));
bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol &&
(_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
needs_to_land;
if (needs_vtol_landing) {
if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
float altitude = _global_pos_sub.get().alt;
if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
altitude = pos_sp_triplet->current.alt;
}
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
_mission_item.vtol_back_transition = true;
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
/* transition to MC */
if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
_mission_item.altitude = _global_pos_sub.get().alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
// make previous setpoint invalid, such that there will be no prev-current line following
// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
} else if (needs_to_land) {
/* move to landing waypoint before descent if necessary */
if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
do_need_move_to_land() &&
(_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT ||
_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
_mission_item.altitude = _global_pos_sub.get().alt;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
// make previous setpoint invalid, such that there will be no prev-current line following.
// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
}
}
bool RtlMissionFast::do_need_move_to_land()
{
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_global_pos_sub.get().lat, _global_pos_sub.get().lon);
return d_current > _navigator->get_acceptance_radius();
}
rtl_time_estimate_s RtlMissionFast::calc_rtl_time_estimate()
{
rtl_time_estimate_s time_estimate;
time_estimate.valid = false;
time_estimate.timestamp = hrt_absolute_time();
return time_estimate;
}

71
src/modules/navigator/rtl_mission_fast.h Normal file
View File

@ -0,0 +1,71 @@
/***************************************************************************
*
* 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 rtl_mission_fast.h
*
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
#include "mission_base.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/rtl_time_estimate.h>
class Navigator;
class RtlMissionFast : public MissionBase
{
public:
RtlMissionFast(Navigator *navigator);
~RtlMissionFast() = default;
void on_activation() override;
void on_active() override;
void on_inactive() override;
rtl_time_estimate_s calc_rtl_time_estimate();
private:
bool setNextMissionItem() override;
void setActiveMissionItems() override;
void handleLanding(WorkItemType &new_work_item_type);
bool do_need_move_to_land();
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
};

268
src/modules/navigator/rtl_mission_fast_reverse.cpp Normal file
View File

@ -0,0 +1,268 @@
/***************************************************************************
*
* 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 rtl_mission_fast_reverse.cpp
*
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "rtl_mission_fast_reverse.h"
#include "navigator.h"
#include <drivers/drv_hrt.h>
static constexpr int32_t DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE = 5;
RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) :
MissionBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE)
{
}
void RtlMissionFastReverse::on_activation()
{
_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
if (_land_detected_sub.get().landed) {
// already landed, no need to do anything, invalidate the position mission item.
_is_current_planned_mission_item_valid = false;
}
MissionBase::on_activation();
}
void RtlMissionFastReverse::on_active()
{
_home_pos_sub.update();
MissionBase::on_active();
}
void RtlMissionFastReverse::on_inactive()
{
_home_pos_sub.update();
MissionBase::on_inactive();
}
bool RtlMissionFastReverse::setNextMissionItem()
{
return (goToPreviousPositionItem(true) == PX4_OK);
}
void RtlMissionFastReverse::setActiveMissionItems()
{
WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// Transition to fixed wing if necessary.
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
_vehicle_status_sub.get().is_vtol &&
!_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
_mission_item.yaw = _navigator->get_local_position()->heading;
// keep current setpoints (FW position controller generates wp to track during transition)
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
} else if (item_contains_position(_mission_item)) {
int32_t next_mission_item_index;
size_t num_found_items = 0;
getPreviousPositionItems(_mission.current_seq, &next_mission_item_index, num_found_items, 1u);
// If the current item is a takeoff item or there is no further position item start landing.
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
num_found_items == 0) {
handleLanding(new_work_item_type);
} else {
// convert mission item to a simple waypoint, keep loiter to alt
if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
}
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
pos_sp_triplet->previous = pos_sp_triplet->current;
}
if (num_found_items > 0) {
const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
mission_item_s next_mission_item;
bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index,
reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
if (success) {
mission_apply_limitation(next_mission_item);
mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
}
}
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
}
issue_command(_mission_item);
/* set current work item type */
_work_item_type = new_work_item_type;
reset_mission_item_reached();
if (_mission_type == MissionType::MISSION_TYPE_MISSION) {
set_mission_result();
}
publish_navigator_mission_item(); // for logging
_navigator->set_position_setpoint_triplet_updated();
}
void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type)
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
bool needs_to_land = !_land_detected_sub.get().landed;
bool vtol_in_fw = _vehicle_status_sub.get().is_vtol &&
(_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
if (needs_to_land) {
if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
// Go to Take off location
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB;
if (!PX4_ISFINITE(_mission_item.altitude)) {
_mission_item.altitude = _global_pos_sub.get().alt;
_mission_item.altitude_is_relative = false;
}
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
_mission_item.vtol_back_transition = true;
pos_sp_triplet->previous = pos_sp_triplet->current;
}
if (vtol_in_fw) {
if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB) {
// Go to home location
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
float altitude = _global_pos_sub.get().alt;
if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
altitude = pos_sp_triplet->current.alt;
}
_mission_item.lat = _home_pos_sub.get().lat;
_mission_item.lon = _home_pos_sub.get().lon;
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
_mission_item.vtol_back_transition = true;
pos_sp_triplet->previous = pos_sp_triplet->current;
}
/* transition to MC */
if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
_mission_item.altitude = _global_pos_sub.get().alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
// make previous setpoint invalid, such that there will be no prev-current line following
// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
} else if ((_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB ||
_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND ||
_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = _home_pos_sub.get().lat;
_mission_item.lon = _home_pos_sub.get().lon;
_mission_item.yaw = NAN;
if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
do_need_move_to_land()) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
_mission_item.altitude = _global_pos_sub.get().alt;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
// make previous setpoint invalid, such that there will be no prev-current line following.
// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
} else {
_mission_item.altitude = _home_pos_sub.get().alt;
_mission_item.altitude_is_relative = false;
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
}
}
}
bool RtlMissionFastReverse::do_need_move_to_land()
{
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_global_pos_sub.get().lat, _global_pos_sub.get().lon);
return d_current > _navigator->get_acceptance_radius();
}
rtl_time_estimate_s RtlMissionFastReverse::calc_rtl_time_estimate()
{
rtl_time_estimate_s time_estimate;
time_estimate.valid = false;
time_estimate.timestamp = hrt_absolute_time();
return time_estimate;
}

71
src/modules/navigator/rtl_mission_fast_reverse.h Normal file
View File

@ -0,0 +1,71 @@
/***************************************************************************
*
* 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 rtl_mission_fast_reverse.h
*
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
#include "mission_base.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/rtl_time_estimate.h>
class Navigator;
class RtlMissionFastReverse : public MissionBase
{
public:
RtlMissionFastReverse(Navigator *navigator);
~RtlMissionFastReverse() = default;
void on_activation() override;
void on_active() override;
void on_inactive() override;
rtl_time_estimate_s calc_rtl_time_estimate();
private:
bool setNextMissionItem() override;
void setActiveMissionItems() override;
void handleLanding(WorkItemType &new_work_item_type);
bool do_need_move_to_land();
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
};