mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
b7e5f56bd7
By opening with O_CLOEXEC we make sure we don't leak the file descriptor when we are exec'ing or calling out subprograms. Right now we currently don't do it so there's no harm, but it's good practice in Linux to have it.
124 lines
3.5 KiB
C++
124 lines
3.5 KiB
C++
/*
|
|
* AP_Gripper_EPM.cpp
|
|
*
|
|
* Created on: DEC 6, 2013
|
|
* Author: Andreas Jochum
|
|
* Pavel Kirienko <pavel.kirienko@zubax.com> - UAVCAN support
|
|
* Peter Barker - moved into AP_Gripper_EPM
|
|
*/
|
|
|
|
#include "AP_Gripper_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;
|
|
|
|
AP_Gripper_EPM::AP_Gripper_EPM(struct AP_Gripper::Backend_Config &_config) :
|
|
AP_Gripper_Backend(_config) { }
|
|
|
|
void AP_Gripper_EPM::init_gripper()
|
|
{
|
|
#ifdef UAVCAN_NODE_FILE
|
|
_uavcan_fd = ::open(UAVCAN_NODE_FILE, O_CLOEXEC);
|
|
// http://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html
|
|
::printf("EPM: UAVCAN fd %d\n", _uavcan_fd);
|
|
#endif
|
|
|
|
// initialise the EPM to the neutral position
|
|
neutral();
|
|
}
|
|
|
|
// Refer to http://uavcan.org/Specification/7._List_of_standard_data_types/#uavcanequipmenthardpoint
|
|
struct UAVCANCommand {
|
|
uint8_t hardpoint_id = 0;
|
|
uint16_t command = 0;
|
|
};
|
|
|
|
// grab - move the epm pwm output to the grab position
|
|
void AP_Gripper_EPM::grab()
|
|
{
|
|
// flag we are active and grabbing cargo
|
|
config.state = AP_Gripper::STATE_GRABBING;
|
|
|
|
// capture time
|
|
_last_grab_or_release = AP_HAL::millis();
|
|
|
|
#ifdef UAVCAN_IOCS_HARDPOINT_SET
|
|
if (should_use_uavcan()) {
|
|
const UAVCANCommand cmd = make_uavcan_command(1);
|
|
(void)ioctl(_uavcan_fd, UAVCAN_IOCS_HARDPOINT_SET, reinterpret_cast<unsigned long>(&cmd));
|
|
}
|
|
else
|
|
#endif
|
|
{
|
|
// move the servo to the release position
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.grab_pwm);
|
|
}
|
|
}
|
|
|
|
// release - move epm pwm output to the release position
|
|
void AP_Gripper_EPM::release()
|
|
{
|
|
// flag we are releasing cargo
|
|
config.state = AP_Gripper::STATE_RELEASING;
|
|
|
|
// capture time
|
|
_last_grab_or_release = AP_HAL::millis();
|
|
|
|
#ifdef UAVCAN_IOCS_HARDPOINT_SET
|
|
if (should_use_uavcan()) {
|
|
const UAVCANCommand cmd = make_uavcan_command(0);
|
|
(void)ioctl(_uavcan_fd, UAVCAN_IOCS_HARDPOINT_SET, reinterpret_cast<unsigned long>(&cmd));
|
|
}
|
|
else
|
|
#endif
|
|
{
|
|
// move the servo to the release position
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.release_pwm);
|
|
}
|
|
}
|
|
|
|
// neutral - return the epm pwm output to the neutral position
|
|
void AP_Gripper_EPM::neutral()
|
|
{
|
|
if (!should_use_uavcan()) {
|
|
// move the servo to the off position
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_gripper, config.neutral_pwm);
|
|
}
|
|
}
|
|
|
|
// update - moves the pwm back to neutral after the timeout has passed
|
|
// should be called at at least 10hz
|
|
void AP_Gripper_EPM::update_gripper()
|
|
{
|
|
// move EPM PWM output back to neutral after the last grab or release
|
|
if (AP_HAL::millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS) {
|
|
if (config.state == AP_Gripper::STATE_GRABBING) {
|
|
neutral();
|
|
config.state = AP_Gripper::STATE_GRABBED;
|
|
} else if (config.state == AP_Gripper::STATE_RELEASING) {
|
|
neutral();
|
|
config.state = AP_Gripper::STATE_RELEASED;
|
|
}
|
|
}
|
|
|
|
// re-grab the cargo intermittently
|
|
if (config.state == AP_Gripper::STATE_GRABBED &&
|
|
(config.regrab_interval > 0) &&
|
|
(AP_HAL::millis() - _last_grab_or_release > ((uint32_t)config.regrab_interval * 1000))) {
|
|
grab();
|
|
}
|
|
}
|
|
|
|
UAVCANCommand AP_Gripper_EPM::make_uavcan_command(uint16_t command) const
|
|
{
|
|
UAVCANCommand cmd;
|
|
cmd.hardpoint_id = config.uavcan_hardpoint_id;
|
|
cmd.command = command;
|
|
return cmd;
|
|
}
|
|
|