Line data    Source code

       1             : /*
       2             :  * AP_Gripper_EPM.cpp
       3             :  *
       4             :  *  Created on: DEC 6, 2013
       5             :  *      Author: Andreas Jochum
       6             :  *              Pavel Kirienko <[email protected]> - UAVCAN support
       7             :  *              Peter Barker - moved into AP_Gripper_EPM
       8             :  */
       9             : 
      10             : #include "AP_Gripper_EPM.h"
      11             : 
      12             : #if AP_GRIPPER_EPM_ENABLED
      13             : 
      14             : #include <AP_HAL/AP_HAL.h>
      15             : #include <AP_BoardConfig/AP_BoardConfig.h>
      16             : #include <GCS_MAVLink/GCS.h>
      17             : #include <AP_Logger/AP_Logger.h>
      18             : #include <SRV_Channel/SRV_Channel.h>
      19             : 
      20             : #ifdef UAVCAN_NODE_FILE
      21             : #include <fcntl.h>
      22             : #include <stdio.h>
      23             : #endif
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27           2 : AP_Gripper_EPM::AP_Gripper_EPM(struct AP_Gripper::Backend_Config &_config) :
      28           2 :     AP_Gripper_Backend(_config) { }
      29             : 
      30           2 : void AP_Gripper_EPM::init_gripper()
      31             : {
      32             : #ifdef UAVCAN_NODE_FILE
      33             :     _uavcan_fd = ::open(UAVCAN_NODE_FILE, O_CLOEXEC);
      34             :     // https://ardupilot.club/dev/docs/learning-ardupilot-uarts-and-the-console.html
      35             :     ::printf("EPM: DroneCAN fd %d\n", _uavcan_fd);
      36             : #endif
      37             : 
      38             :     // initialise the EPM to the neutral position
      39           2 :     neutral();
      40           2 : }
      41             : 
      42             : // Refer to http://uavcan.org/Specification/7._List_of_standard_data_types/#uavcanequipmenthardpoint
      43             : struct UAVCANCommand {
      44             :     uint8_t hardpoint_id = 0;
      45             :     uint16_t command = 0;
      46             : };
      47             : 
      48             : // grab - move the epm pwm output to the grab position
      49           8 : void AP_Gripper_EPM::grab()
      50             : {
      51             :     // flag we are active and grabbing cargo
      52           8 :     config.state = AP_Gripper::STATE_GRABBING;
      53             : 
      54             :     // capture time
      55           8 :     _last_grab_or_release = AP_HAL::millis();
      56             : 
      57             : #ifdef UAVCAN_IOCS_HARDPOINT_SET
      58             :     if (should_use_uavcan()) {
      59             :         const UAVCANCommand cmd = make_uavcan_command(1);
      60             :         (void)ioctl(_uavcan_fd, UAVCAN_IOCS_HARDPOINT_SET, reinterpret_cast<unsigned long>(&cmd));
      61             :     }
      62             :     else
      63             : #endif
      64             :     {
      65             :         // move the servo output to the grab position
      66           8 :         SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm);
      67             :     }
      68           8 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load grabbing");
      69           8 :     LOGGER_WRITE_EVENT(LogEvent::GRIPPER_GRAB);
      70           8 : }
      71             : 
      72             : // release - move epm pwm output to the release position
      73           8 : void AP_Gripper_EPM::release()
      74             : {
      75             :     // flag we are releasing cargo
      76           8 :     config.state = AP_Gripper::STATE_RELEASING;
      77             : 
      78             :     // capture time
      79           8 :     _last_grab_or_release = AP_HAL::millis();
      80             : 
      81             : #ifdef UAVCAN_IOCS_HARDPOINT_SET
      82             :     if (should_use_uavcan()) {
      83             :         const UAVCANCommand cmd = make_uavcan_command(0);
      84             :         (void)ioctl(_uavcan_fd, UAVCAN_IOCS_HARDPOINT_SET, reinterpret_cast<unsigned long>(&cmd));
      85             :     }
      86             :     else
      87             : #endif
      88             :     {
      89             :         // move the servo to the release position
      90           8 :         SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm);
      91             :     }
      92           8 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load releasing");
      93           8 :     LOGGER_WRITE_EVENT(LogEvent::GRIPPER_RELEASE);
      94           8 : }
      95             : 
      96             : // neutral - return the epm pwm output to the neutral position
      97           2 : void AP_Gripper_EPM::neutral()
      98             : {
      99           2 :     if (!should_use_uavcan()) {
     100             :         // move the servo to the off position
     101           2 :         SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.neutral_pwm);
     102             :     }
     103           2 : }
     104             : 
     105             : // update - moves the pwm back to neutral after the timeout has passed
     106             : // should be called at at least 10hz
     107         860 : void AP_Gripper_EPM::update_gripper()
     108             : {
     109             :     // move EPM PWM output back to neutral after the last grab or release
     110         860 :     if (AP_HAL::millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS) {
     111         817 :         if (config.state == AP_Gripper::STATE_GRABBING) {
     112           0 :             neutral();
     113           0 :             config.state = AP_Gripper::STATE_GRABBED;
     114         817 :         } else if (config.state == AP_Gripper::STATE_RELEASING) {
     115           0 :             neutral();
     116           0 :             config.state = AP_Gripper::STATE_RELEASED;
     117             :         }
     118             :     }
     119             : 
     120             :     // re-grab the cargo intermittently
     121        1720 :     if (config.state == AP_Gripper::STATE_GRABBED &&
     122         860 :         (config.regrab_interval > 0) &&
     123           0 :         (AP_HAL::millis() - _last_grab_or_release > ((uint32_t)config.regrab_interval * 1000))) {
     124           0 :         grab();
     125             :     }
     126         860 : }
     127             : 
     128           0 : UAVCANCommand AP_Gripper_EPM::make_uavcan_command(uint16_t command) const
     129             : {
     130           0 :     UAVCANCommand cmd;
     131           0 :     cmd.hardpoint_id = config.uavcan_hardpoint_id;
     132           0 :     cmd.command = command;
     133           0 :     return cmd;
     134             : }
     135             : 
     136             : 
     137           0 : bool AP_Gripper_EPM::released() const
     138             : {
     139             :     // we assume instanteous releasing ATM:
     140           0 :     return (config.state == AP_Gripper::STATE_GRABBED ||
     141           0 :             config.state == AP_Gripper::STATE_GRABBING);
     142             : }
     143             : 
     144           0 : bool AP_Gripper_EPM::grabbed() const
     145             : {
     146             :     // we assume instanteous grabbing ATM:
     147           0 :     return (config.state == AP_Gripper::STATE_GRABBED ||
     148           0 :             config.state == AP_Gripper::STATE_GRABBING);
     149             : }
     150             : 
     151             : #endif  // AP_GRIPPER_EPM_ENABLED