Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: implement FLTMODE_GCSBLOCK bitmask

This commit is contained in:
Iampete1 2023-06-18 16:31:17 +01:00 committed by Peter Hall
parent 61682f6551
commit 6fafb26549
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Rover/Rover.h
View File

@ -360,6 +360,7 @@ private:
void init_ardupilot() override; void init_ardupilot() override;
void startup_ground(void); void startup_ground(void);
void update_ahrs_flyforward(); void update_ahrs_flyforward();
bool gcs_mode_enabled(const Mode::Number mode_num) const;
bool set_mode(Mode &new_mode, ModeReason reason); bool set_mode(Mode &new_mode, ModeReason reason);
bool set_mode(const uint8_t new_mode, ModeReason reason) override; bool set_mode(const uint8_t new_mode, ModeReason reason) override;
uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); } uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); }

30
Rover/system.cpp
View File

@ -223,6 +223,30 @@ void Rover::update_ahrs_flyforward()
ahrs.set_fly_forward(flyforward); ahrs.set_fly_forward(flyforward);
} }
// Check if this mode can be entered from the GCS
bool Rover::gcs_mode_enabled(const Mode::Number mode_num) const
{
// List of modes that can be blocked, index is bit number in parameter bitmask
static const uint8_t mode_list [] {
(uint8_t)Mode::Number::MANUAL,
(uint8_t)Mode::Number::ACRO,
(uint8_t)Mode::Number::STEERING,
(uint8_t)Mode::Number::LOITER,
(uint8_t)Mode::Number::FOLLOW,
(uint8_t)Mode::Number::SIMPLE,
(uint8_t)Mode::Number::CIRCLE,
(uint8_t)Mode::Number::AUTO,
(uint8_t)Mode::Number::RTL,
(uint8_t)Mode::Number::SMART_RTL,
(uint8_t)Mode::Number::GUIDED,
#if MODE_DOCK_ENABLED == ENABLED
(uint8_t)Mode::Number::DOCK
#endif
};
return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list));
}
bool Rover::set_mode(Mode &new_mode, ModeReason reason) bool Rover::set_mode(Mode &new_mode, ModeReason reason)
{ {
if (control_mode == &new_mode) { if (control_mode == &new_mode) {
@ -230,6 +254,12 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason)
return true; return true;
} }
// Check if GCS mode change is disabled via parameter
if ((reason == ModeReason::GCS_COMMAND) && !gcs_mode_enabled((Mode::Number)new_mode.mode_number())) {
gcs().send_text(MAV_SEVERITY_NOTICE,"Mode change to %s denied, GCS entry disabled (FLTMODE_GCSBLOCK)", new_mode.name4());
return false;
}
Mode &old_mode = *control_mode; Mode &old_mode = *control_mode;
if (!new_mode.enter()) { if (!new_mode.enter()) {
// Log error that we failed to enter desired flight mode // Log error that we failed to enter desired flight mode