#include <wg0x.h>
Public Member Functions | |
virtual void | collectDiagnostics (EthercatCom *com) |
void | construct (EtherCAT_SlaveHandler *sh, int &start_address) |
void | diagnostics (diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *) |
For EtherCAT device that only publish one EtherCAT Status message. If sub-class implements multiDiagnostics() then diagnostics() is not used. | |
virtual int | initialize (pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true) |
void | packCommand (unsigned char *buffer, bool halt, bool reset) |
void | program (WG0XActuatorInfo *) |
bool | publishTrace (const string &reason, unsigned level, unsigned delay) |
Asks device to publish (motor) trace. Only works for devices that support it. | |
bool | unpackState (unsigned char *this_buffer, unsigned char *prev_buffer) |
WG0X () | |
virtual | ~WG0X () |
Static Public Member Functions | |
static double | calcEncoderVelocity (int32_t new_position, uint32_t new_timestamp, int32_t old_position, uint32_t old_timestamp) |
static int32_t | positionDiff (int32_t new_position, int32_t old_position) |
static ros::Duration | timediffToDuration (int32_t timediff_usec) |
static int32_t | timestampDiff (uint32_t new_timestamp, uint32_t old_timestamp) |
Protected Types | |
enum | { MODE_OFF = 0x00, MODE_ENABLE = (1 << 0), MODE_CURRENT = (1 << 1), MODE_SAFETY_RESET = (1 << 4), MODE_SAFETY_LOCKOUT = (1 << 5), MODE_UNDERVOLTAGE = (1 << 6), MODE_RESET = (1 << 7) } |
enum | { NO_CALIBRATION = 0, CONTROLLER_CALIBRATION = 1, SAVED_CALIBRATION = 2 } |
enum | AppRamStatus { APP_RAM_PRESENT = 1, APP_RAM_MISSING = 2, APP_RAM_NOT_APPLICABLE = 3 } |
Different possible states for application ram on device. More... | |
Protected Member Functions | |
void | clearErrorFlags (void) |
bool | initializeMotorModel (pr2_hardware_interface::HardwareInterface *hw, const string &device_description, double max_pwm_ratio, double board_resistance, bool poor_measured_motor_voltage) |
Allocates and initialized motor trace for WG0X devices than use it (WG006, WG005). | |
void | publishGeneralDiagnostics (diagnostic_updater::DiagnosticStatusWrapper &d) |
void | publishMailboxDiagnostics (diagnostic_updater::DiagnosticStatusWrapper &d) |
bool | readAppRam (EthercatCom *com, double &zero_offset) |
int | readEeprom (EthercatCom *com) |
int | readMailbox (EthercatCom *com, unsigned address, void *data, unsigned length) |
Read data from WG0X local bus using mailbox communication. | |
int | sendSpiCommand (EthercatCom *com, WG0XSpiEepromCmd const *cmd) |
bool | verifyChecksum (const void *buffer, unsigned size) |
bool | verifyState (WG0XStatus *this_status, WG0XStatus *prev_status) |
bool | writeAppRam (EthercatCom *com, double zero_offset) |
int | writeEeprom (EthercatCom *com) |
int | writeMailbox (EthercatCom *com, unsigned address, void const *data, unsigned length) |
Write data to WG0X local bus using mailbox communication. | |
Static Protected Member Functions | |
static string | modeString (uint8_t mode) |
static string | safetyDisableString (uint8_t status) |
static bool | timestamp_jump (uint32_t timestamp, uint32_t last_timestamp, uint32_t amount) |
Protected Attributes | |
pr2_hardware_interface::Actuator | actuator_ |
WG0XActuatorInfo | actuator_info_ |
AppRamStatus | app_ram_status_ |
uint8_t | board_major_ |
Printed circuit board revision (for this value 0=='A', 1=='B'). | |
uint8_t | board_minor_ |
Printed circuit assembly revision. | |
double | cached_zero_offset_ |
int | calibration_status_ |
WG0XConfigInfo | config_info_ |
pr2_hardware_interface::DigitalOut | digital_out_ |
bool | fpga_internal_reset_detected_ |
uint8_t | fw_major_ |
uint8_t | fw_minor_ |
bool | has_error_ |
bool | in_lockout_ |
unsigned | last_num_encoder_errors_ |
Number of encoder errors the last time motors were reset. | |
uint16_t | max_board_temperature_ |
uint16_t | max_bridge_temperature_ |
double | max_current_ |
min(board current limit, actuator current limit) | |
bool | resetting_ |
ros::Duration | sample_timestamp_ |
bool | status_checksum_error_ |
bool | timestamp_jump_detected_ |
bool | too_many_dropped_packets_ |
Static Protected Attributes | |
static const int | PWM_MAX = 0x4000 |
Private Types | |
enum | { LIMIT_SENSOR_0_STATE = (1 << 0), LIMIT_SENSOR_1_STATE = (1 << 1), LIMIT_ON_TO_OFF = (1 << 2), LIMIT_OFF_TO_ON = (1 << 3) } |
enum | { SAFETY_DISABLED = (1 << 0), SAFETY_UNDERVOLTAGE = (1 << 1), SAFETY_OVER_CURRENT = (1 << 2), SAFETY_BOARD_OVER_TEMP = (1 << 3), SAFETY_HBRIDGE_OVER_TEMP = (1 << 4), SAFETY_OPERATIONAL = (1 << 5), SAFETY_WATCHDOG = (1 << 6) } |
Private Member Functions | |
bool | _readMailboxRepeatRequest (EthercatCom *com) |
bool | clearReadMailbox (EthercatCom *com) |
Clears read mailbox by reading first and last byte. | |
void | diagnoseMailboxError (EthercatCom *com) |
Runs diagnostic on read and write mailboxes. | |
bool | lockMailbox () |
bool | lockWG0XDiagnostics () |
int | readMailbox_ (EthercatCom *com, unsigned address, void *data, unsigned length) |
Internal function. | |
bool | readMailboxInternal (EthercatCom *com, void *data, unsigned length) |
Reads data from read mailbox. | |
bool | readMailboxRepeatRequest (EthercatCom *com) |
bool | tryLockWG0XDiagnostics () |
void | unlockMailbox () |
void | unlockWG0XDiagnostics () |
bool | verifyDeviceStateForMailboxOperation () |
bool | waitForReadMailboxReady (EthercatCom *com) |
Waits until read mailbox is full or timeout. | |
bool | waitForWriteMailboxReady (EthercatCom *com) |
Waits until write mailbox is empty or timeout. | |
int | writeMailbox_ (EthercatCom *com, unsigned address, void const *data, unsigned length) |
Internal function. | |
bool | writeMailboxInternal (EthercatCom *com, void const *data, unsigned length) |
Writes data to mailbox. | |
Private Attributes | |
int | consecutive_drops_ |
bool | disable_motor_model_checking_ |
int | drops_ |
uint32_t | last_last_timestamp_ |
uint32_t | last_timestamp_ |
MbxDiagnostics | mailbox_diagnostics_ |
pthread_mutex_t | mailbox_lock_ |
MbxDiagnostics | mailbox_publish_diagnostics_ |
int | max_consecutive_drops_ |
MotorModel * | motor_model_ |
ethercat_hardware::MotorTraceSample | motor_trace_sample_ |
pr2_hardware_interface::DigitalOut | publish_motor_trace_ |
WG0XDiagnostics | wg0x_collect_diagnostics_ |
pthread_mutex_t | wg0x_diagnostics_lock_ |
WG0XDiagnostics | wg0x_publish_diagnostics_ |
Static Private Attributes | |
static const int | ACTUATOR_INFO_PAGE = 4095 |
static const unsigned | COMMAND_PHY_ADDR = 0x1000 |
static const unsigned | MBX_COMMAND_PHY_ADDR = 0x1400 |
static const unsigned | MBX_COMMAND_SIZE = 512 |
static const unsigned | MBX_COMMAND_SYNCMAN_NUM = 2 |
static const unsigned | MBX_STATUS_PHY_ADDR = 0x2400 |
static const unsigned | MBX_STATUS_SIZE = 512 |
static const unsigned | MBX_STATUS_SYNCMAN_NUM = 3 |
static const unsigned | PDO_COMMAND_SYNCMAN_NUM = 0 |
static const unsigned | PDO_STATUS_SYNCMAN_NUM = 1 |
static const unsigned | PRESSURE_PHY_ADDR = 0x2200 |
static const unsigned | STATUS_PHY_ADDR = 0x2000 |
Definition at line 461 of file wg0x.h.
anonymous enum [protected] |
anonymous enum [protected] |
anonymous enum [private] |
anonymous enum [private] |
enum WG0X::AppRamStatus [protected] |
bool WG0X::_readMailboxRepeatRequest | ( | EthercatCom * | com | ) | [private] |
double WG0X::calcEncoderVelocity | ( | int32_t | new_position, | |
uint32_t | new_timestamp, | |||
int32_t | old_position, | |||
uint32_t | old_timestamp | |||
) | [static] |
bool WG0X::clearReadMailbox | ( | EthercatCom * | com | ) | [private] |
void WG0X::collectDiagnostics | ( | EthercatCom * | com | ) | [virtual] |
Reimplemented from EthercatDevice.
void WG0X::construct | ( | EtherCAT_SlaveHandler * | sh, | |
int & | start_address | |||
) | [virtual] |
void WG0X::diagnoseMailboxError | ( | EthercatCom * | com | ) | [private] |
void WG0X::diagnostics | ( | diagnostic_updater::DiagnosticStatusWrapper & | d, | |
unsigned char * | buffer | |||
) | [virtual] |
For EtherCAT device that only publish one EtherCAT Status message. If sub-class implements multiDiagnostics() then diagnostics() is not used.
d | Diagnostics status wrapper. | |
buffer | Pointer to slave process data. |
Reimplemented from EthercatDevice.
int WG0X::initialize | ( | pr2_hardware_interface::HardwareInterface * | hw, | |
bool | allow_unprogrammed = true | |||
) | [virtual] |
bool WG0X::initializeMotorModel | ( | pr2_hardware_interface::HardwareInterface * | hw, | |
const string & | device_description, | |||
double | max_pwm_ratio, | |||
double | board_resistance, | |||
bool | poor_measured_motor_voltage | |||
) | [protected] |
string WG0X::modeString | ( | uint8_t | mode | ) | [static, protected] |
void WG0X::packCommand | ( | unsigned char * | buffer, | |
bool | halt, | |||
bool | reset | |||
) | [virtual] |
int32_t WG0X::positionDiff | ( | int32_t | new_position, | |
int32_t | old_position | |||
) | [static] |
void WG0X::program | ( | WG0XActuatorInfo * | info | ) |
void WG0X::publishGeneralDiagnostics | ( | diagnostic_updater::DiagnosticStatusWrapper & | d | ) | [protected] |
void WG0X::publishMailboxDiagnostics | ( | diagnostic_updater::DiagnosticStatusWrapper & | d | ) | [protected] |
bool WG0X::publishTrace | ( | const string & | reason, | |
unsigned | level, | |||
unsigned | delay | |||
) | [virtual] |
Asks device to publish (motor) trace. Only works for devices that support it.
reason | Message to put in trace as reason. | |
level | Level to put in trace (aka ERROR=2, WARN=1, OK=0) | |
delay | Publish trace after delay cyles. For 1kHz realtime loop 1cycle = 1ms. |
Reimplemented from EthercatDevice.
bool WG0X::readAppRam | ( | EthercatCom * | com, | |
double & | zero_offset | |||
) | [protected] |
int WG0X::readEeprom | ( | EthercatCom * | com | ) | [protected] |
int WG0X::readMailbox | ( | EthercatCom * | com, | |
unsigned | address, | |||
void * | data, | |||
unsigned | length | |||
) | [protected] |
Read data from WG0X local bus using mailbox communication.
Internally a localbus read is done in two parts. First, a mailbox write of a command header that include local bus address and length. Second, a mailbox read of the result.
com | used to perform communication with device | |
address | WG0X (FPGA) local bus address to read from | |
data | pointer to buffer where read data can be stored, must be at least length in size | |
length | amount of data to read, limited at 511 bytes. |
int WG0X::readMailbox_ | ( | EthercatCom * | com, | |
unsigned | address, | |||
void * | data, | |||
unsigned | length | |||
) | [private] |
Internal function.
Aguments are the same as readMailbox() except that this assumes the mailbox lock is held.
bool WG0X::readMailboxInternal | ( | EthercatCom * | com, | |
void * | data, | |||
unsigned | length | |||
) | [private] |
Reads data from read mailbox.
Will try to conserve bandwidth by reading length bytes of data and last byte of mailbox. Mailbox lock should be held when this function is called.
com | used to perform communication with device | |
data | pointer to buffer where read data is stored. | |
length | amount of data to read from mailbox |
bool WG0X::readMailboxRepeatRequest | ( | EthercatCom * | com | ) | [private] |
string WG0X::safetyDisableString | ( | uint8_t | status | ) | [static, protected] |
int WG0X::sendSpiCommand | ( | EthercatCom * | com, | |
WG0XSpiEepromCmd const * | cmd | |||
) | [protected] |
ros::Duration WG0X::timediffToDuration | ( | int32_t | timediff_usec | ) | [static] |
Convert timestamp difference to ros::Duration. Timestamp is assumed to be in microseconds
It is assumed that each timestamps is exactly 32bit and can wrap around from 0xFFFFFFFF back to 0. In this case 1 - 4294967295 should equal 2 not -4294967294. (Note : 0xFFFFFFFF = 4294967295)
bool WG0X::timestamp_jump | ( | uint32_t | timestamp, | |
uint32_t | last_timestamp, | |||
uint32_t | amount | |||
) | [static, protected] |
int32_t WG0X::timestampDiff | ( | uint32_t | new_timestamp, | |
uint32_t | old_timestamp | |||
) | [static] |
Returns (new_timestamp - old_timestamp). Accounts for wrapping of timestamp values.
It is assumed that each timestamps is exactly 32bit and can wrap around from 0xFFFFFFFF back to 0. In this case 1 - 4294967295 should equal 2 not -4294967294. (Note : 0xFFFFFFFF = 4294967295)
bool WG0X::unpackState | ( | unsigned char * | this_buffer, | |
unsigned char * | prev_buffer | |||
) | [virtual] |
bool WG0X::verifyChecksum | ( | const void * | buffer, | |
unsigned | size | |||
) | [protected] |
bool WG0X::verifyDeviceStateForMailboxOperation | ( | ) | [private] |
bool WG0X::verifyState | ( | WG0XStatus * | this_status, | |
WG0XStatus * | prev_status | |||
) | [protected] |
bool WG0X::waitForReadMailboxReady | ( | EthercatCom * | com | ) | [private] |
bool WG0X::waitForWriteMailboxReady | ( | EthercatCom * | com | ) | [private] |
bool WG0X::writeAppRam | ( | EthercatCom * | com, | |
double | zero_offset | |||
) | [protected] |
int WG0X::writeEeprom | ( | EthercatCom * | com | ) | [protected] |
int WG0X::writeMailbox | ( | EthercatCom * | com, | |
unsigned | address, | |||
void const * | data, | |||
unsigned | length | |||
) | [protected] |
Write data to WG0X local bus using mailbox communication.
First, this puts a command header that include local bus address and length in write mailbox. Second it waits until device actually empties write mailbox.
com | used to perform communication with device | |
address | WG0X (FPGA) local bus address to write data to | |
data | pointer to buffer where write data is stored, must be at least length in size | |
length | amount of data to write, limited at 507 bytes |
int WG0X::writeMailbox_ | ( | EthercatCom * | com, | |
unsigned | address, | |||
void const * | data, | |||
unsigned | length | |||
) | [private] |
Internal function.
Aguments are the same as writeMailbox() except that this assumes the mailbox lock is held.
bool WG0X::writeMailboxInternal | ( | EthercatCom * | com, | |
void const * | data, | |||
unsigned | length | |||
) | [private] |
Writes data to mailbox.
Will try to conserve bandwidth by only length bytes of data and last byte of mailbox. Mailbox lock should be held when this function is called.
com | used to perform communication with device | |
data | pointer to buffer where read data is stored. | |
length | amount of data to read from mailbox |
pr2_hardware_interface::Actuator WG0X::actuator_ [protected] |
WG0XActuatorInfo WG0X::actuator_info_ [protected] |
const int WG0X::ACTUATOR_INFO_PAGE = 4095 [static, private] |
AppRamStatus WG0X::app_ram_status_ [protected] |
uint8_t WG0X::board_major_ [protected] |
uint8_t WG0X::board_minor_ [protected] |
double WG0X::cached_zero_offset_ [protected] |
int WG0X::calibration_status_ [protected] |
const unsigned WG0X::COMMAND_PHY_ADDR = 0x1000 [static, private] |
WG0XConfigInfo WG0X::config_info_ [protected] |
int WG0X::consecutive_drops_ [private] |
pr2_hardware_interface::DigitalOut WG0X::digital_out_ [protected] |
bool WG0X::disable_motor_model_checking_ [private] |
int WG0X::drops_ [private] |
bool WG0X::fpga_internal_reset_detected_ [protected] |
uint8_t WG0X::fw_major_ [protected] |
uint8_t WG0X::fw_minor_ [protected] |
bool WG0X::has_error_ [protected] |
bool WG0X::in_lockout_ [protected] |
uint32_t WG0X::last_last_timestamp_ [private] |
unsigned WG0X::last_num_encoder_errors_ [protected] |
uint32_t WG0X::last_timestamp_ [private] |
MbxDiagnostics WG0X::mailbox_diagnostics_ [private] |
pthread_mutex_t WG0X::mailbox_lock_ [private] |
uint16_t WG0X::max_board_temperature_ [protected] |
uint16_t WG0X::max_bridge_temperature_ [protected] |
int WG0X::max_consecutive_drops_ [private] |
double WG0X::max_current_ [protected] |
const unsigned WG0X::MBX_COMMAND_PHY_ADDR = 0x1400 [static, private] |
const unsigned WG0X::MBX_COMMAND_SIZE = 512 [static, private] |
const unsigned WG0X::MBX_COMMAND_SYNCMAN_NUM = 2 [static, private] |
const unsigned WG0X::MBX_STATUS_PHY_ADDR = 0x2400 [static, private] |
const unsigned WG0X::MBX_STATUS_SIZE = 512 [static, private] |
const unsigned WG0X::MBX_STATUS_SYNCMAN_NUM = 3 [static, private] |
MotorModel* WG0X::motor_model_ [private] |
const unsigned WG0X::PDO_COMMAND_SYNCMAN_NUM = 0 [static, private] |
const unsigned WG0X::PDO_STATUS_SYNCMAN_NUM = 1 [static, private] |
const unsigned WG0X::PRESSURE_PHY_ADDR = 0x2200 [static, private] |
pr2_hardware_interface::DigitalOut WG0X::publish_motor_trace_ [private] |
const int WG0X::PWM_MAX = 0x4000 [static, protected] |
bool WG0X::resetting_ [protected] |
ros::Duration WG0X::sample_timestamp_ [protected] |
The ros::Duration timestamp measures the time since the ethercat process started. It is generated by accumulating diffs between 32bit device timestamps. Device timestamps are assumed to be in microseconds, so 32bit timestamp will overflow every 72 minutes, ros::Duration (int32_t secs, int32_t nsecs) should overflow will overflow after 68 years
bool WG0X::status_checksum_error_ [protected] |
const unsigned WG0X::STATUS_PHY_ADDR = 0x2000 [static, private] |
bool WG0X::timestamp_jump_detected_ [protected] |
bool WG0X::too_many_dropped_packets_ [protected] |
pthread_mutex_t WG0X::wg0x_diagnostics_lock_ [private] |