External/motor-control-sdk
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pull request #12: HDSL : Remove maximum deviation related processing

Browse Source 
Merge in PINDSW/motor_control_sdk from PINDSW-6546_hdsl_remove_maxdev to next

* commit '7bc9f4f7bf2c8ed7852c7e1d057e17b9ff0a72e6':
  am64x/am243x: hdsl: Update VERSION register definition
  am64x/am243x: hdsl: Remove maximum deviation related processing
This commit is contained in:
Dhaval Khandla 2023-08-28 05:19:52 -05:00 committed by Naresh A
commit b6c6293522
6 changed files with 51 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
source/position_sense/hdsl/driver/hdsl_lut.c
View File

@ -1279,11 +1279,7 @@ static void hdsl_configure_register_if(HDSL_Handle hdslHandle)
{ {
hdslHandle->hdslInterface->PC_ADD_H = 0x80; hdslHandle->hdslInterface->PC_ADD_H = 0x80;
hdslHandle->hdslInterface->PC_OFF_H = 0x80; hdslHandle->hdslInterface->PC_OFF_H = 0x80;
hdslHandle->hdslInterface->MAXDEV_H = 0x0; hdslHandle->hdslInterface->ACC_ERR_CNT_THRESH = 0x1F;
hdslHandle->hdslInterface->MAXDEV_L = 0x0;
hdslHandle->hdslInterface->MAXDEV_H_THRES = 0xFF;
hdslHandle->hdslInterface->MAXDEV_L_THRES = 0xFF;
hdslHandle->hdslInterface->ACC_ERR_CNT_THRES = 0x1F;
HW_WR_REG32(((uint32_t)(hdslHandle->baseMemAddr) + 0xac), 0x4cc8115d); HW_WR_REG32(((uint32_t)(hdslHandle->baseMemAddr) + 0xac), 0x4cc8115d);
HW_WR_REG32(((uint32_t)(hdslHandle->baseMemAddr) + 0xac)+4, 0xfb334990); HW_WR_REG32(((uint32_t)(hdslHandle->baseMemAddr) + 0xac)+4, 0xfb334990);

9
source/position_sense/hdsl/firmware/defines.inc
View File

@ -30,14 +30,13 @@
; OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ; OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
; ;
;Firmware version
FIRMWARE_VERSION_CODING .set 0x1 ; bit7..4 major number
; bit5..4 major number
FIRMWARE_VERSION_MAJOR .set 0x0 FIRMWARE_VERSION_MAJOR .set 0x0
; bit3..0 minor number ; bit3..0 minor number
FIRMWARE_VERSION_MINOR .set 0x4 FIRMWARE_VERSION_MINOR .set 0x5
ICSS_FIRMWARE_RELEASE .set ((FIRMWARE_VERSION_CODING << 6) | (FIRMWARE_VERSION_MAJOR << 4) | (FIRMWARE_VERSION_MINOR << 0)) ICSS_FIRMWARE_RELEASE .set ((FIRMWARE_VERSION_MAJOR << 4) | (FIRMWARE_VERSION_MINOR << 0))
;number of cycles for 1 bit ;number of cycles for 1 bit
.if $defined("FREERUN_300_MHZ") .if $defined("FREERUN_300_MHZ")

76
source/position_sense/hdsl/firmware/hdsl_master_icssg_300_mhz_bin.h
View File

@ -55,7 +55,7 @@
*/ */
const unsigned int Hiperface_DSL2_0[]= { const unsigned int Hiperface_DSL2_0[]= {
0x21067700, 0x21065f00,
0x24000125, 0x24000125,
0x2eff818f, 0x2eff818f,
0x24001d8d, 0x24001d8d,
@ -151,7 +151,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0xd104ff00, 0xd104ff00,
0xd703ffff, 0xd703ffff,
0x24002f1e, 0x24002f1e,
0x2304689d, 0x2304509d,
0x05014545, 0x05014545,
0x4f0045d2, 0x4f0045d2,
0x24000070, 0x24000070,
@ -453,7 +453,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x69084502, 0x69084502,
0x21028200, 0x21028200,
0x69074502, 0x69074502,
0x2102df00, 0x2102c700,
0x51000c2b, 0x51000c2b,
0x51015b04, 0x51015b04,
0x100c0c02, 0x100c0c02,
@ -522,14 +522,14 @@ const unsigned int Hiperface_DSL2_0[]= {
0x0b024d00, 0x0b024d00,
0x12006d6d, 0x12006d6d,
0x09064d4d, 0x09064d4d,
0x2103dd00, 0x2103c500,
0xd104ff00, 0xd104ff00,
0xd703ffff, 0xd703ffff,
0x106d6d1e, 0x106d6d1e,
0x511f0d03, 0x511f0d03,
0x51190d02, 0x51190d02,
0x79000003, 0x79000003,
0x23063ed1, 0x230626d1,
0x7900001a, 0x7900001a,
0xd104ff00, 0xd104ff00,
0xd703ffff, 0xd703ffff,
@ -541,7 +541,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x511f0d12, 0x511f0d12,
0x51190d11, 0x51190d11,
0xc901c402, 0xc901c402,
0x21032200, 0x21030a00,
0x910c3c81, 0x910c3c81,
0x240117e0, 0x240117e0,
0x04e1e0e0, 0x04e1e0e0,
@ -636,7 +636,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x1f00c0c0, 0x1f00c0c0,
0x1f008181, 0x1f008181,
0x81505880, 0x81505880,
0x2104f200, 0x2104da00,
0x91042580, 0x91042580,
0xd708e0ff, 0xd708e0ff,
0x91042580, 0x91042580,
@ -717,7 +717,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x2400060d, 0x2400060d,
0x040d6666, 0x040d6666,
0x230252d1, 0x230252d1,
0x79000041, 0x79000029,
0x2400010d, 0x2400010d,
0x000d6666, 0x000d6666,
0x230252d1, 0x230252d1,
@ -729,7 +729,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x2400269f, 0x2400269f,
0x1f020202, 0x1f020202,
0x81521802, 0x81521802,
0x79000035, 0x7900001d,
0x81521802, 0x81521802,
0x0908f3e2, 0x0908f3e2,
0x10727202, 0x10727202,
@ -742,35 +742,11 @@ const unsigned int Hiperface_DSL2_0[]= {
0xc91fe203, 0xc91fe203,
0x1600e2e2, 0x1600e2e2,
0x0101e2e2, 0x0101e2e2,
0x913a18c0, 0x6900e104,
0x10404000, 0x686e5303,
0x10606040,
0x10000060,
0x70c0e204,
0x10222200,
0x10020220,
0x813a1880,
0x914218c0,
0x10404000,
0x10606040,
0x10000060,
0x70c0e208,
0x91501802,
0x1f010202,
0x91043880,
0x1f018080,
0x81041880,
0xc901c002,
0x2400229f,
0x91501802,
0x1d010202,
0x81501802,
0x6900e105,
0x686e5304,
0x813a188d,
0x24000019, 0x24000019,
0x79000002, 0x79000002,
0x2304d2d1, 0x2304bad1,
0x10535300, 0x10535300,
0x10333320, 0x10333320,
0x10131340, 0x10131340,
@ -933,7 +909,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x24003001, 0x24003001,
0xd1066b0e, 0xd1066b0e,
0x2400010d, 0x2400010d,
0x230669d1, 0x230651d1,
0x68ab8d45, 0x68ab8d45,
0x13803b3b, 0x13803b3b,
0x913d1880, 0x913d1880,
@ -946,7 +922,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x81531800, 0x81531800,
0x7900003b, 0x7900003b,
0x2400020d, 0x2400020d,
0x230669d1, 0x230651d1,
0x688b8d38, 0x688b8d38,
0x8137184b, 0x8137184b,
0x13803b3b, 0x13803b3b,
@ -989,7 +965,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x1d09c4c4, 0x1d09c4c4,
0x2400040d, 0x2400040d,
0x24003001, 0x24003001,
0x230669d1, 0x230651d1,
0x15ff8d9c, 0x15ff8d9c,
0x69005c34, 0x69005c34,
0x51009c33, 0x51009c33,
@ -1226,7 +1202,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x108787c7, 0x108787c7,
0x04c98087, 0x04c98087,
0x108080c9, 0x108080c9,
0x2304b9d1, 0x2304a1d1,
0x108b8b9d, 0x108b8b9d,
0x91aa1800, 0x91aa1800,
0x1f018000, 0x1f018000,
@ -1246,7 +1222,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x108787c7, 0x108787c7,
0x04c98087, 0x04c98087,
0x108080c9, 0x108080c9,
0x2304b9d1, 0x2304a1d1,
0x91983880, 0x91983880,
0x10eeeee1, 0x10eeeee1,
0x24000061, 0x24000061,
@ -1335,7 +1311,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x81080580, 0x81080580,
0x24001fc0, 0x24001fc0,
0x810605c0, 0x810605c0,
0x24004400, 0x24000500,
0x810b1800, 0x810b1800,
0x81441800, 0x81441800,
0x2eff8383, 0x2eff8383,
@ -1369,7 +1345,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x9101185b, 0x9101185b,
0x51005b0e, 0x51005b0e,
0x7900000d, 0x7900000d,
0x230626d1, 0x23060ed1,
0x91dc388c, 0x91dc388c,
0x240000e2, 0x240000e2,
0x91e21882, 0x91e21882,
@ -1377,7 +1353,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x100c0c2a, 0x100c0c2a,
0x106c6c4a, 0x106c6c4a,
0x102c2c6a, 0x102c2c6a,
0x230626d1, 0x23060ed1,
0x0b01e2e2, 0x0b01e2e2,
0x0501e2e2, 0x0501e2e2,
0x4f00e2ff, 0x4f00e2ff,
@ -1500,7 +1476,7 @@ const unsigned int Hiperface_DSL2_0[]= {
0x0b077200, 0x0b077200,
0x12001313, 0x12001313,
0x0901f2f2, 0x0901f2f2,
0x230632d1, 0x23061ad1,
0x510e6506, 0x510e6506,
0x6f010df6, 0x6f010df6,
0x10656546, 0x10656546,
@ -1537,16 +1513,16 @@ const unsigned int Hiperface_DSL2_0[]= {
0xd703ffff, 0xd703ffff,
0x24001b8d, 0x24001b8d,
0x23016a9d, 0x23016a9d,
0x2305f89d, 0x2305e09d,
0x230632d1, 0x23061ad1,
0x6f010deb, 0x6f010deb,
0x05012525, 0x05012525,
0x4f0025f8, 0x4f0025f8,
0x24001025, 0x24001025,
0x24001c8d, 0x24001c8d,
0x23016a9d, 0x23016a9d,
0x2305f89d, 0x2305e09d,
0x230632d1, 0x23061ad1,
0x6f010dd8, 0x6f010dd8,
0x05012525, 0x05012525,
0x4f0025fa, 0x4f0025fa,
@ -1713,4 +1689,4 @@ const unsigned int Hiperface_DSL2_0[]= {
0x91003c82, 0x91003c82,
0x1308e2e2, 0x1308e2e2,
0x81003c82, 0x81003c82,
0x2104f200 }; 0x2104da00 };

12
source/position_sense/hdsl/firmware/memory.inc
View File

@ -302,9 +302,9 @@ SAFE_SUM .set 0x36
S_PC_DATA .set 0x37 S_PC_DATA .set 0x37
ACC_ERR_CNT .set 0x38 ACC_ERR_CNT .set 0x38
MAXACC .set 0x39 MAXACC .set 0x39 ; not implemented
MAXDEV_H .set 0x3A MAXDEV_H .set 0x3A ; not implemented
MAXDEV_L .set 0x3B MAXDEV_L .set 0x3B ; not implemented
EVENT_S .set 0x3D EVENT_S .set 0x3D
MASK_S .set 0x3E MASK_S .set 0x3E
@ -312,9 +312,9 @@ MASK_S .set 0x3E
DUMMY .set 0x3F DUMMY .set 0x3F
;extra Master Registers as workaround for different access method depended behaviour (r/w) ;extra Master Registers as workaround for different access method depended behaviour (r/w)
SLAVE_REG_CTRL .set 0x40 SLAVE_REG_CTRL .set 0x40
ACC_ERR_CNT_TRESH .set 0x41 ACC_ERR_CNT_THRESH .set 0x41
MAXDEV_TRESH_H .set 0x42 MAXDEV_THRESH_H .set 0x42 ; not implemented
MAXDEV_TRESH_L .set 0x43 MAXDEV_THRESH_L .set 0x43 ; not implemented
;extra MASTER registers (not in the specification! little endian!) ;extra MASTER registers (not in the specification! little endian!)
NUM_VERT_ERR0 .set 0x90 NUM_VERT_ERR0 .set 0x90
NUM_VERT_ERR1 .set 0x91 NUM_VERT_ERR1 .set 0x91

45
source/position_sense/hdsl/firmware/transport.asm
View File

@ -175,51 +175,16 @@ no_sub_carry:
not REG_TMP2, REG_TMP2 not REG_TMP2, REG_TMP2
add REG_TMP2, REG_TMP2, 1 add REG_TMP2, REG_TMP2, 1
transport_on_v_frame_diff_pos: transport_on_v_frame_diff_pos:
;load MAXDEV_H/L
lbco &REG_TMP0.w2, MASTER_REGS_CONST, MAXDEV_H, 2
mov REG_TMP0.b0, REG_TMP0.b2
mov REG_TMP0.b2, REG_TMP0.b3
mov REG_TMP0.b3, REG_TMP0.b0
;check if it is larger
qbge transport_on_v_frame_dont_update_maxdev, REG_TMP2, REG_TMP0.w2
mov REG_TMP0.b0, REG_TMP2.b1
mov REG_TMP0.b1, REG_TMP2.b0
sbco &REG_TMP0, MASTER_REGS_CONST, MAXDEV_H, 2
transport_on_v_frame_dont_update_maxdev:
;load MAXDEV_TRESH_H/L
.if $defined("HDSL_MULTICHANNEL") .if $defined("HDSL_MULTICHANNEL")
PUSH_FIFO_CONST 0xff PUSH_FIFO_CONST 0xff
PUSH_FIFO_CONST 0xff PUSH_FIFO_CONST 0xff
.endif .endif
lbco &REG_TMP0.w2, MASTER_REGS_CONST, MAXDEV_TRESH_H, 2
mov REG_TMP0.b0, REG_TMP0.b2
mov REG_TMP0.b2, REG_TMP0.b3
mov REG_TMP0.b3, REG_TMP0.b0
;check if it is larger
qbge transport_on_v_frame_dont_update_dte, REG_TMP2, REG_TMP0.w2
; Set EVENT_DTE in ONLINE_STATUS_D register
lbco &REG_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
set REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_D_DTE
; Set EVENT_DTE in EVENT register
lbco &REG_TMP0, MASTER_REGS_CONST, EVENT_H, 4
set REG_TMP0.w0, REG_TMP0.w0, EVENT_DTE
;save events
sbco &REG_TMP0.w0, MASTER_REGS_CONST, EVENT_H, 2
qbbc update_events_no_int6, REG_TMP0.w2, EVENT_DTE
; generate interrupt
ldi r31.w0, PRU0_ARM_IRQ
update_events_no_int6:
transport_on_v_frame_dont_update_dte:
; Clear EVENT_DTE in ONLINE_STATUS_D register
lbco &REG_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
clr REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_D_DTE
sbco &REG_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
;check for diff. is 0 -> estimate if not
;check for diff. is 0 -> estimate if not
qbne transport_on_v_frame_estimate, REG_TMP1, 0 qbne transport_on_v_frame_estimate, REG_TMP1, 0
qbne transport_on_v_frame_estimate, VERT_H.b2, FAST_POSL qbne transport_on_v_frame_estimate, VERT_H.b2, FAST_POSL
;reset MAXDEV_H/L
sbco &REG_FNC.w0, MASTER_REGS_CONST, MAXDEV_H, 2
;reset ALIGN_PH ;reset ALIGN_PH
ldi ALIGN_PH, 0 ldi ALIGN_PH, 0
qba transport_on_v_frame_no_pos_mismatch qba transport_on_v_frame_no_pos_mismatch
@ -1023,7 +988,7 @@ transport_acc_err_inc:
add REG_TMP0.b0, REG_TMP0.b0, 1 add REG_TMP0.b0, REG_TMP0.b0, 1
sbco &REG_TMP0.b0, MASTER_REGS_CONST, ACC_ERR_CNT, 1 sbco &REG_TMP0.b0, MASTER_REGS_CONST, ACC_ERR_CNT, 1
;reset if it is too large ;reset if it is too large
lbco &REG_TMP0.b1, MASTER_REGS_CONST, ACC_ERR_CNT_TRESH, 1 lbco &REG_TMP0.b1, MASTER_REGS_CONST, ACC_ERR_CNT_THRESH, 1
qbgt transport_on_h_frame_no_reset, REG_TMP0.b0, REG_TMP0.b1 qbgt transport_on_h_frame_no_reset, REG_TMP0.b0, REG_TMP0.b1
jmp datalink_abort jmp datalink_abort
transport_on_h_frame_no_reset: transport_on_h_frame_no_reset:
@ -1057,7 +1022,7 @@ transport_on_h_frame_no_reset:
qbbc estimator_acc_sign_extend_dacc1, REG_TMP0.w0, 10 qbbc estimator_acc_sign_extend_dacc1, REG_TMP0.w0, 10
or REG_TMP0.b1, REG_TMP0.b1, 0xf8 or REG_TMP0.b1, REG_TMP0.b1, 0xf8
estimator_acc_sign_extend_dacc1: estimator_acc_sign_extend_dacc1:
; calcuate MAXACC, cap acc ; TODO: calcuate MAXACC, cap acc
;add estimated delta acc to LAST_ACC ;add estimated delta acc to LAST_ACC
add REG_FNC.w0, LAST_ACC, REG_TMP0.w0 add REG_FNC.w0, LAST_ACC, REG_TMP0.w0
;check if estimated acc is neg. or pos. ;check if estimated acc is neg. or pos.

14
source/position_sense/hdsl/include/hdsl_drv.h
View File

@ -185,17 +185,17 @@ typedef struct {
volatile uint8_t SAFE_SUM; /**< Summarized slave status */ volatile uint8_t SAFE_SUM; /**< Summarized slave status */
volatile uint8_t S_PC_DATA; /**< Response of Short message parameters channel Read for safe1 channel */ volatile uint8_t S_PC_DATA; /**< Response of Short message parameters channel Read for safe1 channel */
volatile uint8_t ACC_ERR_CNT; /**< Fast position error counter */ volatile uint8_t ACC_ERR_CNT; /**< Fast position error counter */
volatile uint8_t MAXACC; /**< Fast position acceleration boundary */ volatile uint8_t resvd8; /**< Reserved 8 */
volatile uint8_t MAXDEV_H; /**< Fast position estimator deviation high byte */
volatile uint8_t MAXDEV_L; /**< Fast position estimator deviation low byte */
volatile uint8_t resvd9; /**< Reserved 9 */ volatile uint8_t resvd9; /**< Reserved 9 */
volatile uint8_t resvd10; /**< Reserved 10 */
volatile uint8_t resvd11; /**< Reserved 11 */
volatile uint8_t EVENT_S; /**< Safe Events */ volatile uint8_t EVENT_S; /**< Safe Events */
volatile uint8_t MASK_S; /**< Safe Event Mask */ volatile uint8_t MASK_S; /**< Safe Event Mask */
volatile uint8_t DUMMY; /**< Dummy, no data */ volatile uint8_t DUMMY; /**< Dummy, no data */
volatile uint8_t SLAVE_REG_CTRL; /**< Short message control */ volatile uint8_t SLAVE_REG_CTRL; /**< Short message control */
volatile uint8_t ACC_ERR_CNT_THRES; /**< Fast position error counter threshold */ volatile uint8_t ACC_ERR_CNT_THRESH;/**< Fast position error counter threshold */
volatile uint8_t MAXDEV_H_THRES; /**< Fast position estimator deviation high byte threshold */ volatile uint8_t resvd12; /**< Reserved 12 */
volatile uint8_t MAXDEV_L_THRES; /**< Fast position estimator deviation low byte threshold */ volatile uint8_t resvd13; /**< Reserved 13 */
/*Safe 2 Interface */ /*Safe 2 Interface */
volatile uint8_t VERSION2; /**< Version in Safe Channel 2 */ volatile uint8_t VERSION2; /**< Version in Safe Channel 2 */
volatile uint8_t ENC2_ID; /**< Encoder ID in Safe Channel 2 */ volatile uint8_t ENC2_ID; /**< Encoder ID in Safe Channel 2 */
@ -208,7 +208,7 @@ typedef struct {
volatile uint8_t VPOSCRC2_H; /**< CRC of Safe Position 2, byte 1 */ volatile uint8_t VPOSCRC2_H; /**< CRC of Safe Position 2, byte 1 */
volatile uint8_t VPOSCRC2_L; /**< CRC of Safe Position 2, byte 0 */ volatile uint8_t VPOSCRC2_L; /**< CRC of Safe Position 2, byte 0 */
volatile uint8_t POSTX; /**< Position transmission status */ volatile uint8_t POSTX; /**< Position transmission status */
volatile uint8_t resvd10; /**< Reserved 10 */ volatile uint8_t resvd14; /**< Reserved 14 */
/* Online Status*/ /* Online Status*/
volatile uint8_t ONLINE_STATUS_D_H; /**< Online Status D, high byte*/ volatile uint8_t ONLINE_STATUS_D_H; /**< Online Status D, high byte*/
volatile uint8_t ONLINE_STATUS_D_L; /**< Online Status D, low byte*/ volatile uint8_t ONLINE_STATUS_D_L; /**< Online Status D, low byte*/