Line data    Source code

       1             : #include <AP_Gripper/AP_Gripper_Servo.h>
       2             : 
       3             : #if AP_GRIPPER_SERVO_ENABLED
       4             : 
       5             : #include <GCS_MAVLink/GCS.h>
       6             : #include <AP_Logger/AP_Logger.h>
       7             : #include <SRV_Channel/SRV_Channel.h>
       8             : 
       9             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      10             :   #include <SITL/SITL.h>
      11             : #endif
      12             : 
      13             : extern const AP_HAL::HAL& hal;
      14             : 
      15           6 : void AP_Gripper_Servo::init_gripper()
      16             : {
      17             :     // move the servo to the neutral position
      18           6 :     SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.neutral_pwm);
      19           6 : }
      20             : 
      21           8 : void AP_Gripper_Servo::grab()
      22             : {
      23             :     // check if we are already grabbing
      24           8 :     if (config.state == AP_Gripper::STATE_GRABBING) {
      25             :         // do nothing
      26           0 :         return;
      27             :     }
      28             : 
      29             :     // check if we are already grabbed
      30           8 :     if (config.state == AP_Gripper::STATE_GRABBED) {
      31             :         // inform user that we are already grabbed
      32           0 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load grabbed");
      33           0 :         return;
      34             :     }
      35             : 
      36             :     // flag we are active and grabbing cargo
      37           8 :     config.state = AP_Gripper::STATE_GRABBING;
      38             : 
      39             :     // move the servo to the grab position
      40           8 :     SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
      41           8 :     _last_grab_or_release = AP_HAL::millis();
      42           8 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load grabbing");
      43           8 :     LOGGER_WRITE_EVENT(LogEvent::GRIPPER_GRAB);
      44             : }
      45             : 
      46          11 : void AP_Gripper_Servo::release()
      47             : {
      48             :     // check if we are already releasing
      49          11 :     if (config.state == AP_Gripper::STATE_RELEASING) {
      50             :         // do nothing
      51           0 :         return;
      52             :     }
      53             : 
      54             :     // check if we are already released
      55          11 :     if (config.state == AP_Gripper::STATE_RELEASED) {
      56             :         // inform user that we are already released
      57           0 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load released");
      58           0 :         return;
      59             :     }
      60             :     
      61             :     // flag we are releasing cargo
      62          11 :     config.state = AP_Gripper::STATE_RELEASING;
      63             : 
      64             :     // move the servo to the release position
      65          11 :     SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm);
      66          11 :     _last_grab_or_release = AP_HAL::millis();
      67          11 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load releasing");
      68          11 :     LOGGER_WRITE_EVENT(LogEvent::GRIPPER_RELEASE);
      69             : }
      70             : 
      71          61 : bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const
      72             : {
      73             :     // return true if servo is in position represented by pwm
      74             :     uint16_t current_pwm;
      75          61 :     if (!SRV_Channels::get_output_pwm(SRV_Channel::k_gripper, current_pwm)) {
      76             :         // function not assigned to a channel, perhaps?
      77           0 :         return false;
      78             :     }
      79          61 :     if (current_pwm != pwm) {
      80             :         // last action did not set pwm to the current value
      81             :         // (e.g. last action was a grab not a release)
      82           0 :         return false;
      83             :     }
      84          61 :     if (AP_HAL::millis() - _last_grab_or_release < SERVO_ACTUATION_TIME) {
      85             :         // servo still moving....
      86          58 :         return false;
      87             :     }
      88           3 :     return true;
      89             : }
      90             : 
      91       24003 : bool AP_Gripper_Servo::released() const
      92             : {
      93       24003 :     return (config.state == AP_Gripper::STATE_RELEASED);
      94             : }
      95             : 
      96           0 : bool AP_Gripper_Servo::grabbed() const
      97             : {
      98           0 :     return (config.state == AP_Gripper::STATE_GRABBED);
      99             : }
     100             : 
     101             : // type-specific periodic updates:
     102        5542 : void AP_Gripper_Servo::update_gripper()
     103             : {
     104             :     // Check for successful grabbed or released
     105        5542 :     if (config.state == AP_Gripper::STATE_GRABBING && has_state_pwm(config.grab_pwm)) {
     106           0 :         config.state = AP_Gripper::STATE_GRABBED;
     107           0 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load grabbed");
     108        5542 :     } else if (config.state == AP_Gripper::STATE_RELEASING && has_state_pwm(config.release_pwm)) {
     109           3 :         config.state = AP_Gripper::STATE_RELEASED;
     110           3 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load released");
     111             :     }
     112        5542 : }
     113             : 
     114       24004 : bool AP_Gripper_Servo::valid() const
     115             : {
     116       24004 :     if (!AP_Gripper_Backend::valid()) {
     117           0 :         return false;
     118             :     }
     119       24004 :     if (!SRV_Channels::function_assigned(SRV_Channel::k_gripper)) {
     120           0 :         return false;
     121             :     }
     122       24004 :     return true;
     123             : }
     124             : 
     125             : #endif  // AP_GRIPPER_SERVO_ENABLED