Line data    Source code

       1             : /*
       2             :   Servo controlled mount backend class
       3             :  */
       4             : #pragma once
       5             : 
       6             : #include "AP_Mount_config.h"
       7             : 
       8             : #if HAL_MOUNT_SERVO_ENABLED
       9             : 
      10             : #include "AP_Mount_Backend.h"
      11             : 
      12             : #include <AP_Math/AP_Math.h>
      13             : #include <AP_Common/AP_Common.h>
      14             : 
      15             : class AP_Mount_Servo : public AP_Mount_Backend
      16             : {
      17             : public:
      18             :     // Constructor
      19          98 :     AP_Mount_Servo(AP_Mount &frontend, AP_Mount_Params &params, bool requires_stab, uint8_t instance):
      20             :         AP_Mount_Backend(frontend, params, instance),
      21          98 :         requires_stabilization(requires_stab),
      22          98 :         _roll_idx(SRV_Channel::k_none),
      23          98 :         _tilt_idx(SRV_Channel::k_none),
      24          98 :         _pan_idx(SRV_Channel::k_none)
      25             :     {
      26          98 :     }
      27             : 
      28             :     // init - performs any required initialisation for this instance
      29             :     void init() override;
      30             : 
      31             :     // update mount position - should be called periodically
      32             :     void update() override;
      33             : 
      34             :     // returns true if this mount can control its roll
      35             :     bool has_roll_control() const override;
      36             : 
      37             :     // returns true if this mount can control its tilt
      38             :     bool has_pitch_control() const override;
      39             : 
      40             :     // returns true if this mount can control its pan (required for multicopters)
      41             :     bool has_pan_control() const override;
      42             : 
      43             : protected:
      44             : 
      45             :     // get attitude as a quaternion.  returns true on success
      46             :     bool get_attitude_quaternion(Quaternion& att_quat) override;
      47             : 
      48             :     // servo only natively supports angles:
      49      638511 :     uint8_t natively_supported_mount_target_types() const override {
      50      638511 :         return NATIVE_ANGLES_ONLY;
      51             :     };
      52             : 
      53             : private:
      54             : 
      55             :     // called by the backend to set the servo angles:
      56             :     void send_target_angles(const MountAngleTarget& angle_rad) override;
      57             : 
      58             :     // update body-frame angle outputs from earth-frame targets
      59             :     void update_angle_outputs(const MountAngleTarget& angle_rad);
      60             : 
      61             :     ///  moves servo with the given function id to the specified angle.  all angles are in body-frame and degrees * 10
      62             :     void move_servo(uint8_t rc, int16_t angle, int16_t angle_min, int16_t angle_max);
      63             : 
      64             :     /// Servo gimbals require stabilization, BrushlessPWM gimbals self-stabilize
      65             :     const bool requires_stabilization;
      66             : 
      67             :     // SRV_Channel - different id numbers are used depending upon the instance number
      68             :     SRV_Channel::Function    _roll_idx;  // SRV_Channel mount roll function index
      69             :     SRV_Channel::Function    _tilt_idx;  // SRV_Channel mount tilt function index
      70             :     SRV_Channel::Function    _pan_idx;   // SRV_Channel mount pan  function index
      71             : 
      72             :     Vector3f _angle_bf_output_rad;  // final body frame output angle in radians
      73             : };
      74             : #endif // HAL_MOUNT_SERVO_ENABLED