;***************************************************************************************** ; Mega 128 Scope Controller Program ; (c) Martin Cibulski ;**************************************************************************************** .DSEG mco_z1z2_active: .byte 1 mco_az_per_step: .byte 8 ;Az rad per microstep mco_alt_per_step: .byte 8 ;Alt rad per microstep mco_alt_start: .byte 8 ;Start Altitude at 0 microsteps mco_z1: .byte 8 ;Z1 error mco_z2: .byte 8 ;Z2 error mco_z3: .byte 8 ;Z3 error mco_s1: .byte 8 ;sin(Z1) mco_s2: .byte 8 ;sin(Z2) mco_az_offset: .byte 8 ;fine adjust in RA (radians) mco_alt_offset: .byte 8 ;fine adjust in DEC (radians) mco_test_az: .byte 8 mco_test_alt: .byte 8 mco_test_vec: mco_test_x: .byte 8 mco_test_y: .byte 8 mco_test_z: .byte 8 mco_test_az_res: .byte 8 mco_test_alt_res: .byte 8 mco_test_p: .byte 8 mco_test_q: .byte 8 mco_test_d: .byte 8 mco_test_a1: .byte 8 mco_test_a2: .byte 8 mco_test_a3: .byte 8 mco_test_az_x: .byte 8 mco_test_az_y: .byte 8 mco_test_cosp: .byte 8 mco_test_px: .byte 8 mco_test_py: .byte 8 mco_test_pz: .byte 8 mco_test_cosalt: .byte 8 mco_test_az_len: .byte 8 mco_test_px2: .byte 8 mco_test_py2: .byte 8 mco_test_pz2: .byte 8 ;**************************************************************************************** .CSEG mco_test_init: f_const mco_test__az f_store mco_test_az f_const mco_test__alt f_store mco_test_alt f_const mco_test__z1 f_store mco_z1 f_const mco_test__z2 f_store mco_z2 ret mco_test__az: .db 0x3F,0xE2,0xFD,0x3B,0x29,0xA1,0xB9,0xFA ;0,593412 mco_test__alt: .db 0x3F,0xF2,0xB5,0xBE,0x5D,0x9E,0x40,0xC8 ;1,169371 mco_test__z1: .db 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 ;0 mco_test__z2: .db 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 ;0 mco_test_rad2vec_old: ldi r16 ,high(mco_test_az-8) ldi r17 ,low(mco_test_az-8) ldi r18 ,high(mco_test_vec) ldi r19 ,low(mco_test_vec) jmp mco_rad2vec_old mco_test_rad2vec_new: ldi r16 ,high(mco_test_az-8) ldi r17 ,low(mco_test_az-8) ldi r18 ,high(mco_test_vec) ldi r19 ,low(mco_test_vec) jmp mco_rad2vec_new mco_test_vec2rad_old: ldi r16 ,high(mco_test_vec) ldi r17 ,low(mco_test_vec) ldi r18 ,high(mco_test_az_res) ldi r19 ,low(mco_test_az_res) jmp mco_vec2rad_old mco_test_vec2rad_new: ldi r16 ,high(mco_test_vec) ldi r17 ,low(mco_test_vec) ldi r18 ,high(mco_test_az_res) ldi r19 ,low(mco_test_az_res) jmp mco_vec2rad_new ;**************************************************************************************** .CSEG mco_label_tab: .dw mco_newz12 .db L_XCMD ,"NEWZ12 " .dw mco_showz1 .db L_XCMD ,"Z1 " .dw mco_showz2 .db L_XCMD ,"Z2 " .dw mco_test_init .db L_XCMD ,"MINIT " .dw mco_test_rad2vec_old .db L_XCMD ,"MR2V " .dw mco_test_vec2rad_old .db L_XCMD ,"MV2R " .dw mco_test_rad2vec_new .db L_XCMD ,"MR2VN " .dw mco_test_vec2rad_new .db L_XCMD ,"MV2RN " .dw mco_z1z2_active .db L_BRAM ,"MZ1Z2ACT " .dw mco_az_per_step .db L_FRAM ,"MAZPSTEP " .dw mco_alt_per_step .db L_FRAM ,"MALTPSTEP " .dw mco_alt_start .db L_FRAM ,"MALTSTART " .dw mco_z1 .db L_FRAM ,"MZ1 " .dw mco_z2 .db L_FRAM ,"MZ2 " .dw mco_z3 .db L_FRAM ,"MZ3 " .dw mco_az_offset .db L_FRAM ,"MAZOFFSET " .dw mco_alt_offset .db L_FRAM ,"MALTOFFSET " .dw mco_test_az .db L_FRAM ,"MTESTAZ " .dw mco_test_alt .db L_FRAM ,"MTESTALT " .dw mco_test_x .db L_FRAM ,"MTESTX " .dw mco_test_y .db L_FRAM ,"MTESTY " .dw mco_test_z .db L_FRAM ,"MTESTZ " .dw mco_test_az_res .db L_FRAM ,"MTESTAZR " .dw mco_test_alt_res .db L_FRAM ,"MTESTALTR " .dw mco_test_p .db L_FRAM ,"MTESTP " .dw mco_test_q .db L_FRAM ,"MTESTQ " .dw mco_test_d .db L_FRAM ,"MTESTD " .dw mco_test_a1 .db L_FRAM ,"MTESTA1 " .dw mco_test_a2 .db L_FRAM ,"MTESTA2 " .dw mco_test_a3 .db L_FRAM ,"MTESTA3 " .dw mco_test_az_x .db L_FRAM ,"MTESTAZX " .dw mco_test_az_y .db L_FRAM ,"MTESTAZY " .dw mco_test_cosp .db L_FRAM ,"MTESTCOSP " .dw mco_test_px .db L_FRAM ,"MTESTPX " .dw mco_test_py .db L_FRAM ,"MTESTPY " .dw mco_test_pz .db L_FRAM ,"MTESTPZ " .dw mco_test_cosalt .db L_FRAM ,"MTESTCOSA " .dw mco_test_px2 .db L_FRAM ,"MTESTPX2 " .dw mco_test_py2 .db L_FRAM ,"MTESTPY2 " .dw mco_test_pz2 .db L_FRAM ,"MTESTPZ2 " .dw 0 .db L_END , 0 ;**************************************************************************************** .CSEG mco_newz12: lds r16 ,alg_completed tst r16 brne mco_newz12_00 PUTS_ROM msg_newz12_align_first rjmp mco_newz12_error mco_newz12_00: call next_flt_value brtc mco_newz12_error f_push f_const flt__pi f_mul f_push f_const flt__180 f_div f_store mco_z1 call next_flt_value brtc mco_newz12_error f_push f_const flt__pi f_mul f_push f_const flt__180 f_div f_store mco_z2 rjmp mco_newz12_ok mco_newz12_error: clt ret mco_newz12_ok: lds r16 ,alg_newpos1 cpi r16 ,ALG_POS_NULL breq mco_newz12_01 ldi r16 ,ALG_POS_NEW sts alg_newpos1 ,r16 mco_newz12_01: lds r16 ,alg_newpos2 cpi r16 ,ALG_POS_NULL breq mco_newz12_02 ldi r16 ,ALG_POS_NEW sts alg_newpos2 ,r16 mco_newz12_02: clr r16 sts alg_completed ,r16 ldi r16 ,TGO_ST_NONE sts tgo_status ,r16 call alg_calc call cor_calc call tgo_calc call alg_analysis f_load alg_ana_errsum call flt_print PUTC 13 set ret ;**************************************************************************************** mco_showz1: f_load mco_z1 rjmp mco_showz12_to_deg mco_showz2: f_load mco_z2 mco_showz12_to_deg: f_push f_const flt__180 f_mul f_push f_const flt__pi f_div call flt_print putc 13 ret ;**************************************************************************************** mco_rad2vec: lds r0 ,mco_z1z2_active tst r0 breq mco_rad2vec_old rjmp mco_rad2vec_new ;**************************************************************************************** ;Convert apparent az,alt into vector ; ;r16:r17 az_steps long ; alt_steps long ; az_rad double ; alt_rad double ; ;r18:r19 X double ; Y double ; Z double ;**************************************************************************************** mco_rad2vec_old: .EQU mco_r2v_rptr = 1 .EQU mco_r2v_vptr = 3 .EQU mco_r2v_cosalt = 5 .EQU mco_r2v_lspace = 29 LOCAL mco_r2v_lspace std Y+mco_r2v_rptr ,r16 ;ptr > lAz,lAlt std Y+mco_r2v_rptr+1 ,r17 std Y+mco_r2v_vptr ,r18 ;ptr > X,Y,Z std Y+mco_r2v_vptr+1 ,r19 f_load_array mco_r2v_rptr ,2 ;alt f_push f_load mco_alt_offset ;minus alt_offset f_sub f_cos f_store_l mco_r2v_cosalt ;cos(alt-alt_offset) ;X = cos(alt-alt_offset) * cos(az-az_offset) f_push f_load_array mco_r2v_rptr ,1 ;az f_push f_load mco_az_offset ;minus az_offset f_sub f_cos f_mul f_store_array mco_r2v_vptr ,0 ;X ;Y = cos(alt-alt_offset) * sin(az-az_offset) f_load_l mco_r2v_cosalt ;cosalt f_push f_load_array mco_r2v_rptr ,1 ;az f_push f_load mco_az_offset ;minus az_offset f_sub f_sin f_mul f_store_array mco_r2v_vptr ,1 ;Y ;Z = sin(alt-alt_offset) f_load_array mco_r2v_rptr ,2 ;alt f_push f_load mco_alt_offset ;minus alt_offset f_sub f_sin f_store_array mco_r2v_vptr ,2 ;Z ENDLOCAL mco_r2v_lspace ret ;**************************************************************************************** ;Convert apparent az,alt into vector (incl.Z1,Z2) ; ;r16:r17 az_steps long ; alt_steps long ; az_rad double ; alt_rad double ; ;r18:r19 X double North ; Y double West ; Z double Zenit ;**************************************************************************************** mco_rad2vec_new: ;.EQU mco_r2v_mptr = 1 ;.EQU mco_r2v_vptr = 3 ;.EQU mco_r2v_cosalt = 5 ;.EQU mco_r2v_az = 13 ;.EQU mco_r2v_alt = 21 ;.EQU mco_r2v_lspace = 29 LOCAL mco_r2v_lspace std Y+mco_r2v_rptr ,r16 ;ptr > lAz,lAlt std Y+mco_r2v_rptr+1 ,r17 std Y+mco_r2v_vptr ,r18 ;ptr > X,Y,Z std Y+mco_r2v_vptr+1 ,r19 f_const flt__1 ;start at (1,0,0) north f_store_array mco_r2v_vptr ,0 f_const flt__0 f_store_array mco_r2v_vptr ,1 f_store_array mco_r2v_vptr ,2 f_load mco_z2 ;rotate per tube error Z2 ldi r16 ,high(flt_mat_buffer) ;around Z axis ldi r17 ,low(flt_mat_buffer) call flt_rotmatrix_z ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) ldd r18 ,Y+mco_r2v_vptr ldd r19 ,Y+mco_r2v_vptr+1 movw r21:r20 ,r19:r18 call flt_mat_vec_mul f_load_array mco_r2v_rptr ,2 ;rotate per negative altitude f_push f_load mco_alt_offset ;minus alt_offset f_sub f_chs r16 ldi r16 ,high(flt_mat_buffer) ;around Y axis ldi r17 ,low(flt_mat_buffer) call flt_rotmatrix_y ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) ldd r18 ,Y+mco_r2v_vptr ldd r19 ,Y+mco_r2v_vptr+1 movw r21:r20 ,r19:r18 call flt_mat_vec_mul f_load mco_z1 ;rotate per axis perpendicularity error Z1 ldi r16 ,high(flt_mat_buffer) ;around X axis ldi r17 ,low(flt_mat_buffer) call flt_rotmatrix_x ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) ldd r18 ,Y+mco_r2v_vptr ldd r19 ,Y+mco_r2v_vptr+1 movw r21:r20 ,r19:r18 call flt_mat_vec_mul f_load_array mco_r2v_rptr ,1 ;rotate per azimut around Z axis f_push f_load mco_az_offset ;minus az_offset f_sub ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) call flt_rotmatrix_z ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) ldd r18 ,Y+mco_r2v_vptr ldd r19 ,Y+mco_r2v_vptr+1 movw r21:r20 ,r19:r18 call flt_mat_vec_mul ENDLOCAL mco_r2v_lspace ret ;**************************************************************************************** mco_vec2rad: lds r0 ,mco_z1z2_active tst r0 breq mco_vec2rad_old rjmp mco_vec2rad_new ;**************************************************************************************** ;Convert mount vector into apparent az, alt ; ;r16:r17 X double North ; Y double West ; Z double Zenit ; ;r18:r19 az_rad double ; alt_rad double ;**************************************************************************************** mco_vec2rad_old: .EQU mco_v2m_vptr = 1 .EQU mco_v2m_mptr = 3 .EQU mco_v2m_sign_x = 5 .EQU mco_v2m_lspace = 6 LOCAL mco_v2m_lspace std Y+mco_v2m_vptr ,r16 ;ptr > X,Y,Z std Y+mco_v2m_vptr+1 ,r17 std Y+mco_v2m_mptr ,r18 ;ptr > fAz,fAlt std Y+mco_v2m_mptr+1 ,r19 f_load_array mco_v2m_vptr ,1 ;Y f_push f_load_array mco_v2m_vptr ,0 ;X std Y+mco_v2m_sign_x ,AKKU_S f_div f_atan ;atan(y/x) f_push f_load mco_az_offset ;plus az_offset f_add ldd r0 ,Y+mco_v2m_sign_x ;if x<0 subtract pi (180°) tst r0 brpl mco_v2m_01 f_push f_const flt__pi f_sub mco_v2m_01: tst AKKU_S brpl mco_v2m_02 f_push f_const flt__2pi f_add mco_v2m_02: f_store_array mco_v2m_mptr ,0 ;azimut (radians) f_load_array mco_v2m_vptr ,0 ;X f_push f_mul ;X**2 f_push f_load_array mco_v2m_vptr ,1 ;Y f_push f_mul ;Y**2 f_add f_sqrt f_push f_load_array mco_v2m_vptr ,2 ;Z f_xy f_div f_atan ;atan(Z/sqrt(X*X+Y*Y) f_push f_load mco_alt_offset ;plus alt_offset f_add f_store_array mco_v2m_mptr ,1 ;altitude (radians) ENDLOCAL mco_v2m_lspace ret ;**************************************************************************************** ;Convert mount vector into apparent az, alt (incl Z1,Z2) ; ;r16:r17 X double North ; Y double West ; Z double Zenit ; ;r18:r19 az_rad double ; alt_rad double ;**************************************************************************************** mco_vec2rad_new: .EQU mco_v2r_vptr = 1 .EQU mco_v2r_rptr = 3 .EQU mco_v2r_solcnt = 5 ;loop counter for 2 solutions to try .EQU mco_v2r_signazx = 6 .EQU mco_v2r_p = 8 .EQU mco_v2r_q = 16 .EQU mco_v2r_v11v22 = 24 .EQU mco_v2r_sqrtd = 32 .EQU mco_v2r_a1 = 40 .EQU mco_v2r_a2 = 48 .EQU mco_v2r_a3 = 56 .EQU mco_v2r_az_x = 64 .EQU mco_v2r_az_y = 72 .EQU mco_v2r_cosp = 80 .EQU mco_v2r_px = 88 .EQU mco_v2r_py = 96 .EQU mco_v2r_pz = 104 .EQU mco_v2r_cosalt = 112 .EQU mco_v2r_az_len = 120 .EQU mco_v2r_lspace = 128 LOCAL mco_v2r_lspace std Y+mco_v2r_vptr ,r16 ;ptr > X,Y,Z std Y+mco_v2r_vptr+1 ,r17 std Y+mco_v2r_rptr ,r18 ;ptr > fAz,fAlt std Y+mco_v2r_rptr+1 ,r19 f_load mco_z1 f_sin f_store mco_s1 f_load mco_z2 f_sin f_store mco_s2 ;p = 2*(v1*s2-v1*s1*v3)/(v1*v1+v2*v2) f_load_array mco_v2r_vptr ,0 ;v1 f_push f_load mco_s2 ;s2 f_mul ;v1*s2 f_push f_load_array mco_v2r_vptr ,0 ;v1 f_push f_load mco_s1 ;s1 f_mul ;v1*s1 f_push f_load_array mco_v2r_vptr ,2 ;v3 f_mul ;v1*s1*v3 f_sub ;v1*s2-v1*s1*v3 f_push f_const flt__2 f_mul ;2*(v1*s2-v1*s1*v3) f_push f_load_array mco_v2r_vptr ,0 ;v1 f_push ;v1 f_mul ;v1*v1 f_push f_load_array mco_v2r_vptr ,1 ;v2 f_push ;v2 f_mul ;v2*v2 f_add ;v1*v1+v2*v2 f_store_l mco_v2r_v11v22 f_div f_store_l mco_v2r_p f_store mco_test_p ; q = (v2*v2 * (s1*s1 - 1) + v3*v3*s1*s1 - 2*v3*s1*s2 + s2*s2) / (v1*v1 + v2*v2) f_load_array mco_v2r_vptr ,1 ;v2 f_push ;v2 f_mul ;v1*v1 f_push f_load mco_s1 ;s1 f_push ;s1 f_mul ;s1*s1 f_push f_const flt__1 ;1 f_sub ;s1*s1 - 1 f_mul f_push f_load_array mco_v2r_vptr ,2 ;v3 f_push ;v3 f_mul ;v3*v3 f_push f_load mco_s1 ;s1 f_push ;s1 f_mul ;s1*s1 f_mul ;v3*v3*s1*s1 f_add f_push f_load_array mco_v2r_vptr ,2 ;v3 f_push f_load mco_s1 ;s1 f_mul ;v3*s1 f_push f_load mco_s2 ;s2 f_mul ;v3*s1*s2 f_push f_const flt__2 f_mul ;2*v3*s1*s2 f_sub f_push f_load mco_s2 ;s2 f_push ;s2 f_mul ;s2*s2 f_add f_push f_load_l mco_v2r_v11v22 ;v1*v1+v2*v2 f_div f_store_l mco_v2r_q f_store mco_test_q ; D = 0.25*p*p - q f_load_l mco_v2r_p ;p f_push ;p f_mul ;p*p f_push f_const flt__0_25 f_mul ;0.25*p*p f_push f_load_l mco_v2r_q ;q f_sub ;0.25*p*p - q f_store mco_test_d ;= D tst AKKU_S ;positive brpl mco_v2r_d_positive ;yes, OK clt ;return false rjmp mco_v2r_end mco_v2r_d_positive: f_sqrt ;sqrt (D) f_store_l mco_v2r_sqrtd ldi r16 ,2 ;2 solutions to test std Y+mco_v2r_solcnt ,r16 mco_v2r_calc_a1: f_load_l mco_v2r_p ;p f_chs r16 ;-p f_push f_const flt__0_5 f_mul ;-p/2 f_push f_load_l mco_v2r_sqrtd ;sqrt(D) f_add ;-p/2 + sqrt(D) f_store_l mco_v2r_a1 f_store mco_test_a1 f_store_l mco_v2r_az_y f_store mco_test_az_y ; a2 = (v3*s1 - v1*a1 - s2) / v2 f_load_array mco_v2r_vptr ,2 ;v3 f_push f_load mco_s1 ;s1 f_mul ;v3*s1 f_push f_load_array mco_v2r_vptr ,0 ;v1 f_push f_load_l mco_v2r_a1 ;a1 f_mul ;v1*a1 f_sub f_push f_load mco_s2 ;s2 f_sub f_push f_load_array mco_v2r_vptr ,1 ;v2 f_div f_store_l mco_v2r_a2 f_store mco_test_a2 f_chs r16 f_store_l mco_v2r_az_x f_store mco_test_az_x f_load mco_s1 ;s1 f_chs r16 ;-s1 f_store_l mco_v2r_a3 f_store mco_test_a3 ;cosp = v1*a1 + v2*a2 + v3*a3 f_load_array mco_v2r_vptr ,0 ;v1 f_push f_load_l mco_v2r_a1 ;a1 f_mul ;v2*a2 f_push f_load_array mco_v2r_vptr ,1 ;v2 f_push f_load_l mco_v2r_a2 ;a2 f_mul ;v2*a2 f_add ;v1*a1 + v2*a2 f_push f_load_array mco_v2r_vptr ,2 ;v3 f_push f_load_l mco_v2r_a3 ;a3 f_mul ;v3*a3 f_add ;v1*a1 + v2*a2 + v3*a3 f_store_l mco_v2r_cosp f_store mco_test_cosp ;p1 = v1 - cosp * a1 f_load_array mco_v2r_vptr ,0 ;v1 f_push f_load_l mco_v2r_cosp ;cosp f_push f_load_l mco_v2r_a1 ;a1 f_mul ;cosp*a1 f_sub ;v1 - cosp*a1 f_store_l mco_v2r_px f_store mco_test_px ;p2 = v2 - cosp * a2 f_load_array mco_v2r_vptr ,1 ;v2 f_push f_load_l mco_v2r_cosp ;cosp f_push f_load_l mco_v2r_a2 ;a2 f_mul ;cosp*a2 f_sub ;v2 - cosp*a2 f_store_l mco_v2r_py f_store mco_test_py ;cosalt = azx*px + azy*py f_load_l mco_v2r_az_x ;azx f_push f_load_l mco_v2r_px ;px f_mul ;azx*px f_push f_load_l mco_v2r_az_y ;azy f_push f_load_l mco_v2r_py ;py f_mul ;azy*py f_add ;azx*px + azy*py f_store_l mco_v2r_cosalt f_store mco_test_cosalt ;cosalt > 0 >> OK tst AKKU_S ;cos(alt) > 0, alt < 90° brpl mco_v2r_solution_found ;yes, solution found ldd r16 ,Y+mco_v2r_solcnt ;solution loop counter dec r16 std Y+mco_v2r_solcnt ,r16 brne mco_v2r_next_solution ;still a solution to test clt ;return false rjmp mco_v2r_end mco_v2r_next_solution: f_load_l mco_v2r_sqrtd ;sqrtd = - sqrtD f_chs r16 f_store_l mco_v2r_sqrtd rjmp mco_v2r_calc_a1 ;test second solution mco_v2r_solution_found: ;p3 = v3 - cosp * a3 f_load_array mco_v2r_vptr ,2 ;v3 f_push f_load_l mco_v2r_cosp ;cosp f_push f_load_l mco_v2r_a3 ;a3 f_mul ;cosp*a3 f_sub ;v3 - cosp*a3 f_store_l mco_v2r_pz f_store mco_test_pz f_load_l mco_v2r_az_x f_push f_mul f_push f_load_l mco_v2r_az_y f_push f_mul f_add f_sqrt f_store_l mco_v2r_az_len f_store mco_test_az_len ;az = atan (Y/X) f_load_l mco_v2r_az_y ;Y f_push f_load_l mco_v2r_az_x ;X std Y+mco_v2r_signazx ,AKKU_S f_div f_atan ;atan(y/x) ldd r0 ,Y+mco_v2r_signazx ;if x<0 subtract pi (180°) tst r0 brpl mco_v2r_azx_positive f_push f_const flt__pi f_sub mco_v2r_azx_positive: tst AKKU_S brpl mco_v2r_az_pos f_push f_const flt__2pi f_add mco_v2r_az_pos: f_push f_load mco_az_offset ;plus az_offset f_add f_store_array mco_v2r_rptr ,0 ;azimut (radians) f_store mco_test_az_res f_load_array mco_v2r_rptr ,0 ;rotate per -azimut around Z axis f_chs r16 ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) call flt_rotmatrix_z ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) mov r18 ,yh mov r19 ,yl subi r19 ,low(-mco_v2r_px) sbci r18 ,high(-mco_v2r_px) movw r21:r20 ,r19:r18 call flt_mat_vec_mul f_load mco_z1 ;rotate per Z1 around X axis f_chs r16 ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) call flt_rotmatrix_x ldi r16 ,high(flt_mat_buffer) ldi r17 ,low(flt_mat_buffer) mov r18 ,yh mov r19 ,yl subi r19 ,low(-mco_v2r_px) sbci r18 ,high(-mco_v2r_px) movw r21:r20 ,r19:r18 call flt_mat_vec_mul f_load_l mco_v2r_px f_store mco_test_px2 f_load_l mco_v2r_py f_store mco_test_py2 f_load_l mco_v2r_pz f_store mco_test_pz2 f_load_l mco_v2r_pz ;Z f_push f_load_l mco_v2r_px ;X f_div f_atan ;atan(Z/X) f_push f_load mco_alt_offset ;plus alt_offset f_add f_store_array mco_v2r_rptr ,1 ;altitude (radians) f_store mco_test_alt_res set mco_v2r_end: ENDLOCAL mco_v2r_lspace ret