Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

EPM: add support for ver2, remove support ver 1

This commit is contained in:
Randy Mackay 2014-09-15 15:51:06 +09:00
parent 79f95efb71
commit 1401ee4077
2 changed files with 59 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_EPM/AP_EPM.cpp
View File

@ -7,8 +7,8 @@
* Author: Andreas Jochum
*/
#include <AP_EPM.h>
#include <AP_HAL.h>
#include "AP_EPM.h"
extern const AP_HAL::HAL& hal;
@ -23,7 +23,8 @@ const AP_Param::GroupInfo AP_EPM::var_info[] PROGMEM = {
AP_GROUPEND
};
AP_EPM::AP_EPM(void)
AP_EPM::AP_EPM(void) :
_last_grab_or_release(0)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -31,54 +32,63 @@ AP_EPM::AP_EPM(void)
void AP_EPM::init()
{
// return immediately if not enabled
if (!_enabled || !EPM_SUPPORTED) {
if (!_enabled) {
return;
}
hal.gpio->pinMode(EPM_PIN_1, HAL_GPIO_OUTPUT);
hal.gpio->pinMode(EPM_PIN_2, HAL_GPIO_OUTPUT);
// initialise the EPM to the neutral position
neutral();
}
bool AP_EPM::enabled()
{
if (!EPM_SUPPORTED) {
return false;
}
return _enabled;
}
void AP_EPM::on()
// grab - move the epm pwm output to the grab position
void AP_EPM::grab()
{
// return immediately if not enabled
if (!_enabled || !EPM_SUPPORTED) {
if (!_enabled) {
return;
}
hal.gpio->write(EPM_PIN_1, 1);
hal.gpio->write(EPM_PIN_2, 0);
// capture time
_last_grab_or_release = hal.scheduler->millis();
// move the servo to the release position
RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, EPM_GRAB_PWM);
}
void AP_EPM::off()
// release - move the epm pwm output to the release position
void AP_EPM::release()
{
// return immediately if not enabled
if (!_enabled || !EPM_SUPPORTED) {
if (!_enabled) {
return;
}
hal.gpio->write(EPM_PIN_1, 0);
hal.gpio->write(EPM_PIN_2, 1);
// capture time
_last_grab_or_release = hal.scheduler->millis();
// move the servo to the release position
RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, EPM_RELEASE_PWM);
}
// neutral - return the epm pwm output to the neutral position
void AP_EPM::neutral()
{
// return immediately if not enabled
if (!_enabled || !EPM_SUPPORTED) {
if (!_enabled) {
return;
}
hal.gpio->write(EPM_PIN_1, 0);
hal.gpio->write(EPM_PIN_2, 0);
// move the servo to the off position
RC_Channel_aux::set_radio(RC_Channel_aux::k_epm, EPM_NEUTRAL_PWM);
}
// update - moves the pwm back to neutral after the timeout has passed
// should be called at at least 10hz
void AP_EPM::update()
{
// move EPM PWM output back to neutral 2 seconds after the last grab or release
if ((_last_grab_or_release != 0) && (hal.scheduler->millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS)) {
neutral();
}
}

53
libraries/AP_EPM/AP_EPM.h
View File

@ -13,31 +13,17 @@
#ifndef __AP_EPM_h__
#define __AP_EPM_h__
#include <inttypes.h>
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <RC_Channel.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#define EPM_PIN_1 61 // On pin - AN7
#define EPM_PIN_2 62 // Off pin - AN8
#define EPM_SUPPORTED true
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1
#define EPM_PIN_1 -1 // to be determine
#define EPM_PIN_2 -1 // to be determine
#define EPM_SUPPORTED false
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
#define EPM_PIN_1 -1 // to be determine
#define EPM_PIN_2 -1 // to be determine
#define EPM_SUPPORTED false
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#define EPM_PIN_1 -1 // to be determine
#define EPM_PIN_2 -1 // to be determine
#define EPM_SUPPORTED false
#else
#define EPM_PIN_1 -1 // not supported
#define EPM_PIN_2 -1 // not supported
#define EPM_SUPPORTED false
#endif
// EPM PWM definitions
#define EPM_GRAB_PWM 1900
#define EPM_RELEASE_PWM 1100
#define EPM_NEUTRAL_PWM 1500
// EPM PWM returns to neutral position this many milliseconds after grab or release
#define EPM_RETURN_TO_NEUTRAL_MS 500
/// @class AP_EPM
/// @brief Class to manage the EPM_CargoGripper
@ -45,26 +31,33 @@ class AP_EPM {
public:
AP_EPM();
// setup the epm pins
// setup the epm
void init();
// enabled - true if the epm is enabled
bool enabled();
bool enabled() { return _enabled; }
// activate the EPM
void on();
// grab - move the epm pwm output to the grab position
void grab();
// de-activate the EPM
void off();
// release - move the epm pwm output to the release position
void release();
// do nothing
// neutral - return the epm pwm output to the neutral position
void neutral();
// update - moves the pwm back to neutral after the timeout has passed
// should be called at at least 10hz
void update();
static const struct AP_Param::GroupInfo var_info[];
private:
// parameters
AP_Int8 _enabled;
// internal variables
uint32_t _last_grab_or_release;
};
#endif /* _AP_EPM_H_ */