UAVCAN hardpoint control

ArduPilot · Jun 23, 2016 · 29848fc · 29848fc
1 parent 107691b
commit 29848fc
Show file tree

Hide file tree

Showing 4 changed files with 75 additions and 16 deletions.
diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp
@@ -102,12 +102,6 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 extern "C" int uavcan_main(int argc, const char *argv[]);
-
-#define _UAVCAN_IOCBASE             (0x4000)                        // IOCTL base for module UAVCAN
-#define _UAVCAN_IOC(_n)             (_IOC(_UAVCAN_IOCBASE, _n))
-
-#define UAVCAN_IOCG_NODEID_INPROGRESS  _UAVCAN_IOC(1)               // query if node identification is in progress
-
 #endif
 
 void AP_BoardConfig::init()
@@ -203,9 +197,9 @@ void AP_BoardConfig::init()
             hal.console->printf("UAVCAN: failed to start servers\n");
         } else {
             uint32_t start_wait_ms = AP_HAL::millis();
-            int fd = open("/dev/uavcan/esc", 0); // design flaw of uavcan driver, this should be /dev/uavcan/node one day
+            int fd = open(UAVCAN_NODE_FILE, 0);
             if (fd == -1) {
-                AP_HAL::panic("Configuration invalid - unable to open /dev/uavcan/esc");
+                AP_HAL::panic("Configuration invalid - unable to open " UAVCAN_NODE_FILE);
             }
 
             // delay startup, UAVCAN still discovering nodes

diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h
@@ -4,6 +4,19 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
+#include <sys/ioctl.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
+#define _UAVCAN_IOCBASE             (0x4000)                        // IOCTL base for module UAVCAN
+#define _UAVCAN_IOC(_n)             (_IOC(_UAVCAN_IOCBASE, _n))
+
+#define UAVCAN_IOCG_NODEID_INPROGRESS  _UAVCAN_IOC(1)               // query if node identification is in progress
+#define UAVCAN_IOCS_HARDPOINT_SET      _UAVCAN_IOC(10)              // control hardpoint (e.g. OpenGrab EPM)
+
+#define UAVCAN_NODE_FILE            "/dev/uavcan/esc"   // design flaw of uavcan driver, this should be /dev/uavcan/node one day
+
+#endif
 
 class AP_BoardConfig
 {

diff --git a/libraries/AP_EPM/AP_EPM.cpp b/libraries/AP_EPM/AP_EPM.cpp
@@ -5,10 +5,15 @@
  *
  *  Created on: DEC 6, 2013
  *      Author: Andreas Jochum
+ *              Pavel Kirienko <[email protected]> - UAVCAN support
  */
 
 #include "AP_EPM.h"
 #include <AP_HAL/AP_HAL.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <cstdio>
 
 extern const AP_HAL::HAL& hal;
 
@@ -48,6 +53,13 @@ const AP_Param::GroupInfo AP_EPM::var_info[] = {
     // @Values: 0:Never, 15:every 15 seconds, 30:every 30 seconds, 60:once per minute
     AP_GROUPINFO("REGRAB",  4, AP_EPM, _regrab_interval, EPM_REGRAB_DEFAULT),
 
+    // @Param: REGRAB
+    // @DisplayName: EPM UAVCAN Hardpoint ID
+    // @Description: Refer to https://docs.zubax.com/opengrab_epm_v3#UAVCAN_interface
+    // @User: Standard
+    // @Range: 0 to 255
+    AP_GROUPINFO("UAVCAN_ID", 5, AP_EPM, _uavcan_hardpoint_id, 0),
+
     AP_GROUPEND
 };
 
@@ -59,7 +71,6 @@ AP_EPM::AP_EPM(void) :
     // initialise flags
     _flags.grab = false;
     _flags.active = false;
-
 }
 
 void AP_EPM::init() 
@@ -69,6 +80,10 @@ void AP_EPM::init()
         return;
     }
 
+    _uavcan_fd = open(UAVCAN_NODE_FILE, 0);
+    // http://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html
+    ::printf("EPM: UAVCAN fd %d\n", _uavcan_fd);
+
     // initialise the EPM to the neutral position
     neutral();
 }
@@ -88,8 +103,14 @@ void AP_EPM::grab()
     // capture time
     _last_grab_or_release = AP_HAL::millis();
 
