00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ETHERCAT_HARDWARE__WG0X_H
00036 #define ETHERCAT_HARDWARE__WG0X_H
00037
00038 #include "ethercat_hardware/ethercat_device.h"
00039 #include "ethercat_hardware/motor_model.h"
00040 #include "ethercat_hardware/motor_heating_model.h"
00041 #include "realtime_tools/realtime_publisher.h"
00042 #include "ethercat_hardware/wg_mailbox.h"
00043 #include "ethercat_hardware/wg_eeprom.h"
00044
00045 #include <boost/shared_ptr.hpp>
00046
00047 using namespace ethercat_hardware;
00048
00049
00050 struct WG0XSafetyDisableStatus
00051 {
00052 uint8_t safety_disable_status_;
00053 uint8_t safety_disable_status_hold_;
00054 uint8_t safety_disable_count_;
00055 static const unsigned BASE_ADDR = 0xA1;
00056 } __attribute__ ((__packed__));
00057
00058
00059 struct WG0XSafetyDisableCounters
00060 {
00061 uint8_t undervoltage_count_;
00062 uint8_t over_current_count_;
00063 uint8_t board_over_temp_count_;
00064 uint8_t bridge_over_temp_count_;
00065 uint8_t operate_disable_count_;
00066 uint8_t watchdog_disable_count_;
00067 static const unsigned BASE_ADDR = 0x223;
00068 } __attribute__ ((__packed__));
00069
00070 struct WG0XDiagnosticsInfo
00071 {
00072 int16_t config_offset_current_A_;
00073 int16_t config_offset_current_B_;
00074 uint16_t supply_current_in_;
00075 union {
00076 uint16_t supply_current_out_;
00077 uint16_t voltage_ref_;
00078 } __attribute__ ((__packed__));
00079 int16_t offset_current_A_;
00080 int16_t offset_current_B_;
00081 int16_t adc_current_;
00082 uint8_t unused1[2];
00083 uint8_t lowside_deadtime_;
00084 uint8_t highside_deadtime_;
00085 uint8_t unused2[14];
00086 uint8_t pdo_command_irq_count_;
00087 uint8_t mbx_command_irq_count_;
00088 uint8_t unused3;
00089 WG0XSafetyDisableCounters safety_disable_counters_;
00090 uint8_t unused4;
00091 uint8_t pdi_timeout_error_count_;
00092 uint8_t pdi_checksum_error_count_;
00093 static const unsigned BASE_ADDR = 0x200;
00094 } __attribute__ ((__packed__));
00095
00096 struct WG0XConfigInfo
00097 {
00098 uint32_t product_id_;
00099 union
00100 {
00101 uint32_t revision_;
00102 struct
00103 {
00104 uint8_t firmware_minor_revision_;
00105 uint8_t firmware_major_revision_;
00106 uint8_t pca_revision_;
00107 uint8_t pcb_revision_;
00108 }__attribute__ ((__packed__));
00109 }__attribute__ ((__packed__));
00110 uint32_t device_serial_number_;
00111 uint8_t current_loop_kp_;
00112 uint8_t current_loop_ki_;
00113 uint16_t absolute_current_limit_;
00114 float nominal_current_scale_;
00115 float nominal_voltage_scale_;
00116 uint8_t pad_[8];
00117 uint8_t configuration_status_;
00118 uint8_t safety_disable_status_;
00119 uint8_t safety_disable_status_hold_;
00120 uint8_t safety_disable_count_;
00121 uint16_t watchdog_limit_;
00122
00123 static const unsigned CONFIG_INFO_BASE_ADDR = 0x0080;
00124 }__attribute__ ((__packed__));
00125
00126 struct WG0XUserConfigRam
00127 {
00128 uint8_t version_;
00129 uint8_t unused_[3];
00130 double zero_offset_;
00131 uint32_t crc32_;
00132
00133 static const unsigned BASE_ADDR = 0x00C0;
00134 }__attribute__ ((__packed__));
00135
00136 struct WG0XActuatorInfo
00137 {
00138 uint16_t major_;
00139 uint16_t minor_;
00140 uint32_t id_;
00141 char name_[64];
00142 char robot_name_[32];
00143 char motor_make_[32];
00144 char motor_model_[32];
00145 double max_current_;
00146 double speed_constant_;
00147 double resistance_;
00148 double motor_torque_constant_;
00149 double encoder_reduction_;
00150 uint32_t pulses_per_revolution_;
00151 uint8_t pad1[40];
00152 uint32_t crc32_256_;
00153 uint8_t pad2[4];
00154 uint32_t crc32_264_;
00155
00156 bool verifyCRC(void) const;
00157 void generateCRC(void);
00158 };
00159
00160 struct WG0XStatus
00161 {
00162 uint8_t mode_;
00163 uint8_t digital_out_;
00164 int16_t programmed_pwm_value_;
00165 int16_t programmed_current_;
00166 int16_t measured_current_;
00167 uint32_t timestamp_;
00168 int32_t encoder_count_;
00169 int32_t encoder_index_pos_;
00170 uint16_t num_encoder_errors_;
00171 uint8_t encoder_status_;
00172 uint8_t calibration_reading_;
00173 int32_t last_calibration_rising_edge_;
00174 int32_t last_calibration_falling_edge_;
00175 uint16_t board_temperature_;
00176 uint16_t bridge_temperature_;
00177 uint16_t supply_voltage_;
00178 int16_t motor_voltage_;
00179 uint16_t packet_count_;
00180 uint8_t pad_;
00181 uint8_t checksum_;
00182
00183 static const unsigned SIZE=44;
00184 }__attribute__ ((__packed__));
00185
00186
00187 struct WG0XCommand
00188 {
00189 uint8_t mode_;
00190 uint8_t digital_out_;
00191 int16_t programmed_pwm;
00192 int16_t programmed_current_;
00193 uint8_t pad_;
00194 uint8_t checksum_;
00195 }__attribute__ ((__packed__));
00196
00197 struct MbxDiagnostics
00198 {
00199 MbxDiagnostics();
00200 uint32_t write_errors_;
00201 uint32_t read_errors_;
00202 uint32_t lock_errors_;
00203 uint32_t retries_;
00204 uint32_t retry_errors_;
00205 };
00206
00207 struct WG0XDiagnostics
00208 {
00209 WG0XDiagnostics();
00210 void update(const WG0XSafetyDisableStatus &new_status, const WG0XDiagnosticsInfo &new_diagnostics_info);
00211
00212 bool first_;
00213 bool valid_;
00214 WG0XSafetyDisableStatus safety_disable_status_;
00215
00216 WG0XDiagnosticsInfo diagnostics_info_;
00217
00218 uint32_t safety_disable_total_;
00219 uint32_t undervoltage_total_;
00220 uint32_t over_current_total_;
00221 uint32_t board_over_temp_total_;
00222 uint32_t bridge_over_temp_total_;
00223 uint32_t operate_disable_total_;
00224 uint32_t watchdog_disable_total_;
00225
00226 uint32_t lock_errors_;
00227 uint32_t checksum_errors_;
00228
00229
00230 double zero_offset_;
00231 double cached_zero_offset_;
00232 };
00233
00234 class WG0X : public EthercatDevice
00235 {
00236 public:
00237 void construct(EtherCAT_SlaveHandler *sh, int &start_address);
00238 WG0X();
00239 virtual ~WG0X();
00240
00241 virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
00242
00243 void packCommand(unsigned char *buffer, bool halt, bool reset);
00244 bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
00245
00246 bool program(EthercatCom *com, const WG0XActuatorInfo &actutor_info);
00247 bool program(EthercatCom *com, const MotorHeatingModelParametersEepromConfig &heating_config);
00248
00249 bool readActuatorInfoFromEeprom(EthercatCom *com, WG0XActuatorInfo &actuator_info);
00250 bool readMotorHeatingModelParametersFromEeprom(EthercatCom *com, MotorHeatingModelParametersEepromConfig &config);
00251
00252 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
00253 virtual void collectDiagnostics(EthercatCom *com);
00254
00255 bool publishTrace(const string &reason, unsigned level, unsigned delay);
00256
00257 protected:
00258 uint8_t fw_major_;
00259 uint8_t fw_minor_;
00260 uint8_t board_major_;
00261 uint8_t board_minor_;
00262
00263 WG0XActuatorInfo actuator_info_;
00264 WG0XConfigInfo config_info_;
00265 double max_current_;
00266
00267 ethercat_hardware::ActuatorInfo actuator_info_msg_;
00268 static void copyActuatorInfo(ethercat_hardware::ActuatorInfo &out, const WG0XActuatorInfo &in);
00269
00270 pr2_hardware_interface::Actuator actuator_;
00271 pr2_hardware_interface::DigitalOut digital_out_;
00272
00273 enum
00274 {
00275 MODE_OFF = 0x00,
00276 MODE_ENABLE = (1 << 0),
00277 MODE_CURRENT = (1 << 1),
00278 MODE_SAFETY_RESET = (1 << 4),
00279 MODE_SAFETY_LOCKOUT = (1 << 5),
00280 MODE_UNDERVOLTAGE = (1 << 6),
00281 MODE_RESET = (1 << 7)
00282 };
00283
00284 enum
00285 {
00286 WG05_PRODUCT_CODE = 6805005,
00287 WG06_PRODUCT_CODE = 6805006,
00288 WG021_PRODUCT_CODE = 6805021
00289 };
00290
00291 static string modeString(uint8_t mode);
00292 static string safetyDisableString(uint8_t status);
00293 bool in_lockout_;
00294 bool resetting_;
00295 bool has_error_;
00296 uint16_t max_bridge_temperature_, max_board_temperature_;
00297 bool too_many_dropped_packets_;
00298 bool status_checksum_error_;
00299 bool timestamp_jump_detected_;
00300 bool fpga_internal_reset_detected_;
00301 bool encoder_errors_detected_;
00302
00303 void clearErrorFlags(void);
00304
00305 double cached_zero_offset_;
00306 enum {NO_CALIBRATION=0, CONTROLLER_CALIBRATION=1, SAVED_CALIBRATION=2};
00307 int calibration_status_;
00308
00314 ros::Duration sample_timestamp_;
00315
00317
00318
00319
00320
00321
00322
00323 enum AppRamStatus { APP_RAM_PRESENT=1, APP_RAM_MISSING=2, APP_RAM_NOT_APPLICABLE=3 };
00324 AppRamStatus app_ram_status_;
00325 bool readAppRam(EthercatCom *com, double &zero_offset);
00326 bool writeAppRam(EthercatCom *com, double zero_offset);
00327
00328 bool verifyState(WG0XStatus *this_status, WG0XStatus *prev_status);
00329
00330 int writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length);
00331 int readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length);
00332
00333 void publishGeneralDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
00334 void publishMailboxDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
00335
00336 bool initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw,
00337 const string &device_description,
00338 double max_pwm_ratio,
00339 double board_resistance,
00340 bool poor_measured_motor_voltage);
00341
00342 bool initializeMotorHeatingModel(bool allow_unprogrammed);
00343
00344 bool verifyChecksum(const void* buffer, unsigned size);
00345 static bool timestamp_jump(uint32_t timestamp, uint32_t last_timestamp, uint32_t amount);
00346
00347 static const int PWM_MAX = 0x4000;
00348
00349 protected:
00351 ethercat_hardware::WGMailbox mailbox_;
00352
00354 ethercat_hardware::WGEeprom eeprom_;
00355
00356 static const unsigned COMMAND_PHY_ADDR = 0x1000;
00357 static const unsigned STATUS_PHY_ADDR = 0x2000;
00358
00359 static const unsigned PDO_COMMAND_SYNCMAN_NUM = 0;
00360 static const unsigned PDO_STATUS_SYNCMAN_NUM = 1;
00361
00362 enum
00363 {
00364 LIMIT_SENSOR_0_STATE = (1 << 0),
00365 LIMIT_SENSOR_1_STATE = (1 << 1),
00366 LIMIT_ON_TO_OFF = (1 << 2),
00367 LIMIT_OFF_TO_ON = (1 << 3)
00368 };
00369
00370 enum
00371 {
00372 SAFETY_DISABLED = (1 << 0),
00373 SAFETY_UNDERVOLTAGE = (1 << 1),
00374 SAFETY_OVER_CURRENT = (1 << 2),
00375 SAFETY_BOARD_OVER_TEMP = (1 << 3),
00376 SAFETY_HBRIDGE_OVER_TEMP = (1 << 4),
00377 SAFETY_OPERATIONAL = (1 << 5),
00378 SAFETY_WATCHDOG = (1 << 6)
00379 };
00380
00381
00382
00383 static const unsigned ACTUATOR_INFO_PAGE = 4095;
00384
00385
00386 MotorModel *motor_model_;
00387 bool disable_motor_model_checking_;
00388 ethercat_hardware::MotorTraceSample motor_trace_sample_;
00389 pr2_hardware_interface::DigitalOut publish_motor_trace_;
00390
00391
00392 static boost::shared_ptr<ethercat_hardware::MotorHeatingModelCommon> motor_heating_model_common_;
00393 boost::shared_ptr<ethercat_hardware::MotorHeatingModel> motor_heating_model_;
00394
00395
00396 uint32_t last_timestamp_;
00397 uint32_t last_last_timestamp_;
00398 int drops_;
00399 int consecutive_drops_;
00400 int max_consecutive_drops_;
00401
00402 bool lockWG0XDiagnostics();
00403 bool tryLockWG0XDiagnostics();
00404 void unlockWG0XDiagnostics();
00405 pthread_mutex_t wg0x_diagnostics_lock_;
00406 WG0XDiagnostics wg0x_publish_diagnostics_;
00407 WG0XDiagnostics wg0x_collect_diagnostics_;
00408
00409 public:
00410 static int32_t timestampDiff(uint32_t new_timestamp, uint32_t old_timestamp);
00411 static int32_t positionDiff(int32_t new_position, int32_t old_position);
00412
00413 static ros::Duration timediffToDuration(int32_t timediff_usec);
00414
00415 static double calcEncoderVelocity(int32_t new_position, uint32_t new_timestamp,
00416 int32_t old_position, uint32_t old_timestamp);
00417
00418 static double convertRawTemperature(int16_t raw_temp);
00419 };
00420
00421
00422 #endif // ETHERCAT_HARDWARE__WG0X_H