Line data    Source code

       1             : #include "AP_Mount_config.h"
       2             : 
       3             : #if HAL_SOLO_GIMBAL_ENABLED
       4             : 
       5             : #include "AP_Mount_SoloGimbal.h"
       6             : 
       7             : #include "SoloGimbal.h"
       8             : #include <AP_Logger/AP_Logger.h>
       9             : #include <GCS_MAVLink/GCS_MAVLink.h>
      10             : #include <GCS_MAVLink/GCS.h>
      11             : 
      12           1 : AP_Mount_SoloGimbal::AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance) :
      13             :     AP_Mount_Backend(frontend, params, instance),
      14           1 :     _gimbal()
      15           1 : {}
      16             : 
      17             : // init - performs any required initialisation for this instance
      18           1 : void AP_Mount_SoloGimbal::init()
      19             : {
      20           1 :     _initialised = true;
      21           1 :     AP_Mount_Backend::init();
      22           1 : }
      23             : 
      24       81838 : void AP_Mount_SoloGimbal::update_fast()
      25             : {
      26       81838 :     _gimbal.update_fast();
      27       81838 : }
      28             : 
      29             : // update mount position - should be called periodically
      30       10229 : void AP_Mount_SoloGimbal::update()
      31             : {
      32       10229 :     AP_Mount_Backend::update();
      33             : 
      34             :     // exit immediately if not initialised
      35       10229 :     if (!_initialised) {
      36           0 :         return;
      37             :     }
      38             : 
      39             :     // change to RC_TARGETING mode if RC input has changed
      40       10229 :     set_rctargeting_on_rcinput_change();
      41             : 
      42             :     // update based on mount mode
      43       10229 :     switch(get_mode()) {
      44             :         // move mount to a "retracted" position.  we do not implement a separate servo based retract mechanism
      45           0 :         case MAV_MOUNT_MODE_RETRACT:
      46           0 :             _gimbal.set_lockedToBody(true);
      47           0 :             mnt_target.target_type = MountTargetType::ANGLE;
      48           0 :             mnt_target.angle_rad.set(Vector3f{0,0,0}, false);
      49           0 :             break;
      50             : 
      51             :         // move mount to a neutral position, typically pointing forward
      52        5504 :         case MAV_MOUNT_MODE_NEUTRAL: {
      53        5504 :             _gimbal.set_lockedToBody(false);
      54        5504 :             const Vector3f &target = _params.neutral_angles.get();
      55        5504 :             mnt_target.target_type = MountTargetType::ANGLE;
      56        5504 :             mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
      57        5504 :             break;
      58             :         }
      59             : 
      60             :         // point to the angles given by a mavlink message
      61         200 :         case MAV_MOUNT_MODE_MAVLINK_TARGETING:
      62             :             // targets are stored while handling the incoming mavlink message
      63         200 :             _gimbal.set_lockedToBody(false);
      64         200 :             break;
      65             : 
      66             :         // RC radio manual angle control, but with stabilization from the AHRS
      67        2096 :         case MAV_MOUNT_MODE_RC_TARGETING:
      68        2096 :             _gimbal.set_lockedToBody(false);
      69        2096 :             update_mnt_target_from_rc_target();
      70        2096 :             break;
      71             : 
      72             :         // point mount to a GPS point given by the mission planner
      73         480 :         case MAV_MOUNT_MODE_GPS_POINT:
      74         480 :             if (get_angle_target_to_roi(mnt_target.angle_rad)) {
      75         480 :                 mnt_target.target_type = MountTargetType::ANGLE;
      76         480 :                 _gimbal.set_lockedToBody(false);
      77             :             }
      78         480 :             break;
      79             : 
      80             :         // point mount to Home location
      81           0 :         case MAV_MOUNT_MODE_HOME_LOCATION:
      82           0 :             if (get_angle_target_to_home(mnt_target.angle_rad)) {
      83           0 :                 mnt_target.target_type = MountTargetType::ANGLE;
      84           0 :                 _gimbal.set_lockedToBody(false);
      85             :             }
      86           0 :             break;
      87             : 
      88             :         // point mount to another vehicle
      89        1949 :         case MAV_MOUNT_MODE_SYSID_TARGET:
      90        1949 :             if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
      91         400 :                 mnt_target.target_type = MountTargetType::ANGLE;
      92         400 :                 _gimbal.set_lockedToBody(false);
      93             :             }
      94        1949 :             break;
      95             : 
      96           0 :         default:
      97             :             // we do not know this mode so do nothing
      98           0 :             break;
      99             :     }
     100             : 
     101       10229 :     if (mnt_target.target_type == MountTargetType::RATE) {
     102           0 :         update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
     103             :     }
     104             : }
     105             : 
     106             : // get attitude as a quaternion.  returns true on success
     107        3068 : bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat)
     108             : {
     109        3068 :     if (!_gimbal.aligned()) {
     110         341 :         return false;
     111             :     }
     112        2727 :     att_quat.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw());
     113        2727 :     return true;
     114             : }
     115             : 
     116             : /*
     117             :   handle a GIMBAL_REPORT message
     118             :  */
     119       18195 : void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg)
     120             : {
     121       18195 :     _gimbal.update_target(Vector3f{mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw()});
     122       18195 :     _gimbal.receive_feedback(chan,msg);
     123             : 
     124             : #if HAL_LOGGING_ENABLED
     125       18195 :     AP_Logger *logger = AP_Logger::get_singleton();
     126       18195 :     if (logger == nullptr) {
     127           0 :         return;
     128             :     }
     129             : #endif
     130       36390 :     if(!_params_saved
     131             : #if HAL_LOGGING_ENABLED
     132       18195 :        && logger->logging_started()
     133             : #endif
     134             :         ) {
     135           1 :         _gimbal.fetch_params();       //last parameter save might not be stored in logger so retry
     136           1 :         _params_saved = true;
     137             :     }
     138             : 
     139             : #if HAL_LOGGING_ENABLED
     140       18195 :     if (_gimbal.get_log_dt() > 1.0f/25.0f) {
     141        4545 :         _gimbal.write_logs();
     142             :     }
     143             : #endif
     144             : }
     145             : 
     146          26 : void AP_Mount_SoloGimbal::handle_param_value(const mavlink_message_t &msg)
     147             : {
     148          26 :     _gimbal.handle_param_value(msg);
     149          26 : }
     150             : 
     151             : /*
     152             :   handle a GIMBAL_REPORT message
     153             :  */
     154           0 : void AP_Mount_SoloGimbal::handle_gimbal_torque_report(mavlink_channel_t chan, const mavlink_message_t &msg)
     155             : {
     156           0 :     _gimbal.disable_torque_report();
     157           0 : }
     158             : 
     159             : #endif // HAL_SOLO_GIMBAL_ENABLED