diff --git a/source/position_sense/hdsl/firmware/datalink_init.asm b/source/position_sense/hdsl/firmware/datalink_init.asm index 83426eb..73874df 100644 --- a/source/position_sense/hdsl/firmware/datalink_init.asm +++ b/source/position_sense/hdsl/firmware/datalink_init.asm @@ -376,12 +376,12 @@ datalink_learn_recv_loop_final: sbco ®_TMP2, c25, 0, 4 .endif - READ_CYCLCNT r25 + READ_CYCLCNT REG_TMP2 ; avoid wrap around, need to skip on equal as wait does not work for 0. -; qble datalink_learn_skip_wait, r25, r3 - qble datalink_abort2, r25, r3 - sub REG_TMP11, r3, r25 - MOV r25.b0, REG_TMP11.b0 +; qble datalink_learn_skip_wait, REG_TMP2, r3 + qble datalink_abort2, REG_TMP2, r3 + sub REG_TMP11, r3, REG_TMP2 + MOV REG_TMP2.b0, REG_TMP11.b0 ; WAIT subracts -1 from parameter before compare. On 0 it wraps around!!! WAIT REG_TMP11 datalink_learn_skip_wait: diff --git a/source/position_sense/hdsl/firmware/hdsl_master_icssg_300_mhz_bin.h b/source/position_sense/hdsl/firmware/hdsl_master_icssg_300_mhz_bin.h index b88f492..d15aad9 100644 --- a/source/position_sense/hdsl/firmware/hdsl_master_icssg_300_mhz_bin.h +++ b/source/position_sense/hdsl/firmware/hdsl_master_icssg_300_mhz_bin.h @@ -55,7 +55,7 @@ */ const unsigned int Hiperface_DSL2_0[]= { -0x21066e00, +0x21067800, 0x24000125, 0x2eff818f, 0x24001d8d, @@ -530,7 +530,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x511f0d03, 0x51190d02, 0x79000003, -0x230635d1, +0x23063fd1, 0x7900001a, 0xd104ff00, 0xd703ffff, @@ -637,7 +637,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x1f00c0c0, 0x1f008181, 0x81505880, -0x2104eb00, +0x2104f500, 0x91042580, 0xd708e0ff, 0x91042580, @@ -746,7 +746,6 @@ const unsigned int Hiperface_DSL2_0[]= { 0x10404000, 0x10606040, 0x10000060, -0x91501801, 0x70c0e204, 0x10222200, 0x10020220, @@ -755,22 +754,23 @@ const unsigned int Hiperface_DSL2_0[]= { 0x10404000, 0x10606040, 0x10000060, -0x91501801, -0x70c0e207, -0x1f010101, +0x70c0e208, +0x91501802, +0x1f010202, 0x91043880, 0x1f018080, 0x81041880, 0xc901c002, 0x2400229f, -0x1d010101, -0x81501801, +0x91501802, +0x1d010202, +0x81501802, 0x6900e105, 0x686e5304, 0x813a188d, 0x24000019, 0x79000002, -0x2304d1d1, +0x2304d4d1, 0x10535300, 0x10333320, 0x10131340, @@ -931,7 +931,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x24003001, 0xd1066b0e, 0x2400010d, -0x230660d1, +0x23066ad1, 0x68ab8d45, 0x13803b3b, 0x913d1880, @@ -944,7 +944,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x81531800, 0x7900003b, 0x2400020d, -0x230660d1, +0x23066ad1, 0x688b8d38, 0x8137184b, 0x13803b3b, @@ -987,7 +987,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x1d09c4c4, 0x2400040d, 0x24003001, -0x230660d1, +0x23066ad1, 0x15ff8d9c, 0x69005c34, 0x51009c33, @@ -1196,7 +1196,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0xf020020d, 0x69000d03, 0x24fdfd80, -0x68808427, +0x68808426, 0x91381800, 0x01010000, 0x81381800, @@ -1228,8 +1228,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x108787c7, 0x04c98087, 0x108080c9, -0x2304b8d1, -0x209d0000, +0x2304bbd1, 0x108b8b9d, 0x91aa1800, 0x1f018000, @@ -1249,10 +1248,14 @@ const unsigned int Hiperface_DSL2_0[]= { 0x108787c7, 0x04c98087, 0x108080c9, -0x2304b8d1, +0x2304bbd1, 0x91983880, -0x008e8080, -0x024ec0c0, +0x10eeeee1, +0x24000061, +0xc917ee02, +0x2400ff61, +0x00818080, +0x02c1c0c0, 0x81983880, 0x10efefe0, 0x10eeeee1, @@ -1293,18 +1296,25 @@ const unsigned int Hiperface_DSL2_0[]= { 0x0000526e, 0x0220720f, 0x02c093af, +0xc91fe003, +0x03ff536f, +0x79000002, 0x0300536f, -0x6902190b, -0xc90fc907, +0x6902190f, +0x24000040, +0xc90fc909, 0x1600c980, 0x01018080, 0x0b038080, 0x16008080, 0x01018080, -0x79000002, +0x51008005, +0x13ff4040, +0x79000003, +0x10c9c980, 0x0b038080, 0x00808e8e, -0x03006e6e, +0x02404e4e, 0x20d10000, 0x151f8484, 0x240478e2, @@ -1358,7 +1368,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x9101185b, 0x51005b0e, 0x7900000d, -0x23061dd1, +0x230627d1, 0x91dc388c, 0x240000e2, 0x91e21882, @@ -1366,7 +1376,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x100c0c2a, 0x106c6c4a, 0x102c2c6a, -0x23061dd1, +0x230627d1, 0x0b01e2e2, 0x0501e2e2, 0x4f00e2ff, @@ -1444,10 +1454,10 @@ const unsigned int Hiperface_DSL2_0[]= { 0x1e452121, 0x10000020, 0x2d802101, -0x910c3c99, -0x58e3f933, -0x04f9e3eb, -0x100b0b19, +0x910c3c82, +0x58e3e233, +0x04e2e3eb, +0x100b0b02, 0x0b01ebeb, 0x0501ebeb, 0x4f00ebff, @@ -1489,7 +1499,7 @@ const unsigned int Hiperface_DSL2_0[]= { 0x0b077200, 0x12001313, 0x0901f2f2, -0x230629d1, +0x230633d1, 0x510e6506, 0x6f010df6, 0x10656546, @@ -1528,16 +1538,16 @@ const unsigned int Hiperface_DSL2_0[]= { 0xd703ffff, 0x24001b8d, 0x23016a9d, -0x2305ef9d, -0x230629d1, +0x2305f99d, +0x230633d1, 0x6f010dea, 0x05012525, 0x4f0025f8, 0x24001025, 0x24001c8d, 0x23016a9d, -0x2305ef9d, -0x230629d1, +0x2305f99d, +0x230633d1, 0x6f010dd6, 0x05012525, 0x4f0025fa, @@ -1704,4 +1714,4 @@ const unsigned int Hiperface_DSL2_0[]= { 0x91003c82, 0x1308e2e2, 0x81003c82, -0x2104ea00 }; +0x2104f400 }; diff --git a/source/position_sense/hdsl/firmware/memory.inc b/source/position_sense/hdsl/firmware/memory.inc index af6421a..8138b5b 100644 --- a/source/position_sense/hdsl/firmware/memory.inc +++ b/source/position_sense/hdsl/firmware/memory.inc @@ -202,7 +202,7 @@ LONG_MSG_RECV .sassign r28, long_msg_recv_s .asg r16.b3, BYTE_ERROR .asg r15, FAST_POSH .asg r14.b3, FAST_POSL - .asg r14, SPEED + .asg r14, SPEED ; NOTE: r14.b3 is used as FAST_POSL .asg r17.b0, DISPARITY .asg r17.b1, SEND_PARA .asg r29.w0, RET_ADDR0 @@ -509,4 +509,4 @@ VPOSCRC_TEMP .set 0x60 ; 2 bytes STATUS2_TEMP .set 0x62 ; 1 byte VPOS2_TEMP .set 0x63 ; 5 bytes VPOSCRC2_TEMP .set 0x68 ; 2 bytes -SAFE_SUM_TEMP .set 0x70 ; 1 byte \ No newline at end of file +SAFE_SUM_TEMP .set 0x70 ; 1 byte diff --git a/source/position_sense/hdsl/firmware/transport.asm b/source/position_sense/hdsl/firmware/transport.asm index 4ba2367..3ef3f10 100644 --- a/source/position_sense/hdsl/firmware/transport.asm +++ b/source/position_sense/hdsl/firmware/transport.asm @@ -177,7 +177,6 @@ transport_on_v_frame_diff_pos: mov REG_TMP0.b2, REG_TMP0.b3 mov REG_TMP0.b3, REG_TMP0.b0 ;check if it is larger - lbco ®_TMP1.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1 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 @@ -193,10 +192,10 @@ transport_on_v_frame_dont_update_maxdev: mov REG_TMP0.b2, REG_TMP0.b3 mov REG_TMP0.b3, REG_TMP0.b0 ;check if it is larger - lbco ®_TMP1.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1 qbge transport_on_v_frame_dont_update_dte, REG_TMP2, REG_TMP0.w2 ; Set EVENT_DTE in ONLINE_STATUS_D register - set REG_TMP1.b0, REG_TMP1.b0, ONLINE_STATUS_D_DTE + lbco ®_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 ®_TMP0, MASTER_REGS_CONST, EVENT_H, 4 set REG_TMP0.w0, REG_TMP0.w0, EVENT_DTE @@ -208,8 +207,9 @@ transport_on_v_frame_dont_update_maxdev: update_events_no_int6: transport_on_v_frame_dont_update_dte: ; Clear EVENT_DTE in ONLINE_STATUS_D register - clr REG_TMP1.b0, REG_TMP1.b0, ONLINE_STATUS_D_DTE - sbco ®_TMP1.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1 + lbco ®_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1 + clr REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_D_DTE + sbco ®_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1 ;check for diff. is 0 -> estimate if not qbne transport_on_v_frame_estimate, REG_TMP1, 0 @@ -1073,7 +1073,6 @@ calc_speed_extend_acc1: sub DELTA_ACC0, REG_TMP0.w0, LAST_ACC mov LAST_ACC, REG_TMP0.w0 CALL1 calc_fastpos - RET ;restore return addr mov RET_ADDR0, REG_TMP11.w0 ; Moving the event and online register update during stuffing @@ -1108,8 +1107,14 @@ calc_speed_extend_acc0: transport_on_h_frame_exit: ;calculate rel. pos and store lbco ®_TMP0, MASTER_REGS_CONST, REL_POS0, 4 - add REG_TMP0.w0, REG_TMP0.w0, SPEED.w0 - adc REG_TMP0.w2, REG_TMP0.w2, SPEED.b2 +;sign extend speed to 32 bits and add it to REL_POS + mov REG_TMP1, SPEED + ldi REG_TMP1.b3, 0 + qbbc calc_relpos_extend_vel, SPEED, 23 + ldi REG_TMP1.b3, 0xff +calc_relpos_extend_vel: + add REG_TMP0.w0, REG_TMP0.w0, REG_TMP1.w0 + adc REG_TMP0.w2, REG_TMP0.w2, REG_TMP1.w2 sbco ®_TMP0, MASTER_REGS_CONST, REL_POS0, 4 ;store fast pos. and velocity mov REG_TMP0, FAST_POSH @@ -1205,21 +1210,32 @@ estimator_fpos: add FAST_POSL, VERT_L.b2, REG_TMP0.b0 adc FAST_POSH.b0, VERT_L.b3, REG_TMP0.b1 adc FAST_POSH.w1, VERT_H.w0, REG_TMP0.w2 +;sign extend relative position to 40 bits + qbbc estimator_fpos_add_relpos_positive, REG_TMP0, 31 + adc FAST_POSH.b3, VERT_H.b2, 0xFF + qba estimator_fpos_add_relpos_done +estimator_fpos_add_relpos_positive: adc FAST_POSH.b3, VERT_H.b2, 0 +estimator_fpos_add_relpos_done: qbne estimator_fpos_align_ph_not_2, ALIGN_PH, 2 ;vel = vel+acc/8 - qbbc estimator_fpos_acc_pos, LAST_ACC, 15 + ldi REG_TMP0.b2, 0 + qbbc estimator_fpos_acc_pos, LAST_ACC, 15 not REG_TMP0.w0, LAST_ACC add REG_TMP0.w0, REG_TMP0.w0, 1 lsr REG_TMP0.w0, REG_TMP0.w0, 3 not REG_TMP0.w0, REG_TMP0.w0 add REG_TMP0.w0, REG_TMP0.w0, 1 +;sign extend acceleration to 24 bit -> speed size + qbeq estimator_fpos_acc_sing_check_end, REG_TMP0.w0, 0 + or REG_TMP0.b2, REG_TMP0.b2, 0xFF qba estimator_fpos_acc_sing_check_end estimator_fpos_acc_pos: + mov REG_TMP0.w0, LAST_ACC lsr REG_TMP0.w0, REG_TMP0.w0, 3 estimator_fpos_acc_sing_check_end: add SPEED.w0, SPEED.w0, REG_TMP0.w0 - adc SPEED.b3, SPEED.b3, 0 + adc SPEED.b2, SPEED.b2, REG_TMP0.b2 estimator_fpos_align_ph_not_2: RET1 ;--------------------------------------------------------------------------------------------------