;***************************************************************************************** ; Mega 128 Scope Controller Program ; (c) Martin Cibulski ;**************************************************************************************** ;.db "UMa Alp " .EQU CTEST_AZ_STEPS = 2489263 .EQU CTEST_ALT_STEPS = 530205 .EQU CTEST_RECT_INT = 0x0000 .EQU CTEST_DECL_INT = 0x0000 ;14.57 .EQU CTEST_TIME = 770*128 .DSEG ;current coordintes cor_az_steps: .byte 4 ;steps in az cor_alt_steps: .byte 4 ;steps in alt cor_az_rad: .byte FLT_SIZE cor_alt_rad: .byte FLT_SIZE cor_mntvec: .byte 3 * FLT_SIZE cor_skyvec: .byte 3 * FLT_SIZE cor_rect: .byte FLT_SIZE ;radians cor_decl: .byte FLT_SIZE ;radians cor_time: .byte 4 ;time in IRQs (128 per second) cor_rectint: .byte 2 ;1 step = 0.1 time minute cor_declint: .byte 2 ;1 step = 1.0 arc minute cor_recttext: .byte 8 + 1 ;' 00:00.0' + zero byte cor_decltext: .byte 8 + 1 ;'+00:00 ' + zero byte cor_new_position: .byte 1 ;**************************************************************************************** .CSEG cor_label_tab: .dw cor_calc .db L_XCMD ,"CCALC " .dw cor_az_steps .db L_LRAM ,"CAZSTEPS " .dw cor_alt_steps .db L_LRAM ,"CALTSTEPS " .dw cor_az_rad .db L_FRAM ,"CAZRAD " .dw cor_alt_rad .db L_FRAM ,"CALTRAD " .dw cor_mntvec .db L_FRAM ,"CMNTVEC " .dw cor_skyvec .db L_FRAM ,"CSKYVEC " .dw cor_rect .db L_FRAM ,"CRECT " .dw cor_decl .db L_FRAM ,"CDECL " .dw cor_time .db L_LRAM ,"CTIME " .dw 0 .db L_END , 0 ;**************************************************************************************** cor_init: f_const flt__0 f_store cor_rect f_store cor_decl ldi r16 ,1 sts cor_new_position ,r16 ret ;**************************************************************************************** cor_set_test_data: ldi r16 ,byte4(CTEST_AZ_STEPS) ldi r17 ,byte3(CTEST_AZ_STEPS) ldi r18 ,high(CTEST_AZ_STEPS) ldi r19 ,low(CTEST_AZ_STEPS) sts cor_az_steps ,r16 sts cor_az_steps+1 ,r17 sts cor_az_steps+2 ,r18 sts cor_az_steps+3 ,r19 ldi r16 ,byte4(CTEST_ALT_STEPS) ldi r17 ,byte3(CTEST_ALT_STEPS) ldi r18 ,high(CTEST_ALT_STEPS) ldi r19 ,low(CTEST_ALT_STEPS) sts cor_alt_steps ,r16 sts cor_alt_steps+1 ,r17 sts cor_alt_steps+2 ,r18 sts cor_alt_steps+3 ,r19 ldi r16 ,byte4(CTEST_TIME) ldi r17 ,byte3(CTEST_TIME) ldi r18 ,high(CTEST_TIME) ldi r19 ,low(CTEST_TIME) sts cor_time ,r16 sts cor_time+1 ,r17 sts cor_time+2 ,r18 sts cor_time+3 ,r19 ret ;**************************************************************************************** .CSEG cor__rect2disp: .db 0x40,0xA1,0xE7,0xA9,0x90,0x7E,0x59,0x3B ;24*60*10/(2*PI) = 2291.8311805... cor__decl2disp: .db 0x40,0xAA,0xDB,0x7E,0x58,0xBD,0x85,0xD8 ;360*60/(2*PI) = 3437,7467707... cor_calc: in r16 ,SREG cli lds r0 ,mot_a_pos lds r1 ,mot_a_pos+1 lds r2 ,mot_a_pos+2 lds r3 ,mot_a_pos+3 lds r4 ,mot_b_pos lds r5 ,mot_b_pos+1 lds r6 ,mot_b_pos+2 lds r7 ,mot_b_pos+3 lds r8 ,clock lds r9 ,clock+1 lds r10 ,clock+2 lds r11 ,clock+3 out SREG ,r16 sts cor_az_steps ,r0 sts cor_az_steps+1 ,r1 sts cor_az_steps+2 ,r2 sts cor_az_steps+3 ,r3 sts cor_alt_steps ,r4 sts cor_alt_steps+1 ,r5 sts cor_alt_steps+2 ,r6 sts cor_alt_steps+3 ,r7 sts cor_time ,r8 sts cor_time+1 ,r9 sts cor_time+2 ,r10 sts cor_time+3 ,r11 cor_calc_test: ldi r16 ,high(cor_az_steps) ;convert steps to az/alt ldi r17 ,low(cor_az_steps) ldi r18 ,high(cor_az_rad) ldi r19 ,low(cor_az_rad) call alg_mnt_steps2rad ldi r16 ,high(cor_az_steps) ;convert az/alt to vector ldi r17 ,low(cor_az_steps) ldi r18 ,high(cor_mntvec) ldi r19 ,low(cor_mntvec) call mco_rad2vec lds r16 ,alg_completed tst r16 ;alignment done ? brne cor_calc_aligned ;yes, calculate ra/dec coordinates clr r16 ;no, display 00:00,0 and +00:00 clr r17 ldi xh ,high(cor_recttext) ldi xl ,low(cor_recttext) rcall cor_display_ra clr r16 clr r17 ldi xh ,high(cor_decltext) ldi xl ,low(cor_decltext) rcall cor_display_dec ldi r16 ,TGO_ST_NONE ;no target to track sts tgo_status ,r16 ret cor_calc_aligned: lds r16 ,alg_completed call mot_calc_az_speeds ldi r16 ,high(alg_mat_mnt2sky) ;Calc: ldi r17 ,low(alg_mat_mnt2sky) ;mount position * transformation matrix (mount to sky) ldi r18 ,high(cor_mntvec) ; = sky position 1 ldi r19 ,low(cor_mntvec) ldi r20 ,high(cor_skyvec) ldi r21 ,low(cor_skyvec) call flt_mat_vec_mul ldi r16 ,high(cor_skyvec) ;ptr > X,Y,Z ldi r17 ,low(cor_skyvec) ldi r18 ,high(cor_rect) ;ptr > fRect,fDecl,lTime ldi r19 ,low(cor_rect) rcall cor_vec_to_sky f_load cor_rect f_push f_const cor__rect2disp f_mul call flt_double2word sts cor_rectint ,AKKU_2 sts cor_rectint+1 ,AKKU_3 f_load cor_decl f_push f_const cor__decl2disp f_mul call flt_double2word sts cor_declint ,AKKU_2 sts cor_declint+1 ,AKKU_3 lds r16 ,tgo_status cpi r16 ,TGO_ST_NONE brne cor_calc_goto_in_progress f_load cor_rect f_store tgo_rect f_load cor_decl f_store tgo_decl ldi r16 ,TGO_ST_TRACKING sts tgo_status ,r16 cor_calc_goto_in_progress: lds r16 ,cor_rectint lds r17 ,cor_rectint+1 ldi xh ,high(cor_recttext) ldi xl ,low(cor_recttext) rcall cor_display_ra lds r16 ,cor_declint lds r17 ,cor_declint+1 ldi xh ,high(cor_decltext) ldi xl ,low(cor_decltext) rcall cor_display_dec ret ;*********************************************************************** cor_display_ra: push r18 push r19 push zl push zh movw r19:r18 ,r17:r16 ldi zl ,low(cor_dpr_mask<<1) ldi zh ,high(cor_dpr_mask<<1) ldi r16 ,low(9) ldi r17 ,high(9) call util_read_flash movw r17:r16 ,r19:r18 cpi r17 ,low(24*60*10) ldi r18 ,high(24*60*10) cpc r16 ,r18 brcs cor_dra_no24h subi r17 ,low(24*60*10) sbci r16 ,high(24*60*10) cor_dra_no24h: adiw xh:xl ,1 ldi r18 ,high(10*60*10) ldi r19 ,low(10*60*10) rcall cor_dp_count adiw xh:xl ,1 ldi r18 ,high(1*60*10) ldi r19 ,low(1*60*10) rcall cor_dp_count adiw xh:xl ,2 ldi r18 ,high(10*10) ldi r19 ,low(10*10) rcall cor_dp_count adiw xh:xl ,1 ldi r18 ,high(1*10) ldi r19 ,low(1*10) rcall cor_dp_count adiw xh:xl ,2 ldi r18 ,high(1) ldi r19 ,low(1) ldi r20 ,'0' rcall cor_dp_count pop zh pop zl pop r19 pop r18 ret cor_dpr_mask: .db " 00:00.0" ,0,0 ;*********************************************************************** cor_display_dec: push r18 push r19 push zl push zh movw r19:r18 ,r17:r16 ldi zl ,low(cor_dpd_mask<<1) ldi zh ,high(cor_dpd_mask<<1) ldi r16 ,low(9) ldi r17 ,high(9) call util_read_flash movw r17:r16 ,r19:r18 ldi r20 ,'+' tst r16 brpl cor_dpd_positive com r17 ;make negative com r16 ldi r18 ,0xFF sub r17 ,r18 sbc r16 ,r18 ldi r20 ,'-' cor_dpd_positive: rcall cor_dp_store_digit ldi r18 ,high(10*60) ;10 degrees ldi r19 ,low(10*60) adiw xh:xl ,1 rcall cor_dp_count ldi r18 ,high(1*60) ldi r19 ,low(1*60) adiw xh:xl ,1 rcall cor_dp_count ldi r18 ,high(10) ldi r19 ,low(10) ldi r20 ,'0' adiw xh:xl ,2 rcall cor_dp_count ldi r18 ,high(1) ldi r19 ,low(1) adiw xh:xl ,1 rcall cor_dp_count pop zh pop zl pop r19 pop r18 ret cor_dpd_mask: .db "+00:00 " ,0,0 ;*********************************************************************** cor_dp_count: ldi r20 ,'0' cor_dp_count_01: inc r20 sub r17 ,r19 sbc r16 ,r18 brcc cor_dp_count_01 dec r20 add r17 ,r19 adc r16 ,r18 ;*********************************************************************** cor_dp_store_digit: ld r21 ,X st X ,r20 cp r20 ,r21 breq cor_dp_count_02 ldi r20 ,1 sts cor_new_position ,r20 cor_dp_count_02: ret ;**************************************************************************************** cor_vec_to_sky: .EQU cor_v2s_vptr = 1 .EQU cor_v2s_sptr = 3 .EQU cor_v2s_sign_x = 5 .EQU cor_v2s_lspace = 6 LOCAL cor_v2s_lspace std Y+cor_v2s_vptr ,r16 ;ptr > X,Y,Z std Y+cor_v2s_vptr+1 ,r17 std Y+cor_v2s_sptr ,r18 ;ptr > fRect,fDecl,lTime std Y+cor_v2s_sptr+1 ,r19 f_load_array cor_v2s_vptr ,1 ;Y f_push f_load_array cor_v2s_vptr ,0 ;X std Y+cor_v2s_sign_x ,AKKU_S f_div f_atan ;atan(y/x) ldd r0 ,Y+cor_v2s_sign_x ;if x<0 subtract pi (180°) tst r0 brpl cor_v2s_01 f_push f_const flt__pi f_sub cor_v2s_01: f_push ;add elapsed time to rightascensin ldd xh ,Y+cor_v2s_sptr ldd xl ,Y+cor_v2s_sptr+1 adiw xh:xl ,16 ;lTime in IRQs (128 per second) ld AKKU_2 ,X+ ld AKKU_3 ,X+ ld AKKU_4 ,X+ ld AKKU_5 ,X+ call flt_convert_long ;make double f_push f_const alg__rad_per_irq f_mul ;convert to radians f_add f_push ;> 2*PI, subtract 2*PI f_const flt__2pi f_y_gt_x brts cor_v2s_02 f_pop ;< 0.0, add 2*PI f_x_lt_0 brtc cor_v2s_03 f_push f_const flt__2pi f_add rjmp cor_v2s_03 cor_v2s_02: f_sub cor_v2s_03: f_store_array cor_v2s_sptr ,0 ;rightascension (radians) f_load_array cor_v2s_vptr ,0 ;X f_push f_mul ;X**2 f_push f_load_array cor_v2s_vptr ,1 ;Y f_push f_mul ;Y**2 f_add f_sqrt f_push f_load_array cor_v2s_vptr ,2 ;Z f_xy f_div f_atan ;atan(Z/sqrt(X*X+Y*Y) f_store_array cor_v2s_sptr ,1 ;declination (radians) ENDLOCAL cor_v2s_lspace ret ;****************************************************************************************