We are running a survey to help us improve the experience for all of our members. If you see the survey appear, please take the time to tell us about your experience if you can.
filter_proc PROC push {lr}adc0_filt ; ===================================================================== ; --------------- check for ADC0 result, retrieve ------------------- ; ===================================================================== LDRH r0, [r9] ; Load SAR0 satus reg ANDS r0, r0, #0x00000100 BEQ adc0_filt ; go back if not LDRH r3, [r9, #0x2a0] ; get SAR0 result LDMIA r10, {r1, r2, r4, r5, r8, r12} ; load filter variables SUB r3, r3, #2048 ; shift adc result to center LSL r3, r3, #4 ; ------------------- calculate EGM biquad -------------------------- MOV r6, #71 ; b2 = b0 = 0.5*b1 MUL r5, r5, r6 ; b2*xn-2 MUL r0, r6, r3 ; b0*xn ADD r5, r0, r5 ; yn += b0*xn + b2*xn-2 MUL r0, r6, r4 ; 0.5*b1*xn-1 ADD r6, r5, r0, LSL #1 ; yn += b1*xn-1 MOV r5, #28587 ; a2 MUL r5, r5, r4 ; a2*yn-2 SUB r6, r6, r12 ; yn -= a2*yn-2 MOV r5, #61070 ; -a1 -(-61070) MUL r5, r5, r8 ; -a1*yn-1 ADD r6, r6, r5 ; yn += -a1*yn-1 ASR r6, r6, #16 ; yn = yn >> 16 ; Load sine LUT values ADD r5, r11, #640 ; create shifted pointer for cos LDR r0, [r10, r11] ; load sine LUT value LDR r1, [r10, r5] ; cos(x) = sin(x+T/4) ; Consider changing sine-table to 16 or more bits per sample (why not?) ; ------------------- calculate NavX filter ------------------------- SUB r3, r3, r6 ; subtract DC from NavX signal ADD r7, r7, #24 ; update filt_data offset MUL r1, r1, r3 ; work0 = cos_lut[i] * (adc_samp - dc_offset) ADD r3, r3, r1, ASR #12 ; cos_sum = cos_sum + (work0 >> 12) MUL r0, r0, r3 ; work0 = sin_lut[i] * (adc_samp - dc_offset) ADD r2, r2, r0, ASR #12 ; sin_sum = sin_sum + (work0 >> 12) ; -------------------- store filter variables ----------------------- STMIA r10!, {r1, r2, r3, r4, r6, r8}; write back incremented address adc1_filt ; ===================================================================== ; --------------- check for ADC1 result, retrieve ------------------- ; ===================================================================== LDRH r3, [r9, #0x2a2] ; get SAR0 result LDMIA r10, {r1, r2, r4, r5, r8, r12} ; load filter variables SUB r3, r3, #2048 ; shift adc result to center LSL r3, r3, #4 ; ------------------- calculate EGM biquad -------------------------- MOV r6, #71 ; b2 = b0 = 0.5*b1 MUL r5, r5, r6 ; b2*xn-2 MUL r0, r6, r3 ; b0*xn ADD r5, r0, r5 ; yn += b0*xn + b2*xn-2 MUL r0, r6, r4 ; 0.5*b1*xn-1 ADD r6, r5, r0, LSL #1 ; yn += b1*xn-1 MOV r5, #28587 ; a2 MUL r5, r5, r4 ; a2*yn-2 SUB r6, r6, r12 ; yn -= a2*yn-2 MOV r5, #61070 ; -a1 -(-61070) MUL r5, r5, r8 ; -a1*yn-1 ADD r6, r6, r5 ; yn += -a1*yn-1 ASR r6, r6, #16 ; yn = yn >> 16 MOV r6, r3 ; Load sine LUT values ADD r5, r11, #640 ; create shifted pointer for cos LDR r0, [r10, r11] ; load sine LUT value LDR r1, [r10, r5] ; cos(x) = sin(x+T/4) ; Consider changing sine-table to 16 or more bits per sample (why not?) ; ------------------- calculate NavX filter ------------------------- SUB r3, r3, r6 ; subtract DC from NavX signal ADD r7, r7, #24 ; update filt_data offset ADD r11, r11, #4 ; update sine rom pointer MUL r1, r1, r3 ; work0 = cos_lut[i] * (adc_samp - dc_offset) ADD r3, r3, r1, ASR #12 ; cos_sum = cos_sum + (work0 >> 12) MUL r0, r0, r3 ; work0 = sin_lut[i] * (adc_samp - dc_offset) ADD r2, r2, r0, ASR #12 ; sin_sum = sin_sum + (work0 >> 12) ; -------------------- store filter variables ----------------------- STMIA r10!, {r1, r2, r3, r4, r6, r8}; write back incremented address ; ------------ check if all channels has been processed ------------- SUBS r0, r7, #480 ; have we processed all 20 channels? LDREQ r10, =filt_data ; reset filt_data ptr MOVEQ r7, #0 ; reset filt_data offset ; ----------- check if we have reached termination point ------------ SUBS r0, r11, #960 ; have 120 samples been processed? BNE adc0_filt ; if no, go to filter_proc again MOV r11, #480 ; reset sine LUT index, r11 is in the range ; 480 to 960, for optimization reasons