Line data    Source code

       1             : #include "AP_Mount_config.h"
       2             : 
       3             : #if HAL_MOUNT_SERVO_ENABLED
       4             : 
       5             : #include "AP_Mount_Servo.h"
       6             : 
       7             : #include <AP_AHRS/AP_AHRS.h>
       8             : #include <GCS_MAVLink/GCS_MAVLink.h>
       9             : 
      10             : extern const AP_HAL::HAL& hal;
      11             : 
      12             : // init - performs any required initialisation for this instance
      13          98 : void AP_Mount_Servo::init()
      14             : {
      15          98 :     if (_instance == 0) {
      16          98 :         _roll_idx = SRV_Channel::k_mount_roll;
      17          98 :         _tilt_idx = SRV_Channel::k_mount_tilt;
      18          98 :         _pan_idx  = SRV_Channel::k_mount_pan;
      19             :     } else {
      20             :         // this must be the 2nd mount
      21           0 :         _roll_idx = SRV_Channel::k_mount2_roll;
      22           0 :         _tilt_idx = SRV_Channel::k_mount2_tilt;
      23           0 :         _pan_idx  = SRV_Channel::k_mount2_pan;
      24             :     }
      25          98 :     AP_Mount_Backend::init();
      26          98 : }
      27             : 
      28             : // update mount position - should be called periodically
      29      342483 : void AP_Mount_Servo::update()
      30             : {
      31      342483 :     AP_Mount_Backend::update();
      32             : 
      33      342483 :     update_mnt_target();
      34             : 
      35             :     // have our base class call send_target_angles to command the gimbal:
      36      342483 :     send_target_to_gimbal();
      37      342483 : }
      38             : 
      39             : // called by the backend to set the servo angles:
      40      342483 : void AP_Mount_Servo::send_target_angles(const MountAngleTarget& angle_rad)
      41             : {
      42      342483 :     update_angle_outputs(mnt_target.angle_rad);
      43             : 
      44             :     // write the results to the servos
      45      342483 :     move_servo(_roll_idx, degrees(_angle_bf_output_rad.x)*10, _params.roll_angle_min*10, _params.roll_angle_max*10);
      46      342483 :     move_servo(_tilt_idx, degrees(_angle_bf_output_rad.y)*10, _params.pitch_angle_min*10, _params.pitch_angle_max*10);
      47      342483 :     move_servo(_pan_idx,  degrees(_angle_bf_output_rad.z)*10, _params.yaw_angle_min*10, _params.yaw_angle_max*10);
      48      342483 : }
      49             : 
      50             : // returns true if this mount can control its roll
      51      126267 : bool AP_Mount_Servo::has_roll_control() const
      52             : {
      53      126267 :     return SRV_Channels::function_assigned(_roll_idx) && roll_range_valid();
      54             : }
      55             : 
      56             : // returns true if this mount can control its tilt
      57      126267 : bool AP_Mount_Servo::has_pitch_control() const
      58             : {
      59      126267 :     return SRV_Channels::function_assigned(_tilt_idx) && pitch_range_valid();
      60             : }
      61             : 
      62             : // returns true if this mount can control its pan (required for multicopters)
      63      459205 : bool AP_Mount_Servo::has_pan_control() const
      64             : {
      65      459205 :     return SRV_Channels::function_assigned(_pan_idx) && yaw_range_valid();
      66             : }
      67             : 
      68             : // get attitude as a quaternion.  returns true on success
      69      126267 : bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat)
      70             : {
      71             :     // No feedback from gimbal so simply report demanded servo angles (which is
      72             :     // not the same as target angles).
      73      126267 :     float roll_rad = 0.0f;
      74      126267 :     float pitch_rad = 0.0f;
      75      126267 :     float yaw_rad = 0.0f;
      76      126267 :     if (has_roll_control()) {
      77       21277 :         roll_rad = constrain_float(_angle_bf_output_rad.x, radians(_params.roll_angle_min), radians(_params.roll_angle_max));
      78             :     }
      79      126267 :     if (has_pitch_control()) {
      80      126120 :         pitch_rad = constrain_float(_angle_bf_output_rad.y, radians(_params.pitch_angle_min), radians(_params.pitch_angle_max));
      81             :     }
      82      126267 :     if (has_pan_control()) {
      83       17894 :         yaw_rad = constrain_float(_angle_bf_output_rad.z, radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
      84             :     }
      85             : 
      86             :     // convert to quaternion
      87      126267 :     att_quat.from_euler(roll_rad, pitch_rad, yaw_rad);
      88      126267 :     return true;
      89             : }
      90             : 
      91             : // private methods
      92             : 
      93             : // update body-frame angle outputs from earth-frame angle targets
      94      342483 : void AP_Mount_Servo::update_angle_outputs(const MountAngleTarget& angle_rad)
      95             : {
      96      342483 :     const AP_AHRS &ahrs = AP::ahrs();
      97             : 
      98             :     // get target yaw in body-frame with limits applied
      99      342483 :     const float yaw_bf_rad = constrain_float(angle_rad.get_bf_yaw(), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
     100             : 
     101             :     // default output to target earth-frame roll and pitch angles, body-frame yaw
     102      342483 :     _angle_bf_output_rad.x = angle_rad.roll;
     103      342483 :     _angle_bf_output_rad.y = angle_rad.pitch;
     104      342483 :     _angle_bf_output_rad.z = yaw_bf_rad;
     105             : 
     106             :     // do no stabilization in retract or neutral:
     107      342483 :     switch (mnt_target.target_type) {
     108        9558 :     case MountTargetType::NEUTRAL:
     109             :     case MountTargetType::RETRACTED:
     110      278483 :         return;
     111      332925 :     case MountTargetType::ANGLE:
     112             :     case MountTargetType::RATE:
     113             :     case MountTargetType::LOCATION:
     114      332925 :         break;
     115             :     }
     116             : 
     117             :     // only have to adjust roll/pitch for body frame in self-stabilising brushless gimbals
     118      332925 :     if (!requires_stabilization) {
     119             :         //since this is a shared backend, must call this directly
     120      268925 :         AP_Mount_Backend::adjust_mnt_target_if_RP_locked();
     121      268925 :         return;
     122             :     }
     123             : 
     124             :     // retrieve lean angles from ahrs
     125       64000 :     Vector2f ahrs_angle_rad = {ahrs.get_roll_rad(), ahrs.get_pitch_rad()};
     126             : 
     127             :     // rotate ahrs roll and pitch angles to gimbal yaw
     128       64000 :     if (has_pan_control()) {
     129       48855 :         ahrs_angle_rad.rotate(-yaw_bf_rad);
     130             :     }
     131             : 
     132             :     // add roll and pitch lean angle correction for earth frame
     133       64000 :     if (angle_rad.roll_is_ef){
     134       64000 :         _angle_bf_output_rad.x -= ahrs_angle_rad.x;
     135             :     }
     136             :     
     137       64000 :     if (angle_rad.pitch_is_ef){
     138       64000 :         _angle_bf_output_rad.y -= ahrs_angle_rad.y;
     139             :     } 
     140             : 
     141             :     // lead filter
     142       64000 :     const Vector3f &gyro = ahrs.get_gyro();
     143             : 
     144       64000 :     if (!is_zero(_params.roll_stb_lead) && fabsf(ahrs.get_pitch_rad()) < M_PI/3.0f) {
     145             :         // Compute rate of change of euler roll angle
     146           0 :         float roll_rate = gyro.x + (ahrs.sin_pitch() / ahrs.cos_pitch()) * (gyro.y * ahrs.sin_roll() + gyro.z * ahrs.cos_roll());
     147           0 :         _angle_bf_output_rad.x -= roll_rate * _params.roll_stb_lead;
     148             :     }
     149             : 
     150       64000 :     if (!is_zero(_params.pitch_stb_lead)) {
     151             :         // Compute rate of change of euler pitch angle
     152           0 :         float pitch_rate = ahrs.cos_pitch() * gyro.y - ahrs.sin_roll() * gyro.z;
     153           0 :         _angle_bf_output_rad.y -= pitch_rate * _params.pitch_stb_lead;
     154             :     }
     155             : }
     156             : 
     157             : // move_servo - moves servo with the given id to the specified angle.  all angles are in degrees * 10
     158     1027449 : void AP_Mount_Servo::move_servo(uint8_t function_idx, int16_t angle, int16_t angle_min, int16_t angle_max)
     159             : {
     160     1027449 :         SRV_Channels::move_servo((SRV_Channel::Function)function_idx, angle, angle_min, angle_max);
     161     1027449 : }
     162             : #endif // HAL_MOUNT_SERVO_ENABLED