Calculating dt in assembly
Jay Kickliter
Posts: 446
I'm having an assembly problem. I commented out most of the code that could be causing anomalies, but it still pops. I'm trying to calculate dt, which is how long it takes for the main loop to complete.
Here's the code:
Here's a few lins of output from an upper-level debugging object, the second to last number is dt, and the last is loops/sec.
Most of the time dt is around 0.000373, but every once in a while it drops down real low. I can't account for a random missing 300 usec. At first I though it was a problem with using either sub or subs to calculate dt, or the counter rolling over, but I tried all and the counter wouldn't roll over that fast.
Any ideas?
Here's the code:
:imu_loop mov kt1, raw_ptr 'store Z acceleration in kt3
add kt1, #_Z_AXIS*2
rdword fnumA, kt1
subs fnumA, neutral_axis_offset
call #_ffloat
mov kt3, fnumA
mov kt1, raw_ptr 'store Y acceleration in fnumA
add kt1, #_Y_AXIS*2
rdword fnumA, kt1
subs fnumA, neutral_axis_offset
subs fnumA, #_Y_AXIS_OFFSET
call #_ffloat
mov fnumB, kt3 'calculate roll using Z and Y
call #_atan2
mov fnumB, rad_to_deg
call #_fmul
mov uf_roll,fnumA
'wrlong uf_roll,roll_ptr
mov kt1, raw_ptr 'calcoulate roll rate using X rate
add kt1, #_X_RATE*2
rdword fnumA, kt1
subs fnumA, x_rate_offset
call #_ffloat
mov fnumB, rate_correction_factor
call #_fmul
mov kt2, roll_ptr
add kt2, #4
mov uf_roll_rate, fnumA
'wrlong uf_roll_rate, kt2
mov kt1, raw_ptr 'store X axis in fnumA
add kt1, #_X_AXIS*2
rdword fnumA, kt1
subs fnumA, neutral_axis_offset
subs fnumA, #_X_AXIS_OFFSET
call #_ffloat
mov fnumB, kt3 'calculate pitch using Z and X
call #_atan2
mov fnumB, rad_to_deg
call #_fmul
mov uf_pitch,fnumA
'wrlong uf_pitch,pitch_ptr
mov kt1, raw_ptr 'calcoulate pitch rate using Y rate
add kt1, #_Y_RATE*2
rdword fnumA, kt1
subs fnumA, y_rate_offset
call #_ffloat
mov fnumB, rate_correction_factor
call #_fmul
mov kt2, pitch_ptr
add kt2, #4
mov uf_pitch_rate, fnumA
'wrlong uf_pitch_rate, kt2
mov fnumA, cnt 'calculate dt using current cnt and last_time
subs fnumA, last_time
mov last_time, cnt
call #_ffloat
mov fnumB, inverse_clock_freq
call #_fmul
mov dt, fnumA
wrlong fnumA, misc_ptr
'call #_state_update
'call #_kalman_update
jmp #:imu_loop
Here's a few lins of output from an upper-level debugging object, the second to last number is dt, and the last is loops/sec.
0.00 0.00 0.00 0.00 0.000381 2628.12 0.00 0.00 0.00 0.00 0.000373 2678.81 0.00 0.00 0.00 0.00 0.000373 2678.81 0.00 0.00 0.00 0.00 0.000373 2678.81 0.00 0.00 0.00 0.00 0.000079 12610.34 0.00 0.00 0.00 0.00 0.000079 12706.48 0.00 0.00 0.00 0.00 0.000380 2629.50 0.00 0.00 0.00 0.00 0.000384 2606.20 0.00 0.00 0.00 0.00 0.000373 2680.25
Most of the time dt is around 0.000373, but every once in a while it drops down real low. I can't account for a random missing 300 usec. At first I though it was a problem with using either sub or subs to calculate dt, or the counter rolling over, but I tried all and the counter wouldn't roll over that fast.
Any ideas?
Comments
You don't show your math routines. Is there any chance that they treat, say, zero operands as a special case and don't go through the motions?
-Phil