Storing Variables in Hub Ram
Jay Kickliter
Posts: 446
I'm running out of room longs in my PASM cog. So I decided to move them to hub memory. It's not working, of course.
Here's the two routines for loading/unloading the variables:
As a test, I ran this code:
which should move the variables from hub data, at hub address par + 10 longs, process the data, and send them back to the hub.
But instead, nothing works at all, leading me to beleive that hub memory is being contaminated.
Can anyone see any issues? I've been trying to figure this out on my own for a few days now, but I just can't get it to work, no matter what I try.
Here's the two routines for loading/unloading the variables:
_load_pitch_vars [b]mov[/b] kt1, #9
[b]mov[/b] kt2, #angle
[b]mov[/b] kt3, [b]par[/b]
[b]add[/b] kt3, #40
[b]movs[/b] :reader, #kt3
:loop [b]movd[/b] :reader, kt2
[b]nop[/b]
:reader [b]rdlong[/b] 0, 0
[b]add[/b] kt2, #1
[b]add[/b] kt3, #4
[b]djnz[/b] kt1, :loop
_load_pitch_vars_ret [b]ret[/b]
_unload_pitch_vars [b]mov[/b] kt1, #9
[b]mov[/b] kt2, #angle
[b]mov[/b] kt3, [b]par[/b]
[b]add[/b] kt3, #40
[b]movs[/b] :writer, #kt3
:loop [b]movd[/b] :writer, kt2
[b]nop[/b]
:writer [b]wrlong[/b] 0, 0
[b]add[/b] kt2, #1
[b]add[/b] kt3, #4
[b]djnz[/b] kt1, :loop
_unload_pitch_vars_ret [b]ret[/b]
As a test, I ran this code:
[b]call[/b] #_load_pitch_vars
[b]mov[/b] q_m, uf_pitch_rate
[b]call[/b] #_state_update
[b]mov[/b] angle_m,uf_pitch
[b]call[/b] #_kalman_update
[b]call[/b] #_unload_pitch_vars
which should move the variables from hub data, at hub address par + 10 longs, process the data, and send them back to the hub.
But instead, nothing works at all, leading me to beleive that hub memory is being contaminated.
Can anyone see any issues? I've been trying to figure this out on my own for a few days now, but I just can't get it to work, no matter what I try.
Comments
_load_pitch_vars mov kt1, #9 movd :reader,#angle mov kt2,par add kt2,#40 :reader rdlong 0-0,kt2 add :reader,incrDst add kt2,#4 djnz kt1,#:reader _load_pitch_vars ret incrDst long |< 9rdlong uses pointers so it should read
rdlong cogramvariable,hubramaddress
hubramaddress is a cog register with the address stored in it
The other routine (for unloading) is identical except that you use a WRLONG instead of a RDLONG.
You said " The other routine (for unloading) is identical except that you use a WRLONG instead of a RDLONG. "
It is posible to only initialize .. :reader position with RD/WR long with CALL to "_load_pitch_vars" and have same routine to both directions.
In my opinion that construct spare LONGS in COG if posible.
:reader
rdlong 0-0,kt2
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Nothing is impossible, there are only different degrees of difficulty.
For every stupid question there is at least one intelligent answer.
Don't guess - ask instead.
If you don't ask you won't know.
If your gonna construct something, make it·as simple as·possible yet as versatile as posible.
Sapieha
Jason
Jason, I'm going to try that. First I want to make sure this works. What I really need is 4 entry points, 2 (load, unload) for both pitch and roll variables. Or with a little clean-up, I could probably keep it all in the cog, since I'm just barely running over 495 longs.
Jason
I modified Mike's code to make it a little more compact, does anybody see any glaring collision points when using these two routines?:
_unload_pitch_vars [b]mov[/b] prw, pwriter [b]jmp[/b] #p_enter pwriter [b]wrlong[/b] 0-0, kt2 _load_pitch_vars p_enter [b]mov[/b] kt1, #18 [b]movd[/b] prw, #angle [b]mov[/b] kt2, [b]par[/b] [b]add[/b] kt2, #40 prw [b]rdlong[/b] 0-0, kt2 [b]add[/b] prw, incrDst [b]add[/b] kt2, #4 [b]djnz[/b] kt1, #prw _unload_pitch_vars_ret _load_pitch_vars_ret [b]ret[/b] _unload_roll_vars [b]mov[/b] rrw, rwriter [b]jmp[/b] #r_enter rwriter [b]wrlong[/b] 0-0, kt2 _load_roll_vars r_enter [b]mov[/b] kt1, #18 [b]movd[/b] rrw, #angle [b]mov[/b] kt2, [b]par[/b] [b]add[/b] kt2, #120 rrw [b]rdlong[/b] 0-0, kt2 [b]add[/b] rrw, incrDst [b]add[/b] kt2, #4 [b]djnz[/b] kt1, #rrw _unload_roll_vars_ret _load_roll_vars_ret [b]ret[/b]Here's a section of the main loop where I call the routines:
[b]call[/b] #_load_pitch_vars [b]mov[/b] q_m, uf_pitch_rate [b]call[/b] #_state_update [b]mov[/b] angle_m,uf_pitch [b]call[/b] #_kalman_update [b]call[/b] #_unload_pitch_vars [b]call[/b] #_load_roll_vars [b]mov[/b] q_m, uf_roll_rate [b]call[/b] #_state_update [b]mov[/b] angle_m,uf_roll [b]call[/b] #_kalman_update [b]call[/b] #_unload_roll_varsAnd here is the the Kalman filter:
_state_update 'q := fMath.FSub(q_m, q_bias_Roll) {39 uS} [b]mov[/b] fnumA, q_m [b]mov[/b] fnumB, q_bias [b]call[/b] #_fsub [b]mov[/b] q, fnumA 'Pdot_Roll[noparse][[/noparse]*0] := fMath.FSub(fMath.FSub( Q_angle, P_Roll[noparse][[/noparse]*1] ), P_Roll[noparse][[/noparse]*2]) {39 uS} [b]mov[/b] fnumA, q_angle [b]mov[/b] fnumB, p1 [b]call[/b] #_fsub [b]mov[/b] fnumB, p2 [b]call[/b] #_fsub [b]mov[/b] pd0, fnumA 'Pdot_Roll[noparse][[/noparse]*1] := fMath.FMul(fMath.FNeg(P_Roll[noparse][[/noparse]*3]), dt) {21 uS} [b]mov[/b] fnumA, p3 [b]xor[/b] fnumA, Bit111 [b]mov[/b] fnumB, dt [b]call[/b] #_fmul [b]mov[/b] pd1, fnumA 'rate_Roll := q [b]mov[/b] rate, q 'angle_Roll := fMath.FAdd(angle_Roll, fMath.FMul(q, dt) {46 uS} ) {39 uS} [b]mov[/b] fnumA, q [b]mov[/b] fnumB, dt [b]call[/b] #_fmul [b]mov[/b] fnumB, angle [b]call[/b] #_fadd [b]mov[/b] angle, fnumA 'Angle_Roll_Integer := fmath.ftrunc(fmath.fmul(angle_Roll, 100.00)) 'mov angle 'P_Roll[noparse][[/noparse]*0] := fMath.FAdd(P_Roll[noparse][[/noparse]*0], fMath.FMul(Pdot_Roll[noparse][[/noparse]*0], dt) {46 uS} ) {39 uS} [b]mov[/b] fnumA, pd0 [b]mov[/b] fnumB, dt [b]call[/b] #_fmul [b]mov[/b] fnumB, p0 [b]call[/b] #_fadd [b]mov[/b] p0, fnumA 'P_Roll[noparse][[/noparse]*1] := fMath.FAdd(P_Roll[noparse][[/noparse]*1], Pdot_Roll[noparse][[/noparse]*1] ) [b]mov[/b] fnumA, p1 [b]mov[/b] fnumB, pd1 [b]call[/b] #_fadd [b]mov[/b] p1, fnumA 'P_Roll[noparse][[/noparse]*2] := fMath.FAdd(P_Roll[noparse][[/noparse]*2], Pdot_Roll[noparse][[/noparse]*1] ) [b]mov[/b] fnumA, p2 [b]mov[/b] fnumB, pd1 [b]call[/b] #_fadd [b]mov[/b] p2, fnumA 'P_Roll[noparse][[/noparse]*3] := fMath.FAdd(P_Roll[noparse][[/noparse]*3], fMath.FMul(Q_gyro, dt) {46 uS} ) {39 uS} [b]mov[/b] fnumA, q_gyro [b]mov[/b] fnumB, dt [b]call[/b] #_fmul [b]mov[/b] fnumB, p3 [b]call[/b] #_fadd [b]mov[/b] p3, fnumA _state_update_ret [b]ret[/b] _kalman_update 'angle_err := fMath.FSub(angle_m, angle_Roll) {39 uS} [b]mov[/b] fnumA, angle_m [b]mov[/b] fnumB, angle [b]call[/b] #_fsub [b]mov[/b] angle_err,fnumA 'E := fMath.FAdd(R_angle, P_Roll[noparse][[/noparse]*0]) {39 uS} [b]mov[/b] fnumA, r_angle [b]mov[/b] fnumB, p0 [b]call[/b] #_fadd [b]mov[/b] e, fnumA 'K_0 := fMath.FDiv(P_Roll[noparse][[/noparse]*0], E) {45 uS} [b]mov[/b] fnumA, p0 [b]mov[/b] fnumB, e [b]call[/b] #_fdiv [b]mov[/b] k_0, fnumA 'K_1 := fMath.FDiv(P_Roll[noparse][[/noparse]*2], E) {45 uS} [b]mov[/b] fnumA, p2 [b]mov[/b] fnumB, e [b]call[/b] #_fdiv [b]mov[/b] k_1, fnumA 't_0 := P_Roll[noparse][[/noparse]*0] [b]mov[/b] t_0, p0 't_1 := P_Roll[noparse][[/noparse]*1] [b]mov[/b] t_1, p1 'P_Roll[noparse][[/noparse]*0] := fMath.FSub(P_Roll[noparse][[/noparse]*0], fMath.FMul(K_0, t_0) {46 uS} ) {39 uS} [b]mov[/b] fnumA, k_0 [b]mov[/b] fnumB, t_0 [b]call[/b] #_fmul [b]mov[/b] fnumB, fnumA [b]mov[/b] fnumA, p0 [b]call[/b] #_fsub [b]mov[/b] p0, fnumA 'P_Roll[noparse][[/noparse]*1] := fMath.FSub(P_Roll[noparse][[/noparse]*1], fMath.FMul(K_0, t_1) {46 uS} ) {39 uS} [b]mov[/b] fnumA, k_0 [b]mov[/b] fnumB, t_1 [b]call[/b] #_fmul [b]mov[/b] fnumB, fnumA [b]mov[/b] fnumA, p1 [b]call[/b] #_fsub [b]mov[/b] p1, fnumA 'P_Roll[noparse][[/noparse]*2] := fMath.FSub(P_Roll[noparse][[/noparse]*2], fMath.FMul(K_1, t_0) {46 uS} ) {39 uS} [b]mov[/b] fnumA, k_1 [b]mov[/b] fnumB, t_0 [b]call[/b] #_fmul [b]mov[/b] fnumB, fnumA [b]mov[/b] fnumA, p2 [b]call[/b] #_fsub [b]mov[/b] p2, fnumA 'P_Roll[noparse][[/noparse]*3] := fMath.FSub(P_Roll[noparse][[/noparse]*3], fMath.FMul(K_1, t_1) {46 uS} ) {39 uS} [b]mov[/b] fnumA, k_1 [b]mov[/b] fnumB, t_1 [b]call[/b] #_fmul [b]mov[/b] fnumB, fnumA [b]mov[/b] fnumA, p3 [b]call[/b] #_fsub [b]mov[/b] p3, fnumA 'angle_Roll := fMath.FAdd( angle_Roll, fMath.FMul(K_0, angle_err) {46 uS} ) {39 uS} [b]mov[/b] fnumA, k_0 [b]mov[/b] fnumB, angle_err [b]call[/b] #_fmul [b]mov[/b] fnumB, fnumA [b]mov[/b] fnumA, angle [b]call[/b] #_fadd [b]mov[/b] angle, fnumA 'Angle_Roll_Integer := fmath.ftrunc(fmath.fmul(angle_Roll, 100.00)) 'q_bias_Roll := fMath.FAdd( q_bias_Roll, fMath.FMul(K_1, angle_err) {46 uS} ) {39 uS} [b]mov[/b] fnumA, k_1 [b]mov[/b] fnumB, angle_err [b]call[/b] #_fmul [b]mov[/b] fnumB, fnumA [b]mov[/b] fnumA, q_bias [b]call[/b] #_fadd [b]mov[/b] q_bias, fnumA _kalman_update_ret [b]ret[/b]Sorry for the long post, but I just can't figure out where the collision is coming from. Attached for download is the source.