📄 gac_algorithm.asm
字号:
; generated by Norcroft eCOG C Compiler V1.10 $BuildRevision$
; special options: -g
;
MODULE GAC_Algorithm___GAC_Algorithm
.CODE
.LARGE
; TARGET_MAX_STACK_OVERRUN = 8
;/******************************************************************************
;MODE: AML--#GPS+accelerator+compass
;MCU: Cyan eCOG1k
;COMPILER: CyanIDE v1.41
;FILE NAME: GAC_Algorithm C source code files
;******************************************************************************/
;
;#define GAC_Algorithm_code
;
;/******************************************************************************
;Project level include files.
;******************************************************************************/
;#include "GAC_Algorithm.h"
;#include "cyan_base_driver.h"
;#include "gps.h"
;#include "accelerator.h"
;#include "compass.h"
;
;
;/******************************************************************************
;Declaration of static functions.
;******************************************************************************/
;void GAC_Algorithm_initialization(void);
;double integral_formula(double data_n_1_1,double data_n,double data_n_1_2);
;void distance_integral_formula(void);
;void LSE_get_start_value(double *In);
;void LSE_get_earth_distance(void);
;
;void matrix( void *ma, void *mb, void *mc );
;
;void Get_ACC_average(void);
;void Get_COM_average(void);
;
;void Calculate_roll_pitch( ACC_type *acc );
;void ACC_data_change( ACC_type *acc );
;void Get_ACC_Earth_loc( ACC_type *acc, double *comp );
;
;void GPS_data_STR_to_RADIAN(char *GPS_lat_str,char *GPS_lon_str);
;void GPS_data_change( GPS_DATA *gps, XYZ_type *buf );
;
;void Acc_data_route( ACC_type *acc, double *comp );
;double ACC_data_HEX_to_FLOAT(int ACC_Sdata);
;
;void ACC_COM_reading(void);
;void GPS_reading(void);
;void ACC_COM_insert_reading(void);
;
;/******************************************************************************
;GAC_Algorithm_initialization
; include all var...
;******************************************************************************/
;void GAC_Algorithm_initialization(void)
;
$GAC_Algorithm_initialization:
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?52.1:
st XH,@(-2,Y)
st X,@(-1,Y)
Lstab?0c:
;{
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?53.1:
; r_p_data_buf.roll = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?54.1:
ld AL,#$r_p_data_buf
st Y,@H'fffe
st AL,@H'ffe1 ; %r0
ld Y,@H'ffe1 ; %r0
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
; r_p_data_buf.pitch = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?55.1:
ld AH,#$r_p_data_buf
add AH,#4
st AH,@H'ffe5 ; %r2
st Y,@H'fffe
ld Y,@H'ffe5 ; %r2
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
; GS_write_pointer = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?56.1:
ld AL,#0
st AL,@$GS_write_pointer
; GS_read_pointer = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?57.1:
st AL,@$GS_read_pointer
;
; An_1.x = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?59.1:
ld AH,#$An_1
st AH,@H'ffe5 ; %r2
st Y,@H'fffe
ld Y,@H'ffe5 ; %r2
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
; An_1.y = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?60.1:
ld AH,#$An_1
add AH,#4
st AH,@H'ffe5 ; %r2
st Y,@H'fffe
ld Y,@H'ffe5 ; %r2
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
;// An_1.z = 0;
; Vn_1.x = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?62.1:
ld AH,#$Vn_1
st AH,@H'ffe5 ; %r2
st Y,@H'fffe
ld Y,@H'ffe5 ; %r2
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
; Vn_1.y = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?63.1:
ld AL,#$Vn_1
add AL,#4
st Y,@H'fffe
st AL,@H'ffe1 ; %r0
ld Y,@H'ffe1 ; %r0
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
;// Vn_1.z = 0;
; Sn_1.x = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?65.1:
ld AH,#$Sn_1
st AH,@H'ffe5 ; %r2
st Y,@H'fffe
ld Y,@H'ffe5 ; %r2
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
; Sn_1.y = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?66.1:
ld AL,#$Sn_1
add AL,#4
st Y,@H'fffe
st AL,@H'ffe1 ; %r0
ld Y,@H'ffe1 ; %r0
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
; //Sn_1.z = 0;
;
; X_distance_accumulator = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?69.1:
st Y,@H'fffe
ld Y,#$X_distance_accumulator
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
;
; Y_distance_accumulator = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?71.1:
st Y,@H'fffe
ld Y,#$Y_distance_accumulator
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld Y,@H'fffe
ld AH,#0
st AH,@H'fffe
;
; AS_write_pointer = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?73.1:
ld AL,#0
st AL,@$AS_write_pointer
; AS_read_pointer = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?74.1:
st AL,@$AS_read_pointer
;
; //gps_o.z = 0;
;
; ACC_delay_write_pointer = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?78.1:
st AL,@$ACC_delay_write_pointer
; ACC_delay_read_pointer = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?79.1:
st AL,@$ACC_delay_read_pointer
;
;
; ACC_COM_average_read_pointer = 0;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?82.1:
st AL,@$ACC_COM_average_read_pointer
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?84.1:
ld XH,@(-2,Y)
bra @(-1,Y)
Lstab?1c:
;
;
;}
;
;/******************************************************************************
;Integral formula :
; Vxy[n] = Vxy[n-1] + 1/2 * (Axy[n-1] + Axy[n]) * Ts
; Ts = 40ms = 0.04s, n corresponding data buffer.
; 1/2 * Ts = 0.02s.
;******************************************************************************/
; #define _1_2TS 0.02 // 1/2 *TS = 0.02;
;double integral_formula(double data_n_1_1,double data_n,double data_n_1_2)
;
$integral_formula:
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?93.1:
st AL,@(-2,Y)
st AH,@(-1,Y)
st XH,@(-4,Y)
st X,@(-3,Y)
st AL,@(-5,Y) ; %r16 (__struct_result)
Lstab?2c:
;{
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?94.1:
; //float temp;
; //temp = data_n_1_1 + (float)_1_2TS * (data_n + data_n_1_2);
; return(data_n_1_1 + (double)_1_2TS * (data_n + data_n_1_2));
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?97.1:
add Y,#-26
Lstab?3a:
mov AL,Y
add AL,#33
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
mov AL,Y
add AL,#29
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(-1,Y)
ld AH,@(2,X)
st AH,@(-2,Y)
ld AH,@(1,X)
st AH,@(-3,Y)
ld AH,@(0,X)
st AH,@(-4,Y)
mov AL,Y
add AL,#12
ld AH,@(-4,Y)
add Y,#-3
Lstab?4a:
bsr $__dadd
mov AH,Y
add AH,#15
st AH,@H'ffe5 ; %r2
ld X,@H'ffe5 ; %r2
ld AH,@(3,X)
st AH,@(10,Y)
ld AH,@(2,X)
st AH,@(9,Y)
ld AH,@(1,X)
st AH,@(8,Y)
ld AH,@(0,X)
st AH,@(7,Y)
ld X,#?lit0002
ld AH,@(3,X)
st AH,@(6,Y)
ld AH,@(2,X)
st AH,@(5,Y)
ld AH,@(1,X)
st AH,@(4,Y)
ld AH,@(0,X)
st AH,@(3,Y)
mov AL,Y
add AL,#19
ld AH,@(3,Y)
add Y,#4
Lstab?5a:
bsr $__dmul
mov AL,Y
add AL,#15
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(10,Y)
ld AH,@(2,X)
st AH,@(9,Y)
ld AH,@(1,X)
st AH,@(8,Y)
ld AH,@(0,X)
st AH,@(7,Y)
mov AL,Y
add AL,#24
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(6,Y)
ld AH,@(2,X)
st AH,@(5,Y)
ld AH,@(1,X)
st AH,@(4,Y)
ld AH,@(0,X)
st AH,@(3,Y)
ld AL,@(20,Y) ; %r16 (__struct_result)
ld AH,@(3,Y)
add Y,#4
Lstab?6a:
bsr $__dadd
add Y,#21
ld XH,@(-4,Y)
bra @(-3,Y)
Lstab?7c:
;
;}
;
;/******************************************************************************
;Request X_axis distance formula :
; S[n] = S[n-1] + 1/2 * (V[n-1] + V[n]) * Ts
; Ts = 20ms = 0.02s, n corresponding data buffer.
; 1/2 * Ts = 0.01s.
; S0 = 0
;******************************************************************************/
;void distance_integral_formula(void)
;
$distance_integral_formula:
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?107.1:
st XH,@(-2,Y)
st X,@(-1,Y)
Lstab?8c:
;{
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?108.1:
Lstab?9c:
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?109.1:
ld X,#?lit0001
ld AH,@(3,X)
st AH,@(-3,Y) ; Vrn+3
ld AH,@(2,X)
st AH,@(-4,Y) ; Vrn+2
ld AH,@(1,X)
st AH,@(-5,Y) ; Vrn+1
ld AH,@(0,X)
st AH,@(-6,Y) ; Vrn
; double Vrn = 0;
;
;
; if(MOVE_flag)
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?112.1:
add Y,#-6
Lstab?10a:
ld AL,@$Algorithm_flag
beq ?L4F3
; {
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?113.1:
; Vn.x = integral_formula(Vn_1.x,ACC_data_buf.x,An_1.x);
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?114.1:
ld AL,#$An_1
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(-1,Y)
ld AH,@(2,X)
st AH,@(-2,Y)
ld AH,@(1,X)
st AH,@(-3,Y)
ld AH,@(0,X)
st AH,@(-4,Y)
ld AL,#$ACC_data_buf
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(-5,Y)
ld AH,@(2,X)
st AH,@(-6,Y)
ld AH,@(1,X)
st AH,@(-7,Y)
ld AH,@(0,X)
st AH,@(-8,Y)
add Y,#-12
Lstab?11a:
ld AH,#$Vn_1
st AH,@H'ffe5 ; %r2
ld X,@H'ffe5 ; %r2
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld AL,#$Vn
ld AH,@(0,Y)
add Y,#1
Lstab?12a:
bsr $integral_formula
; Vn.y = integral_formula(Vn_1.y,ACC_data_buf.y,An_1.y);
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?115.1:
ld AL,#$An_1
add AL,#4
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(10,Y)
ld AH,@(2,X)
st AH,@(9,Y)
ld AH,@(1,X)
st AH,@(8,Y)
ld AH,@(0,X)
st AH,@(7,Y)
ld AL,#$ACC_data_buf
add AL,#4
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(6,Y)
ld AH,@(2,X)
st AH,@(5,Y)
ld AH,@(1,X)
st AH,@(4,Y)
ld AH,@(0,X)
st AH,@(3,Y)
ld AH,#$Vn_1
add AH,#4
st AH,@H'ffe5 ; %r2
ld X,@H'ffe5 ; %r2
ld AH,@(3,X)
st AH,@(2,Y)
ld AH,@(2,X)
st AH,@(1,Y)
ld AH,@(1,X)
st AH,@(0,Y)
ld AH,@(0,X)
st AH,@(-1,Y)
ld AL,#$Vn
add AL,#4
ld AH,@(-1,Y)
bsr $integral_formula
add Y,#11
Lstab?13a:
bra ?L8F3
?L4F3:
; }
; else
; {
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?118.1:
; Vn = Vn_1;
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?119.1:
st Y,@H'fffe
ld Y,#$Vn
ld X,#$Vn_1
ld AL,#8
bc
ld Y,@H'fffe
st AL,@H'fffe
?L8F3:
; //Vn.x = 0;
; //Vn.y = 0;
; }
;
; Vrn = sqrt(Vn.x * Vn.x + Vn.y * Vn.y);
^GAC_Algorithm___GAC_Algorithm.'D:\GPS\software\GPS_A_C\GAC_Algorithm.c'.?124.1:
add Y,#-24
Lstab?14a:
ld AH,#$Vn
add AH,#4
st AH,@H'ffe5 ; %r2
ld X,@H'ffe5 ; %r2
ld AH,@(3,X)
st AH,@(3,Y)
ld AH,@(2,X)
st AH,@(2,Y)
ld AH,@(1,X)
st AH,@(1,Y)
ld AH,@(0,X)
st AH,@(0,Y)
ld AL,#$Vn
add AL,#4
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(-1,Y)
ld AH,@(2,X)
st AH,@(-2,Y)
ld AH,@(1,X)
st AH,@(-3,Y)
ld AH,@(0,X)
st AH,@(-4,Y)
mov AL,Y
add AL,#16
ld AH,@(-4,Y)
add Y,#-3
Lstab?15a:
bsr $__dmul
mov AL,Y
add AL,#19
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(10,Y)
ld AH,@(2,X)
st AH,@(9,Y)
ld AH,@(1,X)
st AH,@(8,Y)
ld AH,@(0,X)
st AH,@(7,Y)
ld AH,#$Vn
st AH,@H'ffe5 ; %r2
ld X,@H'ffe5 ; %r2
ld AH,@(3,X)
st AH,@(2,Y)
ld AH,@(2,X)
st AH,@(1,Y)
ld AH,@(1,X)
st AH,@(0,Y)
ld AH,@(0,X)
st AH,@(-1,Y)
ld AL,#$Vn
st AL,@H'ffe1 ; %r0
ld X,@H'ffe1 ; %r0
ld AH,@(3,X)
st AH,@(-2,Y)
ld AH,@(2,X)
st AH,@(-3,Y)
ld AH,@(1,X)
st AH,@(-4,Y)
ld AH,@(0,X)
st AH,@(-5,Y)
mov AL,Y
add AL,#15
ld AH,@(-5,Y)
add Y,#-4
Lstab?16a:
bsr $__dmul
mov AH,Y
add AH,#19
st AH,@H'ffe5 ; %r2
ld X,@H'ffe5 ; %r2
ld AH,@(3,X)
st AH,@(10,Y)
ld AH,@(2,X)
st AH,@(9,Y)
ld AH,@(1,X)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -