mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-13 22:20:35 +08:00
Ege Kural
207456fd35
* CI: enable clang-tidy cppcoreguidelines-virtual-class-destructor Signed-off-by: kuralme <kuralme@protonmail.com> * format fix Signed-off-by: kuralme <kuralme@protonmail.com> --------- Signed-off-by: kuralme <kuralme@protonmail.com>
123 lines
4.8 KiB
C++
123 lines
4.8 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#pragma once
|
|
|
|
#include <uORB/topics/vehicle_status.h>
|
|
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
|
|
|
enum class ModeChangeSource {
|
|
User, ///< RC or MAVLink
|
|
ModeExecutor,
|
|
};
|
|
|
|
class ModeChangeHandler
|
|
{
|
|
public:
|
|
ModeChangeHandler() = default;
|
|
virtual ~ModeChangeHandler() = default;
|
|
|
|
/**
|
|
* Explicitly disable copying/moving
|
|
*/
|
|
ModeChangeHandler(const ModeChangeHandler &) = delete;
|
|
ModeChangeHandler &operator=(const ModeChangeHandler &) = delete;
|
|
ModeChangeHandler(ModeChangeHandler &&) = delete;
|
|
ModeChangeHandler &operator=(ModeChangeHandler &&) = delete;
|
|
|
|
virtual void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) = 0;
|
|
|
|
/**
|
|
* Get the replaced (internal) mode for a given (external) mode
|
|
* @param nav_state
|
|
* @return nav_state or the mode that nav_state replaces
|
|
*/
|
|
virtual uint8_t getReplacedModeIfAny(uint8_t nav_state) = 0;
|
|
|
|
virtual uint8_t onDisarm(uint8_t stored_nav_state) = 0;
|
|
};
|
|
|
|
|
|
class UserModeIntention
|
|
{
|
|
public:
|
|
UserModeIntention(const vehicle_status_s &vehicle_status,
|
|
const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler);
|
|
~UserModeIntention() = default;
|
|
|
|
/**
|
|
* Explicitly disable copying/moving
|
|
*/
|
|
UserModeIntention(const UserModeIntention &) = delete;
|
|
UserModeIntention &operator=(const UserModeIntention &) = delete;
|
|
|
|
/**
|
|
* Change the user intended mode
|
|
* @param user_intended_nav_state new mode
|
|
* @param source calling reason
|
|
* @param allow_fallback allow to fallback to a lower mode if current mode cannot run
|
|
* @param force always set if true
|
|
* @return true if successfully set (also if unchanged)
|
|
*/
|
|
bool change(uint8_t user_intended_nav_state, ModeChangeSource source = ModeChangeSource::User,
|
|
bool allow_fallback = false, bool force = false);
|
|
|
|
uint8_t get() const { return _user_intented_nav_state; }
|
|
|
|
/**
|
|
* Change the user intention to the last user intended mode where arming is possible
|
|
*/
|
|
void onDisarm();
|
|
|
|
/**
|
|
* Returns false if there has not been any mode change yet
|
|
*/
|
|
bool everHadModeChange() const { return _ever_had_mode_change; }
|
|
|
|
bool getHadModeChangeAndClear() { bool ret = _had_mode_change; _had_mode_change = false; return ret; }
|
|
|
|
private:
|
|
bool isArmed() const { return _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; }
|
|
bool isTakeOffIntended(uint8_t user_intented_nav_state) const {return user_intented_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || user_intented_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;}
|
|
|
|
const vehicle_status_s &_vehicle_status;
|
|
const HealthAndArmingChecks &_health_and_arming_checks;
|
|
ModeChangeHandler *const _handler{nullptr};
|
|
|
|
uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode
|
|
uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming
|
|
|
|
bool _ever_had_mode_change{false}; ///< true if there was ever a mode change call (also if the same mode as already set)
|
|
bool _had_mode_change{false}; ///< true if there was a mode change call since the last getHadModeChangeAndClear()
|
|
};
|