;***************************************************************************************** ; Mega 128 Scope Controller Program ; (c) Martin Cibulski ;**************************************************************************************** .EQU TGO_ST_NONE = 0b00000000 ;no tracking/goto .EQU TGO_ST_NEW_TARGET = 0b00000001 ;integer coordinates from object DB .EQU TGO_ST_TRACKING = 0b00000010 ;float coordinates from current position .EQU TGO_SECONDS_AHEAD = 1 .DSEG tgo_counter: .byte 4 ;loop counter tgo_status: .byte 1 tgo_az_steps: .byte 4 ;steps in az tgo_alt_steps: .byte 4 ;steps in alt tgo_rectint: .byte 2 ;1 step = 0.1 time minute tgo_declint: .byte 2 ;1 step = 1.0 arc minute tgo_rect: .byte 8 ;radians tgo_decl: .byte 8 ;radians tgo_time: .byte 4 ;time in IRQs (128 per second) tgo_skyvec: tgo_skyvec_x: .byte 8 tgo_skyvec_y: .byte 8 tgo_skyvec_z: .byte 8 tgo_mntvec: tgo_mntvec_x: .byte 8 tgo_mntvec_y: .byte 8 tgo_mntvec_z: .byte 8 tgo_az: .byte 8 ;az (radians) tgo_alt: .byte 8 ;alt (radians) ;**************************************************************************************** .CSEG tgo_label_tab: .dw tgo_calc .db L_XCMD ,"TCALC " .dw tgo_counter .db L_LRAM ,"TCOUNTER " .dw tgo_status .db L_BRAM ,"TSTATUS " .dw tgo_az .db L_FRAM ,"TAZ " .dw tgo_alt .db L_FRAM ,"TALT " .dw tgo_az_steps .db L_LRAM ,"TAZSTEPS " .dw tgo_alt_steps .db L_LRAM ,"TALTSTEPS " .dw tgo_mntvec_x .db L_FRAM ,"TMNTVEC " .dw tgo_skyvec_x .db L_FRAM ,"TSKYVEC " .dw tgo_rectint .db L_WRAM ,"TRECTINT " .dw tgo_declint .db L_WRAM ,"TDECLINT " .dw tgo_rect .db L_FRAM ,"TRECT " .dw tgo_decl .db L_FRAM ,"TDECL " .dw tgo_time .db L_LRAM ,"TTIME " .dw 0 .db L_END , 0 ;**************************************************************************************** tgo_init: ldi r16 ,TGO_ST_NONE sts tgo_status ,r16 rjmp tgo_calc ;**************************************************************************************** tgo_test: ret ;**************************************************************************************** tgo_calc: .EQU tgo_cal_spfacta = 1 .EQU tgo_cal_spfactb = 9 .EQU tgo_cal_spa = 17 .EQU tgo_cal_spb = 25 .EQU tgo_cal_aztogo = 33 .EQU tgo_cal_lspace = 41 LOCAL tgo_cal_lspace lds r16 ,tgo_status cpi r16 ,TGO_ST_NONE brne tgo_check_int_target ;tgo_calc_no_target: ldi r16 ,0 ;tracking speed = 0 sts mot_a_gssp ,r16 sts mot_a_gssp+1 ,r16 sts mot_a_gssp+2 ,r16 sts mot_a_gssp+3 ,r16 sts mot_b_gssp ,r16 sts mot_b_gssp+1 ,r16 sts mot_b_gssp+2 ,r16 sts mot_b_gssp+3 ,r16 rjmp tgo_calc_end ;**************************************************************************************** ;Target defined from database (integer coordinates) ? tgo_check_int_target: lds r16 ,tgo_status ;goto was commanded ? cpi r16 ,TGO_ST_NEW_TARGET ;(integer coordinates from database) brne tgo_check_flt_target clr AKKU_2 ;convert RA into radians clr AKKU_3 ;and set as new target lds AKKU_4 ,tgo_rectint lds AKKU_5 ,tgo_rectint+1 call flt_convert_long f_push f_const alg__db2rect f_mul f_store tgo_rect lds AKKU_2 ,tgo_declint ;convert DEC into radians lds AKKU_3 ,tgo_declint+1 ;and set as new target call flt_convert_word f_push f_const alg__db2decl f_mul f_store tgo_decl ldi r16 ,TGO_ST_TRACKING sts tgo_status ,r16 ;**************************************************************************************** ;Target defined from current position (RA/DEC in radians) ? tgo_check_flt_target: lds r16 ,tgo_status ;target coordinates set ? cpi r16 ,TGO_ST_TRACKING breq tgo_calc_tracking ;yes, calculate tracking speeds rjmp tgo_calc_end ;nothing to do tgo_calc_tracking: lds r16 ,alg_newpos3 ;position for adjustment ? tst r16 brne tgo_cal_adjust ;yes rjmp tgo_cal_no_adjust ;no, normal tracking calculation tgo_cal_adjust: f_const flt__0 f_store mco_az_offset f_store mco_alt_offset lds r2 ,alg_time3 ;time of position 3 lds r3 ,alg_time3+1 lds r4 ,alg_time3+2 lds r5 ,alg_time3+3 sts tgo_time ,r2 sts tgo_time+1 ,r3 sts tgo_time+2 ,r4 sts tgo_time+3 ,r5 clr AKKU_2 ;RA of position 3 clr AKKU_3 lds AKKU_4 ,alg_rectint3 lds AKKU_5 ,alg_rectint3+1 call flt_convert_long f_push f_const alg__db2rect f_mul f_store tgo_rect lds AKKU_2 ,alg_declint3 ;DEC of position 3 lds AKKU_3 ,alg_declint3+1 call flt_convert_word f_push f_const alg__db2decl f_mul f_store tgo_decl ldi r16 ,high(alg_az_steps3) ldi r17 ,low(alg_az_steps3) ldi r18 ,high(alg_az_rad3) ldi r19 ,low(alg_az_rad3) call alg_mnt_steps2rad rjmp tgo_cal_time_set tgo_cal_no_adjust: cli lds r2 ,cor_time ;current time lds r3 ,cor_time+1 lds r4 ,cor_time+2 lds r5 ,cor_time+3 sei ldi r16 ,high(TGO_SECONDS_AHEAD*128) ;add one second ldi r17 ,low (TGO_SECONDS_AHEAD*128) clr r0 add r5 ,r17 adc r4 ,r16 adc r3 ,r0 adc r2 ,r0 sts tgo_time ,r2 ;set as target time sts tgo_time+1 ,r3 sts tgo_time+2 ,r4 sts tgo_time+3 ,r5 tgo_cal_time_set: ldi r16 ,high(tgo_rect) ;calc target sky vector ldi r17 ,low(tgo_rect) ldi r18 ,high(tgo_skyvec) ldi r19 ,low(tgo_skyvec) call alg_sky_to_vec ldi r16 ,high(alg_mat_sky2mnt) ;Calc: ldi r17 ,low(alg_mat_sky2mnt) ;sky vector * transformation matrix (mount to sky) ldi r18 ,high(tgo_skyvec) ; = mount vector ldi r19 ,low(tgo_skyvec) ldi r20 ,high(tgo_mntvec) ldi r21 ,low(tgo_mntvec) call flt_mat_vec_mul ldi r16 ,high(tgo_mntvec) ;Convert ldi r17 ,low(tgo_mntvec) ;mount coordinate vector > apparent alt,az ldi r18 ,high(tgo_az) ldi r19 ,low(tgo_az) call mco_vec2rad ;**************************************************************************************** lds r16 ,alg_newpos3 ;position for adjustment ? tst r16 breq tgo_cal_normal ;no, continue calculation f_load alg_az_rad3 ;correct az position f_push f_load tgo_az ;calculated az position f_sub f_store mco_az_offset ;new az correction f_load alg_alt_rad3 ;correct alt position f_push f_load tgo_alt ;calculated alt position f_sub f_store mco_alt_offset ;new alt correction clr r16 sts alg_newpos3 ,r16 rjmp tgo_calc_end ;**************************************************************************************** tgo_cal_normal: f_load tgo_az ;Calc tracking way to go f_push f_load cor_az_rad f_sub f_store_l tgo_cal_aztogo f_push f_const flt__pi f_sub tst AKKU_S brmi tgo_cal_lt_180 f_load_l tgo_cal_aztogo f_push f_const flt__2pi f_sub f_store_l tgo_cal_aztogo rjmp tgo_cal_ge_m180 tgo_cal_lt_180: f_push f_const flt__2pi f_add tst AKKU_S brpl tgo_cal_ge_m180 f_load_l tgo_cal_aztogo f_push f_const flt__2pi f_add f_store_l tgo_cal_aztogo tgo_cal_ge_m180: f_load_l tgo_cal_aztogo f_push f_load mco_az_per_step f_div ldi r16 ,16 ;convert to nanosteps add AKKU_E2 ,r16 clr r16 adc AKKU_E1 ,r16 f_push f_const a39__irq_per_sec f_div f_store_l tgo_cal_spa ;**************************************************************************************** f_load tgo_alt f_push f_load mco_alt_start f_sub f_push f_load mco_z3 f_sub f_push f_load mco_alt_per_step f_div f_push lds AKKU_2 ,cor_alt_steps lds AKKU_3 ,cor_alt_steps+1 lds AKKU_4 ,cor_alt_steps+2 lds AKKU_5 ,cor_alt_steps+3 call flt_convert_long f_sub ldi r16 ,16 ;convert to nanosteps add AKKU_E2 ,r16 clr r16 adc AKKU_E1 ,r16 f_push f_const a39__irq_per_sec f_div f_store_l tgo_cal_spb ;**************************************************************************************** ;calculate speed limiting factors to ensure that both motors don't run abowe ;their maximum allowed speed lds AKKU_2 ,mot_a_ssp5 ;max speed of motor A lds AKKU_3 ,mot_a_ssp5+1 lds AKKU_4 ,mot_a_ssp5+2 lds AKKU_5 ,mot_a_ssp5+3 call flt_convert_long f_push f_load_l tgo_cal_spa ;calculated goto speed clr AKKU_S f_div ;limiting factor f_push f_const flt__1 ;maximum allowed factor 1.0 f_x_lt_y ;1.0 < factorA brts tgo_calc_01 ;yes, keep 1.0 as smaller factor call flt_xy ;no, take smaller factor tgo_calc_01: f_store_l tgo_cal_spfacta f_pop ;remove Y from stack lds AKKU_2 ,mot_b_ssp5 ;maximum speed of motor B lds AKKU_3 ,mot_b_ssp5+1 lds AKKU_4 ,mot_b_ssp5+2 lds AKKU_5 ,mot_b_ssp5+3 call flt_convert_long f_push f_load_l tgo_cal_spb ;calculated goto speed clr AKKU_S f_div ;limiting factor f_push f_const flt__1 ;maximum allowed factor 1.0 f_x_lt_y ;1.0 < min(factorA, factorB) brts tgo_calc_02 ;yes, keep 1.0 as smaller factor call flt_xy ;no, take smaller factor tgo_calc_02: f_store_l tgo_cal_spfactb f_pop ;remove Y from stack ;**************************************************************************************** ;multiply the theoretical tracking/goto speed values by the limiting factor ;and set the results as tracking/goto speed f_load_l tgo_cal_spa f_push f_load_l tgo_cal_spfacta f_mul call flt_double2long cli sts mot_a_gssp ,AKKU_2 sts mot_a_gssp+1 ,AKKU_3 sts mot_a_gssp+2 ,AKKU_4 sts mot_a_gssp+3 ,AKKU_5 sei f_load_l tgo_cal_spb f_push f_load_l tgo_cal_spfactb f_mul call flt_double2long cli sts mot_b_gssp ,AKKU_2 sts mot_b_gssp+1 ,AKKU_3 sts mot_b_gssp+2 ,AKKU_4 sts mot_b_gssp+3 ,AKKU_5 sei cli sec clr r17 lds r16 ,tgo_counter+3 adc r16 ,r17 sts tgo_counter+3 ,r16 lds r16 ,tgo_counter+2 adc r16 ,r17 sts tgo_counter+2 ,r16 lds r16 ,tgo_counter+1 adc r16 ,r17 sts tgo_counter+1 ,r16 lds r16 ,tgo_counter adc r16 ,r17 sts tgo_counter ,r16 sei cli lds r16 ,mot_hsp_notzero dec r16 brmi tgo_calc_03 sts mot_hsp_notzero ,r16 ldi r16 ,TGO_ST_NONE sts tgo_status ,r16 tgo_calc_03: sei tgo_calc_end: ENDLOCAL tgo_cal_lspace ret ;****************************************************************************************