From 2bcb503170cc2dee96b912d937a57e2924ac6f0a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 26 Aug 2022 12:42:17 +0900 Subject: [PATCH] AP_Mount: allow users to disable yaw on 3-axis gimbals Also fixes 3-axis gimbal support for SToRM32 serial and MAVLink gimbals --- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 2 +- libraries/AP_Mount/AP_Mount_Backend.h | 6 +++++- libraries/AP_Mount/AP_Mount_Gremsy.h | 2 +- libraries/AP_Mount/AP_Mount_SToRM32.cpp | 7 ------- libraries/AP_Mount/AP_Mount_SToRM32.h | 2 +- libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp | 7 ------- libraries/AP_Mount/AP_Mount_SToRM32_serial.h | 2 +- libraries/AP_Mount/AP_Mount_Servo.cpp | 2 +- 8 files changed, 10 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 0bc0d860f1..9f310483da 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -99,7 +99,7 @@ void AP_Mount_Alexmos::update() // has_pan_control - returns true if this mount can control it's pan (required for multicopters) bool AP_Mount_Alexmos::has_pan_control() const { - return _gimbal_3axis; + return _gimbal_3axis && yaw_range_valid(); } // get attitude as a quaternion. returns true on success diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 452e9d1f8a..65dae03c41 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -46,7 +46,7 @@ public: // return true if healthy virtual bool healthy() const { return true; } - // has_pan_control - returns true if this mount can control it's pan (required for multicopters) + // returns true if this mount can control its pan (required for multicopters) virtual bool has_pan_control() const = 0; // get mount's mode @@ -115,6 +115,10 @@ protected: bool yaw_is_ef; }; + // returns true if user has configured a valid yaw angle range + // allows user to disable yaw even on 3-axis gimbal + bool yaw_range_valid() const { return (_state._pan_angle_min < _state._pan_angle_max); } + // returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal) virtual bool suppress_heartbeat() const { return false; } diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index 01e57c4b07..d5edb5ef1e 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -34,7 +34,7 @@ public: bool healthy() const override; // has_pan_control - bool has_pan_control() const override { return true; } + bool has_pan_control() const override { return yaw_range_valid(); } // handle GIMBAL_DEVICE_INFORMATION message void handle_gimbal_device_information(const mavlink_message_t &msg) override; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index c5ca95590b..477e67046d 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -104,13 +104,6 @@ void AP_Mount_SToRM32::update() } } -// has_pan_control - returns true if this mount can control it's pan (required for multicopters) -bool AP_Mount_SToRM32::has_pan_control() const -{ - // we do not have yaw control - return false; -} - // set_mode - sets mount's mode void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode) { diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h index b8bd402844..0933f11daf 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -30,7 +30,7 @@ public: void update() override; // has_pan_control - returns true if this mount can control it's pan (required for multicopters) - bool has_pan_control() const override; + bool has_pan_control() const override { return yaw_range_valid(); } // set_mode - sets mount's mode void set_mode(enum MAV_MOUNT_MODE mode) override; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 44c4227760..c4915abead 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -133,13 +133,6 @@ void AP_Mount_SToRM32_serial::update() } } -// has_pan_control - returns true if this mount can control it's pan (required for multicopters) -bool AP_Mount_SToRM32_serial::has_pan_control() const -{ - // we do not have yaw control - return false; -} - // set_mode - sets mount's mode void AP_Mount_SToRM32_serial::set_mode(enum MAV_MOUNT_MODE mode) { diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h index fcf85c0409..d50a563bc4 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h @@ -33,7 +33,7 @@ public: void update() override; // has_pan_control - returns true if this mount can control it's pan (required for multicopters) - bool has_pan_control() const override; + bool has_pan_control() const override { return yaw_range_valid(); }; // set_mode - sets mount's mode void set_mode(enum MAV_MOUNT_MODE mode) override; diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index daaa8b812d..87b4964e52 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -117,7 +117,7 @@ void AP_Mount_Servo::update() // returns true if this mount can control its pan (required for multicopters) bool AP_Mount_Servo::has_pan_control() const { - return SRV_Channels::function_assigned(_pan_idx); + return SRV_Channels::function_assigned(_pan_idx) && yaw_range_valid(); } // get attitude as a quaternion. returns true on success