From 4f4e6d646c2d285a76439cdf7805d013db5ba328 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 15 Nov 2023 10:02:53 +1100 Subject: [PATCH] AP_Mount: validate modes before setting them --- libraries/AP_Mount/AP_Mount_Backend.cpp | 20 ++++++++++++++++++++ libraries/AP_Mount/AP_Mount_Backend.h | 7 +++++-- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index f05e7430eb..8545d75b90 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -46,6 +46,26 @@ bool AP_Mount_Backend::has_pitch_control() const return (_params.pitch_angle_min < _params.pitch_angle_max); } +bool AP_Mount_Backend::valid_mode(MAV_MOUNT_MODE mode) const +{ + switch (mode) { + case MAV_MOUNT_MODE_RETRACT...MAV_MOUNT_MODE_HOME_LOCATION: + return true; + case MAV_MOUNT_MODE_ENUM_END: + return false; + } + return false; +} + +bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode) +{ + if (!valid_mode(mode)) { + return false; + } + _mode = mode; + return true; +} + // set angle target in degrees // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 1c873e48e6..b88ddf42bf 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -69,12 +69,15 @@ public: // get angular velocity of mount. Only available on some backends virtual bool get_angular_velocity(Vector3f& rates) { return false; } - + + // returns true if mode is a valid mode, false otherwise: + bool valid_mode(MAV_MOUNT_MODE mode) const; + // get mount's mode enum MAV_MOUNT_MODE get_mode() const { return _mode; } // set mount's mode - void set_mode(enum MAV_MOUNT_MODE mode) { _mode = mode; } + bool set_mode(enum MAV_MOUNT_MODE mode); // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle