00001 """--------------------------------------------------------------------
00002 COPYRIGHT 2013 SEGWAY Inc.
00003
00004 Software License Agreement:
00005
00006 The software supplied herewith by Segway Inc. (the "Company") for its
00007 RMP Robotic Platforms is intended and supplied to you, the Company's
00008 customer, for use solely and exclusively with Segway products. The
00009 software is owned by the Company and/or its supplier, and is protected
00010 under applicable copyright laws. All rights are reserved. Any use in
00011 violation of the foregoing restrictions may subject the user to criminal
00012 sanctions under applicable laws, as well as to civil liability for the
00013 breach of the terms and conditions of this license. The Company may
00014 immediately terminate this Agreement upon your use of the software with
00015 any products that are not Segway products.
00016
00017 The software was written using Python programming language. Your use
00018 of the software is therefore subject to the terms and conditions of the
00019 OSI- approved open source license viewable at http://www.python.org/.
00020 You are solely responsible for ensuring your compliance with the Python
00021 open source license.
00022
00023 You shall indemnify, defend and hold the Company harmless from any claims,
00024 demands, liabilities or expenses, including reasonable attorneys fees, incurred
00025 by the Company as a result of any claim or proceeding against the Company
00026 arising out of or based upon:
00027
00028 (i) The combination, operation or use of the software by you with any hardware,
00029 products, programs or data not supplied or approved in writing by the Company,
00030 if such claim or proceeding would have been avoided but for such combination,
00031 operation or use.
00032
00033 (ii) The modification of the software by or on behalf of you
00034
00035 (iii) Your use of the software.
00036
00037 THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES,
00038 WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED
00039 TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00040 PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT,
00041 IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR
00042 CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
00043
00044 \file system_defines.py
00045
00046 \brief This module defines the interface for the RMP
00047
00048 \Platform: Cross Platform
00049 --------------------------------------------------------------------"""
00050
00051 """
00052 This is the maximum update rate 100Hz
00053 """
00054 MIN_UPDATE_PERIOD_SEC = 0.01
00055
00056 """
00057 Flags for RMPEventHandlers class these are used to signal between threads
00058 """
00059 RMP_KILL = 1
00060 RMP_INIT_FAILED = 2
00061 RMP_IS_DEAD = 3
00062 RMP_TX_RDY = 4
00063 RMP_RSP_DATA_RDY = 5
00064 RMP_GOTO_STANDBY = 6
00065 RMP_GOTO_TRACTOR = 7
00066
00067 """------------------------------------------------------------------------
00068 RMP Feedback dictionaries:
00069 There are three 32-bit user feedback bitmaps that define the content of the
00070 RMP response message. Each bit in a bitmap corresponds to a feedback variable.
00071 If the bit is set in the configurable parameter the item is included in the
00072 response packet at the index defined by its order in the bitmaps. This means
00073 that if the first bit of feedback bitmap one is set it will be the variable
00074 at the first index in the reponse if the last bit of feedback bitmap 3 is set
00075 it will be the variable at the last index (before the CRC) of the response.
00076
00077 Response packets are big endian and each variable consists of 32 bytes. The variable
00078 representation in human readable form is defined by the masks below.
00079 ------------------------------------------------------------------------"""
00080
00081 """
00082 Dictionary for user feedback bitmap 1 and associated masks for
00083 representing data
00084 """
00085 RMP_FORCED_FEEDBACK_1_MASK = 0x00001FFF
00086 RMP_FLOATING_POINT_FEEDBACK_1_MASK = 0xFF7FF900
00087 RMP_HEX_FEEDBACK_1_MASK = 0x008004FF
00088 RMP_IP_FEEDBACK_1_MASK = 0x00000000
00089
00090 feedback_1_bitmap_menu_items = dict({
00091 0x00000001:"fault_status_word_1",
00092 0x00000002:"fault_status_word_2",
00093 0x00000004:"fault_status_word_3",
00094 0x00000008:"fault_status_word_4",
00095 0x00000010:"mcu_0_fault_status",
00096 0x00000020:"mcu_1_fault_status",
00097 0x00000040:"mcu_2_fault_status",
00098 0x00000080:"mcu_3_fault_status",
00099 0x00000100:"operational_time",
00100 0x00000200:"operational_state",
00101 0x00000400:"dynamic_response",
00102 0x00000800:"min_propulsion_batt_soc",
00103 0x00001000:"aux_batt_soc",
00104 0x00002000:"inertial_x_acc_g",
00105 0x00004000:"inertial_y_acc_g",
00106 0x00008000:"inertial_x_rate_rps",
00107 0x00010000:"inertial_y_rate_rps",
00108 0x00020000:"inertial_z_rate_rps",
00109 0x00040000:"pse_pitch_deg",
00110 0x00080000:"pse_pitch_rate_dps",
00111 0x00100000:"pse_roll_deg",
00112 0x00200000:"pse_roll_rate_dps",
00113 0x00400000:"pse_yaw_rate_dps",
00114 0x00800000:"pse_data_is_valid",
00115 0x01000000:"yaw_rate_limit_rps",
00116 0x02000000:"vel_limit_mps",
00117 0x04000000:"linear_accel_mps2",
00118 0x08000000:"linear_vel_mps",
00119 0x10000000:"differential_wheel_vel_rps",
00120 0x20000000:"right_front_vel_mps",
00121 0x40000000:"left_front_vel_mps",
00122 0x80000000:"right_rear_vel_mps"})
00123
00124 """
00125 Dictionary for user feedback bitmap 2 and associated masks for
00126 representing data
00127 """
00128 RMP_FORCED_FEEDBACK_2_MASK = 0x00000000
00129 RMP_FLOATING_POINT_FEEDBACK_2_MASK = 0x3FFFFFFF
00130 RMP_HEX_FEEDBACK_2_MASK = 0xC0000000
00131 RMP_IP_FEEDBACK_2_MASK = 0x00000000
00132
00133 feedback_2_bitmap_menu_items = dict({
00134 0x00000001:"left_rear_vel_mps",
00135 0x00000002:"right_front_pos_m",
00136 0x00000004:"left_front_pos_m",
00137 0x00000008:"right_rear_pos_m",
00138 0x00000010:"left_rear_pos_m",
00139 0x00000020:"linear_pos_m",
00140 0x00000040:"right_front_current_A0pk",
00141 0x00000080:"left_front_current_A0pk",
00142 0x00000100:"right_rear_current_A0pk",
00143 0x00000200:"left_rear_current_A0pk",
00144 0x00000400:"max_motor_current_A0pk",
00145 0x00000800:"right_front_current_limit_A0pk",
00146 0x00001000:"left_front_current_limit_A0pk",
00147 0x00002000:"right_rear_current_limit_A0pk",
00148 0x00004000:"left_rear_current_limit_A0pk",
00149 0x00008000:"min_motor_current_limit_A0pk",
00150 0x00010000:"front_base_batt_1_soc",
00151 0x00020000:"front_base_batt_2_soc",
00152 0x00040000:"rear_base_batt_1_soc",
00153 0x00080000:"rear_base_batt_2_soc",
00154 0x00100000:"front_base_batt_1_temp_degC",
00155 0x00200000:"front_base_batt_2_temp_degC",
00156 0x00400000:"rear_base_batt_1_temp_degC",
00157 0x00800000:"rear_base_batt_2_temp_degC",
00158 0x01000000:"vel_target_mps",
00159 0x02000000:"yaw_rate_target_rps",
00160 0x04000000:"angle_target_deg",
00161 0x08000000:"aux_batt_voltage_V",
00162 0x10000000:"aux_batt_current_A",
00163 0x20000000:"aux_batt_temp_degC",
00164 0x40000000:"abb_system_status",
00165 0x80000000:"aux_batt_status"})
00166
00167 """
00168 Dictionary for user feedback bitmap 3 and associated masks for
00169 representing data
00170 """
00171 RMP_FORCED_FEEDBACK_3_MASK = 0x00000000
00172 RMP_FLOATING_POINT_FEEDBACK_3_MASK = 0x007FFFF2
00173 RMP_HEX_FEEDBACK_3_MASK = 0xF0800001
00174 RMP_IP_FEEDBACK_3_MASK = 0x0D000000
00175
00176 feedback_3_bitmap_menu_items = dict({
00177 0x00000001:"aux_batt_faults",
00178 0x00000002:"7p2V_battery_voltage",
00179 0x00000004:"ccu_sp_sw_build_id",
00180 0x00000008:"ccu_uip_sw_build_id",
00181 0x00000010:"mcu_0_inst_power_W",
00182 0x00000020:"mcu_1_inst_power_W",
00183 0x00000040:"mcu_2_inst_power_W",
00184 0x00000080:"mcu_3_inst_power_W",
00185 0x00000100:"mcu_0_total_energy_Wh",
00186 0x00000200:"mcu_1_total_energy_Wh",
00187 0x00000400:"mcu_2_total_energy_Wh",
00188 0x00000800:"mcu_3_total_energy_Wh",
00189 0x00001000:"fram_vel_limit_mps",
00190 0x00002000:"fram_accel_limit_mps2",
00191 0x00004000:"fram_decel_limit_mps2",
00192 0x00008000:"fram_dtz_decel_limit_mps2",
00193 0x00010000:"fram_coastdown_decel_mps2",
00194 0x00020000:"fram_yaw_rate_limit_rps",
00195 0x00040000:"fram_yaw_accel_limit_rps2",
00196 0x00080000:"fram_tire_diameter_m",
00197 0x00100000:"fram_wheelbase_length_m",
00198 0x00200000:"fram_wheel_track_width_m",
00199 0x00400000:"fram_transmission_ratio",
00200 0x00800000:"fram_config_bitmap",
00201 0x01000000:"fram_eth_ip_address",
00202 0x02000000:"fram_eth_port_number",
00203 0x04000000:"fram_eth_subnet_mask",
00204 0x08000000:"fram_eth_gateway",
00205 0x10000000:"user_feedback_bitmap_1",
00206 0x20000000:"user_feedback_bitmap_2",
00207 0x40000000:"user_feedback_bitmap_3",
00208 0x80000000:"user_feedback_bitmap_4"})
00209
00210 """
00211 Dictionary for user feedback bitmap 4 and associated masks for
00212 representing data
00213 """
00214 RMP_FORCED_FEEDBACK_4_MASK = 0x00000000
00215 RMP_FLOATING_POINT_FEEDBACK_4_MASK = 0x00000000
00216 RMP_HEX_FEEDBACK_4_MASK = 0x00000000
00217 RMP_IP_FEEDBACK_4_MASK = 0x00000000
00218
00219 feedback_4_bitmap_menu_items = dict()
00220
00221
00222 """------------------------------------------------------------------------
00223 RMP Command structures
00224 There are two types of messages the RMP will accept:
00225 1. Motion command message: This message contains two 32-bit IEEE754 floating
00226 point variables. The first is the normalized velocity command (range: -1.0...1.0).
00227 The second is the normalized yaw rate command (range: -1.0...1.0).
00228
00229 2. Configuration message: This message will contain two 32-bit variables. The
00230 first is a 32-bit integer general purpose command which is the ID of the configuration
00231 command (see below). The second is the general purpose parameter which may be integer
00232 or 32-bit IEEE754 floating point value depending on the command
00233 ------------------------------------------------------------------------"""
00234
00235 """
00236 Defines for the structure of commands sent to the RMP via CAN each message
00237 consists of an ID and two 32-bit words. The CAN interface is 8-byte DLC.
00238 The CAN data bytes are big-endian (highest byte in lowest index).
00239 Can ID: 16-bit ID SID.
00240 Bytes 0-3: 32-bit variable 1 (MS byte at index 0 LS byte at index 3)
00241 Bytes 4-7: 32-bit variable 2 (MS byte at index 4 LS byte at index 7)
00242
00243 The format to rmp_interface.py is
00244 [RMP_MOTION_CMD_ID,norm_vel_cmd,norm_yaw_cmd]
00245 [RMP_OMNI_MOTION_CMD_ID,norm_vel_cmd,norm_yaw_cmd,angle_cmd_deg]
00246 [RMP_CFG_CMD_ID,gp_cmd,gp_param]
00247 """
00248 RMP_MOTION_CMD_ID = 0x0500
00249 RMP_OMNI_MOTION_CMD_ID = 0x0600
00250 RMP_CFG_CMD_ID = 0x0501
00251
00252 """
00253 Defines the start address for the CAN response
00254 """
00255 RMP_RESPONSE_START_CAN_ID = 0x502
00256
00257 """
00258 The packet structure for USB and Ethernet.
00259 Bytes 0-1: 16-bit ID (see above).
00260 Bytes 2-5: 32-bit variable 1 (content depends on message type)
00261 Bytes 6-9: 32-bit variable 2 (content depends on message type)
00262 Bytes 10-11: 16-bit CRC value of buffer contents
00263 """
00264 RMP_USB_ETH_CAN_ID_HIGH_INDEX = 0
00265 RMP_USB_ETH_CAN_ID_LOW_INDEX = 1
00266 RMP_USB_ETH_CAN_DATA_0_INDEX = 2
00267 RMP_USB_ETH_CAN_DATA_1_INDEX = 3
00268 RMP_USB_ETH_CAN_DATA_2_INDEX = 4
00269 RMP_USB_ETH_CAN_DATA_3_INDEX = 5
00270 RMP_USB_ETH_CAN_DATA_4_INDEX = 6
00271 RMP_USB_ETH_CAN_DATA_5_INDEX = 7
00272 RMP_USB_ETH_CAN_DATA_6_INDEX = 8
00273 RMP_USB_ETH_CAN_DATA_7_INDEX = 9
00274 RMP_USB_ETH_CHECKSUM_HIGH_BYTE_INDEX = 10
00275 RMP_USB_ETH_CHECKSUM_LOW_BYTE_INDEX = 11
00276
00277 """
00278 Defines the total number of bytes in a USB or Ethernet packet
00279 """
00280 NUM_USB_ETH_BYTES = 12
00281
00282 """------------------------------------------------------------------------
00283 Configuration commands
00284 There are a number of configurable parameters on the RMP. Some are stored in
00285 NV F-RAM memory onboard the machine. Some are runtime variables. The general
00286 purpose command ID and the associated general purpose parameters are defined
00287 below
00288 ------------------------------------------------------------------------"""
00289 config_param_dict = dict({
00290 0:"RMP_CMD_NONE",
00291 1:"RMP_CMD_SET_MAXIMUM_VELOCITY",
00292 2:"RMP_CMD_SET_MAXIMUM_ACCELERATION",
00293 3:"RMP_CMD_SET_MAXIMUM_DECELERATION",
00294 4:"RMP_CMD_SET_MAXIMUM_DTZ_DECEL_RATE",
00295 5:"RMP_CMD_SET_COASTDOWN_ACCEL",
00296 6:"RMP_CMD_SET_MAXIMUM_TURN_RATE",
00297 7:"RMP_CMD_SET_MAXIMUM_TURN_ACCEL",
00298 8:"RMP_CMD_SET_TIRE_DIAMETER",
00299 9:"RMP_CMD_SET_WHEEL_BASE_LENGTH",
00300 10:"RMP_CMD_SET_WHEEL_TRACK_WIDTH",
00301 11:"RMP_CMD_SET_TRANSMISSION_RATIO",
00302 12:"RMP_CMD_SET_INPUT_CONFIG_BITMAP",
00303 13:"RMP_CMD_SET_ETH_IP_ADDRESS",
00304 14:"RMP_CMD_SET_ETH_PORT_NUMBER",
00305 15:"RMP_CMD_SET_ETH_SUBNET_MASK",
00306 16:"RMP_CMD_SET_ETH_GATEWAY",
00307 17:"RMP_CMD_SET_USER_FB_1_BITMAP",
00308 18:"RMP_CMD_SET_USER_FB_2_BITMAP",
00309 19:"RMP_CMD_SET_USER_FB_3_BITMAP",
00310 20:"RMP_CMD_SET_USER_FB_4_BITMAP"})
00311 """
00312 This command results in no action but a response from the RMP. The general
00313 purpose parameter is ignored.
00314 """
00315 RMP_CMD_NONE = (0)
00316
00317
00318 """------------------------------------------------------------------------
00319 Start Variables Stored in NV F-RAM memory
00320 ------------------------------------------------------------------------"""
00321
00322 """
00323 This command updates the maximum velocity limit of the machine. The general
00324 purpose command is a 32-bit IEEE754 floating point variable representing the
00325 maximum velocity limit in m/s.
00326 """
00327 RMP_CMD_SET_MAXIMUM_VELOCITY = (1)
00328
00329 DEFAULT_MAXIMUM_VELOCITY_MPS = 2.2352
00330 MAX_VELOCITY_MPS = 8.047
00331 MIN_VELOCITY_MPS = 0.0
00332
00333 """
00334 This command updates the maximum acceleration limit of the machine. The general
00335 purpose command is a 32-bit IEEE754 floating point variable representing the
00336 maximum acceleration limit in m/s^2.
00337 """
00338 RMP_CMD_SET_MAXIMUM_ACCELERATION = (2)
00339
00340 DEFAULT_MAXIMUM_ACCELERATION_MPS2 = 3.923
00341 MAX_ACCELERATION_MPS2 = 7.848
00342 MIN_ACCELERATION_MPS2 = 0.0
00343
00344 """
00345 This command updates the maximum deceleration limit of the machine. The general
00346 purpose command is a 32-bit IEEE754 floating point variable representing the
00347 maximum deceleration limit in m/s^2.
00348 """
00349 RMP_CMD_SET_MAXIMUM_DECELERATION = (3)
00350
00351 DEFAULT_MAXIMUM_DECELERATION_MPS2 = 3.923
00352 MAX_DECELERATION_MPS2 = 7.848
00353 MIN_DECELERATION_MPS2 = 0.0
00354
00355 """
00356 This command updates the maximum decel to zero response deceleration limit.
00357 The general purpose command is a 32-bit IEEE754 floating point variable
00358 representing the maximum DTZ response deceleration limit in m/s^2.
00359 """
00360 RMP_CMD_SET_MAXIMUM_DTZ_DECEL_RATE = (4)
00361
00362 DEFAULT_MAXIMUM_DTZ_DECEL_RATE_MPS2 = 3.923
00363 MAX_DTZ_DECEL_RATE_MPS2 = 7.848
00364 MIN_DTZ_DECEL_RATE_MPS2 = 0.0
00365
00366 """
00367 This command updates the coastdown acceleration for acceleration based input
00368 mapping. The general purpose command is a 32-bit IEEE754 floating point variable
00369 representing coastdown acceleration in m/s^2.
00370 """
00371 RMP_CMD_SET_COASTDOWN_ACCEL = (5)
00372
00373 DEFAULT_COASTDOWN_ACCEL_MPS2 = 0.1962
00374 MAX_COASTDOWN_ACCEL_MPS2 = 1.961
00375 MIN_COASTDOWN_ACCEL_MPS2 = 0.0
00376
00377 """
00378 This command updates the maximum commandable yaw rate.
00379 The general purpose command is a 32-bit IEEE754 floating point variable
00380 representing the yaw rate limit in rad/s.
00381 """
00382 RMP_CMD_SET_MAXIMUM_TURN_RATE = (6)
00383
00384 DEFAULT_MAXIMUM_YAW_RATE_RPS = 3.0
00385 MAX_YAW_RATE_RPS = 4.5
00386 MIN_YAW_RATE_RPS = 0.0
00387
00388 """
00389 This command updates the maximum commandable yaw acceleration.
00390 The general purpose command is a 32-bit IEEE754 floating point variable
00391 representing yaw acceleration in rad/s^2.
00392 """
00393 RMP_CMD_SET_MAXIMUM_TURN_ACCEL = (7)
00394
00395 DEFAULT_MAX_YAW_ACCEL_RPS2 = 28.274
00396 MAX_YAW_ACCEL_RPS2 = 28.274
00397 MIN_YAW_ACCEL_RPS2 = 0.0
00398
00399 """
00400 This command updates the machine tire diameter used in software to calculate
00401 velocity, position, differential wheel speed (yaw rate) and accelerations.
00402 The general purpose command is a 32-bit IEEE754 floating point variable
00403 representing tire diameter in meters.
00404 """
00405 RMP_CMD_SET_TIRE_DIAMETER = (8)
00406
00407 I2_TIRE_DIAMETER_M = 0.46228
00408 X2_TIRE_DIAMETER_M = 0.483616
00409 OMNI_TIRE_DIAMETER_M = 0.254
00410 DEFAULT_TIRE_DIAMETER_M = X2_TIRE_DIAMETER_M
00411 MAX_TIRE_DIAMETER_M = 1.0
00412 MIN_TIRE_DIAMETER_M = 0.1524
00413
00414 """
00415 This command updates the machine wheel base length used in software to calculate
00416 velocity, position, differential wheel speed (yaw rate) and accelerations.
00417 The general purpose command is a 32-bit IEEE754 floating point variable
00418 representing wheel base length in meters
00419 """
00420 RMP_CMD_SET_WHEEL_BASE_LENGTH = (9)
00421 DEFAULT_WHEEL_BASE_LENGTH_M = 0.5842
00422 OMNI_WHEEL_BASE_WIDTH_M = 0.572
00423 FLEXOMNI_WHEEL_BASE_WIDTH_M = 0.693
00424 MAX_WHEEL_BASE_LENGTH_M = 1.0
00425 MIN_WHEEL_BASE_LENGTH_M = 0.4142
00426
00427 """
00428 This command updates the machine track width (lateral distance between the tires)
00429 used in software to calculate differential wheel speeds (yaw rate).
00430 The general purpose command is a 32-bit IEEE754 floating point variable
00431 representing track width in meters.
00432 """
00433 RMP_CMD_SET_WHEEL_TRACK_WIDTH = (10)
00434
00435 I2_WHEEL_TRACK_WIDTH_M = 0.569976
00436 X2_WHEEL_TRACK_WIDTH_M = 0.7112
00437 OMNI_WHEEL_TRACK_WIDTH_M = 0.693
00438 DEFAULT_WHEEL_TRACK_WIDTH_M = X2_WHEEL_TRACK_WIDTH_M
00439 MAX_WHEEL_TRACK_WIDTH_M = 1.0
00440 MIN_WHEEL_TRACK_WIDTH_M = 0.506476
00441
00442 """
00443 This command updates the machine transmission ratio (gearbox ratio)
00444 used in software to convert from motor speed to gearbox output speed
00445 The general purpose command is a 32-bit IEEE754 floating point variable
00446 representing transmission ratio (unitless).
00447 """
00448 RMP_CMD_SET_TRANSMISSION_RATIO = (11)
00449
00450 I2_TRANSMISSION_RATIO = 24.2667
00451 X2_TRANSMISSION_RATIO = 24.2667
00452 OMNI_TRANSMISSION_RATIO = 24.2667
00453 DEFAULT_TRANSMISSION_RATIO = X2_TRANSMISSION_RATIO
00454 MAX_TRANSMISSION_RATIO = 200.0
00455 MIN_TRANSMISSION_RATIO = 1.0
00456
00457 """
00458 This command updates the machine configuration bitmap. This bitmap sets
00459 the controller input mapping for vel/yaw controllers and whether the machine
00460 should use audio.
00461 The general purpose command is a 32-bit integer variable with bits representing
00462 each variable.
00463 """
00464 RMP_CMD_SET_INPUT_CONFIG_BITMAP = (12)
00465
00466 YAW_ALAT_SCALE_MAPPING = 0
00467 YAW_ALAT_LIMIT_MAPPING = 1
00468
00469 VELOCITY_BASED_MAPPING = 0
00470 ACCELERATION_BASED_MAPPING = 1
00471
00472 ALLOW_MACHINE_AUDIO = 0
00473 SILENCE_MACHINE_AUDIO = 1
00474
00475 DISABLE_AC_PRESENT_CSI = 1
00476 ENABLE_AC_PRESENT_CSI = 0
00477
00478 BALANCE_MODE_DISABLED = 1
00479 BALANCE_MODE_ENABLE = 0
00480
00481 BALANCE_GAINS_DEFAULT = (0x00000000)
00482 BALANCE_GAINS_LIGHT = (0x00000001)
00483 BALANCE_GAINS_TALL = (0x00000002)
00484 BALANCE_GAINS_HEAVY = (0x00000004)
00485 BALANCE_GAINS_CUSTOM = (0x00000008)
00486 VALID_BALANCE_GAINS_MASK = (0x0000000F)
00487
00488 YAW_INPUT_MAPPING_SHIFT = 0
00489 VEL_INPUT_MAPPING_SHIFT = 1
00490 AUDIO_SILENCE_REQUEST_SHIFT = 2
00491 DISABLE_AC_PRESENT_CSI_SHIFT = 3
00492 BALANCE_GAIN_SCHEDULE_SHIFT = 4
00493 BALANCE_MODE_LOCKOUT_SHIFT = 8
00494
00495
00496 DEFAULT_CONFIG_BITMAP = ((YAW_ALAT_LIMIT_MAPPING << YAW_INPUT_MAPPING_SHIFT) |
00497 (VELOCITY_BASED_MAPPING << VEL_INPUT_MAPPING_SHIFT) |
00498 (ALLOW_MACHINE_AUDIO << AUDIO_SILENCE_REQUEST_SHIFT)|
00499 (ENABLE_AC_PRESENT_CSI << DISABLE_AC_PRESENT_CSI_SHIFT)|
00500 (BALANCE_GAINS_DEFAULT << BALANCE_GAIN_SCHEDULE_SHIFT)|
00501 (BALANCE_MODE_DISABLED << BALANCE_MODE_LOCKOUT_SHIFT))
00502
00503 """
00504 This command updates the machine ethernet IP address.
00505 The general purpose command is a 32-bit integer variable representing a
00506 dotted quad.
00507 The conversion is:
00508 integer = (first octet * 16777216) + (second octet * 65536) + (third octet * 256) + (fourth octet)
00509 Bounds for this item are valid IP addresses
00510 """
00511 RMP_CMD_SET_ETH_IP_ADDRESS = (13)
00512
00513 DEFAULT_IP_ADDRESS = '192.168.0.40'
00514
00515 """
00516 This command updates the machine ethernet port number.
00517 The general purpose command is a 32-bit integer variable
00518 Bounds for this item are valid ethernet ports
00519 """
00520 RMP_CMD_SET_ETH_PORT_NUMBER = (14)
00521
00522 DEFAULT_PORT_NUMBER = 8080
00523
00524 """
00525 This command updates the machine ethernet IP subnet mask.
00526 The general purpose command is a 32-bit integer variable representing a
00527 dotted quad.
00528 The conversion is:
00529 integer = (first octet * 16777216) + (second octet * 65536) + (third octet * 256) + (fourth octet)
00530 Bounds for this item are valid IP subnet masks
00531 """
00532 RMP_CMD_SET_ETH_SUBNET_MASK = (15)
00533
00534 DEFAULT_SUBNET_MASK = '255.255.255.0'
00535
00536 """
00537 This command updates the machine ethernet IP gateway.
00538 The general purpose command is a 32-bit integer variable representing a
00539 dotted quad.
00540 The conversion is:
00541 integer = (first octet * 16777216) + (second octet * 65536) + (third octet * 256) + (fourth octet)
00542 Bounds for this item are valid IP gateway
00543 """
00544 RMP_CMD_SET_ETH_GATEWAY = (16)
00545
00546 DEFAULT_GATEWAY = '192.168.0.1'
00547
00548 """
00549 This command updates user feedback bitmap 1. It is used to
00550 select feedback from the dictionary at the top of the file.
00551 The general purpose parameter is a 32 bit integer
00552 """
00553 RMP_CMD_SET_USER_FB_1_BITMAP = (17)
00554 DEFAULT_USER_FB_1_BITMAP = 0xFFFFFFFF
00555 VALID_USER_FB_1_BITMAP_MASK = 0xFFFFFFFF
00556
00557 """
00558 This command updates user feedback bitmap 2. It is used to
00559 select feedback from the dictionary at the top of the file.
00560 The general purpose parameter is a 32 bit integer
00561 """
00562 RMP_CMD_SET_USER_FB_2_BITMAP = (18)
00563 DEFAULT_USER_FB_2_BITMAP = 0xFFFFFFFF
00564 VALID_USER_FB_2_BITMAP_MASK = 0xFFFFFFFF
00565
00566 """
00567 This command updates user feedback bitmap 3. It is used to
00568 select feedback from the dictionary at the top of the file.
00569 The general purpose parameter is a 32 bit integer
00570 """
00571 RMP_CMD_SET_USER_FB_3_BITMAP = (19)
00572 DEFAULT_USER_FB_3_BITMAP = 0xFFFFFFFF
00573 VALID_USER_FB_3_BITMAP_MASK = 0xFFFFFFFF
00574
00575 """
00576 This command updates user feedback bitmap 3. It is used to
00577 select feedback from the dictionary at the top of the file.
00578 The general purpose parameter is a 32 bit integer
00579 """
00580 RMP_CMD_SET_USER_FB_4_BITMAP = (20)
00581 DEFAULT_USER_FB_4_BITMAP = 0x00000000
00582 VALID_USER_FB_4_BITMAP_MASK = 0x00000000
00583
00584
00585 """
00586 When the RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS command is set this is the
00587 size of the response array. The index of the items matches the parameter number -1
00588 for the items defined above. +1 is for the CRC
00589 """
00590 NUMBER_OF_NVM_CONFIG_PARAMS = 20
00591 FORCED_CONFIG_FEEDBACK_ITEMS = NUMBER_OF_NVM_CONFIG_PARAMS + 1
00592
00593
00594 """------------------------------------------------------------------------
00595 End Variables Stored in NV F-RAM memory
00596 ------------------------------------------------------------------------"""
00597
00598 """
00599 This command forces the feedback to be the configurable items stored in NV memory above.
00600 It is used when verifying that paremeters have been successfully set or just general verification
00601 at startup. The general purpose parameter is 1 to force the feedback to contain configurable items
00602 and 0 to stop forcing the feedback.
00603 """
00604 RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS = (30)
00605
00606 """
00607 This command requests audio songs on the machine. The general purpose parameter
00608 is a 32 bit integer representing the song request ID. Some songs are presistant (ie they must be
00609 manually cleared by sending the NO_SONG after set). Most are momentary.
00610 """
00611 RMP_CMD_SET_AUDIO_COMMAND = (31)
00612
00613 MOTOR_AUDIO_PLAY_NO_SONG = (0)
00614 MOTOR_AUDIO_PLAY_POWER_ON_SONG = (1)
00615 MOTOR_AUDIO_PLAY_POWER_OFF_SONG = (2)
00616 MOTOR_AUDIO_PLAY_ALARM_SONG = (3)
00617 MOTOR_AUDIO_PLAY_MODE_UP_SONG = (4)
00618 MOTOR_AUDIO_PLAY_MODE_DOWN_SONG = (5)
00619 MOTOR_AUDIO_PLAY_ENTER_ALARM_SONG = (6)
00620 MOTOR_AUDIO_PLAY_EXIT_ALARM_SONG = (7)
00621 MOTOR_AUDIO_PLAY_FINAL_SHUTDOWN_SONG = (8)
00622 MOTOR_AUDIO_PLAY_CORRECT_ISSUE = (9)
00623 MOTOR_AUDIO_PLAY_ISSUE_CORRECTED = (10)
00624 MOTOR_AUDIO_PLAY_CORRECT_ISSUE_REPEATING = (11)
00625 MOTOR_AUDIO_PLAY_BEGINNER_ACK = (12)
00626 MOTOR_AUDIO_PLAY_EXPERT_ACK = (13)
00627 MOTOR_AUDIO_ENTER_FOLLOW = (14)
00628 MOTOR_AUDIO_TEST_SWEEP = (15)
00629 MOTOR_AUDIO_SIMULATE_MOTOR_NOISE = (16)
00630
00631 """
00632 This command updates the operational mode request on the machine. The general purpose parameter
00633 is a 32 bit integer representing the mode request ID.
00634 """
00635 RMP_CMD_SET_OPERATIONAL_MODE = (32)
00636
00637 DISABLE_REQUEST = 1
00638 POWERDOWN_REQUEST = 2
00639 DTZ_REQUEST = 3
00640 STANDBY_REQUEST = 4
00641 TRACTOR_REQUEST = 5
00642 BALANCE_REQUEST = 6
00643
00644 """
00645 This command requests the faultlog from the machine. The general purpose parameter
00646 is 1 indicating this is a new request, or 0 indicating it is a subsequent request.
00647 The entire faultlog requires 6 packets, the first request should have the general
00648 purpose command set to 1 the next 6 should have the general purpose command set to 0.
00649 """
00650 RMP_CMD_SEND_SP_FAULTLOG = (33)
00651
00652 FAULTLOG_INITIAL_REQUEST = 1
00653 FAULTLOG_NORMAL_REQUEST = 0
00654
00655 """
00656 Define the number of faultlog packets and the size (in 32-bit words) of each packet.
00657 """
00658 FAULTLOG_NUM_OF_PACKETS = 6
00659 FAULTLOG_PACKET_NUM_OF_WORDS = 53
00660
00661 """
00662 This command resets the position data on the machine. The general purpose command
00663 is a bitmap of which integrators are to be reset.
00664 """
00665 RMP_CMD_RESET_INTEGRATORS = (34)
00666
00667 RESET_LINEAR_POSITION = 0x00000001
00668 RESET_RIGHT_FRONT_POSITION = 0x00000002
00669 RESET_LEFT_FRONT_POSITION = 0x00000004
00670 RESET_RIGHT_REAR_POSITION = 0x00000008
00671 RESET_LEFT_REAR_POSITION = 0x00000010
00672 RESET_ALL_POSITION_DATA = 0x0000001F
00673
00674 """
00675 This command resets all configurable parameters to their default values.
00676 The general purpose parameter is ignored for this request.
00677 """
00678 RMP_CMD_RESET_PARAMS_TO_DEFAULT = (35)
00679
00680
00681 """------------------------------------------------------------------------
00682 RMP Fault definitions
00683 This section is used to define the decoding of fault status words sent
00684 by the RMP. The meaning of specific faults can be found in the interface
00685 guide.
00686 ------------------------------------------------------------------------"""
00687 NO_FAULT = 0x00000000
00688 ALL_FAULTS = 0xFFFFFFFF
00689
00690 """
00691 Transient faults: These faults are not latching and can be asserted and then
00692 cleared during runtime. There are currently no transient faults for the RMP
00693 """
00694 transient_fault_decode = dict({
00695 0x00000000: ""})
00696
00697 """
00698 Critical faults: These faults are latching.
00699 """
00700 critical_fault_decode = dict({
00701 0x00000000: "",
00702 0x00000001:"CRITICAL_FAULT_INIT",
00703 0x00000002:"CRITICAL_FAULT_INIT_UIP_COMM",
00704 0x00000004:"CRITICAL_FAULT_INIT_PROPULSION",
00705 0x00000008:"CRITICAL_FAULT_INIT_TIMEOUT",
00706 0x00000010:"CRITICAL_FAULT_FORW_SPEED_LIMITER_HAZARD",
00707 0x00000020:"CRITICAL_FAULT_AFT_SPEED_LIMITER_HAZARD",
00708 0x00000040:"CRITICAL_FAULT_CHECK_STARTUP",
00709 0x00000080:"CRITICAL_FAULT_APP_VELOCITY_CTL_FAILED",
00710 0x00000100:"CRITICAL_FAULT_APP_POSITION_CTL_FAILED",
00711 0x00000200:"CRITICAL_FAULT_ABB_SHUTDOWN",
00712 0x00000400:"CRITICAL_FAULT_AP_MODE_TRANS_TIMEOUT",
00713 0x00000800:"CRITICAL_FAULT_PITCH_ANGLE_EXCEEDED",
00714 0x00001000:"CRITICAL_FAULT_ROLL_ANGLE_EXCEEDED",
00715 0x00002000:"CRITICAL_FAULT_BSB_INIT_FAILED",
00716 0x00004000:"CRITICAL_FAULT_BSB_COMM_FAILED",
00717 0x00008000:"CRITICAL_FAULT_BSB_LOST_POWER",
00718 0x00010000:"CRITICAL_FAULT_BSB_HW_FAULT"})
00719
00720 """
00721 Communication faults: These faults are latching.
00722 """
00723 comm_fault_decode = dict({
00724 0x00000000: "",
00725 0x00000001:"COMM_FAULT_UIP_MISSING_UIP_DATA",
00726 0x00000002:"COMM_FAULT_UIP_UNKNOWN_MESSAGE_RECEIVED",
00727 0x00000004:"COMM_FAULT_UIP_BAD_CHECKSUM",
00728 0x00000008:"COMM_FAULT_UIP_TRANSMIT",
00729 0x00000010:"COMM_FAULT_UI_BAD_MOTION_CMD",
00730 0x00000020:"COMM_FAULT_UI_UNKOWN_CMD",
00731 0x00000040:"COMM_FAULT_UI_BAD_PACKET_CHECKSUM"})
00732
00733
00734 """
00735 MCU faults: These faults are latching.
00736 """
00737 mcu_fault_decode = dict({
00738 0x00000000: "",
00739 0x00000001:"MCU_FAULT_MCU_0_IS_DEGRADED",
00740 0x00000002:"MCU_FAULT_MCU_0_IS_FAILED",
00741 0x00000004:"MCU_FAULT_MCU_0_REQUESTS_REDUCED_PERFORMANCE",
00742 0x00000008:"MCU_FAULT_MCU_0_REQUESTS_ZERO_SPEED",
00743 0x00000010:"MCU_FAULT_MCU_1_IS_DEGRADED",
00744 0x00000020:"MCU_FAULT_MCU_1_IS_FAILED",
00745 0x00000040:"MCU_FAULT_MCU_1_REQUESTS_REDUCED_PERFORMANCE",
00746 0x00000080:"MCU_FAULT_MCU_1_REQUESTS_ZERO_SPEED",
00747 0x00000100:"MCU_FAULT_MCU_2_IS_DEGRADED",
00748 0x00000200:"MCU_FAULT_MCU_2_IS_FAILED",
00749 0x00000400:"MCU_FAULT_MCU_2_REQUESTS_REDUCED_PERFORMANCE",
00750 0x00000800:"MCU_FAULT_MCU_2_REQUESTS_ZERO_SPEED",
00751 0x00001000:"MCU_FAULT_MCU_3_IS_DEGRADED",
00752 0x00002000:"MCU_FAULT_MCU_3_IS_FAILED",
00753 0x00004000:"MCU_FAULT_MCU_3_REQUESTS_REDUCED_PERFORMANCE",
00754 0x00008000:"MCU_FAULT_MCU_3_REQUESTS_ZERO_SPEED",
00755 0x00010000:"MCU_FAULT_MISSING_MCU_0_DATA",
00756 0x00020000:"MCU_FAULT_MISSING_MCU_1_DATA",
00757 0x00040000:"MCU_FAULT_MISSING_MCU_2_DATA",
00758 0x00080000:"MCU_FAULT_MISSING_MCU_3_DATA",
00759 0x00100000:"MCU_FAULT_UNKNOWN_MESSAGE_RECEIVED"})
00760
00761 """
00762 Define a mask to indicate that the CCU has detected the fault and not the MCU
00763 """
00764 CCU_DETECTED_MCU_FAULT_MASK = 0x001F0000
00765
00766 """
00767 Sensor faults: These faults are latching.
00768 """
00769 sensor_fault_decode = dict({
00770 0x00000000: "",
00771 0x00000001:"SENSOR_FAULT_2P5V_VREF_RANGE_FAULT",
00772 0x00000002:"SENSOR_FAULT_7P2V_VBAT_RANGE_FAULT",
00773 0x00000004:"SENSOR_FAULT_7P2V_VBAT_WARNING",
00774 0x00000008:"SENSOR_FAULT_7P2V_BATT_INBALANCE_FAULT",
00775 0x00000010:"SENSOR_FAULT_7P2V_BATT_TEMPERATURE_FAULT",
00776 0x00000020:"SENSOR_FAULT_DIGITAL_INPUT",
00777 0x00000040:"SENSOR_FAULT_RANGE",
00778 0x00000080:"SENSOR_FAULT_DEFAULT",
00779 0x00000100:"SENSOR_FAULT_5V_MONITOR_RANGE_FAULT",
00780 0x00000200:"SENSOR_FAULT_12V_MONITOR_RANGE_FAULT"})
00781
00782 """
00783 BSA faults: These faults are latching.
00784 """
00785 bsa_fault_decode = dict({
00786 0x00000000: "",
00787 0x00000001:"BSA_FAULT_SIDE_A_MISSING_BSA_DATA",
00788 0x00000002:"BSA_FAULT_SIDE_B_MISSING_BSA_DATA",
00789 0x00000004:"BSA_FAULT_UNKNOWN_MESSAGE_RECEIVED",
00790 0x00000008:"BSA_FAULT_TRANSMIT_A_FAILED",
00791 0x00000010:"BSA_FAULT_TRANSMIT_B_FAILED",
00792 0x00000020:"BSA_FAULT_DEFAULT",
00793 0x00000040:"BSA_FAULT_SIDE_A_RATE_SENSOR_SATURATED",
00794 0x00000080:"BSA_FAULT_SIDE_B_RATE_SENSOR_SATURATED",
00795 0x00000100:"BSA_FAULT_SIDE_A_TILT_SENSOR_SATURATED",
00796 0x00000200:"BSA_FAULT_SIDE_B_TILT_SENSOR_SATURATED",
00797 0x00000400:"PSE_FAULT_COMPARISON"})
00798
00799 """
00800 Architecture faults: These faults are latching.
00801 """
00802 arch_fault_decode = dict({
00803 0x00000000: "",
00804 0x00000001:"ARCHITECT_FAULT_SPI_RECEIVE",
00805 0x00000002:"ARCHITECT_FAULT_SPI_TRANSMIT",
00806 0x00000004:"ARCHITECT_FAULT_SPI_RECEIVE_OVERRUN",
00807 0x00000008:"ARCHITECT_FAULT_SPI_RX_BUFFER_OVERRUN",
00808 0x00000010:"ARCHITECT_FAULT_COMMANDED_SAFETY_SHUTDOWN",
00809 0x00000020:"ARCHITECT_FAULT_COMMANDED_DISABLE",
00810 0x00000040:"ARCHITECT_FAULT_KILL_SWITCH_ACTIVE",
00811 0x00000080:"ARCHITECT_FAULT_FRAM_CONFIG_INIT_FAILED",
00812 0x00000100:"ARCHITECT_FAULT_FRAM_CONFIG_SET_FAILED",
00813 0x00000200:"ARCHITECT_FAULT_BAD_MODEL_IDENTIFIER",
00814 0x00000400:"ARCHITECT_FAULT_BAD_CCU_HW_REV",
00815 0x00000800:"ARCHITECT_FAULT_DECEL_SWITCH_ACTIVE"})
00816
00817 """
00818 Internal faults: These faults are latching.
00819 """
00820 internal_fault_decode = dict({
00821 0x00000000: "",
00822 0x00000001:"INTERNAL_FAULT_HIT_DEFAULT_CONDITION",
00823 0x00000002:"INTERNAL_FAULT_HIT_SPECIAL_CASE"})
00824
00825 """
00826 MCU specific faults: These faults are detected locally by the MCU
00827 """
00828 mcu_specific_fault_decode = dict({
00829 0x00000000: "",
00830 0x00000001:"MCU_TRANS_BATTERY_TEMP_WARNING",
00831 0x00000002:"MCU_TRANS_BATTERY_COLD_REGEN",
00832 0x00000004:"MCU_UNKNOWN",
00833 0x00000008:"MCU_UNKNOWN",
00834 0x00000010:"MCU_TRANS_LOW_BATTERY",
00835 0x00000020:"MCU_TRANS_BATT_OVERVOLTAGE",
00836 0x00000040:"MCU_CRITICAL_BATT_OVERVOLTAGE",
00837 0x00000080:"MCU_CRITICAL_EMPTY_BATTERY",
00838 0x00000100:"MCU_CRITICAL_BATTERY_TEMP",
00839 0x00000200:"MCU_COMM_CU_BCU_LINK_DOWN",
00840 0x00000400:"MCU_COMM_INITIALIZATION_FAILED",
00841 0x00000800:"MCU_COMM_FAILED_CAL_EEPROM",
00842 0x00001000:"MCU_POWER_SUPPLY_TRANSIENT_FAULT",
00843 0x00002000:"MCU_POWER_SUPPLY_12V_FAULT",
00844 0x00004000:"MCU_POWER_SUPPLY_5V_FAULT",
00845 0x00008000:"MCU_POWER_SUPPLY_3V_FAULT",
00846 0x00010000:"MCU_JUNCTION_TEMP_FAULT",
00847 0x00020000:"MCU_MOTOR_WINDING_TEMP_FAULT",
00848 0x00040000:"MCU_MOTOR_DRIVE_FAULT",
00849 0x00080000:"MCU_MOTOR_DRIVE_HALL_FAULT",
00850 0x00100000:"MCU_MOTOR_DRIVE_AMP_FAULT",
00851 0x00200000:"MCU_MOTOR_DRIVE_AMP_ENABLE_FAULT",
00852 0x00400000:"MCU_MOTOR_DRIVE_AMP_OVERCURRENT_FAULT",
00853 0x00800000:"MCU_MOTOR_DRIVE_VOLTAGE_FEEDBACK_FAULT",
00854 0x01000000:"MCU_FRAME_FAULT",
00855 0x02000000:"MCU_BATTERY_FAULT",
00856 0x08000000:"MCU_MOTOR_STUCK_RELAY_FAULT",
00857 0x10000000:"MCU_ACTUATOR_POWER_CONSISTENCY_FAULT",
00858 0x20000000:"MCU_ACTUATOR_HALT_PROCESSOR_FAULT",
00859 0x40000000:"MCU_ACTUATOR_DEGRADED_FAULT"})
00860
00861 """
00862 All the fault groups are packed into four 32-bit fault status words. The following
00863 defines how they are packed into the words
00864 """
00865
00866 """
00867 Fault status word 0
00868 """
00869 FSW_ARCH_FAULTS_INDEX = 0
00870 FSW_ARCH_FAULTS_SHIFT = 0
00871 FSW_ARCH_FAULTS_MASK = 0x00000FFF
00872 FSW_CRITICAL_FAULTS_INDEX = 0
00873 FSW_CRITICAL_FAULTS_SHIFT = 12
00874 FSW_CRITICAL_FAULTS_MASK = 0xFFFFF000
00875 """
00876 Fault status word 1
00877 """
00878 FSW_COMM_FAULTS_INDEX = 1
00879 FSW_COMM_FAULTS_SHIFT = 0
00880 FSW_COMM_FAULTS_MASK = 0x0000FFFF
00881 FSW_INTERNAL_FAULTS_INDEX = 1
00882 FSW_INTERNAL_FAULTS_SHIFT = 16
00883 FSW_INTERNAL_FAULTS_MASK = 0x000F0000
00884 """
00885 Fault status word 2
00886 """
00887 FSW_SENSORS_FAULTS_INDEX = 2
00888 FSW_SENSORS_FAULTS_SHIFT = 0
00889 FSW_SENSORS_FAULTS_MASK = 0x0000FFFF
00890 FSW_BSA_FAULTS_INDEX = 2
00891 FSW_BSA_FAULTS_SHIFT = 16
00892 FSW_BSA_FAULTS_MASK = 0xFFFF0000
00893 """
00894 Fault status word 3
00895 """
00896 FSW_MCU_FAULTS_INDEX = 3
00897 FSW_MCU_FAULTS_SHIFT = 0
00898 FSW_MCU_FAULTS_MASK = 0xFFFFFFFF
00899
00900 """
00901 Fault group index definitions
00902 """
00903 FAULTGROUP_TRANSIENT = 0
00904 FAULTGROUP_CRITICAL = 1
00905 FAULTGROUP_COMM = 2
00906 FAULTGROUP_SENSORS = 3
00907 FAULTGROUP_BSA = 4
00908 FAULTGROUP_MCU = 5
00909 FAULTGROUP_ARCHITECTURE = 6
00910 FAULTGROUP_INTERNAL = 7
00911 NUM_OF_FAULTGROUPS = 8