-    // move the servo to the release position
-    RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _grab_pwm);
+    if (should_use_uavcan()) {
+        ::printf("EPM: UAVCAN GRAB\n");
+        const UAVCANCommand cmd = make_uavcan_command(1);
+        (void)ioctl(_uavcan_fd, UAVCAN_IOCS_HARDPOINT_SET, reinterpret_cast<unsigned long>(&cmd));
+    } else {
+        // move the servo to the release position
+        RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _grab_pwm);
+    }
 }
 
 // release - move the epm pwm output to the release position
@@ -107,8 +128,14 @@ void AP_EPM::release()
     // capture time
     _last_grab_or_release = AP_HAL::millis();
 
-    // move the servo to the release position
-    RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _release_pwm);
+    if (should_use_uavcan()) {
+        ::printf("EPM: UAVCAN RELEASE\n");
+        const UAVCANCommand cmd = make_uavcan_command(0);
+        (void)ioctl(_uavcan_fd, UAVCAN_IOCS_HARDPOINT_SET, reinterpret_cast<unsigned long>(&cmd));
+    } else {
+        // move the servo to the release position
+        RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _release_pwm);
+    }
 }
 
 // neutral - return the epm pwm output to the neutral position
@@ -122,8 +149,10 @@ void AP_EPM::neutral()
     // flag we are inactive (i.e. not grabbing or releasing cargo)
     _flags.active = false;
 
-    // move the servo to the off position
-    RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _neutral_pwm);
+    if (!should_use_uavcan()) {
+        // move the servo to the off position
+        RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, _neutral_pwm);
+    }
 }
 
 // update - moves the pwm back to neutral after the timeout has passed
@@ -136,7 +165,8 @@ void AP_EPM::update()
     }
 
     // re-grab the cargo intermittently
-    if (_flags.grab && (_regrab_interval > 0) && (AP_HAL::millis() - _last_grab_or_release > ((uint32_t)_regrab_interval * 1000))) {
+    if (_flags.grab && (_regrab_interval > 0) &&
+        (AP_HAL::millis() - _last_grab_or_release > ((uint32_t)_regrab_interval * 1000))) {
         grab();
     }
 }
diff --git a/libraries/AP_EPM/AP_EPM.h b/libraries/AP_EPM/AP_EPM.h
@@ -5,8 +5,10 @@
  *
  *  Created on: DEC 06, 2013
  *      Author: Andreas Jochum
+ *              Pavel Kirienko <[email protected]> - UAVCAN support
  *
  *      Set-up Wiki: http://copter.ardupilot.org/wiki/common-electro-permanent-magnet-gripper/
+ *      EPM docs:    https://docs.zubax.com/opengrab_epm_v3
  */
 
 /// @file	AP_EPM.h
@@ -53,18 +55,38 @@ class AP_EPM {
     // neutral - return the EPM pwm output to the neutral position
     void        neutral();
 
+    bool should_use_uavcan() const { return _uavcan_fd >= 0; }
+
+    // Refer to http://uavcan.org/Specification/7._List_of_standard_data_types/#uavcanequipmenthardpoint
+    struct UAVCANCommand {
+        uint8_t hardpoint_id = 0;
+        uint16_t command = 0;
+    };
+
+    UAVCANCommand make_uavcan_command(uint16_t command) const
+    {
+        UAVCANCommand cmd;
+        cmd.hardpoint_id = _uavcan_hardpoint_id;
+        cmd.command = command;
+        return cmd;
+    }
+
     // EPM flags
     struct EPM_Flags {
         uint8_t grab    : 1;    // true if we think we have grabbed onto cargo, false if we think we've released it
         uint8_t active  : 1;    // true if we are actively sending grab or release PWM to EPM to activate grabbing or releasing, false if we are sending neutral pwm
     } _flags;
 
+    // UAVCAN driver fd
+    int _uavcan_fd = -1;
+
     // parameters
     AP_Int8     _enabled;               // EPM enable/disable
     AP_Int16    _grab_pwm;              // PWM value sent to EPM to initiate grabbing the cargo
     AP_Int16    _release_pwm;           // PWM value sent to EPM to release the cargo
     AP_Int16    _neutral_pwm;           // PWM value sent to EPM when not grabbing or releasing
     AP_Int8     _regrab_interval;       // Time in seconds that gripper will regrab the cargo to ensure grip has not weakend
+    AP_Int16    _uavcan_hardpoint_id;
 
     // internal variables
     uint32_t    _last_grab_or_release;