CCS PCH C Compiler, Version 4.039u, 37276 20-Aug-07 21:35 Filename: C:\Program Files\PICC\Projects\SWATH\main.lst ROM used: 27418 bytes (84%) Largest free fragment is 5346 RAM used: 978 (64%) at main() level 1202 (79%) worst case Stack: 8 worst case (5 in main + 3 for interrupts) * 0000: GOTO 6816 * 0008: MOVWF 05 000A: MOVFF FD8,06 000E: MOVFF FE0,07 0012: MOVLB 0 0014: MOVFF FE9,0D 0018: MOVFF FEA,08 001C: MOVFF FE1,09 0020: MOVFF FE2,0A 0024: MOVFF FD9,0B 0028: MOVFF FDA,0C 002C: MOVFF FF3,14 0030: MOVFF FF4,15 0034: MOVFF FFA,16 0038: MOVFF 00,0F 003C: MOVFF 01,10 0040: MOVFF 02,11 0044: MOVFF 03,12 0048: MOVFF 04,13 004C: BTFSS F9D.5 004E: GOTO 0058 0052: BTFSC F9E.5 0054: GOTO 06B4 0058: MOVFF 0F,00 005C: MOVFF 10,01 0060: MOVFF 11,02 0064: MOVFF 12,03 0068: MOVFF 13,04 006C: BSF 0E.7 006E: MOVFF 0D,FE9 0072: MOVFF 08,FEA 0076: MOVFF 09,FE1 007A: MOVFF 0A,FE2 007E: MOVFF 0B,FD9 0082: MOVFF 0C,FDA 0086: MOVFF 14,FF3 008A: MOVFF 15,FF4 008E: MOVFF 16,FFA 0092: MOVF 05,W 0094: MOVFF 07,FE0 0098: MOVFF 06,FD8 009C: RETFIE 0 .................... #include "main.h" .................... #include <18F4520.h> .................... //////// Standard Header file for the PIC18F4520 device //////////////// .................... #device PIC18F4520 .................... #list .................... .................... #device adc=8 .................... #device icd=true //8bit ADC .................... .................... #FUSES WDT128 //Watch Dog Timer uses 1:128 Postscale .................... #FUSES STVREN //Stack full/underflow will cause reset .................... #FUSES BROWNOUT //Reset when brownout detected .................... #FUSES BORV25 //Brownout reset at 2.5V .................... #FUSES HS //High speed Osc (> 4mhz) .................... #FUSES NOCPD //No EE protection .................... #FUSES STVREN //Stack full/underflow will cause reset .................... #FUSES MCLR //Master Clear pin enabled .................... .................... #use delay(clock=20000000,RESTART_WDT) //20MHZ CLOCK * 073E: MOVLW 03 0740: MOVWF FEA 0742: MOVLW D2 0744: MOVWF FE9 0746: MOVF FEF,W 0748: BZ 076C 074A: MOVLW 06 074C: MOVWF 01 074E: MOVLW BF 0750: MOVWF 00 0752: CLRWDT 0754: DECFSZ 00,F 0756: BRA 0752 0758: DECFSZ 01,F 075A: BRA 074E 075C: MOVLW 7A 075E: MOVWF 00 0760: DECFSZ 00,F 0762: BRA 0760 0764: BRA 0766 0766: CLRWDT 0768: DECFSZ FEF,F 076A: BRA 074A 076C: GOTO 6AD0 (RETURN) .................... #use rs232(baud=19200,parity=N,xmit=PIN_C1,rcv=PIN_C0,bits=8,stream=BASESTREAM,ERRORS,DISABLE_INTS) * 05BA: MOVFF FF2,4B0 05BE: BCF FF2.7 05C0: BCF F94.1 05C2: BCF F8B.1 05C4: MOVLW 08 05C6: MOVWF 01 05C8: BRA 05CA 05CA: NOP 05CC: BSF 01.7 05CE: BRA 05F0 05D0: BCF 01.7 05D2: MOVLB 4 05D4: RRCF xAF,F 05D6: MOVLB 0 05D8: BTFSC FD8.0 05DA: BSF F8B.1 05DC: BTFSS FD8.0 05DE: BCF F8B.1 05E0: BSF 01.6 05E2: BRA 05F0 05E4: BCF 01.6 05E6: DECFSZ 01,F 05E8: BRA 05D2 05EA: BRA 05EC 05EC: NOP 05EE: BSF F8B.1 05F0: MOVLW 50 05F2: MOVWF FE9 05F4: DECFSZ FE9,F 05F6: BRA 05F4 05F8: BRA 05FA 05FA: BTFSC 01.7 05FC: BRA 05D0 05FE: BTFSC 01.6 0600: BRA 05E4 0602: MOVLB 4 0604: BTFSC xB0.7 0606: BSF FF2.7 0608: MOVLB 0 060A: RETLW 00 * 08C6: BSF F94.0 08C8: BTFSC F82.0 08CA: BRA 08C8 08CC: MOVLW 08 08CE: MOVWF 00 08D0: MOVLB 3 08D2: CLRF xD8 08D4: MOVFF FF2,3D9 08D8: BCF FF2.7 08DA: BSF 00.7 08DC: BRA 08FA 08DE: BCF 00.7 08E0: BRA 08FA 08E2: BCF FD8.0 08E4: BTFSC F82.0 08E6: BSF FD8.0 08E8: RRCF xD8,F 08EA: BSF 00.6 08EC: BRA 08FA 08EE: BCF 00.6 08F0: DECFSZ 00,F 08F2: BRA 08E2 08F4: MOVFF 3D8,01 08F8: BRA 0912 08FA: MOVLW 50 08FC: BTFSC 00.7 08FE: MOVLW 15 0900: MOVWF 01 0902: DECFSZ 01,F 0904: BRA 0902 0906: NOP 0908: BTFSC 00.7 090A: BRA 08DE 090C: BTFSC 00.6 090E: BRA 08EE 0910: BRA 08E2 0912: BTFSC xD9.7 0914: BSF FF2.7 0916: MOVLB 0 0918: GOTO 095E (RETURN) .................... .................... #use i2c(Master,Fast,sda=PIN_C4,scl=PIN_C3) .................... .................... .................... #include .................... /////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2007 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef _STDLIB .................... #define _STDLIB .................... .................... //--------------------------------------------------------------------------- .................... // Definitions and types .................... //--------------------------------------------------------------------------- .................... .................... #ifndef RAND_MAX .................... #define RAND_MAX 32767 // The value of which is the maximum value .................... // ... returned by the rand function .................... #endif .................... .................... #IF (sizeof(int16*)>1) .................... #DEFINE LONG_POINTERS 1 .................... #ELSE .................... #DEFINE LONG_POINTERS 0 .................... #ENDIF .................... .................... .................... typedef struct { .................... signed int quot; .................... signed int rem; .................... } div_t; .................... .................... typedef struct { .................... signed long quot; .................... signed long rem; .................... } ldiv_t; .................... .................... #include .................... /////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef _STDDEF .................... .................... #define _STDDEF .................... .................... #if sizeof(int *)==1 .................... #define ptrdiff_t int .................... #else .................... #define ptrdiff_t long .................... #endif .................... .................... #define size_t int .................... #define wchar_t char .................... #define NULL 0 .................... .................... #define offsetof(s,f) (offsetofbit(s,f)/8) .................... .................... #endif .................... .................... .................... //--------------------------------------------------------------------------- .................... // String conversion functions .................... //--------------------------------------------------------------------------- .................... .................... /* Standard template: float atof(char * s) .................... * converts the initial portion of the string s to a float. .................... * returns the converted value if any, 0 otherwise .................... */ .................... float atof(char * s); .................... .................... /* Standard template: float atoe(char * s) .................... * converts the initial portion of the string s to a float. .................... * returns the converted value if any, 0 otherwise .................... * also handles E format numbers .................... */ .................... float atoe(char * s); .................... .................... /* Standard template: signed int atoi(char * s) .................... * converts the initial portion of the string s to a signed int .................... * returns the converted value if any, 0 otherwise .................... */ .................... signed int atoi(char *s); .................... .................... /* Syntax: signed int32 atoi32(char * s) .................... converts the initial portion of the string s to a signed int32 .................... returns the converted value if any, 0 otherwise*/ .................... signed int32 atoi32(char *s); .................... .................... /* Syntax: char * itoa(signed int32 num, int8 base, char * s) .................... converts the signed int32 to a string and .................... returns the converted value if any, 0 otherwise*/ .................... char * itoa(signed int32 num, int8 base, char * s); .................... .................... /* Standard template: signed long atol(char * s) .................... * converts the initial portion of the string s to a signed long .................... * returns the converted value if any, 0 otherwise .................... */ .................... signed long atol(char *s); .................... .................... /* Standard template: float strtol(char * s,char *endptr) .................... * converts the initial portion of the string s to a float .................... * returns the converted value if any, 0 otherwise .................... * the final string is returned in the endptr, if endptr is not null .................... */ .................... float strtod(char *s,char *endptr); .................... .................... /* Standard template: long strtoul(char * s,char *endptr,signed int base) .................... * converts the initial portion of the string s, represented as an .................... * integral value of radix base to a signed long. .................... * Returns the converted value if any, 0 otherwise .................... * the final string is returned in the endptr, if endptr is not null .................... */ .................... signed long strtol(char *s,char *endptr,signed int base); .................... .................... /* Standard template: long strtoul(char * s,char *endptr,signed int base) .................... * converts the initial portion of the string s, represented as an .................... * integral value of radix base to a unsigned long. .................... * returns the converted value if any, 0 otherwise .................... * the final string is returned in the endptr, if endptr is not null .................... */ .................... long strtoul(char *s,char *endptr,signed int base); .................... .................... //--------------------------------------------------------------------------- .................... // Pseudo-random sequence generation functions .................... //--------------------------------------------------------------------------- .................... .................... /* The rand function computes a sequence of pseudo-random integers in .................... * the range 0 to RAND_MAX .................... * .................... * Parameters: .................... * (none) .................... * .................... * Returns: .................... * The pseudo-random integer .................... */ .................... long rand(void); .................... .................... /* The srand function uses the argument as a seed for a new sequence of .................... * pseudo-random numbers to be returned by subsequent calls to rand. .................... * .................... * Parameters: .................... * [in] seed: The seed value to start from. You might need to pass .................... * .................... * Returns: .................... * (none) .................... * .................... * Remarks .................... * The srand function sets the starting point for generating .................... * a series of pseudorandom integers. To reinitialize the .................... * generator, use 1 as the seed argument. Any other value for .................... * seed sets the generator to a random starting point. rand .................... * retrieves the pseudorandom numbers that are generated. .................... * Calling rand before any call to srand generates the same .................... * sequence as calling srand with seed passed as 1. .................... * Usually, you need to pass a time here from outer source .................... * so that the numbers will be different every time you run. .................... */ .................... void srand(unsigned int32 seed); .................... .................... //--------------------------------------------------------------------------- .................... // Memory management functions .................... //--------------------------------------------------------------------------- .................... .................... // Comming soon .................... .................... //--------------------------------------------------------------------------- .................... // Communication with the environment .................... //--------------------------------------------------------------------------- .................... .................... /* The function returns 0 always .................... */ .................... signed int system(char *string); .................... .................... //--------------------------------------------------------------------------- .................... // Searching and sorting utilities .................... //--------------------------------------------------------------------------- .................... .................... /* Performs a binary search of a sorted array.. .................... * .................... * Parameters: .................... * [in] key: Object to search for .................... * [in] base: Pointer to base of search data .................... * [in] num: Number of elements .................... * [in] width: Width of elements .................... * [in] compare: Function that compares two elements .................... * .................... * Returns: .................... * bsearch returns a pointer to an occurrence of key in the array pointed .................... * to by base. If key is not found, the function returns NULL. If the .................... * array is not in order or contains duplicate records with identical keys, .................... * the result is unpredictable. .................... */ .................... //void *bsearch(const void *key, const void *base, size_t num, size_t width, .................... // int (*compare)(const void *, const void *)); .................... .................... /* Performs the shell-metzner sort (not the quick sort algorithm). The contents .................... * of the array are sorted into ascending order according to a comparison .................... * function pointed to by compar. .................... * .................... * Parameters: .................... * [in] base: Pointer to base of search data .................... * [in] num: Number of elements .................... * [in] width: Width of elements .................... * [in] compare: Function that compares two elements .................... * .................... * Returns: .................... * (none) .................... */ .................... //void *qsort(const void *base, size_t num, size_t width, .................... // int (*compare)(const void *, const void *)); .................... .................... //--------------------------------------------------------------------------- .................... // Integer arithmetic functions .................... //--------------------------------------------------------------------------- .................... .................... #define labs abs .................... .................... div_t div(signed int numer,signed int denom); .................... ldiv_t ldiv(signed long numer,signed long denom); .................... .................... //--------------------------------------------------------------------------- .................... // Multibyte character functions .................... //--------------------------------------------------------------------------- .................... .................... // Not supported .................... .................... //--------------------------------------------------------------------------- .................... // Multibyte string functions .................... //--------------------------------------------------------------------------- .................... .................... // Not supported .................... .................... .................... //--------------------------------------------------------------------------- .................... // Internal implementation .................... //--------------------------------------------------------------------------- .................... .................... #include .................... /////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef _STDDEF .................... .................... #define _STDDEF .................... .................... #if sizeof(int *)==1 .................... #define ptrdiff_t int .................... #else .................... #define ptrdiff_t long .................... #endif .................... .................... #define size_t int .................... #define wchar_t char .................... #define NULL 0 .................... .................... #define offsetof(s,f) (offsetofbit(s,f)/8) .................... .................... #endif .................... .................... #include .................... //////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... //////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef _STRING .................... #define _STRING .................... #include .................... /////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef _STDDEF .................... .................... #define _STDDEF .................... .................... #if sizeof(int *)==1 .................... #define ptrdiff_t int .................... #else .................... #define ptrdiff_t long .................... #endif .................... .................... #define size_t int .................... #define wchar_t char .................... #define NULL 0 .................... .................... #define offsetof(s,f) (offsetofbit(s,f)/8) .................... .................... #endif .................... .................... #include .................... //////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... //////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef _CTYPE .................... #define _CTYPE .................... .................... #define islower(x) isamong(x,"abcdefghijklmnopqrstuvwxyz") .................... #define isupper(x) isamong(x,"ABCDEFGHIJKLMNOPQRSTUVWXYZ") .................... #define isalnum(x) isamong(x,"0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz") .................... #define isalpha(x) isamong(x,"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz") .................... #define isdigit(x) isamong(x,"0123456789") .................... #define isspace(x) ((x)==' ') .................... #define isxdigit(x) isamong(x,"0123456789ABCDEFabcdef") .................... #define iscntrl(x) ((x)<' ') .................... #define isprint(x) ((x)>=' ') .................... #define isgraph(x) ((x)>' ') .................... #define ispunct(x) (((x)>' ')&&!isalnum(x)) .................... .................... #endif .................... .................... .................... .................... .................... .................... ////////////////////////////////////////////// .................... //// Uncomment the following define to //// .................... //// allow some functions to use a //// .................... //// quicker algorithm, but use more ROM //// .................... //// //// .................... //// #define FASTER_BUT_MORE_ROM //// .................... ////////////////////////////////////////////// .................... .................... .................... .................... /*Copying functions*/ .................... /* standard template: .................... void *memmove(void *s1, void *s2, size_t n). .................... Copies max of n characters safely (not following ending '\0') .................... from s2 in s1; if s2 has less than n characters, appends 0 */ .................... .................... char *memmove(void *s1,char *s2,size_t n) .................... { .................... char *sc1; .................... char *sc2; .................... sc1=s1; .................... sc2=s2; .................... if(sc2 0 && *s2 != '\0'; n--) * 2BB4: MOVFF 482,487 2BB8: MOVFF 481,486 2BBC: MOVLB 4 2BBE: MOVF x85,F 2BC0: BZ 2C0A 2BC2: MOVFF 484,03 2BC6: MOVFF 483,FE9 2BCA: MOVFF 484,FEA 2BCE: MOVF FEF,F 2BD0: BZ 2C0A .................... *s++ = *s2++; 2BD2: MOVFF 487,03 2BD6: MOVF x86,W 2BD8: INCF x86,F 2BDA: BTFSC FD8.2 2BDC: INCF x87,F 2BDE: MOVWF x88 2BE0: MOVFF 03,489 2BE4: MOVFF 484,03 2BE8: MOVF x83,W 2BEA: INCF x83,F 2BEC: BTFSC FD8.2 2BEE: INCF x84,F 2BF0: MOVWF FE9 2BF2: MOVFF 03,FEA 2BF6: MOVFF FEF,48A 2BFA: MOVFF 489,FEA 2BFE: MOVFF 488,FE9 2C02: MOVFF 48A,FEF 2C06: DECF x85,F 2C08: BRA 2BBE .................... for (; n > 0; n--) 2C0A: MOVF x85,F 2C0C: BZ 2C26 .................... *s++ = '\0'; 2C0E: MOVFF 487,03 2C12: MOVF x86,W 2C14: INCF x86,F 2C16: BTFSC FD8.2 2C18: INCF x87,F 2C1A: MOVWF FE9 2C1C: MOVFF 03,FEA 2C20: CLRF FEF 2C22: DECF x85,F 2C24: BRA 2C0A .................... .................... return(s1); 2C26: MOVFF 481,01 2C2A: MOVFF 482,02 .................... } 2C2E: MOVLB 0 2C30: RETLW 00 .................... /***********************************************************/ .................... .................... /*concatenation functions*/ .................... /* standard template: char *strcat(char *s1, const char *s2) .................... appends s2 to s1*/ .................... .................... char *strcat(char *s1, char *s2) .................... { .................... char *s; .................... .................... for (s = s1; *s != '\0'; ++s); * 56B2: MOVFF 3DF,3E3 56B6: MOVFF 3DE,3E2 56BA: MOVFF 3E3,03 56BE: MOVLB 3 56C0: MOVFF 3E2,FE9 56C4: MOVFF 3E3,FEA 56C8: MOVF FEF,F 56CA: BZ 56D8 56CC: INCF xE2,F 56CE: BTFSC FD8.2 56D0: INCF xE3,F 56D2: MOVLB 0 56D4: BRA 56BA 56D6: MOVLB 3 .................... while(*s2 != '\0') .................... { 56D8: MOVFF 3E1,03 56DC: MOVFF 3E0,FE9 56E0: MOVFF 3E1,FEA 56E4: MOVF FEF,F 56E6: BZ 570E .................... *s = *s2; 56E8: MOVFF 3E0,FE9 56EC: MOVFF 3E1,FEA 56F0: MOVFF FEF,3E6 56F4: MOVFF 3E3,FEA 56F8: MOVFF 3E2,FE9 56FC: MOVFF 3E6,FEF .................... ++s; 5700: INCF xE2,F 5702: BTFSC FD8.2 5704: INCF xE3,F .................... ++s2; 5706: INCF xE0,F 5708: BTFSC FD8.2 570A: INCF xE1,F .................... } 570C: BRA 56D8 .................... .................... *s = '\0'; 570E: MOVFF 3E2,FE9 5712: MOVFF 3E3,FEA 5716: CLRF FEF .................... return(s1); 5718: MOVFF 3DE,01 571C: MOVFF 3DF,02 .................... } 5720: MOVLB 0 5722: RETLW 00 .................... /* standard template: char *strncat(char *s1, char *s2,size_t n) .................... appends not more than n characters from s2 to s1*/ .................... .................... char *strncat(char *s1, char *s2, size_t n) .................... { .................... char *s; .................... .................... for (s = s1; *s != '\0'; ++s); .................... while(*s2 != '\0' && 0s2 */ .................... .................... signed int memcmp(void * s1,char *s2,size_t n) .................... { .................... char *su1, *su2; .................... for(su1=s1, su2=s2; 0s2 */ .................... .................... signed int strcmp(char *s1, char *s2) .................... { .................... for (; *s1 == *s2; s1++, s2++) * 0C26: MOVLB 3 0C28: MOVFF 3EF,FE9 0C2C: MOVFF 3F0,FEA 0C30: MOVFF FEF,3F3 0C34: MOVFF 3F2,03 0C38: MOVFF 3F1,FE9 0C3C: MOVFF 3F2,FEA 0C40: MOVF FEF,W 0C42: SUBWF xF3,W 0C44: BNZ 0C70 .................... if (*s1 == '\0') 0C46: MOVFF 3F0,03 0C4A: MOVFF 3EF,FE9 0C4E: MOVFF 03,FEA 0C52: MOVF FEF,F 0C54: BNZ 0C5C .................... return(0); 0C56: MOVLW 00 0C58: MOVWF 01 0C5A: BRA 0C9C 0C5C: MOVFF 3F0,03 0C60: MOVF xEF,W 0C62: INCF xEF,F 0C64: BTFSC FD8.2 0C66: INCF xF0,F 0C68: INCF xF1,F 0C6A: BTFSC FD8.2 0C6C: INCF xF2,F 0C6E: BRA 0C28 .................... return((*s1 < *s2) ? -1: 1); 0C70: MOVFF 3F0,03 0C74: MOVFF 3EF,FE9 0C78: MOVFF 3F0,FEA 0C7C: MOVFF FEF,3F3 0C80: MOVFF 3F2,03 0C84: MOVFF 3F1,FE9 0C88: MOVFF 3F2,FEA 0C8C: MOVF FEF,W 0C8E: SUBWF xF3,W 0C90: BC 0C98 0C92: MOVLW FF 0C94: MOVWF 03 0C96: BRA 0C9A 0C98: MOVLW 01 0C9A: MOVWF 01 .................... } 0C9C: MOVLB 0 0C9E: RETLW 00 .................... /* standard template: int strcoll(const char *s1, const char *s2). .................... Compares s1 & s2; returns -1 if s1s2 */ .................... .................... signed int strcoll(char *s1, char *s2) .................... { .................... for (; *s1 == *s2; s1++, s2++) .................... if (*s1 == '\0') .................... return(0); .................... return((*s1 < *s2) ? -1: 1); .................... } .................... .................... /* standard template: .................... int strncmp(const char *s1, const char *s2, size_t n). .................... Compares max of n characters (not following 0) from s1 to s2; .................... returns same as strcmp */ .................... .................... signed int strncmp(char *s1, char *s2, size_t n) .................... { .................... for (; n > 0; s1++, s2++, n--) .................... if (*s1 != *s2) .................... return((*s1 <*s2) ? -1: 1); .................... else if (*s1 == '\0') .................... return(0); .................... return(0); .................... } .................... /* standard template: .................... int strxfrm(const char *s1, const char *s2, size_t n). .................... transforms maximum of n characters from s2 and places them into s1*/ .................... size_t strxfrm(char *s1, char *s2, size_t n) .................... { .................... char *s; .................... int n1; .................... n1=n; .................... for (s = s1; n > 0 && *s2 != '\0'; n--) .................... *s++ = *s2++; .................... for (; n > 0; n--) .................... *s++ = '\0'; .................... .................... return(n1); .................... } .................... .................... .................... .................... .................... .................... /***********************************************************/ .................... /*Search functions*/ .................... /* standard template: void *memchr(const char *s, int c). .................... Finds first occurrence of c in n characters of s */ .................... .................... char *memchr(void *s,int c,size_t n) .................... { .................... char uc; .................... char *su; .................... uc=c; .................... for(su=s;0= 'A' && *p <='Z') .................... *p += 'a' - 'A'; .................... return(s); .................... } .................... .................... .................... /************************************************************/ .................... .................... .................... #endif .................... .................... .................... div_t div(signed int numer,signed int denom) .................... { .................... div_t val; .................... val.quot = numer / denom; .................... val.rem = numer - (denom * val.quot); .................... return (val); .................... } .................... .................... ldiv_t ldiv(signed long numer,signed long denom) .................... { .................... ldiv_t val; .................... val.quot = numer / denom; .................... val.rem = numer - (denom * val.quot); .................... return (val); .................... } .................... .................... float atof(char * s) .................... { .................... float pow10 = 1.0; * 11C4: MOVLW 7F 11C6: MOVLB 4 11C8: MOVWF x87 11CA: CLRF x88 11CC: CLRF x89 11CE: CLRF x8A .................... float result = 0.0; 11D0: CLRF x8B 11D2: CLRF x8C 11D4: CLRF x8D 11D6: CLRF x8E .................... int sign = 0; 11D8: CLRF x8F .................... char c; .................... int ptr = 0; 11DA: CLRF x91 .................... .................... c = s[ptr++]; 11DC: MOVF x91,W 11DE: INCF x91,F 11E0: CLRF 03 11E2: ADDWF x85,W 11E4: MOVWF FE9 11E6: MOVF x86,W 11E8: ADDWFC 03,W 11EA: MOVWF FEA 11EC: MOVFF FEF,490 .................... .................... if ((c>='0' && c<='9') || c=='+' || c=='-' || c=='.') { 11F0: MOVF x90,W 11F2: SUBLW 2F 11F4: BC 11FC 11F6: MOVF x90,W 11F8: SUBLW 39 11FA: BC 1210 11FC: MOVF x90,W 11FE: SUBLW 2B 1200: BZ 1210 1202: MOVF x90,W 1204: SUBLW 2D 1206: BZ 1210 1208: MOVF x90,W 120A: SUBLW 2E 120C: BTFSS FD8.2 120E: BRA 13E2 .................... if(c == '-') { 1210: MOVF x90,W 1212: SUBLW 2D 1214: BNZ 122E .................... sign = 1; 1216: MOVLW 01 1218: MOVWF x8F .................... c = s[ptr++]; 121A: MOVF x91,W 121C: INCF x91,F 121E: CLRF 03 1220: ADDWF x85,W 1222: MOVWF FE9 1224: MOVF x86,W 1226: ADDWFC 03,W 1228: MOVWF FEA 122A: MOVFF FEF,490 .................... } .................... if(c == '+') 122E: MOVF x90,W 1230: SUBLW 2B 1232: BNZ 1248 .................... c = s[ptr++]; 1234: MOVF x91,W 1236: INCF x91,F 1238: CLRF 03 123A: ADDWF x85,W 123C: MOVWF FE9 123E: MOVF x86,W 1240: ADDWFC 03,W 1242: MOVWF FEA 1244: MOVFF FEF,490 .................... .................... while((c >= '0' && c <= '9')) { 1248: MOVF x90,W 124A: SUBLW 2F 124C: BC 1310 124E: MOVF x90,W 1250: SUBLW 39 1252: BNC 1310 .................... result = 10*result + c - '0'; 1254: CLRF x95 1256: CLRF x94 1258: MOVLW 20 125A: MOVWF x93 125C: MOVLW 82 125E: MOVWF x92 1260: MOVFF 48E,499 1264: MOVFF 48D,498 1268: MOVFF 48C,497 126C: MOVFF 48B,496 1270: MOVLB 0 1272: RCALL 0CC2 1274: MOVFF 00,492 1278: MOVFF 01,493 127C: MOVFF 02,494 1280: MOVFF 03,495 1284: MOVLB 4 1286: CLRF x97 1288: MOVFF 490,496 128C: MOVLB 0 128E: RCALL 0DB8 1290: BCF FD8.1 1292: MOVFF 495,49D 1296: MOVFF 494,49C 129A: MOVFF 493,49B 129E: MOVFF 492,49A 12A2: MOVFF 03,4A1 12A6: MOVFF 02,4A0 12AA: MOVFF 01,49F 12AE: MOVFF 00,49E 12B2: RCALL 0DEE 12B4: MOVFF 00,496 12B8: MOVFF 01,497 12BC: MOVFF 02,498 12C0: MOVFF 03,499 12C4: BSF FD8.1 12C6: MOVFF 03,49D 12CA: MOVFF 02,49C 12CE: MOVFF 01,49B 12D2: MOVFF 00,49A 12D6: MOVLB 4 12D8: CLRF xA1 12DA: CLRF xA0 12DC: MOVLW 40 12DE: MOVWF x9F 12E0: MOVLW 84 12E2: MOVWF x9E 12E4: MOVLB 0 12E6: RCALL 0DEE 12E8: MOVFF 03,48E 12EC: MOVFF 02,48D 12F0: MOVFF 01,48C 12F4: MOVFF 00,48B .................... c = s[ptr++]; 12F8: MOVLB 4 12FA: MOVF x91,W 12FC: INCF x91,F 12FE: CLRF 03 1300: ADDWF x85,W 1302: MOVWF FE9 1304: MOVF x86,W 1306: ADDWFC 03,W 1308: MOVWF FEA 130A: MOVFF FEF,490 .................... } 130E: BRA 1248 .................... .................... if (c == '.') { 1310: MOVF x90,W 1312: SUBLW 2E 1314: BNZ 13E2 .................... c = s[ptr++]; 1316: MOVF x91,W 1318: INCF x91,F 131A: CLRF 03 131C: ADDWF x85,W 131E: MOVWF FE9 1320: MOVF x86,W 1322: ADDWFC 03,W 1324: MOVWF FEA 1326: MOVFF FEF,490 .................... while((c >= '0' && c <= '9')) { 132A: MOVF x90,W 132C: SUBLW 2F 132E: BC 13E2 1330: MOVF x90,W 1332: SUBLW 39 1334: BNC 13E2 .................... pow10 = pow10*10; 1336: MOVFF 48A,495 133A: MOVFF 489,494 133E: MOVFF 488,493 1342: MOVFF 487,492 1346: CLRF x99 1348: CLRF x98 134A: MOVLW 20 134C: MOVWF x97 134E: MOVLW 82 1350: MOVWF x96 1352: MOVLB 0 1354: RCALL 0CC2 1356: MOVFF 03,48A 135A: MOVFF 02,489 135E: MOVFF 01,488 1362: MOVFF 00,487 .................... result += (c - '0')/pow10; 1366: MOVLW 30 1368: MOVLB 4 136A: SUBWF x90,W 136C: CLRF x97 136E: MOVWF x96 1370: MOVLB 0 1372: RCALL 0DB8 1374: MOVFF 03,496 1378: MOVFF 02,495 137C: MOVFF 01,494 1380: MOVFF 00,493 1384: MOVFF 48A,49A 1388: MOVFF 489,499 138C: MOVFF 488,498 1390: MOVFF 487,497 1394: RCALL 1066 1396: BCF FD8.1 1398: MOVFF 48E,49D 139C: MOVFF 48D,49C 13A0: MOVFF 48C,49B 13A4: MOVFF 48B,49A 13A8: MOVFF 03,4A1 13AC: MOVFF 02,4A0 13B0: MOVFF 01,49F 13B4: MOVFF 00,49E 13B8: RCALL 0DEE 13BA: MOVFF 03,48E 13BE: MOVFF 02,48D 13C2: MOVFF 01,48C 13C6: MOVFF 00,48B .................... c = s[ptr++]; 13CA: MOVLB 4 13CC: MOVF x91,W 13CE: INCF x91,F 13D0: CLRF 03 13D2: ADDWF x85,W 13D4: MOVWF FE9 13D6: MOVF x86,W 13D8: ADDWFC 03,W 13DA: MOVWF FEA 13DC: MOVFF FEF,490 .................... } 13E0: BRA 132A .................... } .................... .................... } .................... .................... if (sign == 1) 13E2: DECFSZ x8F,W 13E4: BRA 1418 .................... result = -1*result; 13E6: CLRF x95 13E8: CLRF x94 13EA: MOVLW 80 13EC: MOVWF x93 13EE: MOVLW 7F 13F0: MOVWF x92 13F2: MOVFF 48E,499 13F6: MOVFF 48D,498 13FA: MOVFF 48C,497 13FE: MOVFF 48B,496 1402: MOVLB 0 1404: RCALL 0CC2 1406: MOVFF 03,48E 140A: MOVFF 02,48D 140E: MOVFF 01,48C 1412: MOVFF 00,48B 1416: MOVLB 4 .................... return(result); 1418: MOVFF 48B,00 141C: MOVFF 48C,01 1420: MOVFF 48D,02 1424: MOVFF 48E,03 .................... } 1428: MOVLB 0 142A: RETLW 00 .................... .................... float atoe(char * s) .................... { .................... float pow10 = 1.0; .................... float result = 0.0; .................... int sign = 0; .................... int expsign = 0; .................... char c; .................... int ptr = 0; .................... int i; .................... float exp = 1.0; .................... int expcnt = 0; .................... .................... c = s[ptr++]; .................... .................... if ((c>='0' && c<='9') || c=='+' || c=='-' || c=='.' || c=='E' || c=='e') { .................... if(c == '-') { .................... sign = 1; .................... c = s[ptr++]; .................... } .................... if(c == '+') .................... c = s[ptr++]; .................... .................... while((c >= '0' && c <= '9')) { .................... result = 10*result + c - '0'; .................... c = s[ptr++]; .................... } .................... .................... if (c == '.') { .................... c = s[ptr++]; .................... while((c >= '0' && c <= '9')) { .................... pow10 = pow10*10; .................... result += (c - '0')/pow10; .................... c = s[ptr++]; .................... } .................... } .................... .................... // Handling the exponent .................... if (c=='e' || c=='E') { .................... c = s[ptr++]; .................... .................... if(c == '-') { .................... expsign = 1; .................... c = s[ptr++]; .................... } .................... if(c == '+') .................... c = s[ptr++]; .................... .................... while((c >= '0' && c <= '9')) { .................... expcnt = 10*expcnt + c - '0'; .................... c = s[ptr++]; .................... } .................... .................... for(i=0;i= '0' && c <= '9') 14E2: MOVF xF5,W 14E4: SUBLW 2F 14E6: BTFSC FD8.0 14E8: BRA 160A 14EA: MOVF xF5,W 14EC: SUBLW 39 14EE: BTFSS FD8.0 14F0: BRA 160A .................... { .................... .................... // Check for hexa number .................... if (c == '0' && (s[index] == 'x' || s[index] == 'X')) 14F2: MOVF xF5,W 14F4: SUBLW 30 14F6: BNZ 153A 14F8: CLRF 03 14FA: MOVF xF4,W 14FC: ADDWF xEF,W 14FE: MOVWF FE9 1500: MOVF xF0,W 1502: ADDWFC 03,W 1504: MOVWF FEA 1506: MOVF FEF,W 1508: SUBLW 78 150A: BZ 1520 150C: CLRF 03 150E: MOVF xF4,W 1510: ADDWF xEF,W 1512: MOVWF FE9 1514: MOVF xF0,W 1516: ADDWFC 03,W 1518: MOVWF FEA 151A: MOVF FEF,W 151C: SUBLW 58 151E: BNZ 153A .................... { .................... base = 16; 1520: MOVLW 10 1522: MOVWF xF3 .................... index++; 1524: INCF xF4,F .................... c = s[index++]; 1526: MOVF xF4,W 1528: INCF xF4,F 152A: CLRF 03 152C: ADDWF xEF,W 152E: MOVWF FE9 1530: MOVF xF0,W 1532: ADDWFC 03,W 1534: MOVWF FEA 1536: MOVFF FEF,3F5 .................... } .................... .................... // The number is a decimal number .................... if (base == 10) 153A: MOVF xF3,W 153C: SUBLW 0A 153E: BNZ 157A .................... { .................... while (c >= '0' && c <= '9') .................... { 1540: MOVF xF5,W 1542: SUBLW 2F 1544: BC 1578 1546: MOVF xF5,W 1548: SUBLW 39 154A: BNC 1578 .................... result = 10*result + (c - '0'); 154C: MOVLW 0A 154E: MOVWF xF6 1550: MOVFF 3F1,3F7 1554: MOVLB 0 1556: BRA 142C 1558: MOVLW 30 155A: MOVLB 3 155C: SUBWF xF5,W 155E: ADDWF 01,W 1560: MOVWF xF1 .................... c = s[index++]; 1562: MOVF xF4,W 1564: INCF xF4,F 1566: CLRF 03 1568: ADDWF xEF,W 156A: MOVWF FE9 156C: MOVF xF0,W 156E: ADDWFC 03,W 1570: MOVWF FEA 1572: MOVFF FEF,3F5 .................... } 1576: BRA 1540 .................... } .................... else if (base == 16) // The number is a hexa number 1578: BRA 160A 157A: MOVF xF3,W 157C: SUBLW 10 157E: BNZ 160A .................... { .................... c = toupper(c); 1580: MOVF xF5,W 1582: SUBLW 60 1584: BC 1592 1586: MOVF xF5,W 1588: SUBLW 7A 158A: BNC 1592 158C: MOVF xF5,W 158E: ANDLW DF 1590: BRA 1594 1592: MOVF xF5,W 1594: MOVWF xF5 .................... while ( (c >= '0' && c <= '9') || (c >= 'A' && c<='F')) .................... { 1596: MOVF xF5,W 1598: SUBLW 2F 159A: BC 15A2 159C: MOVF xF5,W 159E: SUBLW 39 15A0: BC 15AE 15A2: MOVF xF5,W 15A4: SUBLW 40 15A6: BC 160A 15A8: MOVF xF5,W 15AA: SUBLW 46 15AC: BNC 160A .................... if (c >= '0' && c <= '9') 15AE: MOVF xF5,W 15B0: SUBLW 2F 15B2: BC 15CC 15B4: MOVF xF5,W 15B6: SUBLW 39 15B8: BNC 15CC .................... result = (result << 4) + (c - '0'); 15BA: SWAPF xF1,W 15BC: MOVWF xF6 15BE: MOVLW F0 15C0: ANDWF xF6,F 15C2: MOVLW 30 15C4: SUBWF xF5,W 15C6: ADDWF xF6,W 15C8: MOVWF xF1 .................... else 15CA: BRA 15DE .................... result = (result << 4) + (c - 'A' + 10); 15CC: SWAPF xF1,W 15CE: MOVWF xF6 15D0: MOVLW F0 15D2: ANDWF xF6,F 15D4: MOVLW 41 15D6: SUBWF xF5,W 15D8: ADDLW 0A 15DA: ADDWF xF6,W 15DC: MOVWF xF1 .................... .................... c = s[index++]; 15DE: MOVF xF4,W 15E0: INCF xF4,F 15E2: CLRF 03 15E4: ADDWF xEF,W 15E6: MOVWF FE9 15E8: MOVF xF0,W 15EA: ADDWFC 03,W 15EC: MOVWF FEA 15EE: MOVFF FEF,3F5 .................... c = toupper(c); 15F2: MOVF xF5,W 15F4: SUBLW 60 15F6: BC 1604 15F8: MOVF xF5,W 15FA: SUBLW 7A 15FC: BNC 1604 15FE: MOVF xF5,W 1600: ANDLW DF 1602: BRA 1606 1604: MOVF xF5,W 1606: MOVWF xF5 .................... } 1608: BRA 1596 .................... } .................... } .................... .................... if (sign == 1 && base == 10) 160A: DECFSZ xF2,W 160C: BRA 1616 160E: MOVF xF3,W 1610: SUBLW 0A 1612: BNZ 1616 .................... result = -result; 1614: NEGF xF1 .................... .................... return(result); 1616: MOVFF 3F1,01 .................... } 161A: MOVLB 0 161C: RETLW 00 .................... .................... signed long atol(char *s) .................... { .................... signed long result; .................... int sign, base, index; .................... char c; .................... .................... index = 0; .................... sign = 0; .................... base = 10; .................... result = 0; .................... .................... if (!s) .................... return 0; .................... c = s[index++]; .................... .................... // increase index if either positive or negative sign is detected .................... if (c == '-') .................... { .................... sign = 1; // Set the sign to negative .................... c = s[index++]; .................... } .................... else if (c == '+') .................... { .................... c = s[index++]; .................... } .................... .................... if (c >= '0' && c <= '9') .................... { .................... if (c == '0' && (s[index] == 'x' || s[index] == 'X')) .................... { .................... base = 16; .................... index++; .................... c = s[index++]; .................... } .................... .................... // The number is a decimal number .................... if (base == 10) .................... { .................... while (c >= '0' && c <= '9') .................... { .................... result = 10*result + (c - '0'); .................... c = s[index++]; .................... } .................... } .................... else if (base == 16) // The number is a hexa number .................... { .................... c = toupper(c); .................... while ( (c >= '0' && c <= '9') || (c >= 'A' && c <='F')) .................... { .................... if (c >= '0' && c <= '9') .................... result = (result << 4) + (c - '0'); .................... else .................... result = (result << 4) + (c - 'A' + 10); .................... .................... c = s[index++];c = toupper(c); .................... } .................... } .................... } .................... .................... if (base == 10 && sign == 1) .................... result = -result; .................... .................... return(result); .................... } .................... .................... /* A fast routine to multiply by 10 .................... */ .................... signed int32 mult_with10(int32 num) .................... { .................... return ( (num << 1) + (num << 3) ); .................... } .................... .................... signed int32 atoi32(char *s) .................... { .................... signed int32 result; .................... int sign, base, index; .................... char c; .................... .................... index = 0; .................... sign = 0; .................... base = 10; .................... result = 0; .................... .................... if (!s) .................... return 0; .................... c = s[index++]; .................... .................... // increase index if either positive or negative sign is detected .................... if (c == '-') .................... { .................... sign = 1; // Set the sign to negative .................... c = s[index++]; .................... } .................... else if (c == '+') .................... { .................... c = s[index++]; .................... } .................... .................... if (c >= '0' && c <= '9') .................... { .................... if (c == '0' && (s[index] == 'x' || s[index] == 'X')) .................... { .................... base = 16; .................... index++; .................... c = s[index++]; .................... } .................... .................... // The number is a decimal number .................... if (base == 10) .................... { .................... while (c >= '0' && c <= '9') { .................... result = (result << 1) + (result << 3); // result *= 10; .................... result += (c - '0'); .................... c = s[index++]; .................... } .................... } .................... else if (base == 16) // The number is a hexa number .................... { .................... c = toupper(c); .................... while ((c >= '0' && c <= '9') || (c >= 'A' && c <='F')) .................... { .................... if (c >= '0' && c <= '9') .................... result = (result << 4) + (c - '0'); .................... else .................... result = (result << 4) + (c - 'A' + 10); .................... .................... c = s[index++];c = toupper(c); .................... } .................... } .................... } .................... .................... if (base == 10 && sign == 1) .................... result = -result; .................... .................... return(result); .................... } .................... .................... char * itoa(signed int32 num, int8 base, char *s) .................... { .................... int32 temp=1; .................... int8 i,sign=0,cnt=0; .................... char c; .................... .................... if(num<0) { .................... sign=1; // Check for negative number .................... num*=-1; .................... } .................... .................... while(temp>0) { .................... temp=(num/base); .................... s[cnt]=(num%base)+'0'; // Conversion .................... .................... if(s[cnt]>0x39) .................... s[cnt]+=0x7; .................... .................... cnt++; .................... num=temp; .................... } .................... .................... if(sign==1) { .................... s[cnt]=0x2D; // Negative sign .................... cnt++; .................... } .................... .................... for(i = 0;i<(int8)(cnt/2);i++) { .................... .................... c=s[i]; .................... s[i]=s[cnt-i-1]; // Reverse the number .................... s[cnt-i-1]=c; .................... } .................... s[cnt]='\0'; // End the string .................... return s; .................... } .................... .................... float strtod(char *s,char *endptr) { .................... float pow10 = 1.0; .................... float result = 0.0; .................... int sign = 0, point = 0; .................... char c; .................... int ptr = 0; .................... .................... if (!s) .................... return 0; .................... c=s[ptr++]; .................... .................... .................... while((c>='0' && c<='9') || c=='+' || c=='-' || c=='.') { .................... if(c == '-') { .................... sign = 1; .................... c = s[ptr++]; .................... } .................... .................... while((c >= '0' && c <= '9') && point == 0) { .................... result = 10*result + c - '0'; .................... c = s[ptr++]; .................... } .................... .................... if (c == '.') { .................... point = 1; .................... c = s[ptr++]; .................... } .................... .................... while((c >= '0' && c <= '9') && point == 1) { .................... pow10 = pow10*10; .................... result += (c - '0')/pow10; .................... c = s[ptr++]; .................... } .................... .................... if (c == '+') { .................... c = s[ptr++]; .................... } .................... } .................... .................... if (sign == 1) .................... result = -1*result; .................... if(endptr) .................... { .................... if (ptr) { .................... ptr--; .................... #IF LONG_POINTERS .................... *((long *)endptr)= s+ptr; .................... #ELSE .................... *((char *)endptr)=s+ptr; .................... #ENDIF .................... } .................... else .................... { .................... #IF LONG_POINTERS .................... *((long *)endptr)= s; .................... #ELSE .................... *((char *)endptr)=s; .................... #ENDIF .................... } .................... } .................... .................... return(result); .................... } .................... .................... long strtoul(char *s,char *endptr,signed int base) .................... { .................... char *sc,*s1,*sd; .................... unsigned long x=0; .................... char sign; .................... char digits[]="0123456789abcdefghijklmnopqstuvwxyz"; .................... for(sc=s;isspace(*sc);++sc); .................... sign=*sc=='-'||*sc=='+'?*sc++:'+'; .................... if(sign=='-' || base <0 || base ==1|| base >36) // invalid base .................... goto StrtoulGO; .................... .................... else if (base) .................... { .................... if(base==16 && *sc =='0'&&(sc[1]=='x' || sc[1]=='X')) .................... sc+=2; .................... if(base==8 && *sc =='0') .................... sc+=1; .................... if(base==2 && *sc =='0'&&sc[1]=='b') .................... sc+=2; .................... .................... } .................... else if(*sc!='0') // base is 0, find base .................... base=10; .................... else if (sc[1]=='x' || sc[1]=='X') .................... base =16,sc+=2; .................... else if(sc[1]=='b') .................... base=2,sc+=2; .................... else .................... base=8; .................... for (s1=sc;*sc=='0';++sc);// skip leading zeroes .................... sd=memchr(digits,tolower(*sc),base); .................... for(; sd!=0; ) .................... { .................... x=x*base+(int16)(sd-digits); .................... ++sc; .................... sd=memchr(digits,tolower(*sc),base); .................... } .................... if(s1==sc) .................... { .................... StrtoulGO: .................... if (endptr) .................... { .................... #IF LONG_POINTERS .................... *((long *)endptr)= s; .................... #ELSE .................... *((char *)endptr)=s; .................... #ENDIF .................... } .................... return 0; .................... } .................... if (endptr) .................... { .................... #IF LONG_POINTERS .................... *((long *)endptr)= sc; .................... #ELSE .................... *((char *)endptr)=sc; .................... #ENDIF .................... } .................... return x; .................... } .................... .................... .................... signed long strtol(char *s,char *endptr,signed int base) .................... { .................... char *sc,*s1,*sd; .................... signed long x=0; .................... char sign; .................... char digits[]="0123456789abcdefghijklmnopqstuvwxyz"; .................... for(sc=s;isspace(*sc);++sc); .................... sign=*sc=='-'||*sc=='+'?*sc++:'+'; .................... if (base <0 || base ==1|| base >36) // invalid base .................... goto StrtolGO; .................... else if (base) .................... { .................... if(base==16 && *sc =='0'&&(sc[1]=='x' || sc[1]=='X')) .................... sc+=2; .................... if(base==8 && *sc =='0') .................... sc+=1; .................... if(base==2 && *sc =='0'&&sc[1]=='b') .................... sc+=2; .................... .................... } .................... else if(*sc!='0') // base is 0, find base .................... base=10; .................... else if (sc[1]=='x' || sc[1]=='X') .................... base =16,sc+=2; .................... else if(sc[1]=='b') .................... base=2,sc+=2; .................... else .................... base=8; .................... for (s1=sc;*sc=='0';++sc);// skip leading zeroes .................... .................... sd=memchr(digits,tolower(*sc),base); .................... for(;sd!=0;) .................... { .................... x=x*base+(int16)(sd-digits); .................... ++sc; .................... sd=memchr(digits,tolower(*sc),base); .................... } .................... if(s1==sc) .................... { .................... StrtolGO: .................... if (endptr) .................... { .................... #IF LONG_POINTERS .................... *((long *)endptr)= s; .................... #ELSE .................... *((char *)endptr)=s; .................... #ENDIF .................... } .................... return 0; .................... } .................... if(sign=='-') .................... x =-x; .................... if (endptr) .................... { .................... #IF LONG_POINTERS .................... *((long *)endptr)= sc; .................... #ELSE .................... *((char *)endptr)=sc; .................... #ENDIF .................... } .................... return x; .................... } .................... .................... signed int system(char *string) .................... { .................... return 0; .................... } .................... .................... int mblen(char *s,size_t n) .................... { .................... return strlen(s); .................... } .................... .................... int mbtowc(wchar_t *pwc,char *s,size_t n) .................... { .................... *pwc=*s; .................... return 1; .................... } .................... .................... int wctomb(char *s,wchar_t wchar) .................... { .................... *s=wchar; .................... return 1; .................... } .................... .................... size_t mbstowcs(wchar_t *pwcs,char *s,size_t n) .................... { .................... strncpy(pwcs,s,n); .................... return strlen(pwcs); .................... } .................... .................... size_t wcstombs(char *s,wchar_t *pwcs,size_t n) .................... { .................... strncpy(s,pwcs,n); .................... return strlen(s); .................... } .................... .................... //--------------------------------------------------------------------------- .................... // The random number implementation .................... //--------------------------------------------------------------------------- .................... .................... unsigned int32 _Randseed; .................... .................... long rand(void) .................... { .................... _Randseed = _Randseed * 1103515245 + 12345; .................... return ((unsigned long)(_Randseed >> 16) % RAND_MAX); .................... } .................... .................... void srand(unsigned int32 seed) .................... { .................... _Randseed = seed; .................... } .................... .................... //--------------------------------------------------------------------------- .................... // Searching and sorting utilities implementation .................... //--------------------------------------------------------------------------- .................... .................... typedef signed int (*_Cmpfun)(char * p1,char * p2); .................... .................... void qsort(char * qdata, int qitems, int qsize, _Cmpfun cmp) { .................... int m,j,i,l; .................... short done; .................... BYTE t[16]; .................... .................... m = qitems/2; .................... while( m > 0 ) { .................... for(j=0; j<(qitems-m); ++j) { .................... i = j; .................... do .................... { .................... done=TRUE; .................... l = i+m; .................... if( (*cmp)(qdata+i*qsize, qdata+l*qsize) > 0 ) { .................... memcpy(t, qdata+i*qsize, qsize); .................... memcpy(qdata+i*qsize, qdata+l*qsize, qsize); .................... memcpy(qdata+l*qsize, t, qsize); .................... if(m <= i) .................... i -= m; .................... done = FALSE; .................... } .................... } while(!done); .................... } .................... m = m/2; .................... } .................... } .................... .................... .................... char *bsearch(char *key, char *base, size_t num, size_t width,_Cmpfun cmp) .................... { .................... char *p, *q; .................... size_t n; .................... size_t pivot; .................... signed int val; .................... .................... p = base; .................... n = num; .................... .................... while (n > 0) .................... { .................... pivot = n >> 1; .................... q = p + width * pivot; .................... .................... val = (*cmp)(key, q); .................... .................... if (val < 0) .................... n = pivot; .................... else if (val == 0) .................... return ((char *)q); .................... else { .................... p = q + width; .................... n -= pivot + 1; .................... } .................... } .................... .................... return NULL; // There's no match .................... } .................... .................... .................... #endif .................... .................... #include .................... //////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2007 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... //////////////////////////////////////////////////////////////////////////// .................... //// //// .................... //// History: //// .................... //// * 9/20/2001 : Improvments are made to sin/cos code. //// .................... //// The code now is small, much faster, //// .................... //// and more accurate. //// .................... //// * 2/21/2007 : Compiler handles & operator differently and does .................... //// not return generic (int8 *) so type cast is done //// .................... //// //// .................... //////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef MATH_H .................... #define MATH_H .................... .................... #ifdef PI .................... #undef PI .................... #endif .................... #define PI 3.141592654 .................... .................... .................... #define SQRT2 1.41421356 .................... .................... //float const ps[4] = {5.9304945, 21.125224, 8.9403076, 0.29730279}; .................... //float const qs[4] = {1.0000000, 15.035723, 17.764134, 2.4934718}; .................... .................... ///////////////////////////// Round Functions ////////////////////////////// .................... .................... float CEIL_FLOOR(float x, int n) .................... { .................... float y, res; .................... long l; .................... int1 s; .................... .................... s = 0; * 4994: MOVLB 4 4996: BCF x15.0 .................... y = x; 4998: MOVFF 409,40E 499C: MOVFF 408,40D 49A0: MOVFF 407,40C 49A4: MOVFF 406,40B .................... .................... if (x < 0) 49A8: MOVFF 409,43D 49AC: MOVFF 408,43C 49B0: MOVFF 407,43B 49B4: MOVFF 406,43A 49B8: CLRF x41 49BA: CLRF x40 49BC: CLRF x3F 49BE: CLRF x3E 49C0: MOVLB 0 49C2: CALL 2360 49C6: BNC 49D2 .................... { .................... s = 1; 49C8: MOVLB 4 49CA: BSF x15.0 .................... y = -y; 49CC: MOVF x0C,W 49CE: XORLW 80 49D0: MOVWF x0C .................... } .................... .................... if (y <= 32768.0) 49D2: MOVFF 40E,43D 49D6: MOVFF 40D,43C 49DA: MOVFF 40C,43B 49DE: MOVFF 40B,43A 49E2: MOVLB 4 49E4: CLRF x41 49E6: CLRF x40 49E8: CLRF x3F 49EA: MOVLW 8E 49EC: MOVWF x3E 49EE: MOVLB 0 49F0: CALL 2360 49F4: BC 49F8 49F6: BNZ 4A2A .................... res = (float)(long)y; 49F8: MOVFF 40E,43D 49FC: MOVFF 40D,43C 4A00: MOVFF 40C,43B 4A04: MOVFF 40B,43A 4A08: CALL 2F98 4A0C: MOVFF 02,497 4A10: MOVFF 01,496 4A14: CALL 0DB8 4A18: MOVFF 03,412 4A1C: MOVFF 02,411 4A20: MOVFF 01,410 4A24: MOVFF 00,40F .................... .................... else if (y < 10000000.0) 4A28: BRA 4BD0 4A2A: MOVFF 40E,43D 4A2E: MOVFF 40D,43C 4A32: MOVFF 40C,43B 4A36: MOVFF 40B,43A 4A3A: MOVLW 80 4A3C: MOVLB 4 4A3E: MOVWF x41 4A40: MOVLW 96 4A42: MOVWF x40 4A44: MOVLW 18 4A46: MOVWF x3F 4A48: MOVLW 96 4A4A: MOVWF x3E 4A4C: MOVLB 0 4A4E: CALL 2360 4A52: BTFSS FD8.0 4A54: BRA 4BC0 .................... { .................... l = (long)(y/32768.0); 4A56: MOVFF 40E,496 4A5A: MOVFF 40D,495 4A5E: MOVFF 40C,494 4A62: MOVFF 40B,493 4A66: MOVLB 4 4A68: CLRF x9A 4A6A: CLRF x99 4A6C: CLRF x98 4A6E: MOVLW 8E 4A70: MOVWF x97 4A72: MOVLB 0 4A74: CALL 1066 4A78: MOVFF 03,43D 4A7C: MOVFF 02,43C 4A80: MOVFF 01,43B 4A84: MOVFF 00,43A 4A88: CALL 2F98 4A8C: MOVFF 02,414 4A90: MOVFF 01,413 .................... y = 32768.0*(y/32768.0 - (float)l); 4A94: MOVFF 40E,496 4A98: MOVFF 40D,495 4A9C: MOVFF 40C,494 4AA0: MOVFF 40B,493 4AA4: MOVLB 4 4AA6: CLRF x9A 4AA8: CLRF x99 4AAA: CLRF x98 4AAC: MOVLW 8E 4AAE: MOVWF x97 4AB0: MOVLB 0 4AB2: CALL 1066 4AB6: MOVFF 00,416 4ABA: MOVFF 01,417 4ABE: MOVFF 02,418 4AC2: MOVFF 03,419 4AC6: MOVFF 414,497 4ACA: MOVFF 413,496 4ACE: CALL 0DB8 4AD2: BSF FD8.1 4AD4: MOVFF 419,49D 4AD8: MOVFF 418,49C 4ADC: MOVFF 417,49B 4AE0: MOVFF 416,49A 4AE4: MOVFF 03,4A1 4AE8: MOVFF 02,4A0 4AEC: MOVFF 01,49F 4AF0: MOVFF 00,49E 4AF4: CALL 0DEE 4AF8: MOVLB 4 4AFA: CLRF x95 4AFC: CLRF x94 4AFE: CLRF x93 4B00: MOVLW 8E 4B02: MOVWF x92 4B04: MOVFF 03,499 4B08: MOVFF 02,498 4B0C: MOVFF 01,497 4B10: MOVFF 00,496 4B14: MOVLB 0 4B16: CALL 0CC2 4B1A: MOVFF 03,40E 4B1E: MOVFF 02,40D 4B22: MOVFF 01,40C 4B26: MOVFF 00,40B .................... res = 32768.0*(float)l; 4B2A: MOVFF 414,497 4B2E: MOVFF 413,496 4B32: CALL 0DB8 4B36: MOVLB 4 4B38: CLRF x95 4B3A: CLRF x94 4B3C: CLRF x93 4B3E: MOVLW 8E 4B40: MOVWF x92 4B42: MOVFF 03,499 4B46: MOVFF 02,498 4B4A: MOVFF 01,497 4B4E: MOVFF 00,496 4B52: MOVLB 0 4B54: CALL 0CC2 4B58: MOVFF 03,412 4B5C: MOVFF 02,411 4B60: MOVFF 01,410 4B64: MOVFF 00,40F .................... res += (float)(long)y; 4B68: MOVFF 40E,43D 4B6C: MOVFF 40D,43C 4B70: MOVFF 40C,43B 4B74: MOVFF 40B,43A 4B78: CALL 2F98 4B7C: MOVFF 02,497 4B80: MOVFF 01,496 4B84: CALL 0DB8 4B88: BCF FD8.1 4B8A: MOVFF 412,49D 4B8E: MOVFF 411,49C 4B92: MOVFF 410,49B 4B96: MOVFF 40F,49A 4B9A: MOVFF 03,4A1 4B9E: MOVFF 02,4A0 4BA2: MOVFF 01,49F 4BA6: MOVFF 00,49E 4BAA: CALL 0DEE 4BAE: MOVFF 03,412 4BB2: MOVFF 02,411 4BB6: MOVFF 01,410 4BBA: MOVFF 00,40F .................... } .................... .................... else 4BBE: BRA 4BD0 .................... res = y; 4BC0: MOVFF 40E,412 4BC4: MOVFF 40D,411 4BC8: MOVFF 40C,410 4BCC: MOVFF 40B,40F .................... .................... y = y - (float)(long)y; 4BD0: MOVFF 40E,43D 4BD4: MOVFF 40D,43C 4BD8: MOVFF 40C,43B 4BDC: MOVFF 40B,43A 4BE0: CALL 2F98 4BE4: MOVFF 02,497 4BE8: MOVFF 01,496 4BEC: CALL 0DB8 4BF0: BSF FD8.1 4BF2: MOVFF 40E,49D 4BF6: MOVFF 40D,49C 4BFA: MOVFF 40C,49B 4BFE: MOVFF 40B,49A 4C02: MOVFF 03,4A1 4C06: MOVFF 02,4A0 4C0A: MOVFF 01,49F 4C0E: MOVFF 00,49E 4C12: CALL 0DEE 4C16: MOVFF 03,40E 4C1A: MOVFF 02,40D 4C1E: MOVFF 01,40C 4C22: MOVFF 00,40B .................... .................... if (s) 4C26: MOVLB 4 4C28: BTFSS x15.0 4C2A: BRA 4C32 .................... res = -res; 4C2C: MOVF x10,W 4C2E: XORLW 80 4C30: MOVWF x10 .................... .................... if (y != 0) 4C32: MOVFF 40E,43D 4C36: MOVFF 40D,43C 4C3A: MOVFF 40C,43B 4C3E: MOVFF 40B,43A 4C42: CLRF x41 4C44: CLRF x40 4C46: CLRF x3F 4C48: CLRF x3E 4C4A: MOVLB 0 4C4C: CALL 2360 4C50: BZ 4CCA .................... { .................... if (s == 1 && n == 0) 4C52: MOVLB 4 4C54: BTFSS x15.0 4C56: BRA 4C90 4C58: MOVF x0A,F 4C5A: BNZ 4C90 .................... res -= 1.0; 4C5C: BSF FD8.1 4C5E: MOVFF 412,49D 4C62: MOVFF 411,49C 4C66: MOVFF 410,49B 4C6A: MOVFF 40F,49A 4C6E: CLRF xA1 4C70: CLRF xA0 4C72: CLRF x9F 4C74: MOVLW 7F 4C76: MOVWF x9E 4C78: MOVLB 0 4C7A: CALL 0DEE 4C7E: MOVFF 03,412 4C82: MOVFF 02,411 4C86: MOVFF 01,410 4C8A: MOVFF 00,40F 4C8E: MOVLB 4 .................... .................... if (s == 0 && n == 1) 4C90: BTFSC x15.0 4C92: BRA 4CCC 4C94: DECFSZ x0A,W 4C96: BRA 4CCC .................... res += 1.0; 4C98: BCF FD8.1 4C9A: MOVFF 412,49D 4C9E: MOVFF 411,49C 4CA2: MOVFF 410,49B 4CA6: MOVFF 40F,49A 4CAA: CLRF xA1 4CAC: CLRF xA0 4CAE: CLRF x9F 4CB0: MOVLW 7F 4CB2: MOVWF x9E 4CB4: MOVLB 0 4CB6: CALL 0DEE 4CBA: MOVFF 03,412 4CBE: MOVFF 02,411 4CC2: MOVFF 01,410 4CC6: MOVFF 00,40F 4CCA: MOVLB 4 .................... } .................... if (x == 0) 4CCC: MOVFF 409,43D 4CD0: MOVFF 408,43C 4CD4: MOVFF 407,43B 4CD8: MOVFF 406,43A 4CDC: CLRF x41 4CDE: CLRF x40 4CE0: CLRF x3F 4CE2: CLRF x3E 4CE4: MOVLB 0 4CE6: CALL 2360 4CEA: BNZ 4CF8 .................... res = 0; 4CEC: MOVLB 4 4CEE: CLRF x12 4CF0: CLRF x11 4CF2: CLRF x10 4CF4: CLRF x0F 4CF6: MOVLB 0 .................... .................... return (res); 4CF8: MOVFF 40F,00 4CFC: MOVFF 410,01 4D00: MOVFF 411,02 4D04: MOVFF 412,03 .................... } 4D08: RETLW 00 .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float floor(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : rounds down the number x. .................... // Date : N/A .................... // .................... float floor(float x) .................... { .................... return CEIL_FLOOR(x, 0); * 4D28: MOVFF 405,409 4D2C: MOVFF 404,408 4D30: MOVFF 403,407 4D34: MOVFF 402,406 4D38: MOVLB 4 4D3A: CLRF x0A 4D3C: MOVLB 0 4D3E: RCALL 4994 .................... } 4D40: GOTO 4E4C (RETURN) .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float ceil(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : rounds up the number x. .................... // Date : N/A .................... // .................... float ceil(float x) .................... { .................... return CEIL_FLOOR(x, 1); * 4D0A: MOVFF 404,409 4D0E: MOVFF 403,408 4D12: MOVFF 402,407 4D16: MOVFF 401,406 4D1A: MOVLW 01 4D1C: MOVLB 4 4D1E: MOVWF x0A 4D20: MOVLB 0 4D22: RCALL 4994 .................... } 4D24: GOTO 4E04 (RETURN) .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float fabs(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the absolute value of floating point number x .................... // Returns : returns the absolute value of x .................... // Date : N/A .................... // .................... #define fabs abs .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float fmod(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the floating point remainder of x/y .................... // Returns : returns the value of x= i*y, for some integer i such that, if y .................... // is non zero, the result has the same isgn of x na dmagnitude less than the .................... // magnitude of y. If y is zero then a domain error occurs. .................... // Date : N/A .................... // .................... .................... float fmod(float x,float y) .................... { .................... float i; .................... if (y!=0.0) * 4D44: MOVFF 3F4,43D 4D48: MOVFF 3F3,43C 4D4C: MOVFF 3F2,43B 4D50: MOVFF 3F1,43A 4D54: MOVLB 4 4D56: CLRF x41 4D58: CLRF x40 4D5A: CLRF x3F 4D5C: CLRF x3E 4D5E: MOVLB 0 4D60: CALL 2360 4D64: BTFSC FD8.2 4D66: BRA 4EAA .................... { .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); 4D68: MOVFF 3F0,496 4D6C: MOVFF 3EF,495 4D70: MOVFF 3EE,494 4D74: MOVFF 3ED,493 4D78: MOVFF 3F4,49A 4D7C: MOVFF 3F3,499 4D80: MOVFF 3F2,498 4D84: MOVFF 3F1,497 4D88: CALL 1066 4D8C: MOVFF 00,3F9 4D90: MOVFF 01,3FA 4D94: MOVFF 02,3FB 4D98: MOVFF 03,3FC 4D9C: MOVFF 03,43D 4DA0: MOVFF 02,43C 4DA4: MOVFF 01,43B 4DA8: MOVFF 00,43A 4DAC: MOVLB 4 4DAE: CLRF x41 4DB0: CLRF x40 4DB2: CLRF x3F 4DB4: CLRF x3E 4DB6: MOVLB 0 4DB8: CALL 2360 4DBC: BNC 4E06 4DBE: MOVFF 3F0,496 4DC2: MOVFF 3EF,495 4DC6: MOVFF 3EE,494 4DCA: MOVFF 3ED,493 4DCE: MOVFF 3F4,49A 4DD2: MOVFF 3F3,499 4DD6: MOVFF 3F2,498 4DDA: MOVFF 3F1,497 4DDE: CALL 1066 4DE2: MOVFF 00,3FD 4DE6: MOVFF 01,3FE 4DEA: MOVFF 02,3FF 4DEE: MOVFF 03,400 4DF2: MOVFF 03,404 4DF6: MOVFF 02,403 4DFA: MOVFF 01,402 4DFE: MOVFF 00,401 4E02: BRA 4D0A 4E04: BRA 4E4C 4E06: MOVFF 3F0,496 4E0A: MOVFF 3EF,495 4E0E: MOVFF 3EE,494 4E12: MOVFF 3ED,493 4E16: MOVFF 3F4,49A 4E1A: MOVFF 3F3,499 4E1E: MOVFF 3F2,498 4E22: MOVFF 3F1,497 4E26: CALL 1066 4E2A: MOVFF 00,3FE 4E2E: MOVFF 01,3FF 4E32: MOVFF 02,400 4E36: MOVFF 03,401 4E3A: MOVFF 03,405 4E3E: MOVFF 02,404 4E42: MOVFF 01,403 4E46: MOVFF 00,402 4E4A: BRA 4D28 4E4C: MOVFF 03,3F8 4E50: MOVFF 02,3F7 4E54: MOVFF 01,3F6 4E58: MOVFF 00,3F5 .................... return(x-(i*y)); 4E5C: MOVFF 3F8,495 4E60: MOVFF 3F7,494 4E64: MOVFF 3F6,493 4E68: MOVFF 3F5,492 4E6C: MOVFF 3F4,499 4E70: MOVFF 3F3,498 4E74: MOVFF 3F2,497 4E78: MOVFF 3F1,496 4E7C: CALL 0CC2 4E80: BSF FD8.1 4E82: MOVFF 3F0,49D 4E86: MOVFF 3EF,49C 4E8A: MOVFF 3EE,49B 4E8E: MOVFF 3ED,49A 4E92: MOVFF 03,4A1 4E96: MOVFF 02,4A0 4E9A: MOVFF 01,49F 4E9E: MOVFF 00,49E 4EA2: CALL 0DEE 4EA6: BRA 4EAA .................... } .................... else 4EA8: BRA 4EAA .................... { .................... #ifdef _ERRNO .................... { .................... errno=EDOM; .................... } .................... #endif .................... } .................... } 4EAA: GOTO 5328 (RETURN) .................... .................... //////////////////// Exponential and logarithmic functions //////////////////// .................... .................... #define LN2 0.6931471806 .................... .................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093, .................... 0.0554965651, 0.240227138, 0.693147172}; .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float exp(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the value (e^x) .................... // Date : N/A .................... // .................... float exp(float x) .................... { .................... float y, res, r; .................... signed int n; .................... int1 s; .................... #ifdef _ERRNO .................... if(x > 88.722838) .................... { .................... errno=ERANGE; .................... return(0); .................... } .................... #endif .................... n = (signed long)(x/LN2); .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... n = -n; .................... y = -y; .................... } .................... .................... res = 0.0; .................... *((int8 *)(&res)) = n + 0x7F; .................... .................... y = y/LN2 - (float)n; .................... .................... r = pe[0]*y + pe[1]; .................... r = r*y + pe[2]; .................... r = r*y + pe[3]; .................... r = r*y + pe[4]; .................... r = r*y + pe[5]; .................... .................... res = res*(1.0 + y*r); .................... .................... if (s) .................... res = 1.0/res; .................... return(res); .................... } .................... .................... /************************************************************/ .................... .................... float const pl[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; .................... float const ql[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943}; .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float log(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the the natural log of x .................... // Date : N/A .................... // .................... float log(float x) .................... { .................... float y, res, r, y2; .................... .................... signed n; .................... #ifdef _ERRNO .................... if(x <0) .................... { .................... errno=EDOM; .................... } .................... if(x ==0) .................... { .................... errno=ERANGE; .................... return(0); .................... } .................... #endif .................... y = x; * 349E: MOVFF 404,408 34A2: MOVFF 403,407 34A6: MOVFF 402,406 34AA: MOVFF 401,405 .................... .................... if (y != 1.0) 34AE: MOVFF 408,43D 34B2: MOVFF 407,43C 34B6: MOVFF 406,43B 34BA: MOVFF 405,43A 34BE: MOVLB 4 34C0: CLRF x41 34C2: CLRF x40 34C4: CLRF x3F 34C6: MOVLW 7F 34C8: MOVWF x3E 34CA: MOVLB 0 34CC: CALL 2360 34D0: BTFSC FD8.2 34D2: BRA 3960 .................... { .................... *((int8 *)(&y)) = 0x7E; 34D4: MOVLW 04 34D6: MOVLB 4 34D8: MOVWF x17 34DA: MOVLW 05 34DC: MOVWF FE9 34DE: MOVFF 417,FEA 34E2: MOVLW 7E 34E4: MOVWF FEF .................... .................... y = (y - 1.0)/(y + 1.0); 34E6: BSF FD8.1 34E8: MOVFF 408,49D 34EC: MOVFF 407,49C 34F0: MOVFF 406,49B 34F4: MOVFF 405,49A 34F8: CLRF xA1 34FA: CLRF xA0 34FC: CLRF x9F 34FE: MOVLW 7F 3500: MOVWF x9E 3502: MOVLB 0 3504: CALL 0DEE 3508: MOVFF 00,416 350C: MOVFF 01,417 3510: MOVFF 02,418 3514: MOVFF 03,419 3518: BCF FD8.1 351A: MOVFF 408,49D 351E: MOVFF 407,49C 3522: MOVFF 406,49B 3526: MOVFF 405,49A 352A: MOVLB 4 352C: CLRF xA1 352E: CLRF xA0 3530: CLRF x9F 3532: MOVLW 7F 3534: MOVWF x9E 3536: MOVLB 0 3538: CALL 0DEE 353C: MOVFF 419,496 3540: MOVFF 418,495 3544: MOVFF 417,494 3548: MOVFF 416,493 354C: MOVFF 03,49A 3550: MOVFF 02,499 3554: MOVFF 01,498 3558: MOVFF 00,497 355C: CALL 1066 3560: MOVFF 03,408 3564: MOVFF 02,407 3568: MOVFF 01,406 356C: MOVFF 00,405 .................... .................... y2=y*y; 3570: MOVFF 408,495 3574: MOVFF 407,494 3578: MOVFF 406,493 357C: MOVFF 405,492 3580: MOVFF 408,499 3584: MOVFF 407,498 3588: MOVFF 406,497 358C: MOVFF 405,496 3590: CALL 0CC2 3594: MOVFF 03,414 3598: MOVFF 02,413 359C: MOVFF 01,412 35A0: MOVFF 00,411 .................... .................... res = pl[0]*y2 + pl[1]; 35A4: MOVLW BC 35A6: MOVLB 4 35A8: MOVWF x95 35AA: MOVLW 24 35AC: MOVWF x94 35AE: MOVLW 67 35B0: MOVWF x93 35B2: MOVLW 7D 35B4: MOVWF x92 35B6: MOVFF 414,499 35BA: MOVFF 413,498 35BE: MOVFF 412,497 35C2: MOVFF 411,496 35C6: MOVLB 0 35C8: CALL 0CC2 35CC: MOVFF 00,416 35D0: MOVFF 01,417 35D4: MOVFF 02,418 35D8: MOVFF 03,419 35DC: BCF FD8.1 35DE: MOVFF 03,49D 35E2: MOVFF 02,49C 35E6: MOVFF 01,49B 35EA: MOVFF 00,49A 35EE: MOVLW E3 35F0: MOVLB 4 35F2: MOVWF xA1 35F4: MOVLW E4 35F6: MOVWF xA0 35F8: MOVLW 90 35FA: MOVWF x9F 35FC: MOVLW 82 35FE: MOVWF x9E 3600: MOVLB 0 3602: CALL 0DEE 3606: MOVFF 03,40C 360A: MOVFF 02,40B 360E: MOVFF 01,40A 3612: MOVFF 00,409 .................... res = res*y2 + pl[2]; 3616: MOVFF 40C,495 361A: MOVFF 40B,494 361E: MOVFF 40A,493 3622: MOVFF 409,492 3626: MOVFF 414,499 362A: MOVFF 413,498 362E: MOVFF 412,497 3632: MOVFF 411,496 3636: CALL 0CC2 363A: MOVFF 00,416 363E: MOVFF 01,417 3642: MOVFF 02,418 3646: MOVFF 03,419 364A: BCF FD8.1 364C: MOVFF 03,49D 3650: MOVFF 02,49C 3654: MOVFF 01,49B 3658: MOVFF 00,49A 365C: MOVLW 1C 365E: MOVLB 4 3660: MOVWF xA1 3662: MOVLW 87 3664: MOVWF xA0 3666: MOVLW 57 3668: MOVWF x9F 366A: MOVLW 83 366C: MOVWF x9E 366E: MOVLB 0 3670: CALL 0DEE 3674: MOVFF 03,40C 3678: MOVFF 02,40B 367C: MOVFF 01,40A 3680: MOVFF 00,409 .................... res = res*y2 + pl[3]; 3684: MOVFF 40C,495 3688: MOVFF 40B,494 368C: MOVFF 40A,493 3690: MOVFF 409,492 3694: MOVFF 414,499 3698: MOVFF 413,498 369C: MOVFF 412,497 36A0: MOVFF 411,496 36A4: CALL 0CC2 36A8: MOVFF 00,416 36AC: MOVFF 01,417 36B0: MOVFF 02,418 36B4: MOVFF 03,419 36B8: BCF FD8.1 36BA: MOVFF 03,49D 36BE: MOVFF 02,49C 36C2: MOVFF 01,49B 36C6: MOVFF 00,49A 36CA: MOVLW AB 36CC: MOVLB 4 36CE: MOVWF xA1 36D0: MOVLW E1 36D2: MOVWF xA0 36D4: MOVLW 9E 36D6: MOVWF x9F 36D8: MOVLW 83 36DA: MOVWF x9E 36DC: MOVLB 0 36DE: CALL 0DEE 36E2: MOVFF 03,40C 36E6: MOVFF 02,40B 36EA: MOVFF 01,40A 36EE: MOVFF 00,409 .................... .................... r = ql[0]*y2 + ql[1]; 36F2: MOVLB 4 36F4: CLRF x95 36F6: CLRF x94 36F8: CLRF x93 36FA: MOVLW 7F 36FC: MOVWF x92 36FE: MOVFF 414,499 3702: MOVFF 413,498 3706: MOVFF 412,497 370A: MOVFF 411,496 370E: MOVLB 0 3710: CALL 0CC2 3714: MOVFF 00,416 3718: MOVFF 01,417 371C: MOVFF 02,418 3720: MOVFF 03,419 3724: BCF FD8.1 3726: MOVFF 03,49D 372A: MOVFF 02,49C 372E: MOVFF 01,49B 3732: MOVFF 00,49A 3736: MOVLW B4 3738: MOVLB 4 373A: MOVWF xA1 373C: MOVLW 2A 373E: MOVWF xA0 3740: MOVLW 82 3742: MOVWF x9F 3744: MOVWF x9E 3746: MOVLB 0 3748: CALL 0DEE 374C: MOVFF 03,410 3750: MOVFF 02,40F 3754: MOVFF 01,40E 3758: MOVFF 00,40D .................... r = r*y2 + ql[2]; 375C: MOVFF 410,495 3760: MOVFF 40F,494 3764: MOVFF 40E,493 3768: MOVFF 40D,492 376C: MOVFF 414,499 3770: MOVFF 413,498 3774: MOVFF 412,497 3778: MOVFF 411,496 377C: CALL 0CC2 3780: MOVFF 00,416 3784: MOVFF 01,417 3788: MOVFF 02,418 378C: MOVFF 03,419 3790: BCF FD8.1 3792: MOVFF 03,49D 3796: MOVFF 02,49C 379A: MOVFF 01,49B 379E: MOVFF 00,49A 37A2: MOVLW 80 37A4: MOVLB 4 37A6: MOVWF xA1 37A8: MOVLW 3E 37AA: MOVWF xA0 37AC: MOVLW 06 37AE: MOVWF x9F 37B0: MOVLW 83 37B2: MOVWF x9E 37B4: MOVLB 0 37B6: CALL 0DEE 37BA: MOVFF 03,410 37BE: MOVFF 02,40F 37C2: MOVFF 01,40E 37C6: MOVFF 00,40D .................... r = r*y2 + ql[3]; 37CA: MOVFF 410,495 37CE: MOVFF 40F,494 37D2: MOVFF 40E,493 37D6: MOVFF 40D,492 37DA: MOVFF 414,499 37DE: MOVFF 413,498 37E2: MOVFF 412,497 37E6: MOVFF 411,496 37EA: CALL 0CC2 37EE: MOVFF 00,416 37F2: MOVFF 01,417 37F6: MOVFF 02,418 37FA: MOVFF 03,419 37FE: BCF FD8.1 3800: MOVFF 03,49D 3804: MOVFF 02,49C 3808: MOVFF 01,49B 380C: MOVFF 00,49A 3810: MOVLW AB 3812: MOVLB 4 3814: MOVWF xA1 3816: MOVLW E1 3818: MOVWF xA0 381A: MOVLW 9E 381C: MOVWF x9F 381E: MOVLW 82 3820: MOVWF x9E 3822: MOVLB 0 3824: CALL 0DEE 3828: MOVFF 03,410 382C: MOVFF 02,40F 3830: MOVFF 01,40E 3834: MOVFF 00,40D .................... .................... res = y*res/r; 3838: MOVFF 408,495 383C: MOVFF 407,494 3840: MOVFF 406,493 3844: MOVFF 405,492 3848: MOVFF 40C,499 384C: MOVFF 40B,498 3850: MOVFF 40A,497 3854: MOVFF 409,496 3858: CALL 0CC2 385C: MOVFF 00,416 3860: MOVFF 01,417 3864: MOVFF 02,418 3868: MOVFF 03,419 386C: MOVFF 03,496 3870: MOVFF 02,495 3874: MOVFF 01,494 3878: MOVFF 00,493 387C: MOVFF 410,49A 3880: MOVFF 40F,499 3884: MOVFF 40E,498 3888: MOVFF 40D,497 388C: CALL 1066 3890: MOVFF 03,40C 3894: MOVFF 02,40B 3898: MOVFF 01,40A 389C: MOVFF 00,409 .................... .................... n = *((int8 *)(&x)) - 0x7E; 38A0: MOVLW 04 38A2: MOVLB 4 38A4: MOVWF x17 38A6: MOVLW 01 38A8: MOVWF FE9 38AA: MOVFF 417,FEA 38AE: MOVLW 7E 38B0: SUBWF FEF,W 38B2: MOVWF x15 .................... .................... if (n<0) 38B4: BTFSC x15.7 38B6: BRA 38BA 38B8: BRA 38E2 .................... r = -(float)-n; 38BA: MOVLW 00 38BC: BSF FD8.0 38BE: SUBFWB x15,W 38C0: CLRF x17 38C2: MOVWF x16 38C4: BTFSC x16.7 38C6: DECF x17,F 38C8: MOVLB 0 38CA: RCALL 344E 38CC: MOVFF 00,40D 38D0: MOVF 01,W 38D2: XORLW 80 38D4: MOVLB 4 38D6: MOVWF x0E 38D8: MOVFF 02,40F 38DC: MOVFF 03,410 .................... else 38E0: BRA 3902 .................... r = (float)n; 38E2: CLRF x17 38E4: MOVFF 415,416 38E8: BTFSC x16.7 38EA: DECF x17,F 38EC: MOVLB 0 38EE: RCALL 344E 38F0: MOVFF 03,410 38F4: MOVFF 02,40F 38F8: MOVFF 01,40E 38FC: MOVFF 00,40D 3900: MOVLB 4 .................... .................... res += r*LN2; 3902: MOVFF 410,495 3906: MOVFF 40F,494 390A: MOVFF 40E,493 390E: MOVFF 40D,492 3912: MOVLW 18 3914: MOVWF x99 3916: MOVLW 72 3918: MOVWF x98 391A: MOVLW 31 391C: MOVWF x97 391E: MOVLW 7E 3920: MOVWF x96 3922: MOVLB 0 3924: CALL 0CC2 3928: BCF FD8.1 392A: MOVFF 40C,49D 392E: MOVFF 40B,49C 3932: MOVFF 40A,49B 3936: MOVFF 409,49A 393A: MOVFF 03,4A1 393E: MOVFF 02,4A0 3942: MOVFF 01,49F 3946: MOVFF 00,49E 394A: CALL 0DEE 394E: MOVFF 03,40C 3952: MOVFF 02,40B 3956: MOVFF 01,40A 395A: MOVFF 00,409 .................... } .................... .................... else 395E: BRA 396C .................... res = 0.0; 3960: MOVLB 4 3962: CLRF x0C 3964: CLRF x0B 3966: CLRF x0A 3968: CLRF x09 396A: MOVLB 0 .................... .................... return(res); 396C: MOVFF 409,00 3970: MOVFF 40A,01 3974: MOVFF 40B,02 3978: MOVFF 40C,03 .................... } 397C: RETLW 00 .................... .................... #define LN10 2.30258509 .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float log10(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the the log base 10 of x .................... // Date : N/A .................... // .................... float log10(float x) .................... { .................... float r; .................... .................... r = log(x); .................... r = r/LN10; .................... return(r); .................... } .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float modf(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description :breaks the argument value int integral and fractional parts, .................... // ach of which have the same sign as the argument. It stores the integral part .................... // as a float in the object pointed to by the iptr .................... // Returns : returns the signed fractional part of value. .................... // Date : N/A .................... // .................... .................... float modf(float value,float *iptr) .................... { .................... *iptr=(value < 0.0)? ceil(value): floor(value); .................... return(value - *iptr); .................... } .................... .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float pwr(float x,float y) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the value (x^y) .................... // Date : N/A .................... // .................... float pwr(float x,float y) .................... { .................... if(x>=0) .................... return( exp(y*log(x)) ); .................... else .................... return( -exp(y*log(-x)) ); .................... } .................... .................... .................... //////////////////// Power functions //////////////////// .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float pow(float x,float y) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the value (x^y) .................... // Date : N/A .................... // .................... float pow(float x,float y) .................... { .................... if(x>=0) .................... return( exp(y*log(x)) ); .................... else .................... return( -exp(y*log(-x)) ); .................... } .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float sqrt(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the square root of x .................... // Date : N/A .................... // .................... float sqrt(float x) .................... { .................... float y, res; .................... BYTE *p; .................... .................... #ifdef _ERRNO .................... if(x < 0) .................... { .................... errno=EDOM; .................... } .................... #endif .................... .................... if( x<=0.0) 397E: MOVFF 3F8,43D 3982: MOVFF 3F7,43C 3986: MOVFF 3F6,43B 398A: MOVFF 3F5,43A 398E: MOVLB 4 3990: CLRF x41 3992: CLRF x40 3994: CLRF x3F 3996: CLRF x3E 3998: MOVLB 0 399A: CALL 2360 399E: BC 39A2 39A0: BNZ 39AC .................... return(0.0); 39A2: CLRF 00 39A4: CLRF 01 39A6: CLRF 02 39A8: CLRF 03 39AA: BRA 3AA6 .................... .................... y=x; 39AC: MOVFF 3F8,3FC 39B0: MOVFF 3F7,3FB 39B4: MOVFF 3F6,3FA 39B8: MOVFF 3F5,3F9 .................... p=&y; 39BC: MOVLW 03 39BE: MOVLB 4 39C0: MOVWF x02 39C2: MOVLW F9 39C4: MOVWF x01 .................... (*p)=(BYTE)((((int16)(*p)) + 127) >> 1); 39C6: MOVFF 401,FE9 39CA: MOVFF 402,FEA 39CE: MOVF FEF,W 39D0: CLRF x06 39D2: MOVWF x05 39D4: MOVLW 7F 39D6: ADDWF x05,W 39D8: MOVWF x07 39DA: MOVLW 00 39DC: ADDWFC x06,W 39DE: MOVWF x08 39E0: BCF FD8.0 39E2: RRCF x08,W 39E4: RRCF x07,W 39E6: MOVFF 402,FEA 39EA: MOVFF 401,FE9 39EE: MOVWF FEF .................... .................... do { .................... res=y; 39F0: MOVFF 3FC,400 39F4: MOVFF 3FB,3FF 39F8: MOVFF 3FA,3FE 39FC: MOVFF 3F9,3FD .................... y+=(x/y); 3A00: MOVFF 3F8,496 3A04: MOVFF 3F7,495 3A08: MOVFF 3F6,494 3A0C: MOVFF 3F5,493 3A10: MOVFF 3FC,49A 3A14: MOVFF 3FB,499 3A18: MOVFF 3FA,498 3A1C: MOVFF 3F9,497 3A20: MOVLB 0 3A22: CALL 1066 3A26: BCF FD8.1 3A28: MOVFF 3FC,49D 3A2C: MOVFF 3FB,49C 3A30: MOVFF 3FA,49B 3A34: MOVFF 3F9,49A 3A38: MOVFF 03,4A1 3A3C: MOVFF 02,4A0 3A40: MOVFF 01,49F 3A44: MOVFF 00,49E 3A48: CALL 0DEE 3A4C: MOVFF 03,3FC 3A50: MOVFF 02,3FB 3A54: MOVFF 01,3FA 3A58: MOVFF 00,3F9 .................... (*p)--; 3A5C: MOVLB 4 3A5E: MOVFF 401,FE9 3A62: MOVFF 402,FEA 3A66: DECF FEF,F .................... } while(res != y); 3A68: MOVFF 400,43D 3A6C: MOVFF 3FF,43C 3A70: MOVFF 3FE,43B 3A74: MOVFF 3FD,43A 3A78: MOVFF 3FC,441 3A7C: MOVFF 3FB,440 3A80: MOVFF 3FA,43F 3A84: MOVFF 3F9,43E 3A88: MOVLB 0 3A8A: CALL 2360 3A8E: BTFSC FD8.2 3A90: BRA 3A96 3A92: MOVLB 4 3A94: BRA 39F0 .................... .................... return(res); 3A96: MOVFF 3FD,00 3A9A: MOVFF 3FE,01 3A9E: MOVFF 3FF,02 3AA2: MOVFF 400,03 .................... } 3AA6: GOTO 415E (RETURN) .................... .................... .................... .................... ////////////////////////////// Trig Functions ////////////////////////////// .................... #ifdef PI_DIV_BY_TWO .................... #undef PI_DIV_BY_TWO .................... #endif .................... #define PI_DIV_BY_TWO 1.570796326794896 .................... #ifdef TWOBYPI .................... #undef TWOBYPI .................... #define TWOBYPI 0.6366197724 .................... #endif .................... //////////////////////////////////////////////////////////////////////////// .................... // float cos(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the cosine value of the angle x, which is in radian .................... // Date : 9/20/2001 .................... // .................... float cos(float x) .................... { .................... float y, t, t2 = 1.0; * 2FD4: MOVLW 7F 2FD6: MOVLB 4 2FD8: MOVWF x20 2FDA: CLRF x21 2FDC: CLRF x22 2FDE: CLRF x23 .................... int quad, i; .................... float frac; .................... float p[4] = { .................... -0.499999993585, .................... 0.041666636258, .................... -0.0013888361399, .................... 0.00002476016134 .................... }; 2FE0: MOVLW 7E 2FE2: MOVWF x2A 2FE4: MOVLW 80 2FE6: MOVWF x2B 2FE8: CLRF x2C 2FEA: CLRF x2D 2FEC: MOVLW 7A 2FEE: MOVWF x2E 2FF0: MOVLW 2A 2FF2: MOVWF x2F 2FF4: MOVLW AA 2FF6: MOVWF x30 2FF8: MOVLW A3 2FFA: MOVWF x31 2FFC: MOVLW 75 2FFE: MOVWF x32 3000: MOVLW B6 3002: MOVWF x33 3004: MOVLW 09 3006: MOVWF x34 3008: MOVLW 9C 300A: MOVWF x35 300C: MOVLW 6F 300E: MOVWF x36 3010: MOVLW 4F 3012: MOVWF x37 3014: MOVLW B4 3016: MOVWF x38 3018: MOVLW 0B 301A: MOVWF x39 .................... .................... if (x < 0) x = -x; // absolute value of input 301C: MOVFF 417,43D 3020: MOVFF 416,43C 3024: MOVFF 415,43B 3028: MOVFF 414,43A 302C: CLRF x41 302E: CLRF x40 3030: CLRF x3F 3032: CLRF x3E 3034: MOVLB 0 3036: CALL 2360 303A: BNC 3044 303C: MOVLB 4 303E: MOVF x15,W 3040: XORLW 80 3042: MOVWF x15 .................... .................... quad = (int)(x / PI_DIV_BY_TWO); // quadrant 3044: MOVFF 417,496 3048: MOVFF 416,495 304C: MOVFF 415,494 3050: MOVFF 414,493 3054: MOVLW DB 3056: MOVLB 4 3058: MOVWF x9A 305A: MOVLW 0F 305C: MOVWF x99 305E: MOVLW 49 3060: MOVWF x98 3062: MOVLW 7F 3064: MOVWF x97 3066: MOVLB 0 3068: CALL 1066 306C: MOVFF 03,43D 3070: MOVFF 02,43C 3074: MOVFF 01,43B 3078: MOVFF 00,43A 307C: RCALL 2F98 307E: MOVFF 01,424 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input 3082: MOVFF 417,496 3086: MOVFF 416,495 308A: MOVFF 415,494 308E: MOVFF 414,493 3092: MOVLW DB 3094: MOVLB 4 3096: MOVWF x9A 3098: MOVLW 0F 309A: MOVWF x99 309C: MOVLW 49 309E: MOVWF x98 30A0: MOVLW 7F 30A2: MOVWF x97 30A4: MOVLB 0 30A6: CALL 1066 30AA: MOVFF 00,43A 30AE: MOVFF 01,43B 30B2: MOVFF 02,43C 30B6: MOVFF 03,43D 30BA: MOVLB 4 30BC: CLRF x97 30BE: MOVFF 424,496 30C2: MOVLB 0 30C4: CALL 0DB8 30C8: BSF FD8.1 30CA: MOVFF 43D,49D 30CE: MOVFF 43C,49C 30D2: MOVFF 43B,49B 30D6: MOVFF 43A,49A 30DA: MOVFF 03,4A1 30DE: MOVFF 02,4A0 30E2: MOVFF 01,49F 30E6: MOVFF 00,49E 30EA: CALL 0DEE 30EE: MOVFF 03,429 30F2: MOVFF 02,428 30F6: MOVFF 01,427 30FA: MOVFF 00,426 .................... quad = quad % 4; // quadrant (0 to 3) 30FE: MOVLW 03 3100: MOVLB 4 3102: ANDWF x24,F .................... .................... if (quad == 0 || quad == 2) 3104: MOVF x24,F 3106: BZ 310E 3108: MOVF x24,W 310A: SUBLW 02 310C: BNZ 3148 .................... t = frac * PI_DIV_BY_TWO; 310E: MOVFF 429,495 3112: MOVFF 428,494 3116: MOVFF 427,493 311A: MOVFF 426,492 311E: MOVLW DB 3120: MOVWF x99 3122: MOVLW 0F 3124: MOVWF x98 3126: MOVLW 49 3128: MOVWF x97 312A: MOVLW 7F 312C: MOVWF x96 312E: MOVLB 0 3130: CALL 0CC2 3134: MOVFF 03,41F 3138: MOVFF 02,41E 313C: MOVFF 01,41D 3140: MOVFF 00,41C .................... else if (quad == 1) 3144: BRA 3224 3146: MOVLB 4 3148: DECFSZ x24,W 314A: BRA 31BA .................... t = (1-frac) * PI_DIV_BY_TWO; 314C: BSF FD8.1 314E: CLRF x9D 3150: CLRF x9C 3152: CLRF x9B 3154: MOVLW 7F 3156: MOVWF x9A 3158: MOVFF 429,4A1 315C: MOVFF 428,4A0 3160: MOVFF 427,49F 3164: MOVFF 426,49E 3168: MOVLB 0 316A: CALL 0DEE 316E: MOVFF 00,43A 3172: MOVFF 01,43B 3176: MOVFF 02,43C 317A: MOVFF 03,43D 317E: MOVFF 03,495 3182: MOVFF 02,494 3186: MOVFF 01,493 318A: MOVFF 00,492 318E: MOVLW DB 3190: MOVLB 4 3192: MOVWF x99 3194: MOVLW 0F 3196: MOVWF x98 3198: MOVLW 49 319A: MOVWF x97 319C: MOVLW 7F 319E: MOVWF x96 31A0: MOVLB 0 31A2: CALL 0CC2 31A6: MOVFF 03,41F 31AA: MOVFF 02,41E 31AE: MOVFF 01,41D 31B2: MOVFF 00,41C .................... else // should be 3 31B6: BRA 3224 31B8: MOVLB 4 .................... t = (frac-1) * PI_DIV_BY_TWO; 31BA: BSF FD8.1 31BC: MOVFF 429,49D 31C0: MOVFF 428,49C 31C4: MOVFF 427,49B 31C8: MOVFF 426,49A 31CC: CLRF xA1 31CE: CLRF xA0 31D0: CLRF x9F 31D2: MOVLW 7F 31D4: MOVWF x9E 31D6: MOVLB 0 31D8: CALL 0DEE 31DC: MOVFF 00,43A 31E0: MOVFF 01,43B 31E4: MOVFF 02,43C 31E8: MOVFF 03,43D 31EC: MOVFF 03,495 31F0: MOVFF 02,494 31F4: MOVFF 01,493 31F8: MOVFF 00,492 31FC: MOVLW DB 31FE: MOVLB 4 3200: MOVWF x99 3202: MOVLW 0F 3204: MOVWF x98 3206: MOVLW 49 3208: MOVWF x97 320A: MOVLW 7F 320C: MOVWF x96 320E: MOVLB 0 3210: CALL 0CC2 3214: MOVFF 03,41F 3218: MOVFF 02,41E 321C: MOVFF 01,41D 3220: MOVFF 00,41C .................... .................... y = 0.999999999781; 3224: MOVLB 4 3226: CLRF x1B 3228: CLRF x1A 322A: CLRF x19 322C: MOVLW 7F 322E: MOVWF x18 .................... t = t * t; 3230: MOVFF 41F,495 3234: MOVFF 41E,494 3238: MOVFF 41D,493 323C: MOVFF 41C,492 3240: MOVFF 41F,499 3244: MOVFF 41E,498 3248: MOVFF 41D,497 324C: MOVFF 41C,496 3250: MOVLB 0 3252: CALL 0CC2 3256: MOVFF 03,41F 325A: MOVFF 02,41E 325E: MOVFF 01,41D 3262: MOVFF 00,41C .................... for (i = 0; i <= 3; i++) 3266: MOVLB 4 3268: CLRF x25 326A: MOVF x25,W 326C: SUBLW 03 326E: BNC 333C .................... { .................... t2 = t2 * t; 3270: MOVFF 423,495 3274: MOVFF 422,494 3278: MOVFF 421,493 327C: MOVFF 420,492 3280: MOVFF 41F,499 3284: MOVFF 41E,498 3288: MOVFF 41D,497 328C: MOVFF 41C,496 3290: MOVLB 0 3292: CALL 0CC2 3296: MOVFF 03,423 329A: MOVFF 02,422 329E: MOVFF 01,421 32A2: MOVFF 00,420 .................... y = y + p[i] * t2; 32A6: MOVLB 4 32A8: MOVF x25,W 32AA: MULLW 04 32AC: MOVF FF3,W 32AE: CLRF 03 32B0: ADDLW 2A 32B2: MOVWF FE9 32B4: MOVLW 04 32B6: ADDWFC 03,W 32B8: MOVWF FEA 32BA: MOVFF FEF,43A 32BE: MOVFF FEC,43B 32C2: MOVFF FEC,43C 32C6: MOVFF FEC,43D 32CA: MOVFF 43D,495 32CE: MOVFF 43C,494 32D2: MOVFF 43B,493 32D6: MOVFF 43A,492 32DA: MOVFF 423,499 32DE: MOVFF 422,498 32E2: MOVFF 421,497 32E6: MOVFF 420,496 32EA: MOVLB 0 32EC: CALL 0CC2 32F0: MOVFF FEA,43F 32F4: MOVFF FE9,43E 32F8: BCF FD8.1 32FA: MOVFF 41B,49D 32FE: MOVFF 41A,49C 3302: MOVFF 419,49B 3306: MOVFF 418,49A 330A: MOVFF 03,4A1 330E: MOVFF 02,4A0 3312: MOVFF 01,49F 3316: MOVFF 00,49E 331A: CALL 0DEE 331E: MOVFF 43F,FEA 3322: MOVFF 43E,FE9 3326: MOVFF 03,41B 332A: MOVFF 02,41A 332E: MOVFF 01,419 3332: MOVFF 00,418 .................... } 3336: MOVLB 4 3338: INCF x25,F 333A: BRA 326A .................... .................... if (quad == 2 || quad == 1) 333C: MOVF x24,W 333E: SUBLW 02 3340: BZ 3346 3342: DECFSZ x24,W 3344: BRA 334C .................... y = -y; // correct sign 3346: MOVF x19,W 3348: XORLW 80 334A: MOVWF x19 .................... .................... return (y); 334C: MOVFF 418,00 3350: MOVFF 419,01 3354: MOVFF 41A,02 3358: MOVFF 41B,03 .................... } 335C: MOVLB 0 335E: RETLW 00 .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float sin(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the sine value of the angle x, which is in radian .................... // Date : 9/20/2001 .................... // .................... float sin(float x) .................... { .................... return cos(x - PI_DIV_BY_TWO); 3360: BSF FD8.1 3362: MOVFF 40F,49D 3366: MOVFF 40E,49C 336A: MOVFF 40D,49B 336E: MOVFF 40C,49A 3372: MOVLW DB 3374: MOVLB 4 3376: MOVWF xA1 3378: MOVLW 0F 337A: MOVWF xA0 337C: MOVLW 49 337E: MOVWF x9F 3380: MOVLW 7F 3382: MOVWF x9E 3384: MOVLB 0 3386: CALL 0DEE 338A: MOVFF 00,410 338E: MOVFF 01,411 3392: MOVFF 02,412 3396: MOVFF 03,413 339A: MOVFF 03,417 339E: MOVFF 02,416 33A2: MOVFF 01,415 33A6: MOVFF 00,414 33AA: RCALL 2FD4 .................... } 33AC: GOTO 3418 (RETURN) .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float tan(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the tangent value of the angle x, which is in radian .................... // Date : 9/20/2001 .................... // .................... float tan(float x) .................... { .................... float c, s; .................... .................... c = cos(x); 33B0: MOVFF 403,417 33B4: MOVFF 402,416 33B8: MOVFF 401,415 33BC: MOVFF 400,414 33C0: RCALL 2FD4 33C2: MOVFF 03,407 33C6: MOVFF 02,406 33CA: MOVFF 01,405 33CE: MOVFF 00,404 .................... if (c == 0.0) 33D2: MOVFF 407,43D 33D6: MOVFF 406,43C 33DA: MOVFF 405,43B 33DE: MOVFF 404,43A 33E2: MOVLB 4 33E4: CLRF x41 33E6: CLRF x40 33E8: CLRF x3F 33EA: CLRF x3E 33EC: MOVLB 0 33EE: CALL 2360 33F2: BNZ 3406 .................... return (1.0e+36); 33F4: MOVLW F6 33F6: MOVWF 00 33F8: MOVLW 40 33FA: MOVWF 01 33FC: MOVLW 97 33FE: MOVWF 02 3400: MOVLW CE 3402: MOVWF 03 3404: BRA 344C .................... .................... s = sin(x); 3406: MOVFF 403,40F 340A: MOVFF 402,40E 340E: MOVFF 401,40D 3412: MOVFF 400,40C 3416: BRA 3360 3418: MOVFF 03,40B 341C: MOVFF 02,40A 3420: MOVFF 01,409 3424: MOVFF 00,408 .................... return(s/c); 3428: MOVFF 40B,496 342C: MOVFF 40A,495 3430: MOVFF 409,494 3434: MOVFF 408,493 3438: MOVFF 407,49A 343C: MOVFF 406,499 3440: MOVFF 405,498 3444: MOVFF 404,497 3448: CALL 1066 .................... } 344C: RETLW 00 .................... .................... .................... .................... float const pas[3] = {0.49559947, -4.6145309, 5.6036290}; .................... float const qas[3] = {1.0000000, -5.5484666, 5.6036290}; .................... .................... float ASIN_COS(float x, int n) .................... { .................... float y, res, r, y2; .................... int1 s; .................... #ifdef _ERRNO .................... if(x <-1 || x > 1) .................... { .................... errno=EDOM; .................... } .................... #endif .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y > 0.5) .................... { .................... y = sqrt((1.0 - y)/2.0); .................... n += 2; .................... } .................... .................... y2=y*y; .................... .................... res = pas[0]*y2 + pas[1]; .................... res = res*y2 + pas[2]; .................... .................... r = qas[0]*y2 + qas[1]; .................... r = r*y2 + qas[2]; .................... .................... res = y*res/r; .................... .................... if (n & 2) // |x| > 0.5 .................... res = PI_DIV_BY_TWO - 2.0*res; .................... if (s) .................... res = -res; .................... if (n & 1) // take arccos .................... res = PI_DIV_BY_TWO - res; .................... .................... return(res); .................... } .................... .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float asin(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the arcsine value of the value x. .................... // Date : N/A .................... // .................... float asin(float x) .................... { .................... float r; .................... .................... r = ASIN_COS(x, 0); .................... return(r); .................... } .................... .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float acos(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the arccosine value of the value x. .................... // Date : N/A .................... // .................... float acos(float x) .................... { .................... float r; .................... .................... r = ASIN_COS(x, 1); .................... return(r); .................... } .................... .................... float const pat[4] = {0.17630401, 5.6710795, 22.376096, 19.818457}; .................... float const qat[4] = {1.0000000, 11.368190, 28.982246, 19.818457}; .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float atan(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the arctangent value of the value x. .................... // Date : N/A .................... // .................... float atan(float x) .................... { .................... float y, res, r; .................... int1 s, flag; .................... .................... s = 0; * 41AE: MOVLB 4 41B0: BCF x0F.0 .................... flag = 0; 41B2: BCF x0F.1 .................... y = x; 41B4: MOVFF 402,406 41B8: MOVFF 401,405 41BC: MOVFF 400,404 41C0: MOVFF 3FF,403 .................... .................... if (x < 0) 41C4: MOVFF 402,43D 41C8: MOVFF 401,43C 41CC: MOVFF 400,43B 41D0: MOVFF 3FF,43A 41D4: CLRF x41 41D6: CLRF x40 41D8: CLRF x3F 41DA: CLRF x3E 41DC: MOVLB 0 41DE: CALL 2360 41E2: BNC 41EE .................... { .................... s = 1; 41E4: MOVLB 4 41E6: BSF x0F.0 .................... y = -y; 41E8: MOVF x04,W 41EA: XORLW 80 41EC: MOVWF x04 .................... } .................... .................... if (y > 1.0) 41EE: MOVLB 4 41F0: CLRF x3D 41F2: CLRF x3C 41F4: CLRF x3B 41F6: MOVLW 7F 41F8: MOVWF x3A 41FA: MOVFF 406,441 41FE: MOVFF 405,440 4202: MOVFF 404,43F 4206: MOVFF 403,43E 420A: MOVLB 0 420C: CALL 2360 4210: BNC 4248 .................... { .................... y = 1.0/y; 4212: MOVLB 4 4214: CLRF x96 4216: CLRF x95 4218: CLRF x94 421A: MOVLW 7F 421C: MOVWF x93 421E: MOVFF 406,49A 4222: MOVFF 405,499 4226: MOVFF 404,498 422A: MOVFF 403,497 422E: MOVLB 0 4230: CALL 1066 4234: MOVFF 03,406 4238: MOVFF 02,405 423C: MOVFF 01,404 4240: MOVFF 00,403 .................... flag = 1; 4244: MOVLB 4 4246: BSF x0F.1 .................... } .................... .................... res = pat[0]*y*y + pat[1]; 4248: MOVLW 0A 424A: MOVLB 4 424C: MOVWF x95 424E: MOVLW 89 4250: MOVWF x94 4252: MOVLW 34 4254: MOVWF x93 4256: MOVLW 7C 4258: MOVWF x92 425A: MOVFF 406,499 425E: MOVFF 405,498 4262: MOVFF 404,497 4266: MOVFF 403,496 426A: MOVLB 0 426C: CALL 0CC2 4270: MOVFF 00,410 4274: MOVFF 01,411 4278: MOVFF 02,412 427C: MOVFF 03,413 4280: MOVFF 03,495 4284: MOVFF 02,494 4288: MOVFF 01,493 428C: MOVFF 00,492 4290: MOVFF 406,499 4294: MOVFF 405,498 4298: MOVFF 404,497 429C: MOVFF 403,496 42A0: CALL 0CC2 42A4: MOVFF 00,414 42A8: MOVFF 01,415 42AC: MOVFF 02,416 42B0: MOVFF 03,417 42B4: BCF FD8.1 42B6: MOVFF 03,49D 42BA: MOVFF 02,49C 42BE: MOVFF 01,49B 42C2: MOVFF 00,49A 42C6: MOVLW 7C 42C8: MOVLB 4 42CA: MOVWF xA1 42CC: MOVLW 79 42CE: MOVWF xA0 42D0: MOVLW 35 42D2: MOVWF x9F 42D4: MOVLW 81 42D6: MOVWF x9E 42D8: MOVLB 0 42DA: CALL 0DEE 42DE: MOVFF 03,40A 42E2: MOVFF 02,409 42E6: MOVFF 01,408 42EA: MOVFF 00,407 .................... res = res*y*y + pat[2]; 42EE: MOVFF 40A,495 42F2: MOVFF 409,494 42F6: MOVFF 408,493 42FA: MOVFF 407,492 42FE: MOVFF 406,499 4302: MOVFF 405,498 4306: MOVFF 404,497 430A: MOVFF 403,496 430E: CALL 0CC2 4312: MOVFF 00,410 4316: MOVFF 01,411 431A: MOVFF 02,412 431E: MOVFF 03,413 4322: MOVFF 03,495 4326: MOVFF 02,494 432A: MOVFF 01,493 432E: MOVFF 00,492 4332: MOVFF 406,499 4336: MOVFF 405,498 433A: MOVFF 404,497 433E: MOVFF 403,496 4342: CALL 0CC2 4346: MOVFF 00,414 434A: MOVFF 01,415 434E: MOVFF 02,416 4352: MOVFF 03,417 4356: BCF FD8.1 4358: MOVFF 03,49D 435C: MOVFF 02,49C 4360: MOVFF 01,49B 4364: MOVFF 00,49A 4368: MOVLW 3F 436A: MOVLB 4 436C: MOVWF xA1 436E: MOVLW 02 4370: MOVWF xA0 4372: MOVLW 33 4374: MOVWF x9F 4376: MOVLW 83 4378: MOVWF x9E 437A: MOVLB 0 437C: CALL 0DEE 4380: MOVFF 03,40A 4384: MOVFF 02,409 4388: MOVFF 01,408 438C: MOVFF 00,407 .................... res = res*y*y + pat[3]; 4390: MOVFF 40A,495 4394: MOVFF 409,494 4398: MOVFF 408,493 439C: MOVFF 407,492 43A0: MOVFF 406,499 43A4: MOVFF 405,498 43A8: MOVFF 404,497 43AC: MOVFF 403,496 43B0: CALL 0CC2 43B4: MOVFF 00,410 43B8: MOVFF 01,411 43BC: MOVFF 02,412 43C0: MOVFF 03,413 43C4: MOVFF 03,495 43C8: MOVFF 02,494 43CC: MOVFF 01,493 43D0: MOVFF 00,492 43D4: MOVFF 406,499 43D8: MOVFF 405,498 43DC: MOVFF 404,497 43E0: MOVFF 403,496 43E4: CALL 0CC2 43E8: MOVFF 00,414 43EC: MOVFF 01,415 43F0: MOVFF 02,416 43F4: MOVFF 03,417 43F8: BCF FD8.1 43FA: MOVFF 03,49D 43FE: MOVFF 02,49C 4402: MOVFF 01,49B 4406: MOVFF 00,49A 440A: MOVLW 33 440C: MOVLB 4 440E: MOVWF xA1 4410: MOVLW 8C 4412: MOVWF xA0 4414: MOVLW 1E 4416: MOVWF x9F 4418: MOVLW 83 441A: MOVWF x9E 441C: MOVLB 0 441E: CALL 0DEE 4422: MOVFF 03,40A 4426: MOVFF 02,409 442A: MOVFF 01,408 442E: MOVFF 00,407 .................... .................... r = qat[0]*y*y + qat[1]; 4432: MOVLB 4 4434: CLRF x95 4436: CLRF x94 4438: CLRF x93 443A: MOVLW 7F 443C: MOVWF x92 443E: MOVFF 406,499 4442: MOVFF 405,498 4446: MOVFF 404,497 444A: MOVFF 403,496 444E: MOVLB 0 4450: CALL 0CC2 4454: MOVFF 00,410 4458: MOVFF 01,411 445C: MOVFF 02,412 4460: MOVFF 03,413 4464: MOVFF 03,495 4468: MOVFF 02,494 446C: MOVFF 01,493 4470: MOVFF 00,492 4474: MOVFF 406,499 4478: MOVFF 405,498 447C: MOVFF 404,497 4480: MOVFF 403,496 4484: CALL 0CC2 4488: MOVFF 00,414 448C: MOVFF 01,415 4490: MOVFF 02,416 4494: MOVFF 03,417 4498: BCF FD8.1 449A: MOVFF 03,49D 449E: MOVFF 02,49C 44A2: MOVFF 01,49B 44A6: MOVFF 00,49A 44AA: MOVLW 1B 44AC: MOVLB 4 44AE: MOVWF xA1 44B0: MOVLW E4 44B2: MOVWF xA0 44B4: MOVLW 35 44B6: MOVWF x9F 44B8: MOVLW 82 44BA: MOVWF x9E 44BC: MOVLB 0 44BE: CALL 0DEE 44C2: MOVFF 03,40E 44C6: MOVFF 02,40D 44CA: MOVFF 01,40C 44CE: MOVFF 00,40B .................... r = r*y*y + qat[2]; 44D2: MOVFF 40E,495 44D6: MOVFF 40D,494 44DA: MOVFF 40C,493 44DE: MOVFF 40B,492 44E2: MOVFF 406,499 44E6: MOVFF 405,498 44EA: MOVFF 404,497 44EE: MOVFF 403,496 44F2: CALL 0CC2 44F6: MOVFF 00,410 44FA: MOVFF 01,411 44FE: MOVFF 02,412 4502: MOVFF 03,413 4506: MOVFF 03,495 450A: MOVFF 02,494 450E: MOVFF 01,493 4512: MOVFF 00,492 4516: MOVFF 406,499 451A: MOVFF 405,498 451E: MOVFF 404,497 4522: MOVFF 403,496 4526: CALL 0CC2 452A: MOVFF 00,414 452E: MOVFF 01,415 4532: MOVFF 02,416 4536: MOVFF 03,417 453A: BCF FD8.1 453C: MOVFF 03,49D 4540: MOVFF 02,49C 4544: MOVFF 01,49B 4548: MOVFF 00,49A 454C: MOVLW A4 454E: MOVLB 4 4550: MOVWF xA1 4552: MOVLW DB 4554: MOVWF xA0 4556: MOVLW 67 4558: MOVWF x9F 455A: MOVLW 83 455C: MOVWF x9E 455E: MOVLB 0 4560: CALL 0DEE 4564: MOVFF 03,40E 4568: MOVFF 02,40D 456C: MOVFF 01,40C 4570: MOVFF 00,40B .................... r = r*y*y + qat[3]; 4574: MOVFF 40E,495 4578: MOVFF 40D,494 457C: MOVFF 40C,493 4580: MOVFF 40B,492 4584: MOVFF 406,499 4588: MOVFF 405,498 458C: MOVFF 404,497 4590: MOVFF 403,496 4594: CALL 0CC2 4598: MOVFF 00,410 459C: MOVFF 01,411 45A0: MOVFF 02,412 45A4: MOVFF 03,413 45A8: MOVFF 03,495 45AC: MOVFF 02,494 45B0: MOVFF 01,493 45B4: MOVFF 00,492 45B8: MOVFF 406,499 45BC: MOVFF 405,498 45C0: MOVFF 404,497 45C4: MOVFF 403,496 45C8: CALL 0CC2 45CC: MOVFF 00,414 45D0: MOVFF 01,415 45D4: MOVFF 02,416 45D8: MOVFF 03,417 45DC: BCF FD8.1 45DE: MOVFF 03,49D 45E2: MOVFF 02,49C 45E6: MOVFF 01,49B 45EA: MOVFF 00,49A 45EE: MOVLW 33 45F0: MOVLB 4 45F2: MOVWF xA1 45F4: MOVLW 8C 45F6: MOVWF xA0 45F8: MOVLW 1E 45FA: MOVWF x9F 45FC: MOVLW 83 45FE: MOVWF x9E 4600: MOVLB 0 4602: CALL 0DEE 4606: MOVFF 03,40E 460A: MOVFF 02,40D 460E: MOVFF 01,40C 4612: MOVFF 00,40B .................... .................... res = y*res/r; 4616: MOVFF 406,495 461A: MOVFF 405,494 461E: MOVFF 404,493 4622: MOVFF 403,492 4626: MOVFF 40A,499 462A: MOVFF 409,498 462E: MOVFF 408,497 4632: MOVFF 407,496 4636: CALL 0CC2 463A: MOVFF 00,410 463E: MOVFF 01,411 4642: MOVFF 02,412 4646: MOVFF 03,413 464A: MOVFF 03,496 464E: MOVFF 02,495 4652: MOVFF 01,494 4656: MOVFF 00,493 465A: MOVFF 40E,49A 465E: MOVFF 40D,499 4662: MOVFF 40C,498 4666: MOVFF 40B,497 466A: CALL 1066 466E: MOVFF 03,40A 4672: MOVFF 02,409 4676: MOVFF 01,408 467A: MOVFF 00,407 .................... .................... .................... if (flag) // for |x| > 1 467E: MOVLB 4 4680: BTFSS x0F.1 4682: BRA 46BE .................... res = PI_DIV_BY_TWO - res; 4684: BSF FD8.1 4686: MOVLW DB 4688: MOVWF x9D 468A: MOVLW 0F 468C: MOVWF x9C 468E: MOVLW 49 4690: MOVWF x9B 4692: MOVLW 7F 4694: MOVWF x9A 4696: MOVFF 40A,4A1 469A: MOVFF 409,4A0 469E: MOVFF 408,49F 46A2: MOVFF 407,49E 46A6: MOVLB 0 46A8: CALL 0DEE 46AC: MOVFF 03,40A 46B0: MOVFF 02,409 46B4: MOVFF 01,408 46B8: MOVFF 00,407 46BC: MOVLB 4 .................... if (s) 46BE: BTFSS x0F.0 46C0: BRA 46C8 .................... res = -res; 46C2: MOVF x08,W 46C4: XORLW 80 46C6: MOVWF x08 .................... .................... return(res); 46C8: MOVFF 407,00 46CC: MOVFF 408,01 46D0: MOVFF 409,02 46D4: MOVFF 40A,03 .................... } 46D8: MOVLB 0 46DA: RETLW 00 .................... .................... ///////////////////////////////////////////////////////////////////////////// .................... // float atan2(float y, float x) .................... ///////////////////////////////////////////////////////////////////////////// .................... // Description :computes the principal value of arc tangent of y/x, using the .................... // signs of both the arguments to determine the quadrant of the return value .................... // Returns : returns the arc tangent of y/x. .................... // Date : N/A .................... // .................... .................... .................... float atan2(float y,float x) .................... { .................... float z; .................... int1 sign; .................... int quad; .................... sign=0; 46DC: MOVLB 3 46DE: BCF xFD.0 .................... quad=0; //quadrant 46E0: CLRF xFE .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 46E2: MOVFF 3F4,43D 46E6: MOVFF 3F3,43C 46EA: MOVFF 3F2,43B 46EE: MOVFF 3F1,43A 46F2: MOVLB 4 46F4: CLRF x41 46F6: CLRF x40 46F8: CLRF x3F 46FA: CLRF x3E 46FC: MOVLB 0 46FE: CALL 2360 4702: BC 4706 4704: BNZ 4732 4706: MOVFF 3F8,43D 470A: MOVFF 3F7,43C 470E: MOVFF 3F6,43B 4712: MOVFF 3F5,43A 4716: MOVLB 4 4718: CLRF x41 471A: CLRF x40 471C: CLRF x3F 471E: CLRF x3E 4720: MOVLB 0 4722: CALL 2360 4726: BC 472A 4728: BNZ 472E 472A: MOVLW 03 472C: BRA 4730 472E: MOVLW 04 4730: BRA 475A 4732: MOVFF 3F8,43D 4736: MOVFF 3F7,43C 473A: MOVFF 3F6,43B 473E: MOVFF 3F5,43A 4742: MOVLB 4 4744: CLRF x41 4746: CLRF x40 4748: CLRF x3F 474A: CLRF x3E 474C: MOVLB 0 474E: CALL 2360 4752: BNC 4758 4754: MOVLW 02 4756: BRA 475A 4758: MOVLW 01 475A: MOVLB 3 475C: MOVWF xFE .................... if(y<0.0) 475E: MOVFF 3F4,43D 4762: MOVFF 3F3,43C 4766: MOVFF 3F2,43B 476A: MOVFF 3F1,43A 476E: MOVLB 4 4770: CLRF x41 4772: CLRF x40 4774: CLRF x3F 4776: CLRF x3E 4778: MOVLB 0 477A: CALL 2360 477E: BNC 478A .................... { .................... sign=1; 4780: MOVLB 3 4782: BSF xFD.0 .................... y=-y; 4784: MOVF xF2,W 4786: XORLW 80 4788: MOVWF xF2 .................... } .................... if(x<0.0) 478A: MOVFF 3F8,43D 478E: MOVFF 3F7,43C 4792: MOVFF 3F6,43B 4796: MOVFF 3F5,43A 479A: MOVLB 4 479C: CLRF x41 479E: CLRF x40 47A0: CLRF x3F 47A2: CLRF x3E 47A4: MOVLB 0 47A6: CALL 2360 47AA: BNC 47B4 .................... { .................... x=-x; 47AC: MOVLB 3 47AE: MOVF xF6,W 47B0: XORLW 80 47B2: MOVWF xF6 .................... } .................... if (x==0.0) 47B4: MOVFF 3F8,43D 47B8: MOVFF 3F7,43C 47BC: MOVFF 3F6,43B 47C0: MOVFF 3F5,43A 47C4: MOVLB 4 47C6: CLRF x41 47C8: CLRF x40 47CA: CLRF x3F 47CC: CLRF x3E 47CE: MOVLB 0 47D0: CALL 2360 47D4: BNZ 482A .................... { .................... if(y==0.0) 47D6: MOVFF 3F4,43D 47DA: MOVFF 3F3,43C 47DE: MOVFF 3F2,43B 47E2: MOVFF 3F1,43A 47E6: MOVLB 4 47E8: CLRF x41 47EA: CLRF x40 47EC: CLRF x3F 47EE: CLRF x3E 47F0: MOVLB 0 47F2: CALL 2360 47F6: BNZ 47FA .................... { .................... #ifdef _ERRNO .................... { .................... errno=EDOM; .................... } .................... #endif .................... } .................... else 47F8: BRA 4828 .................... { .................... if(sign) 47FA: MOVLB 3 47FC: BTFSS xFD.0 47FE: BRA 4814 .................... { .................... return (-(PI_DIV_BY_TWO)); 4800: MOVLW 7F 4802: MOVWF 00 4804: MOVLW C9 4806: MOVWF 01 4808: MOVLW 0F 480A: MOVWF 02 480C: MOVLW DB 480E: MOVWF 03 4810: BRA 4962 .................... } .................... else 4812: BRA 4826 .................... { .................... return (PI_DIV_BY_TWO); 4814: MOVLW 7F 4816: MOVWF 00 4818: MOVLW 49 481A: MOVWF 01 481C: MOVLW 0F 481E: MOVWF 02 4820: MOVLW DB 4822: MOVWF 03 4824: BRA 4962 4826: MOVLB 0 .................... } .................... } .................... } .................... else 4828: BRA 4962 .................... { .................... z=y/x; 482A: MOVFF 3F4,496 482E: MOVFF 3F3,495 4832: MOVFF 3F2,494 4836: MOVFF 3F1,493 483A: MOVFF 3F8,49A 483E: MOVFF 3F7,499 4842: MOVFF 3F6,498 4846: MOVFF 3F5,497 484A: CALL 1066 484E: MOVFF 03,3FC 4852: MOVFF 02,3FB 4856: MOVFF 01,3FA 485A: MOVFF 00,3F9 .................... switch(quad) .................... { 485E: MOVLW 01 4860: MOVLB 3 4862: SUBWF xFE,W 4864: ADDLW FC 4866: BTFSC FD8.0 4868: BRA 4962 486A: ADDLW 04 486C: MOVLB 0 486E: GOTO 4968 .................... case 1: .................... { .................... return atan(z); 4872: MOVFF 3FC,402 4876: MOVFF 3FB,401 487A: MOVFF 3FA,400 487E: MOVFF 3F9,3FF 4882: RCALL 41AE 4884: MOVLB 3 4886: BRA 4962 .................... break; 4888: BRA 4962 488A: MOVLB 0 .................... } .................... case 2: .................... { .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 .................... return (PI-atan(z)); 488C: MOVFF 3FC,402 4890: MOVFF 3FB,401 4894: MOVFF 3FA,400 4898: MOVFF 3F9,3FF 489C: RCALL 41AE 489E: MOVFF FEA,400 48A2: MOVFF FE9,3FF 48A6: BSF FD8.1 48A8: MOVLW DB 48AA: MOVLB 4 48AC: MOVWF x9D 48AE: MOVLW 0F 48B0: MOVWF x9C 48B2: MOVLW 49 48B4: MOVWF x9B 48B6: MOVLW 80 48B8: MOVWF x9A 48BA: MOVFF 03,4A1 48BE: MOVFF 02,4A0 48C2: MOVFF 01,49F 48C6: MOVFF 00,49E 48CA: MOVLB 0 48CC: CALL 0DEE 48D0: MOVFF 400,FEA 48D4: MOVFF 3FF,FE9 48D8: MOVLB 3 48DA: BRA 4962 .................... break; 48DC: BRA 4962 48DE: MOVLB 0 .................... } .................... case 3: .................... { .................... return (atan(z)-PI); 48E0: MOVFF 3FC,402 48E4: MOVFF 3FB,401 48E8: MOVFF 3FA,400 48EC: MOVFF 3F9,3FF 48F0: RCALL 41AE 48F2: MOVFF 00,3FF 48F6: MOVFF 01,400 48FA: MOVFF 02,401 48FE: MOVFF 03,402 4902: MOVFF FEA,404 4906: MOVFF FE9,403 490A: BSF FD8.1 490C: MOVFF 03,49D 4910: MOVFF 02,49C 4914: MOVFF 01,49B 4918: MOVFF 00,49A 491C: MOVLW DB 491E: MOVLB 4 4920: MOVWF xA1 4922: MOVLW 0F 4924: MOVWF xA0 4926: MOVLW 49 4928: MOVWF x9F 492A: MOVLW 80 492C: MOVWF x9E 492E: MOVLB 0 4930: CALL 0DEE 4934: MOVFF 404,FEA 4938: MOVFF 403,FE9 493C: MOVLB 3 493E: BRA 4962 .................... break; 4940: BRA 4962 4942: MOVLB 0 .................... } .................... case 4: .................... { .................... return (-atan(z)); 4944: MOVFF 3FC,402 4948: MOVFF 3FB,401 494C: MOVFF 3FA,400 4950: MOVFF 3F9,3FF 4954: RCALL 41AE 4956: MOVLW 80 4958: XORWF 01,F 495A: MOVLB 3 495C: BRA 4962 .................... break; 495E: BRA 4962 4960: MOVLB 3 4962: MOVLB 0 .................... } .................... } .................... } .................... } 4964: GOTO 52F2 (RETURN) .................... .................... //////////////////// Hyperbolic functions //////////////////// .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float cosh(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the hyperbolic cosine value of x .................... // Returns : returns the hyperbolic cosine value of x .................... // Date : N/A .................... // .................... .................... float cosh(float x) .................... { .................... return ((exp(x)+exp(-x))/2); .................... } .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float sinh(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the hyperbolic sine value of x .................... // Returns : returns the hyperbolic sine value of x .................... // Date : N/A .................... // .................... .................... float sinh(float x) .................... { .................... .................... return ((exp(x) - exp(-x))/2); .................... } .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float tanh(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the hyperbolic tangent value of x .................... // Returns : returns the hyperbolic tangent value of x .................... // Date : N/A .................... // .................... .................... float tanh(float x) .................... { .................... return(sinh(x)/cosh(x)); .................... } .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float frexp(float x, signed int *exp) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : breaks a floating point number into a normalized fraction and an integral .................... // power of 2. It stores the integer in the signed int object pointed to by exp. .................... // Returns : returns the value x, such that x is a double with magnitude in the interval .................... // [1/2,1) or zero, and value equals x times 2 raised to the power *exp.If value is zero, .................... // both parts of the result are zero. .................... // Date : N/A .................... // .................... .................... .................... .................... #define LOG2 .30102999566398119521 .................... float frexp(float x, signed int *exp) .................... { .................... float res; .................... int1 sign = 0; .................... if(x == 0.0) .................... { .................... *exp=0; .................... return (0.0); .................... } .................... if(x < 0.0) .................... { .................... x=-x; .................... sign=1; .................... } .................... if (x > 1.0) .................... { .................... *exp=(ceil(log10(x)/LOG2)); .................... res=x/(pow(2, *exp)); .................... if (res == 1) .................... { .................... *exp=*exp+1; .................... res=.5; .................... } .................... } .................... else .................... { .................... if(x < 0.5) .................... { .................... *exp=-1; .................... res=x*2; .................... } .................... else .................... { .................... *exp=0; .................... res=x; .................... } .................... } .................... if(sign) .................... { .................... res=-res; .................... } .................... return res; .................... } .................... .................... ////////////////////////////////////////////////////////////////////////////// .................... // float ldexp(float x, signed int *exp) .................... ////////////////////////////////////////////////////////////////////////////// .................... // Description : multiplies a floating point number by an integral power of 2. .................... // Returns : returns the value of x times 2 raised to the power exp. .................... // Date : N/A .................... // .................... .................... float ldexp(float value, signed int exp) .................... { .................... return (value * pow(2,exp)); .................... } .................... #endif .................... .................... #include .................... //////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... //////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef _STRING .................... #define _STRING .................... #include .................... #include .................... .................... .................... .................... ////////////////////////////////////////////// .................... //// Uncomment the following define to //// .................... //// allow some functions to use a //// .................... //// quicker algorithm, but use more ROM //// .................... //// //// .................... //// #define FASTER_BUT_MORE_ROM //// .................... ////////////////////////////////////////////// .................... .................... .................... .................... /*Copying functions*/ .................... /* standard template: .................... void *memmove(void *s1, void *s2, size_t n). .................... Copies max of n characters safely (not following ending '\0') .................... from s2 in s1; if s2 has less than n characters, appends 0 */ .................... .................... char *memmove(void *s1,char *s2,size_t n) .................... { .................... char *sc1; .................... char *sc2; .................... sc1=s1; .................... sc2=s2; .................... if(sc2 0 && *s2 != '\0'; n--) .................... *s++ = *s2++; .................... for (; n > 0; n--) .................... *s++ = '\0'; .................... .................... return(s1); .................... } .................... /***********************************************************/ .................... .................... /*concatenation functions*/ .................... /* standard template: char *strcat(char *s1, const char *s2) .................... appends s2 to s1*/ .................... .................... char *strcat(char *s1, char *s2) .................... { .................... char *s; .................... .................... for (s = s1; *s != '\0'; ++s); .................... while(*s2 != '\0') .................... { .................... *s = *s2; .................... ++s; .................... ++s2; .................... } .................... .................... *s = '\0'; .................... return(s1); .................... } .................... /* standard template: char *strncat(char *s1, char *s2,size_t n) .................... appends not more than n characters from s2 to s1*/ .................... .................... char *strncat(char *s1, char *s2, size_t n) .................... { .................... char *s; .................... .................... for (s = s1; *s != '\0'; ++s); .................... while(*s2 != '\0' && 0s2 */ .................... .................... signed int memcmp(void * s1,char *s2,size_t n) .................... { .................... char *su1, *su2; .................... for(su1=s1, su2=s2; 0s2 */ .................... .................... signed int strcmp(char *s1, char *s2) .................... { .................... for (; *s1 == *s2; s1++, s2++) .................... if (*s1 == '\0') .................... return(0); .................... return((*s1 < *s2) ? -1: 1); .................... } .................... /* standard template: int strcoll(const char *s1, const char *s2). .................... Compares s1 & s2; returns -1 if s1s2 */ .................... .................... signed int strcoll(char *s1, char *s2) .................... { .................... for (; *s1 == *s2; s1++, s2++) .................... if (*s1 == '\0') .................... return(0); .................... return((*s1 < *s2) ? -1: 1); .................... } .................... .................... /* standard template: .................... int strncmp(const char *s1, const char *s2, size_t n). .................... Compares max of n characters (not following 0) from s1 to s2; .................... returns same as strcmp */ .................... .................... signed int strncmp(char *s1, char *s2, size_t n) .................... { .................... for (; n > 0; s1++, s2++, n--) .................... if (*s1 != *s2) .................... return((*s1 <*s2) ? -1: 1); .................... else if (*s1 == '\0') .................... return(0); .................... return(0); .................... } .................... /* standard template: .................... int strxfrm(const char *s1, const char *s2, size_t n). .................... transforms maximum of n characters from s2 and places them into s1*/ .................... size_t strxfrm(char *s1, char *s2, size_t n) .................... { .................... char *s; .................... int n1; .................... n1=n; .................... for (s = s1; n > 0 && *s2 != '\0'; n--) .................... *s++ = *s2++; .................... for (; n > 0; n--) .................... *s++ = '\0'; .................... .................... return(n1); .................... } .................... .................... .................... .................... .................... .................... /***********************************************************/ .................... /*Search functions*/ .................... /* standard template: void *memchr(const char *s, int c). .................... Finds first occurrence of c in n characters of s */ .................... .................... char *memchr(void *s,int c,size_t n) .................... { .................... char uc; .................... char *su; .................... uc=c; .................... for(su=s;0= 'A' && *p <='Z') .................... *p += 'a' - 'A'; .................... return(s); .................... } .................... .................... .................... /************************************************************/ .................... .................... .................... #endif .................... .................... #include "globals.c" .................... /** .................... * ROBO is a global variable containing the status of the roboteq controller .................... * .................... **/ .................... struct robo_status .................... { .................... char M1[4]; //Motor Command 1 format: A00 to a7F .................... char M2[4]; //Motor Command 2 format: B7F to b00 .................... int amps1; //amps being consumed by motor1 .................... int amps2; //amps being consumed by motor2 .................... int volt1; //Voltage of motors power supply .................... int volt2; //Voltage of electronics power supply .................... } ROBO; .................... .................... /** .................... * hCONTROL struct contains all the parameters for heading control .................... **/ .................... struct .................... { .................... int1 enabled; //flag that enables / disables heading control .................... float desired; //desired heading .................... float error; //current error .................... float pError; //previous error .................... signed int16 iVal; //integral component .................... signed int16 pVal; //proportional component .................... signed int16 dVal; //derivitive component .................... int iGain; //integral gain .................... int pGain; //proportional gain .................... int dGain; //derivative gain .................... signed int16 PIDtot; //total value of P+I+D components .................... .................... } hCONTROL; .................... .................... /** .................... * This struct contains all the parameters for velocity control .................... **/ .................... struct .................... { .................... int1 enabled; //flag that enables / disables velocity control .................... float desired; //desired velocity .................... float error; //current error .................... float pError; //previous error .................... signed int16 iVal; //integral component .................... signed int16 pVal; //proportional component .................... signed int16 dVal; //derivitive component .................... int iGain; //integral gain .................... int pGain; //proportional gain .................... int dGain; //derivative gain .................... signed int16 PIDtot; //total value of P+I+D components .................... .................... } vCONTROL; .................... .................... #include "waypoints.c" .................... //----OPTIONS---- .................... #define NUM_WAYPOINTS 64 .................... //this is the type of a waypoint .................... typedef struct waypoint .................... { .................... float lat; //waypoint lattitude .................... float lon; //waypoint longitude .................... }; .................... /** .................... * This struct contains all the current waypoints recieved from base .................... * .................... **/ .................... struct .................... { .................... struct waypoint point[NUM_WAYPOINTS]; //an array of waypoints - length is te number of points to support .................... int current; //the index of current waypoint being headed towards .................... float distance; //the distance to the current waypoint from current position in nautical miles .................... float tolerance; //point at which we're close enough to current waypoint to procede to next .................... .................... } WAYPOINTS; .................... .................... #include "prototypes.c" .................... /** .................... * .................... * Function Prototypes for important global functions are put in this file .................... * .................... **/ .................... void parse_gps(); .................... void query_base(); .................... void update_robo(); .................... void query_robo(); .................... void update_compass(); .................... void report_base(); .................... .................... #include "gps.c" .................... //This defines the pins and properties of the GPS Serial Stream .................... #use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8,stream=GPSSTREAM,ERRORS) * 05A2: BTFSS F9E.5 05A4: BRA 05A2 05A6: MOVFF FAB,1F 05AA: MOVFF FAE,01 05AE: BTFSS 1F.1 05B0: BRA 05B6 05B2: BCF FAB.4 05B4: BSF FAB.4 05B6: GOTO 06B6 (RETURN) .................... //----GLOBAL VARIABLES---- .................... char GPSbuffer[72]=""; //this is where the incoming string will be stored .................... int8 BuffIndex = 0; //this is the index of the char array .................... .................... //Test NMEA String to use for debugging .................... //char gps_test[83] = "$GPRMC,184547,A,3648.1513,N,12147.2863,W,000.0,239.9,050707,014.8,E*67"; .................... .................... //GPRMC string used for calculations .................... char GPRMC[83] = ""; .................... .................... //-----FLAGS------- .................... int1 GPSneedUpdate = 0; //raise this flag if GPSbuffer needs to be written to GPRMC after noUpdate flag is lowered .................... int1 GPSRecieving = FALSE; //this is true if we're accepting characters .................... .................... /** .................... * This struct is nested in the GPS struct .................... * It contains all the GPS parameters .................... **/ .................... struct gps_data .................... { .................... char time[7]; //the UTC time in HHMMSS .................... char status; //Status of Reciever, A=valid V=warning .................... float lat; //Lattitude in Degrees ie:36.80252166666 degrees .................... char latHem; //Lattitude Hemisphere N or S .................... float lon; //Longitude in Degrees, ie: 121.788105 degrees .................... char lonHem; //Longitude Hemisphere E or W .................... float sog; //speed over ground (SOG) in Knots .................... float course; //Course over ground 000.0 to 359.9 degrees true .................... char date[7]; //UTC date in ddmmyy .................... float magVar; //Magnetic variation in degrees 000.0 to 180.000 .................... char magVarDir; //Magnetic Variation Direction E or W .................... .................... } GPS; .................... .................... /** .................... * This struct contains all the data for the current and last state .................... * access properties of this by GPS.now.course .................... **/ .................... //struct gps .................... //{ .................... // struct gps_data now; //current state of the system .................... // struct gps_data last; //The last state of the system .................... //} GPS; .................... .................... /** .................... * .................... * This function takes in a GRPMC STRING and parses it .................... * Written by Todd Berk, toddberk@gmail.com 2007 .................... **/ .................... void parse_gps() .................... { .................... //Define Local Variables .................... char separators[3] = ","; //separators in GPRMC string * 2C32: MOVLW 2C 2C34: MOVLB 3 2C36: MOVWF xD1 2C38: CLRF xD2 2C3A: CLRF xD3 .................... char parts[12][13]; //The parts of the GPRMC string, ie .................... char *part; //Pointer Used while splitting string .................... int i=0; //counter used in loops 2C3C: MOVLB 4 2C3E: CLRF x72 .................... char degrees[4]=""; //used for converting ddmm.mmmm to degrees 2C40: CLRF x73 2C42: CLRF x74 2C44: CLRF x75 2C46: CLRF x76 .................... char minutes[10]=""; //used for converting ddmm.mmmm to degrees 2C48: CLRF x77 2C4A: CLRF x78 2C4C: CLRF x79 2C4E: CLRF x7A 2C50: CLRF x7B 2C52: CLRF x7C 2C54: CLRF x7D 2C56: CLRF x7E 2C58: CLRF x7F 2C5A: CLRF x80 .................... .................... //check if we've recieved new GPS string to process .................... //perform update if flag is raised .................... if(GPSneedUpdate && !GPSRecieving) 2C5C: MOVLB 3 2C5E: BTFSS x05.0 2C60: BRA 2C82 2C62: BTFSC x05.1 2C64: BRA 2C82 .................... { .................... //copy buffer to string .................... strcpy(GPRMC,GPSbuffer); 2C66: MOVLW 02 2C68: MOVWF FEA 2C6A: MOVLW B2 2C6C: MOVWF FE9 2C6E: MOVLW 02 2C70: MOVWF FE2 2C72: MOVLW 69 2C74: MOVWF FE1 2C76: MOVF FE7,F 2C78: MOVFF FE6,FEE 2C7C: BNZ 2C76 .................... //lower needupdate flag .................... GPSneedUpdate = 0; 2C7E: BCF x05.0 .................... } else 2C80: BRA 2C84 .................... { .................... //nothings changed, so we return .................... return; 2C82: BRA 2F92 .................... } .................... .................... //First Split string into array of string parameters .................... //this first call ensures the string contains separators .................... part = strtok(GPRMC,separators); 2C84: MOVLW 02 2C86: MOVLB 4 2C88: MOVWF x82 2C8A: MOVLW B2 2C8C: MOVWF x81 2C8E: MOVLW 03 2C90: MOVWF x84 2C92: MOVLW D1 2C94: MOVWF x83 2C96: MOVLB 0 2C98: CALL 0B76 2C9C: MOVFF 02,471 2CA0: MOVFF 01,470 .................... while(part!=0) .................... { 2CA4: MOVLB 4 2CA6: MOVF x70,F 2CA8: BNZ 2CAE 2CAA: MOVF x71,F 2CAC: BZ 2CFE .................... strcpy(parts[i], part);//*REPLACE THIS WILL CALL TO GPSAPPEND(part,i) function 2CAE: MOVF x72,W 2CB0: MULLW 0D 2CB2: MOVF FF3,W 2CB4: CLRF 03 2CB6: ADDLW D4 2CB8: MOVWF 01 2CBA: MOVLW 03 2CBC: ADDWFC 03,F 2CBE: MOVFF 01,481 2CC2: MOVFF 03,482 2CC6: MOVFF 03,FEA 2CCA: MOVFF 01,FE9 2CCE: MOVFF 471,FE2 2CD2: MOVFF 470,FE1 2CD6: MOVF FE7,F 2CD8: MOVFF FE6,FEE 2CDC: BNZ 2CD6 .................... part = strtok(0,separators); 2CDE: CLRF x82 2CE0: CLRF x81 2CE2: MOVLW 03 2CE4: MOVWF x84 2CE6: MOVLW D1 2CE8: MOVWF x83 2CEA: MOVLB 0 2CEC: CALL 0B76 2CF0: MOVFF 02,471 2CF4: MOVFF 01,470 .................... i++; 2CF8: MOVLB 4 2CFA: INCF x72,F .................... } 2CFC: BRA 2CA6 .................... .................... //*BELOW HERE SHOULD BE SPLIT INTO A NEW FUNCTION AND CALLED .................... //FROM while(part!=0) TO REDUCE MEMORY FROM 12x13 ARRAY .................... .................... //Append new parameters to GPS .................... //time .................... strcpy(GPS.time,parts[1]); 2CFE: MOVLW 03 2D00: MOVWF FEA 2D02: MOVLW 06 2D04: MOVWF FE9 2D06: MOVLW 03 2D08: MOVWF FE2 2D0A: MOVLW E1 2D0C: MOVWF FE1 2D0E: MOVF FE7,F 2D10: MOVFF FE6,FEE 2D14: BNZ 2D0E .................... //status .................... GPS.status = parts[2][0]; 2D16: MOVFF 3EE,30D .................... //lattitude .................... //We need to convert from ddmm.mmmm to degrees .................... //point to lat .................... part=&parts[3][0]; 2D1A: MOVLW 03 2D1C: MOVWF x71 2D1E: MOVLW FB 2D20: MOVWF x70 .................... //copy first two character to degrees .................... strncpy(degrees,part,2); 2D22: MOVLW 04 2D24: MOVWF x82 2D26: MOVLW 73 2D28: MOVWF x81 2D2A: MOVFF 471,484 2D2E: MOVFF 470,483 2D32: MOVLW 02 2D34: MOVWF x85 2D36: MOVLB 0 2D38: RCALL 2BB4 .................... //increment part twice .................... part++;part++; 2D3A: MOVLB 4 2D3C: INCF x70,F 2D3E: BTFSC FD8.2 2D40: INCF x71,F 2D42: INCF x70,F 2D44: BTFSC FD8.2 2D46: INCF x71,F .................... //copy rest of string to minutes .................... strcpy(minutes,part); 2D48: MOVLW 04 2D4A: MOVWF FEA 2D4C: MOVLW 77 2D4E: MOVWF FE9 2D50: MOVFF 471,FE2 2D54: MOVFF 470,FE1 2D58: MOVF FE7,F 2D5A: MOVFF FE6,FEE 2D5E: BNZ 2D58 .................... //convert and set lattitude in degrees .................... GPS.lat = atof(degrees)+(atof(minutes)/60); 2D60: MOVLW 04 2D62: MOVWF x86 2D64: MOVLW 73 2D66: MOVWF x85 2D68: MOVLB 0 2D6A: CALL 11C4 2D6E: MOVFF 00,481 2D72: MOVFF 01,482 2D76: MOVFF 02,483 2D7A: MOVFF 03,484 2D7E: MOVLW 04 2D80: MOVLB 4 2D82: MOVWF x86 2D84: MOVLW 77 2D86: MOVWF x85 2D88: MOVLB 0 2D8A: CALL 11C4 2D8E: MOVFF 00,485 2D92: MOVFF 01,486 2D96: MOVFF 02,487 2D9A: MOVFF 03,488 2D9E: MOVFF 03,496 2DA2: MOVFF 02,495 2DA6: MOVFF 01,494 2DAA: MOVFF 00,493 2DAE: MOVLB 4 2DB0: CLRF x9A 2DB2: CLRF x99 2DB4: MOVLW 70 2DB6: MOVWF x98 2DB8: MOVLW 84 2DBA: MOVWF x97 2DBC: MOVLB 0 2DBE: CALL 1066 2DC2: MOVFF FEA,48A 2DC6: MOVFF FE9,489 2DCA: BCF FD8.1 2DCC: MOVFF 484,49D 2DD0: MOVFF 483,49C 2DD4: MOVFF 482,49B 2DD8: MOVFF 481,49A 2DDC: MOVFF 03,4A1 2DE0: MOVFF 02,4A0 2DE4: MOVFF 01,49F 2DE8: MOVFF 00,49E 2DEC: CALL 0DEE 2DF0: MOVFF 48A,FEA 2DF4: MOVFF 489,FE9 2DF8: MOVFF 03,311 2DFC: MOVFF 02,310 2E00: MOVFF 01,30F 2E04: MOVFF 00,30E .................... //lattitude hemisphere .................... GPS.latHem = parts[4][0]; 2E08: MOVFF 408,312 .................... //Longitude .................... //We need to convert from ddmm.mmmm to degrees .................... //point to lng .................... part=&parts[5][0]; 2E0C: MOVLW 04 2E0E: MOVLB 4 2E10: MOVWF x71 2E12: MOVLW 15 2E14: MOVWF x70 .................... //copy first three characters to degrees .................... strncpy(degrees,part,3); 2E16: MOVLW 04 2E18: MOVWF x82 2E1A: MOVLW 73 2E1C: MOVWF x81 2E1E: MOVFF 471,484 2E22: MOVFF 470,483 2E26: MOVLW 03 2E28: MOVWF x85 2E2A: MOVLB 0 2E2C: RCALL 2BB4 .................... //increment part thrice .................... part++;part++;part++; 2E2E: MOVLB 4 2E30: INCF x70,F 2E32: BTFSC FD8.2 2E34: INCF x71,F 2E36: INCF x70,F 2E38: BTFSC FD8.2 2E3A: INCF x71,F 2E3C: INCF x70,F 2E3E: BTFSC FD8.2 2E40: INCF x71,F .................... //copy rest of string to minutes .................... strcpy(minutes,part); 2E42: MOVLW 04 2E44: MOVWF FEA 2E46: MOVLW 77 2E48: MOVWF FE9 2E4A: MOVFF 471,FE2 2E4E: MOVFF 470,FE1 2E52: MOVF FE7,F 2E54: MOVFF FE6,FEE 2E58: BNZ 2E52 .................... //convert and set longitude in degrees .................... GPS.lon = atof(degrees)+atof(minutes)/60; 2E5A: MOVLW 04 2E5C: MOVWF x86 2E5E: MOVLW 73 2E60: MOVWF x85 2E62: MOVLB 0 2E64: CALL 11C4 2E68: MOVFF 00,481 2E6C: MOVFF 01,482 2E70: MOVFF 02,483 2E74: MOVFF 03,484 2E78: MOVLW 04 2E7A: MOVLB 4 2E7C: MOVWF x86 2E7E: MOVLW 77 2E80: MOVWF x85 2E82: MOVLB 0 2E84: CALL 11C4 2E88: MOVFF 00,485 2E8C: MOVFF 01,486 2E90: MOVFF 02,487 2E94: MOVFF 03,488 2E98: MOVFF 03,496 2E9C: MOVFF 02,495 2EA0: MOVFF 01,494 2EA4: MOVFF 00,493 2EA8: MOVLB 4 2EAA: CLRF x9A 2EAC: CLRF x99 2EAE: MOVLW 70 2EB0: MOVWF x98 2EB2: MOVLW 84 2EB4: MOVWF x97 2EB6: MOVLB 0 2EB8: CALL 1066 2EBC: MOVFF FEA,48A 2EC0: MOVFF FE9,489 2EC4: BCF FD8.1 2EC6: MOVFF 484,49D 2ECA: MOVFF 483,49C 2ECE: MOVFF 482,49B 2ED2: MOVFF 481,49A 2ED6: MOVFF 03,4A1 2EDA: MOVFF 02,4A0 2EDE: MOVFF 01,49F 2EE2: MOVFF 00,49E 2EE6: CALL 0DEE 2EEA: MOVFF 48A,FEA 2EEE: MOVFF 489,FE9 2EF2: MOVFF 03,316 2EF6: MOVFF 02,315 2EFA: MOVFF 01,314 2EFE: MOVFF 00,313 .................... //longitude hemisphere .................... GPS.lonHem = parts[6][0]; 2F02: MOVFF 422,317 .................... //velocity .................... GPS.sog = atof(parts[7]); 2F06: MOVLW 04 2F08: MOVLB 4 2F0A: MOVWF x86 2F0C: MOVLW 2F 2F0E: MOVWF x85 2F10: MOVLB 0 2F12: CALL 11C4 2F16: MOVFF 03,31B 2F1A: MOVFF 02,31A 2F1E: MOVFF 01,319 2F22: MOVFF 00,318 .................... //course .................... GPS.course = atof(parts[8]); 2F26: MOVLW 04 2F28: MOVLB 4 2F2A: MOVWF x86 2F2C: MOVLW 3C 2F2E: MOVWF x85 2F30: MOVLB 0 2F32: CALL 11C4 2F36: MOVFF 03,31F 2F3A: MOVFF 02,31E 2F3E: MOVFF 01,31D 2F42: MOVFF 00,31C .................... //date .................... strcpy(GPS.date,parts[9]); 2F46: MOVLW 03 2F48: MOVWF FEA 2F4A: MOVLW 20 2F4C: MOVWF FE9 2F4E: MOVLW 04 2F50: MOVWF FE2 2F52: MOVLW 49 2F54: MOVWF FE1 2F56: MOVF FE7,F 2F58: MOVFF FE6,FEE 2F5C: BNZ 2F56 .................... //magnetic variation .................... GPS.magVar = atof(parts[10]); 2F5E: MOVLW 04 2F60: MOVLB 4 2F62: MOVWF x86 2F64: MOVLW 56 2F66: MOVWF x85 2F68: MOVLB 0 2F6A: CALL 11C4 2F6E: MOVFF 03,32A 2F72: MOVFF 02,329 2F76: MOVFF 01,328 2F7A: MOVFF 00,327 .................... //magnetic variation direction .................... GPS.magVarDir = parts[11][0]; 2F7E: MOVFF 463,32B .................... .................... //---error handing--- .................... //If status == 'V' then we don't have a GPS lock, and SOG is being falsely reported .................... if(GPS.status == 'V') 2F82: MOVLB 3 2F84: MOVF x0D,W 2F86: SUBLW 56 2F88: BNZ 2F92 .................... { .................... GPS.sog = 0; 2F8A: CLRF x1B 2F8C: CLRF x1A 2F8E: CLRF x19 2F90: CLRF x18 .................... } .................... } 2F92: MOVLB 0 2F94: GOTO 6AFC (RETURN) .................... .................... #include "interrupts.c" .................... /** .................... *THIS IS A SERIAL INTERRUPT .................... *IT OCCURS WHEN THE GPS IS OUTPUTTING A STRING .................... *it inputs individual characters .................... **/ .................... .................... #int_rda .................... void rda_isr(void) .................... { .................... //ch is the incoming character .................... char ch; .................... //get incoming character from GPS Stream .................... ch = fgetc(GPSSTREAM); * 06B4: BRA 05A2 06B6: MOVFF 01,4AB .................... if(ch == '$') //$ Marks beginning of a packet 06BA: MOVLB 4 06BC: MOVF xAB,W 06BE: SUBLW 24 06C0: BNZ 06CA .................... { .................... //start accepting packets .................... GPSRecieving = TRUE; 06C2: MOVLB 3 06C4: BSF x05.1 .................... BuffIndex = 0; // Reset buffer index 06C6: MOVLB 2 06C8: CLRF xB1 .................... } .................... //accept packets if we are recieving .................... if(GPSRecieving) 06CA: MOVLB 3 06CC: BTFSS x05.1 06CE: BRA 0736 .................... { .................... if(ch==13 || ch==10) //Got a \r\n end of packet 06D0: MOVLB 4 06D2: MOVF xAB,W 06D4: SUBLW 0D 06D6: BZ 06DE 06D8: MOVF xAB,W 06DA: SUBLW 0A 06DC: BNZ 06EA .................... { .................... //stop recieving .................... GPSRecieving = FALSE; 06DE: MOVLB 3 06E0: BCF x05.1 .................... //we'll let the function update the string by raising a flag .................... GPSneedUpdate = 1; 06E2: BSF x05.0 .................... return; 06E4: BRA 0736 .................... } else { 06E6: BRA 0700 06E8: MOVLB 4 .................... .................... GPSbuffer[BuffIndex] = ch; // Add char to buffer 06EA: CLRF 03 06EC: MOVLB 2 06EE: MOVF xB1,W 06F0: ADDLW 69 06F2: MOVWF FE9 06F4: MOVLW 02 06F6: ADDWFC 03,W 06F8: MOVWF FEA 06FA: MOVFF 4AB,FEF .................... BuffIndex++; // Advance buffer pointer 06FE: INCF xB1,F .................... .................... } .................... .................... if(BuffIndex > 72) //If buffer is full, this is an error 0700: MOVLB 2 0702: MOVF xB1,W 0704: SUBLW 48 0706: BC 0734 .................... { .................... //*HANDLE BUFFER OVERRUN ERROR .................... fprintf(BASESTREAM,"!ERROR:BUFFER-OVERRUN%u",BuffIndex); 0708: MOVLB 4 070A: CLRF xAC 070C: MOVF xAC,W 070E: MOVLB 0 0710: RCALL 009E 0712: MOVLB 4 0714: INCF xAC,F 0716: MOVWF 00 0718: MOVWF xAF 071A: MOVLB 0 071C: RCALL 05BA 071E: MOVLW 15 0720: MOVLB 4 0722: SUBWF xAC,W 0724: BNZ 070C 0726: MOVFF 2B1,4AD 072A: MOVLW 1B 072C: MOVWF xAE 072E: MOVLB 0 0730: RCALL 0638 0732: MOVLB 2 0734: MOVLB 3 .................... } .................... } .................... } .................... .................... .................... #include "query_base.c" .................... //-----OPTIONS----- .................... #define BASE_TIMEOUT 50000 //time to wait for characters in us .................... #define BASE_WATCHDOG_COUNTS 2 //number of timeouts before kill mode takes effect .................... #define BASESTR_LENGTH 96 .................... .................... //-----BUFFER---- 0736: BCF F9E.5 0738: MOVLB 0 073A: GOTO 0058 .................... char basestr[BASESTR_LENGTH] = ""; //rs232 buffer for base stream .................... .................... //-----FLAGS----- .................... int1 base_timeout_error=FALSE; //raised when timeout occurs waiting for incoming rs232 .................... int base_timeout_counts = 0; //number of consecutive base timeouts - used for watchdog timer .................... .................... /** .................... * This struct contains the most recent parameters recieved from base .................... * .................... **/ .................... struct .................... { .................... char mode; //the mode, M=manual, A=autonomous, K=kill (do nothing) .................... int pScale; //power scale to apply to motors .................... } PARAMS; .................... .................... /** .................... * This function will wait for a incoming character a certain amount of time .................... * until it times out and raises the timeout_error flag .................... **/ .................... char base_timed_getc() { .................... int32 timeout=0; * 091C: MOVLB 3 091E: CLRF xD4 0920: CLRF xD5 0922: CLRF xD6 0924: CLRF xD7 .................... base_timeout_error=FALSE; 0926: BCF x05.2 .................... while(!kbhit(BASESTREAM)&&(timeout= BASE_WATCHDOG_COUNTS*/) 201A: MOVF x8C,F 201C: BZ 20A4 .................... { .................... //the shit has hit the fan, stop the boat .................... PARAMS.mode = 'K'; //kill mode 201E: MOVLW 4B 2020: MOVWF x8D .................... //set motors to 0! .................... //set motor 1 command; .................... //for roboteq .................... strcpy(ROBO.M1,"A00"); 2022: CLRF FEA 2024: MOVLW 24 2026: MOVWF FE9 2028: MOVFF FF2,3D2 202C: BCF FF2.7 202E: MOVLW 00 2030: MOVLB 0 2032: CALL 02A8 2036: TBLRD*- 2038: TBLRD*+ 203A: MOVF FF5,W 203C: MOVWF FEE 203E: IORLW 00 2040: BNZ 2038 2042: MOVLB 3 2044: BTFSC xD2.7 2046: BSF FF2.7 .................... //set motor 1 command .................... //for roboteq .................... strcpy(ROBO.M2,"B00"); 2048: CLRF FEA 204A: MOVLW 28 204C: MOVWF FE9 204E: MOVFF FF2,3D2 2052: BCF FF2.7 2054: MOVLW 00 2056: MOVLB 0 2058: CALL 02C6 205C: TBLRD*- 205E: TBLRD*+ 2060: MOVF FF5,W 2062: MOVWF FEE 2064: IORLW 00 2066: BNZ 205E 2068: MOVLB 3 206A: BTFSC xD2.7 206C: BSF FF2.7 .................... //OK, update motors .................... update_robo(); 206E: MOVLB 0 2070: CALL 0974 .................... //let base know we watchdogged .................... fprintf(BASESTREAM,"WATCHDOG KILLED"); 2074: MOVLB 3 2076: CLRF xD2 2078: MOVF xD2,W 207A: MOVLB 0 207C: CALL 02E4 2080: MOVLB 3 2082: INCF xD2,F 2084: MOVWF 00 2086: CLRF 18 2088: BTFSC FF2.7 208A: BSF 18.7 208C: BCF FF2.7 208E: MOVFF FE8,4AF 2092: MOVLB 0 2094: CALL 05BA 2098: BTFSC 18.7 209A: BSF FF2.7 209C: MOVLW 0F 209E: MOVLB 3 20A0: SUBWF xD2,W 20A2: BNZ 2078 .................... } .................... fprintf(BASESTREAM,"BASE TIMEOUT ERROR\r\n"); 20A4: CLRF xD2 20A6: MOVF xD2,W 20A8: MOVLB 0 20AA: CALL 030E 20AE: MOVLB 3 20B0: INCF xD2,F 20B2: MOVWF 00 20B4: CLRF 18 20B6: BTFSC FF2.7 20B8: BSF 18.7 20BA: BCF FF2.7 20BC: MOVFF FE8,4AF 20C0: MOVLB 0 20C2: CALL 05BA 20C6: BTFSC 18.7 20C8: BSF FF2.7 20CA: MOVLW 14 20CC: MOVLB 3 20CE: SUBWF xD2,W 20D0: BNZ 20A6 .................... } else 20D2: BRA 20DC .................... { .................... base_timeout_counts = 0; 20D4: CLRF x8C .................... //!debug output buffer .................... //fprintf(BASESTREAM,"BUFFER:%s\r\n",basestr); .................... .................... //parse data recieved from base .................... parse_base(); 20D6: MOVLB 0 20D8: BRA 1EAC 20DA: MOVLB 3 .................... } .................... .................... } 20DC: MOVLB 0 20DE: GOTO 6AF4 (RETURN) .................... .................... #include "roboteq.c" .................... //DEFINE THE PINS USED FOR THE ROBOTEQ CONTROLLER UART .................... //MUST BE 9600 bause 7 Bit Even Parity .................... #use rs232(baud=9600,xmit=PIN_B5,rcv=PIN_B4,bits=7,PARITY=E,stream=ROBOSTREAM,ERRORS,DISABLE_INTS) * 07C2: MOVFF FF2,3E0 07C6: BCF FF2.7 07C8: MOVLB 3 07CA: CLRF xDF 07CC: BCF F93.5 07CE: BCF F8A.5 07D0: MOVLW 07 07D2: MOVWF 01 07D4: BRA 07D6 07D6: NOP 07D8: BSF 01.7 07DA: BRA 0818 07DC: BCF 01.7 07DE: MOVF xDE,W 07E0: XORWF xDF,F 07E2: RRCF xDE,F 07E4: MOVLB 0 07E6: BTFSC FD8.0 07E8: BSF F8A.5 07EA: BTFSS FD8.0 07EC: BCF F8A.5 07EE: BSF 01.6 07F0: BRA 0816 07F2: BCF 01.6 07F4: DECFSZ 01,F 07F6: BRA 07FA 07F8: BRA 07FE 07FA: MOVLB 3 07FC: BRA 07DE 07FE: BRA 0800 0800: NOP 0802: MOVLB 3 0804: BTFSC xDF.0 0806: BSF F8A.5 0808: BTFSS xDF.0 080A: BCF F8A.5 080C: BSF 01.5 080E: MOVLB 0 0810: BRA 0816 0812: BCF 01.5 0814: BSF F8A.5 0816: MOVLB 3 0818: MOVLW A6 081A: MOVWF FE9 081C: DECFSZ FE9,F 081E: BRA 081C 0820: BRA 0822 0822: NOP 0824: MOVLB 0 0826: BTFSS 01.7 0828: BRA 082E 082A: MOVLB 3 082C: BRA 07DC 082E: BTFSC 01.6 0830: BRA 07F2 0832: BTFSC 01.5 0834: BRA 0812 0836: MOVLB 3 0838: BTFSC xE0.7 083A: BSF FF2.7 083C: MOVLB 0 083E: RETLW 00 * 5FA4: BSF F93.4 5FA6: BTFSC F81.4 5FA8: BRA 5FA6 5FAA: MOVLW 07 5FAC: MOVWF 00 5FAE: MOVLB 3 5FB0: CLRF xE1 5FB2: MOVFF FF2,3E2 5FB6: BCF FF2.7 5FB8: BSF 00.7 5FBA: BRA 5FE0 5FBC: BCF 00.7 5FBE: BRA 5FE0 5FC0: BCF FD8.0 5FC2: BTFSC F81.4 5FC4: BSF FD8.0 5FC6: RRCF xE1,F 5FC8: BSF 00.6 5FCA: BRA 5FE0 5FCC: BCF 00.6 5FCE: DECFSZ 00,F 5FD0: BRA 5FC0 5FD2: BSF 00.5 5FD4: BRA 5FE0 5FD6: BCF 00.5 5FD8: MOVFF 3E1,01 5FDC: RRCF 01,F 5FDE: BRA 5FFA 5FE0: MOVLW A7 5FE2: BTFSC 00.7 5FE4: MOVLW 2D 5FE6: MOVWF 01 5FE8: DECFSZ 01,F 5FEA: BRA 5FE8 5FEC: BTFSC 00.7 5FEE: BRA 5FBC 5FF0: BTFSC 00.6 5FF2: BRA 5FCC 5FF4: BTFSC 00.5 5FF6: BRA 5FD6 5FF8: BRA 5FC0 5FFA: BTFSC xE2.7 5FFC: BSF FF2.7 5FFE: MOVLB 0 6000: GOTO 604A (RETURN) .................... .................... //-------Options------ .................... #define ROBOTEQ_AX3500 1 //Set to 1 if AX3500 controller is being used .................... #define ROBO_TIMEOUT 3500 //Time in microseconds to wait for incoming characters .................... .................... //--------FLAGS------ .................... int1 robo_timeout_error=FALSE; //raised when timeout occurs waiting for serial characters .................... .................... /** .................... * This function will wait for a incoming character a certain amount of time .................... * until it times out and raises the timeout_error flag .................... **/ .................... char robo_timed_getc() { .................... int32 timeout=0; 6004: MOVLB 3 6006: CLRF xDD 6008: CLRF xDE 600A: CLRF xDF 600C: CLRF xE0 .................... robo_timeout_error=FALSE; 600E: BCF x05.3 .................... while(!kbhit(ROBOSTREAM)&&(timeout= 0x30 && rxchar <= 0x39) || (rxchar >=41 && rxchar <=46) ) 6126: MOVLB 3 6128: MOVF xD2,W 612A: SUBLW 2F 612C: BC 6134 612E: MOVF xD2,W 6130: SUBLW 39 6132: BC 6140 6134: MOVF xD2,W 6136: SUBLW 28 6138: BC 615C 613A: MOVF xD2,W 613C: SUBLW 2E 613E: BNC 615C .................... { .................... //accept character .................... rxvalues[i] = rxchar; 6140: CLRF 03 6142: MOVF xD1,W 6144: ADDLW D3 6146: MOVWF FE9 6148: MOVLW 03 614A: ADDWFC 03,W 614C: MOVWF FEA 614E: MOVFF 3D2,FEF .................... i++; 6152: INCF xD1,F .................... //we only need four characters .................... //if we have accepted four good characters, break out of while .................... if(i>=4) 6154: MOVF xD1,W 6156: SUBLW 03 6158: BC 615C .................... { .................... //!DEBUG .................... //fprintf(BASESTREAM,"BROKEOUT\r\n"); .................... break; 615A: BRA 615E .................... } .................... } .................... } 615C: BRA 611A .................... //------Validity Check---------- .................... //if we didn't recieve four characters, we timed out, and should ignore what was returned .................... if(strlen(rxvalues) < 4) 615E: MOVLW 03 6160: MOVWF xDE 6162: MOVLW D3 6164: MOVWF xDD 6166: MOVLB 0 6168: BRA 6060 616A: MOVFF 02,3DE 616E: MOVFF 01,3DD 6172: MOVLB 3 6174: MOVF xDE,F 6176: BNZ 6180 6178: MOVF xDD,W 617A: SUBLW 03 617C: BNC 6180 .................... { .................... return; 617E: BRA 61C0 .................... } .................... //------Parse Value Recieved----- .................... //!DEBUG .................... //fprintf(BASESTREAM,"AMPS1:%X,%X,%X,%X\r\n",rxvalues[0],rxvalues[1],rxvalues[2],rxvalues[3]); .................... //construct value for motor 1 .................... value[2] = rxvalues[0]; 6180: MOVFF 3D3,3DA .................... value[3] = rxvalues[1]; 6184: MOVFF 3D4,3DB .................... //set value for motor 1 .................... ROBO.amps1 = atoi(value); 6188: MOVLW 03 618A: MOVWF xF0 618C: MOVLW D8 618E: MOVWF xEF 6190: MOVLB 0 6192: CALL 147C 6196: MOVFF 01,2C .................... //construct value for motor2 .................... value[2] = rxvalues[2]; 619A: MOVFF 3D5,3DA .................... value[3] = rxvalues[3]; 619E: MOVFF 3D6,3DB .................... //set value for motor 2 .................... ROBO.amps2 = atoi(value); 61A2: MOVLW 03 61A4: MOVLB 3 61A6: MOVWF xF0 61A8: MOVLW D8 61AA: MOVWF xEF 61AC: MOVLB 0 61AE: CALL 147C 61B2: MOVFF 01,2D .................... //divide by 2 if AX3500 - see datasheet .................... if(ROBOTEQ_AX3500) .................... { .................... ROBO.amps1 /= 2; 61B6: BCF FD8.0 61B8: RRCF 2C,F .................... ROBO.amps2 /= 2; 61BA: BCF FD8.0 61BC: RRCF 2D,F 61BE: MOVLB 3 .................... } .................... .................... } 61C0: MOVLB 0 61C2: GOTO 6B18 (RETURN) .................... .................... .................... .................... .................... #include "TCMcompass.c" .................... #include "CRC.c" .................... /////////////////////// Driver to generate CRC ////////////////////////// .................... //// //// .................... //// generate_8bit_crc(data, length, pattern) //// .................... //// Generates 8 bit crc from the data using the pattern. //// .................... //// //// .................... //// generate_16bit_crc(data, length, pattern) //// .................... //// Generates 16 bit crc from the data using the pattern. //// .................... //// //// .................... //// generate_32bit_crc(data, length, pattern) //// .................... //// Generates 32 bit crc from the data using the pattern. //// .................... //// //// .................... ///////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS //// .................... //// C compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, //// .................... //// reproduction or distribution is permitted without written //// .................... //// permission. Derivative programs created using this software //// .................... //// in object code form are not restricted in any way. //// .................... ///////////////////////////////////////////////////////////////////////// .................... .................... #define CRC_16 0x8005 //bit pattern (1)1000 0000 0000 0101 .................... #define CRC_CCITT 0x1021 //bit pattern (1)0001 0000 0010 0001 .................... #define CRC_32 0x04C11DB7 //bit pattern (1)0000 0100 1100 0001 0001 1101 1011 0111 .................... .................... .................... int generate_8bit_crc(char* data, int16 length, int pattern) .................... { .................... int *current_data; .................... int crc_byte; .................... int16 byte_counter; .................... int bit_counter; .................... .................... current_data = data; .................... crc_byte = *current_data++; .................... .................... for(byte_counter=0; byte_counter < (length-1); byte_counter++) .................... { .................... for(bit_counter=0; bit_counter < 8; bit_counter++) .................... { .................... if(!bit_test(crc_byte,7)) .................... { .................... crc_byte <<= 1; .................... bit_test(*current_data, 7 - bit_counter) ? .................... bit_set(crc_byte,0) : bit_clear(crc_byte,0); .................... continue; .................... } .................... crc_byte <<= 1; .................... bit_test(*current_data, 7 - bit_counter) ? .................... bit_set(crc_byte,0) : bit_clear(crc_byte,0); .................... crc_byte ^= pattern; .................... } .................... current_data++; .................... } .................... for(bit_counter=0; bit_counter < 8; bit_counter++) .................... { .................... if(!bit_test(crc_byte,7)) .................... { .................... crc_byte <<= 1; .................... continue; .................... } .................... crc_byte <<= 1; .................... crc_byte ^= pattern; .................... } .................... return crc_byte; .................... } .................... .................... .................... int16 generate_16bit_crc(char* data, int16 length, int16 pattern) .................... { .................... int *current_data; .................... int16 crc_Dbyte; .................... int16 byte_counter; .................... int bit_counter; .................... .................... current_data = data + 2; * 21F6: MOVLW 02 21F8: MOVLB 3 21FA: ADDWF xD9,W 21FC: MOVWF xDF 21FE: MOVLW 00 2200: ADDWFC xDA,W 2202: MOVWF xE0 .................... crc_Dbyte = make16(data[0], data[1]); 2204: MOVFF 3D9,FE9 2208: MOVFF 3DA,FEA 220C: MOVFF FEF,3E6 2210: MOVLW 01 2212: ADDWF xD9,W 2214: MOVWF FE9 2216: MOVLW 00 2218: ADDWFC xDA,W 221A: MOVWF FEA 221C: MOVFF FEF,3E7 2220: MOVFF 3E6,3E2 2224: MOVFF 3E7,3E1 .................... .................... for(byte_counter=0; byte_counter < (length-2); byte_counter++) 2228: CLRF xE4 222A: CLRF xE3 222C: MOVLW 02 222E: SUBWF xDB,W 2230: MOVWF 00 2232: MOVLW 00 2234: SUBWFB xDC,W 2236: MOVWF 03 2238: MOVFF 00,01 223C: MOVF xE4,W 223E: SUBWF 03,W 2240: BNC 22EA 2242: BNZ 224A 2244: MOVF 01,W 2246: SUBWF xE3,W 2248: BC 22EA .................... { .................... for(bit_counter=0; bit_counter < 8; bit_counter++) 224A: CLRF xE5 224C: MOVF xE5,W 224E: SUBLW 07 2250: BNC 22DC .................... { .................... if(!bit_test(crc_Dbyte,15)) 2252: BTFSC xE2.7 2254: BRA 2294 .................... { .................... crc_Dbyte <<= 1; 2256: BCF FD8.0 2258: RLCF xE1,F 225A: RLCF xE2,F .................... bit_test(*current_data, 7 - bit_counter) ? .................... bit_set(crc_Dbyte,0) : bit_clear(crc_Dbyte,0); 225C: MOVFF 3E0,03 2260: MOVFF 3DF,FE9 2264: MOVFF 3E0,FEA 2268: MOVFF FEF,3E6 226C: MOVLW 07 226E: BSF FD8.0 2270: SUBFWB xE5,W 2272: MOVWF xE7 2274: MOVFF 3E6,00 2278: MOVF xE7,W 227A: MOVWF 01 227C: BZ 2286 227E: BCF FD8.0 2280: RRCF 00,F 2282: DECFSZ 01,F 2284: BRA 227E 2286: BTFSS 00.0 2288: BRA 2290 228A: BSF xE1.0 228C: CLRF 03 228E: BRA 2292 2290: BCF xE1.0 .................... continue; 2292: BRA 22D8 .................... } .................... crc_Dbyte <<= 1; 2294: BCF FD8.0 2296: RLCF xE1,F 2298: RLCF xE2,F .................... bit_test(*current_data, 7 - bit_counter) ? .................... bit_set(crc_Dbyte,0) : bit_clear(crc_Dbyte,0); 229A: MOVFF 3E0,03 229E: MOVFF 3DF,FE9 22A2: MOVFF 3E0,FEA 22A6: MOVFF FEF,3E6 22AA: MOVLW 07 22AC: BSF FD8.0 22AE: SUBFWB xE5,W 22B0: MOVWF xE7 22B2: MOVFF 3E6,00 22B6: MOVF xE7,W 22B8: MOVWF 01 22BA: BZ 22C4 22BC: BCF FD8.0 22BE: RRCF 00,F 22C0: DECFSZ 01,F 22C2: BRA 22BC 22C4: BTFSS 00.0 22C6: BRA 22CE 22C8: BSF xE1.0 22CA: CLRF 03 22CC: BRA 22D0 22CE: BCF xE1.0 .................... crc_Dbyte ^= pattern; 22D0: MOVF xDD,W 22D2: XORWF xE1,F 22D4: MOVF xDE,W 22D6: XORWF xE2,F .................... } 22D8: INCF xE5,F 22DA: BRA 224C .................... current_data++; 22DC: INCF xDF,F 22DE: BTFSC FD8.2 22E0: INCF xE0,F .................... } 22E2: INCF xE3,F 22E4: BTFSC FD8.2 22E6: INCF xE4,F 22E8: BRA 222C .................... .................... for(bit_counter=0; bit_counter < 16; bit_counter++) 22EA: CLRF xE5 22EC: MOVF xE5,W 22EE: SUBLW 0F 22F0: BNC 2310 .................... { .................... if(!bit_test(crc_Dbyte,15)) 22F2: BTFSC xE2.7 22F4: BRA 22FE .................... { .................... crc_Dbyte <<= 1; 22F6: BCF FD8.0 22F8: RLCF xE1,F 22FA: RLCF xE2,F .................... continue; 22FC: BRA 230C .................... } .................... crc_Dbyte <<= 1; 22FE: BCF FD8.0 2300: RLCF xE1,F 2302: RLCF xE2,F .................... crc_Dbyte ^= pattern; 2304: MOVF xDD,W 2306: XORWF xE1,F 2308: MOVF xDE,W 230A: XORWF xE2,F .................... } 230C: INCF xE5,F 230E: BRA 22EC .................... .................... return crc_Dbyte; 2310: MOVFF 3E1,01 2314: MOVFF 3E2,02 .................... } 2318: MOVLB 0 231A: GOTO 2984 (RETURN) .................... .................... int32 generate_32bit_crc(char* data, int16 length, int32 pattern) .................... { .................... int *current_data; .................... int32 crc_Dbyte; .................... int16 byte_counter; .................... int bit_counter; .................... .................... current_data = data + 4; .................... crc_Dbyte = make32(data[0], data[1], data[2], data[3]); .................... .................... for(byte_counter=0; byte_counter < (length-4); byte_counter++) .................... { .................... for(bit_counter=0; bit_counter < 8; bit_counter++) .................... { .................... if(!bit_test(crc_Dbyte,31)) .................... { .................... crc_Dbyte <<= 1; .................... bit_test(*current_data, 7 - bit_counter) ? .................... bit_set(crc_Dbyte,0) : bit_clear(crc_Dbyte,0); .................... continue; .................... } .................... crc_Dbyte <<= 1; .................... bit_test(*current_data, 7 - bit_counter) ? .................... bit_set(crc_Dbyte,0) : bit_clear(crc_Dbyte,0); .................... crc_Dbyte ^= pattern; .................... } .................... current_data++; .................... } .................... .................... for(bit_counter=0; bit_counter < 32; bit_counter++) .................... { .................... if(!bit_test(crc_Dbyte,31)) .................... { .................... crc_Dbyte <<= 1; .................... continue; .................... } .................... crc_Dbyte <<= 1; .................... crc_Dbyte ^= pattern; .................... } .................... .................... return crc_Dbyte; .................... } .................... .................... #include .................... /////////////////////////////////////////////////////////////////////////// .................... //// ieeefloat.c //// .................... //// //// .................... //// This library converts IEEE float format to and fro from //// .................... //// the CCS PIC format. //// .................... //// (C) Copyright 1996,2006 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... /* .................... int32 f_PICtoIEEE(float f) .................... PURPOSE: This function converts float from CCS -PIC format to IEEE format in little endian mode .................... PARAMS: Floating point number .................... RETURNS: 32 bit number .................... */ .................... int32 f_PICtoIEEE(float f) .................... { .................... .................... int32 * ret; .................... #locate f=0x20 .................... .................... ret = &f; .................... .................... #if defined(__PCB__) || defined(__PCM__) .................... #asm .................... .................... // switch the bits .................... rlf 0x21 .................... rrf 0x20 .................... rrf 0x21 .................... .................... #endasm .................... #elif defined(__PCH__) .................... #asm .................... .................... // switch the bits .................... rlcf 0x21 .................... rrcf 0x20 .................... rrcf 0x21 .................... .................... #endasm .................... #else .................... #error Invalid complier defined .................... #endif .................... .................... #asm .................... .................... // swap the outer bytes .................... movf 0x20,W .................... xorwf 0x23,W .................... xorwf 0x23,F .................... xorwf 0x23,W .................... movwf 0x20 .................... .................... // swap the inner bytes .................... movf 0x21,W .................... xorwf 0x22,W .................... xorwf 0x22,F .................... xorwf 0x22,W .................... movwf 0x21 .................... .................... #endasm .................... .................... return *ret; .................... } .................... .................... /* .................... float f_IEEEtoPIC(int32 f) .................... PURPOSE: This function converts IEEE format to a float .................... PARAMS: 32 bit number .................... RETURNS: Floating point number .................... */ .................... float f_IEEEtoPIC(int32 f) .................... { .................... .................... float * ret; .................... #locate f=0x20 .................... .................... ret = &f; 231E: MOVLB 3 2320: CLRF xEF 2322: MOVLW 20 2324: MOVWF xEE .................... .................... #asm .................... .................... // swap the outer bytes .................... movf 0x20,W 2326: MOVF 20,W .................... xorwf 0x23,W 2328: XORWF 23,W .................... xorwf 0x23,F 232A: XORWF 23,F .................... xorwf 0x23,W 232C: XORWF 23,W .................... movwf 0x20 232E: MOVWF 20 .................... .................... // swap the inner bytes .................... movf 0x21,W 2330: MOVF 21,W .................... xorwf 0x22,W 2332: XORWF 22,W .................... xorwf 0x22,F 2334: XORWF 22,F .................... xorwf 0x22,W 2336: XORWF 22,W .................... movwf 0x21 2338: MOVWF 21 .................... .................... #endasm .................... .................... #if defined(__PCB__) || defined(__PCM__) .................... #asm .................... .................... // switch the bits .................... rlf 0x21 .................... rlf 0x20 .................... rrf 0x21 .................... .................... #endasm .................... #elif defined(__PCH__) .................... #asm .................... .................... // switch the bits .................... rlcf 0x21 233A: RLCF 21,F .................... rlcf 0x20 233C: RLCF 20,F .................... rrcf 0x21 233E: RRCF 21,F .................... .................... #endasm .................... #else .................... #error Invalid complier defined .................... #endif .................... .................... return *ret; 2340: MOVFF 3EF,03 2344: MOVFF 3EE,FE9 2348: MOVFF 3EF,FEA 234C: MOVFF FEF,00 2350: MOVFF FEC,01 2354: MOVFF FEC,02 2358: MOVFF FEC,03 .................... } 235C: MOVLB 0 235E: RETLW 00 .................... .................... #use rs232(baud=9600,parity=N,xmit=PIN_B1,rcv=PIN_B0,bits=8,stream=COMPSTREAM,ERRORS,DISABLE_INTS) * 0770: MOVFF FF2,3DB 0774: BCF FF2.7 0776: BCF F93.1 0778: BCF F8A.1 077A: MOVLW 08 077C: MOVWF 01 077E: BRA 0780 0780: NOP 0782: BSF 01.7 0784: BRA 07A6 0786: BCF 01.7 0788: MOVLB 3 078A: RRCF xDA,F 078C: MOVLB 0 078E: BTFSC FD8.0 0790: BSF F8A.1 0792: BTFSS FD8.0 0794: BCF F8A.1 0796: BSF 01.6 0798: BRA 07A6 079A: BCF 01.6 079C: DECFSZ 01,F 079E: BRA 0788 07A0: BRA 07A2 07A2: NOP 07A4: BSF F8A.1 07A6: MOVLW A7 07A8: MOVWF FE9 07AA: DECFSZ FE9,F 07AC: BRA 07AA 07AE: BRA 07B0 07B0: BTFSC 01.7 07B2: BRA 0786 07B4: BTFSC 01.6 07B6: BRA 079A 07B8: MOVLB 3 07BA: BTFSC xDB.7 07BC: BSF FF2.7 07BE: MOVLB 0 07C0: RETLW 00 * 20E2: BSF F93.0 20E4: BTFSC F81.0 20E6: BRA 20E4 20E8: MOVLW 08 20EA: MOVWF 00 20EC: MOVLB 3 20EE: CLRF xDE 20F0: MOVFF FF2,3DF 20F4: BCF FF2.7 20F6: BSF 00.7 20F8: BRA 2116 20FA: BCF 00.7 20FC: BRA 2116 20FE: BCF FD8.0 2100: BTFSC F81.0 2102: BSF FD8.0 2104: RRCF xDE,F 2106: BSF 00.6 2108: BRA 2116 210A: BCF 00.6 210C: DECFSZ 00,F 210E: BRA 20FE 2110: MOVFF 3DE,01 2114: BRA 212E 2116: MOVLW A7 2118: BTFSC 00.7 211A: MOVLW 2D 211C: MOVWF 01 211E: DECFSZ 01,F 2120: BRA 211E 2122: NOP 2124: BTFSC 00.7 2126: BRA 20FA 2128: BTFSC 00.6 212A: BRA 210A 212C: BRA 20FE 212E: BTFSC xDF.7 2130: BSF FF2.7 2132: MOVLB 0 2134: GOTO 217E (RETURN) .................... .................... /** .................... * This struct holds the data parsed from the compass .................... * .................... **/ .................... struct .................... { .................... float heading; //heading of compass .................... float temp; //temperature .................... float pitch; //pitch .................... float roll; //roll .................... } COMPASS; .................... .................... //------OPTIONS---- .................... #define COMP_TIMEOUT 12000 //Time to wait for incoming rs232 characters .................... #define IGNORE_CRC 0 //IGNORE CRC ERRORS .................... #define OUTPUT_ERR 1 //OUTPUT ERRORS OVER SERIAL .................... .................... .................... //-----FLAGS------- .................... int1 comp_awake = 0; //is compass awake .................... int1 dataset_sent = 0; //have we specified what data we want, only needs to be done once per startup .................... int1 comp_timeout_error = 0; //raised when timeout waiting for character .................... int1 comp_bad_crc = 0; //raised when CRC check doesn't match - this doesn't always mean bad data .................... .................... //-----Bad Data Thresholds------ .................... //these values are the reasonable boundaries for which BAD CRC data will be accepted .................... #define HEADING_THRESH 5 .................... #define TEMP_THRESH 2 .................... #define PITCH_THRESH 3 .................... #define ROLL_THRESH 3 .................... .................... //----COMM STRINGS--- .................... int const awaken[5] = {0x00,0x05,0x17,0x9D,0x23}; //wakes up compass .................... int const dataset[10] = {0x00,0x0A,0x03,0x04,0x05,0x07,0x18,0x19,0x84,0xBF}; //sets data components to recieve, currently heading, temp, pitch, roll .................... int const polldata[5] = {0x00,0x05,0x04,0xBF,0x71}; //requests the data .................... .................... //-----BUFFER------ .................... int compstr[48] = ""; //holds incoming compass data .................... .................... /** .................... * This function will wait for a incoming character a certain amount of time .................... * until it times out and raises the timeout_error flag .................... **/ .................... char comp_timed_getc() { .................... int32 timeout=0; 2138: MOVLB 3 213A: CLRF xDA 213C: CLRF xDB 213E: CLRF xDC 2140: CLRF xDD .................... comp_timeout_error=FALSE; 2142: BCF x05.6 .................... while(!kbhit(COMPSTREAM)&&(timeout 360)) 2422: MOVLB 3 2424: BTFSS x05.7 2426: BRA 24AE 2428: BSF FD8.1 242A: MOVFF 392,49D 242E: MOVFF 391,49C 2432: MOVFF 390,49B 2436: MOVFF 38F,49A 243A: MOVFF 3ED,4A1 243E: MOVFF 3EC,4A0 2442: MOVFF 3EB,49F 2446: MOVFF 3EA,49E 244A: MOVLB 0 244C: CALL 0DEE 2450: MOVFF 00,3EE 2454: MOVFF 01,3EF 2458: MOVFF 02,3F0 245C: MOVFF 03,3F1 2460: MOVFF 00,00 2464: MOVFF 01,01 2468: MOVFF 02,02 246C: MOVFF 03,03 2470: BCF 01.7 2472: MOVFF 00,3EF 2476: MOVFF 01,3F0 247A: MOVFF 02,3F1 247E: MOVFF 03,3F2 2482: MOVFF 03,43D 2486: MOVFF 02,43C 248A: MOVFF 01,43B 248E: MOVFF 00,43A 2492: MOVLB 4 2494: CLRF x41 2496: CLRF x40 2498: MOVLW 20 249A: MOVWF x3F 249C: MOVLW 81 249E: MOVWF x3E 24A0: MOVLB 0 24A2: RCALL 2360 24A4: BTFSS FD8.0 24A6: BRA 24AC 24A8: MOVLB 3 24AA: BRA 24AE 24AC: BNZ 24FA 24AE: MOVFF 3ED,43D 24B2: MOVFF 3EC,43C 24B6: MOVFF 3EB,43B 24BA: MOVFF 3EA,43A 24BE: MOVLW 0A 24C0: MOVLB 4 24C2: MOVWF x41 24C4: MOVLW D7 24C6: MOVWF x40 24C8: MOVLW 23 24CA: MOVWF x3F 24CC: MOVLW 78 24CE: MOVWF x3E 24D0: MOVLB 0 24D2: RCALL 2360 24D4: BC 24FA 24D6: MOVLB 4 24D8: CLRF x3D 24DA: CLRF x3C 24DC: MOVLW 34 24DE: MOVWF x3B 24E0: MOVLW 87 24E2: MOVWF x3A 24E4: MOVFF 3ED,441 24E8: MOVFF 3EC,440 24EC: MOVFF 3EB,43F 24F0: MOVFF 3EA,43E 24F4: MOVLB 0 24F6: RCALL 2360 24F8: BNC 24FC .................... { //data unaceptable .................... return; 24FA: BRA 2770 .................... } .................... COMPASS.heading = f; 24FC: MOVFF 3ED,392 2500: MOVFF 3EC,391 2504: MOVFF 3EB,390 2508: MOVFF 3EA,38F .................... break; 250C: BRA 2770 .................... case 0x07: //TEMPERATURE .................... f = f_IEEEtoPIC(value); 250E: MOVFF 3E9,23 2512: MOVFF 3E8,22 2516: MOVFF 3E7,21 251A: MOVFF 3E6,20 251E: RCALL 231E 2520: MOVFF 03,3ED 2524: MOVFF 02,3EC 2528: MOVFF 01,3EB 252C: MOVFF 00,3EA .................... if(comp_bad_crc && !(abs(COMPASS.temp-f)<=TEMP_THRESH)) 2530: MOVLB 3 2532: BTFSS x05.7 2534: BRA 25C4 2536: BSF FD8.1 2538: MOVFF 396,49D 253C: MOVFF 395,49C 2540: MOVFF 394,49B 2544: MOVFF 393,49A 2548: MOVFF 3ED,4A1 254C: MOVFF 3EC,4A0 2550: MOVFF 3EB,49F 2554: MOVFF 3EA,49E 2558: MOVLB 0 255A: CALL 0DEE 255E: MOVFF 00,3EE 2562: MOVFF 01,3EF 2566: MOVFF 02,3F0 256A: MOVFF 03,3F1 256E: MOVFF 00,00 2572: MOVFF 01,01 2576: MOVFF 02,02 257A: MOVFF 03,03 257E: BCF 01.7 2580: MOVFF 00,3EF 2584: MOVFF 01,3F0 2588: MOVFF 02,3F1 258C: MOVFF 03,3F2 2590: MOVFF 03,43D 2594: MOVFF 02,43C 2598: MOVFF 01,43B 259C: MOVFF 00,43A 25A0: MOVLB 4 25A2: CLRF x41 25A4: CLRF x40 25A6: CLRF x3F 25A8: MOVLW 80 25AA: MOVWF x3E 25AC: MOVLB 0 25AE: RCALL 2360 25B0: BTFSS FD8.0 25B2: BRA 25B8 25B4: MOVLB 3 25B6: BRA 25C4 25B8: BTFSS FD8.2 25BA: BRA 25C0 25BC: MOVLB 3 25BE: BRA 25C4 .................... { //data unaceptable .................... return; 25C0: BRA 2770 25C2: MOVLB 3 .................... } .................... COMPASS.temp = f; 25C4: MOVFF 3ED,396 25C8: MOVFF 3EC,395 25CC: MOVFF 3EB,394 25D0: MOVFF 3EA,393 .................... break; 25D4: MOVLB 0 25D6: BRA 2770 .................... case 0x18: //PITCH ANGLE .................... f = f_IEEEtoPIC(value); 25D8: MOVFF 3E9,23 25DC: MOVFF 3E8,22 25E0: MOVFF 3E7,21 25E4: MOVFF 3E6,20 25E8: RCALL 231E 25EA: MOVFF 03,3ED 25EE: MOVFF 02,3EC 25F2: MOVFF 01,3EB 25F6: MOVFF 00,3EA .................... if(comp_bad_crc && !(abs(COMPASS.pitch-f)<=PITCH_THRESH)) 25FA: MOVLB 3 25FC: BTFSS x05.7 25FE: BRA 2690 2600: BSF FD8.1 2602: MOVFF 39A,49D 2606: MOVFF 399,49C 260A: MOVFF 398,49B 260E: MOVFF 397,49A 2612: MOVFF 3ED,4A1 2616: MOVFF 3EC,4A0 261A: MOVFF 3EB,49F 261E: MOVFF 3EA,49E 2622: MOVLB 0 2624: CALL 0DEE 2628: MOVFF 00,3EE 262C: MOVFF 01,3EF 2630: MOVFF 02,3F0 2634: MOVFF 03,3F1 2638: MOVFF 00,00 263C: MOVFF 01,01 2640: MOVFF 02,02 2644: MOVFF 03,03 2648: BCF 01.7 264A: MOVFF 00,3EF 264E: MOVFF 01,3F0 2652: MOVFF 02,3F1 2656: MOVFF 03,3F2 265A: MOVFF 03,43D 265E: MOVFF 02,43C 2662: MOVFF 01,43B 2666: MOVFF 00,43A 266A: MOVLB 4 266C: CLRF x41 266E: CLRF x40 2670: MOVLW 40 2672: MOVWF x3F 2674: MOVLW 80 2676: MOVWF x3E 2678: MOVLB 0 267A: RCALL 2360 267C: BTFSS FD8.0 267E: BRA 2684 2680: MOVLB 3 2682: BRA 2690 2684: BTFSS FD8.2 2686: BRA 268C 2688: MOVLB 3 268A: BRA 2690 .................... { //data unaceptable .................... return; 268C: BRA 2770 268E: MOVLB 3 .................... } .................... COMPASS.pitch = f; 2690: MOVFF 3ED,39A 2694: MOVFF 3EC,399 2698: MOVFF 3EB,398 269C: MOVFF 3EA,397 .................... break; 26A0: MOVLB 0 26A2: BRA 2770 .................... case 0x19: //ROLL ANGLE .................... f = f_IEEEtoPIC(value); 26A4: MOVFF 3E9,23 26A8: MOVFF 3E8,22 26AC: MOVFF 3E7,21 26B0: MOVFF 3E6,20 26B4: RCALL 231E 26B6: MOVFF 03,3ED 26BA: MOVFF 02,3EC 26BE: MOVFF 01,3EB 26C2: MOVFF 00,3EA .................... if(comp_bad_crc && !(abs(COMPASS.roll-f)<=ROLL_THRESH)) 26C6: MOVLB 3 26C8: BTFSS x05.7 26CA: BRA 275C 26CC: BSF FD8.1 26CE: MOVFF 39E,49D 26D2: MOVFF 39D,49C 26D6: MOVFF 39C,49B 26DA: MOVFF 39B,49A 26DE: MOVFF 3ED,4A1 26E2: MOVFF 3EC,4A0 26E6: MOVFF 3EB,49F 26EA: MOVFF 3EA,49E 26EE: MOVLB 0 26F0: CALL 0DEE 26F4: MOVFF 00,3EE 26F8: MOVFF 01,3EF 26FC: MOVFF 02,3F0 2700: MOVFF 03,3F1 2704: MOVFF 00,00 2708: MOVFF 01,01 270C: MOVFF 02,02 2710: MOVFF 03,03 2714: BCF 01.7 2716: MOVFF 00,3EF 271A: MOVFF 01,3F0 271E: MOVFF 02,3F1 2722: MOVFF 03,3F2 2726: MOVFF 03,43D 272A: MOVFF 02,43C 272E: MOVFF 01,43B 2732: MOVFF 00,43A 2736: MOVLB 4 2738: CLRF x41 273A: CLRF x40 273C: MOVLW 40 273E: MOVWF x3F 2740: MOVLW 80 2742: MOVWF x3E 2744: MOVLB 0 2746: RCALL 2360 2748: BTFSS FD8.0 274A: BRA 2750 274C: MOVLB 3 274E: BRA 275C 2750: BTFSS FD8.2 2752: BRA 2758 2754: MOVLB 3 2756: BRA 275C .................... { //data unaceptable .................... return; 2758: BRA 2770 275A: MOVLB 3 .................... } .................... COMPASS.roll = f; 275C: MOVFF 3ED,39E 2760: MOVFF 3EC,39D 2764: MOVFF 3EB,39C 2768: MOVFF 3EA,39B .................... break; 276C: MOVLB 0 276E: BRA 2770 .................... } .................... } 2770: GOTO 2B18 (RETURN) .................... .................... /** .................... * This function updates the global COMPASS variable .................... * by polling the compass for data .................... **/ .................... void update_compass() .................... { .................... //Variable definitions .................... int i=0; //used for outside looping 2774: MOVLB 3 2776: CLRF xD1 .................... int length=0; //length of bytes recieved 2778: CLRF xD2 .................... int expect=0; //expected number of bytes to be recieved 277A: CLRF xD3 .................... int16 lCRC=0; //the local calculated CRC16 277C: CLRF xD4 277E: CLRF xD5 .................... int16 rCRC=0; //the remote CRC 2780: CLRF xD6 2782: CLRF xD7 .................... //lower flags in case they're raised .................... comp_bad_crc = 0; 2784: BCF x05.7 .................... comp_timeout_error = 0; 2786: BCF x05.6 .................... //check if compass is awake, .................... if(!comp_awake) 2788: BTFSC x05.4 278A: BRA 27BC .................... { .................... //awaken compass .................... for(i=0;i<5;i++) 278C: CLRF xD1 278E: MOVF xD1,W 2790: SUBLW 04 2792: BNC 27BA .................... { .................... fputc(awaken[i],COMPSTREAM); 2794: CLRF 03 2796: MOVF xD1,W 2798: MOVFF FF2,3D8 279C: BCF FF2.7 279E: MOVLB 0 27A0: CALL 037A 27A4: MOVLB 3 27A6: BTFSC xD8.7 27A8: BSF FF2.7 27AA: MOVWF xD9 27AC: MOVWF xDA 27AE: MOVLB 0 27B0: CALL 0770 .................... } 27B4: MOVLB 3 27B6: INCF xD1,F 27B8: BRA 278E .................... //set flag .................... comp_awake = 1; 27BA: BSF x05.4 .................... } .................... .................... //check if dataset has been sent .................... if(!dataset_sent) 27BC: BTFSC x05.5 27BE: BRA 27F0 .................... { .................... //send it .................... for(i=0;i<10;i++) 27C0: CLRF xD1 27C2: MOVF xD1,W 27C4: SUBLW 09 27C6: BNC 27EE .................... { .................... fputc(dataset[i],COMPSTREAM); 27C8: CLRF 03 27CA: MOVF xD1,W 27CC: MOVFF FF2,3D8 27D0: BCF FF2.7 27D2: MOVLB 0 27D4: CALL 039A 27D8: MOVLB 3 27DA: BTFSC xD8.7 27DC: BSF FF2.7 27DE: MOVWF xD9 27E0: MOVWF xDA 27E2: MOVLB 0 27E4: CALL 0770 .................... } 27E8: MOVLB 3 27EA: INCF xD1,F 27EC: BRA 27C2 .................... //set flag .................... dataset_sent = 1; 27EE: BSF x05.5 .................... } .................... .................... //clear existing buffer .................... for(i=0;i<50;i++) 27F0: CLRF xD1 27F2: MOVF xD1,W 27F4: SUBLW 31 27F6: BNC 280C .................... { .................... compstr[i] = 0x00; 27F8: CLRF 03 27FA: MOVF xD1,W 27FC: ADDLW 9F 27FE: MOVWF FE9 2800: MOVLW 03 2802: ADDWFC 03,W 2804: MOVWF FEA 2806: CLRF FEF .................... } 2808: INCF xD1,F 280A: BRA 27F2 .................... .................... //poll compass for data .................... for(i=0;i<5;i++) 280C: CLRF xD1 280E: MOVF xD1,W 2810: SUBLW 04 2812: BNC 283A .................... { .................... fputc(polldata[i],COMPSTREAM); 2814: CLRF 03 2816: MOVF xD1,W 2818: MOVFF FF2,3D8 281C: BCF FF2.7 281E: MOVLB 0 2820: CALL 03BE 2824: MOVLB 3 2826: BTFSC xD8.7 2828: BSF FF2.7 282A: MOVWF xD9 282C: MOVWF xDA 282E: MOVLB 0 2830: CALL 0770 .................... } 2834: MOVLB 3 2836: INCF xD1,F 2838: BRA 280E .................... .................... //------recieve data----- .................... i=0; //reset counter 283A: CLRF xD1 .................... comp_timeout_error = 0; //reset timeout flag 283C: BCF x05.6 .................... while(!comp_timeout_error) .................... { 283E: BTFSC x05.6 2840: BRA 2890 .................... compstr[i] = comp_timed_getc(); //fills compstr with data 2842: CLRF 03 2844: MOVF xD1,W 2846: ADDLW 9F 2848: MOVWF 01 284A: MOVLW 03 284C: ADDWFC 03,F 284E: MOVFF 01,3D8 2852: MOVFF 03,3D9 2856: MOVLB 0 2858: BRA 2138 285A: MOVFF 3D9,FEA 285E: MOVFF 3D8,FE9 2862: MOVFF 01,FEF .................... //only accept the number of bytes specified by the second bit .................... if(i==1) 2866: MOVLB 3 2868: DECFSZ xD1,W 286A: BRA 287E .................... { .................... expect = compstr[i]; //byte count 286C: CLRF 03 286E: MOVF xD1,W 2870: ADDLW 9F 2872: MOVWF FE9 2874: MOVLW 03 2876: ADDWFC 03,W 2878: MOVWF FEA 287A: MOVFF FEF,3D3 .................... } .................... i++; //increment i, which now equals number of bytes recieved 287E: INCF xD1,F .................... //if number of bytes recieved equals expected, break loop .................... if(expect == i && i != 0) 2880: MOVF xD1,W 2882: SUBWF xD3,W 2884: BNZ 288E 2886: MOVF xD1,F 2888: BZ 288E .................... { .................... //we've recieved all bytes we're expecting .................... //increment i to make length definition below match .................... i++; 288A: INCF xD1,F .................... break; 288C: BRA 2890 .................... } .................... } 288E: BRA 283E .................... length=i-1; //set length equal to the number of bytes recieved 2890: MOVLW 01 2892: SUBWF xD1,W 2894: MOVWF xD2 .................... .................... //-----verify data------- .................... //make sure we recieved bytes .................... if(length == 0) 2896: MOVF xD2,F 2898: BNZ 28CA .................... { .................... if(OUTPUT_ERR) .................... { .................... fprintf(BASESTREAM,"$ERR14:NO COMP BYTES\r\n"); 289A: CLRF xD8 289C: MOVF xD8,W 289E: MOVLB 0 28A0: CALL 03DE 28A4: MOVLB 3 28A6: INCF xD8,F 28A8: MOVWF 00 28AA: CLRF 18 28AC: BTFSC FF2.7 28AE: BSF 18.7 28B0: BCF FF2.7 28B2: MOVFF FE8,4AF 28B6: MOVLB 0 28B8: CALL 05BA 28BC: BTFSC 18.7 28BE: BSF FF2.7 28C0: MOVLW 16 28C2: MOVLB 3 28C4: SUBWF xD8,W 28C6: BNZ 289C .................... return; 28C8: BRA 2BAE .................... } .................... } .................... //make sure byte count matches .................... //compstr[1] = byte count .................... if(compstr[1] != length) 28CA: MOVF xD2,W 28CC: SUBWF xA0,W 28CE: BZ 2964 .................... { //there was an error in reciept, report it .................... if(OUTPUT_ERR) .................... fprintf(BASESTREAM,"$ERR15:BYTE CHECK FAILED,%X,%X\r\n",compstr[1],length); 28D0: CLRF xD8 28D2: MOVF xD8,W 28D4: MOVLB 0 28D6: CALL 0410 28DA: MOVLB 3 28DC: INCF xD8,F 28DE: MOVWF 00 28E0: CLRF 18 28E2: BTFSC FF2.7 28E4: BSF 18.7 28E6: BCF FF2.7 28E8: MOVFF FE8,4AF 28EC: MOVLB 0 28EE: CALL 05BA 28F2: BTFSC 18.7 28F4: BSF FF2.7 28F6: MOVLW 19 28F8: MOVLB 3 28FA: SUBWF xD8,W 28FC: BNZ 28D2 28FE: MOVFF 3A0,3D9 2902: MOVLW 37 2904: MOVWF xDA 2906: MOVLB 0 2908: RCALL 2194 290A: CLRF 18 290C: BTFSC FF2.7 290E: BSF 18.7 2910: BCF FF2.7 2912: MOVLW 2C 2914: MOVLB 4 2916: MOVWF xAF 2918: MOVLB 0 291A: CALL 05BA 291E: BTFSC 18.7 2920: BSF FF2.7 2922: MOVFF 3D2,3D9 2926: MOVLW 37 2928: MOVLB 3 292A: MOVWF xDA 292C: MOVLB 0 292E: RCALL 2194 2930: CLRF 18 2932: BTFSC FF2.7 2934: BSF 18.7 2936: BCF FF2.7 2938: MOVLW 0D 293A: MOVLB 4 293C: MOVWF xAF 293E: MOVLB 0 2940: CALL 05BA 2944: BTFSC 18.7 2946: BSF FF2.7 2948: CLRF 18 294A: BTFSC FF2.7 294C: BSF 18.7 294E: BCF FF2.7 2950: MOVLW 0A 2952: MOVLB 4 2954: MOVWF xAF 2956: MOVLB 0 2958: CALL 05BA 295C: BTFSC 18.7 295E: BSF FF2.7 .................... return; 2960: MOVLB 3 2962: BRA 2BAE .................... } .................... //check if CRC16 Matches .................... lCRC = generate_16bit_crc(compstr, length-2, CRC_CCITT); //local crc 2964: MOVLW 02 2966: SUBWF xD2,W 2968: MOVWF xD8 296A: MOVLW 03 296C: MOVWF xDA 296E: MOVLW 9F 2970: MOVWF xD9 2972: CLRF xDC 2974: MOVFF 3D8,3DB 2978: MOVLW 10 297A: MOVWF xDE 297C: MOVLW 21 297E: MOVWF xDD 2980: MOVLB 0 2982: BRA 21F6 2984: MOVFF 02,3D5 2988: MOVFF 01,3D4 .................... rCRC = make16(compstr[length-2],compstr[length-1]); //remote crc 298C: MOVLW 02 298E: MOVLB 3 2990: SUBWF xD2,W 2992: CLRF 03 2994: ADDLW 9F 2996: MOVWF FE9 2998: MOVLW 03 299A: ADDWFC 03,W 299C: MOVWF FEA 299E: MOVFF FEF,3D9 29A2: MOVLW 01 29A4: SUBWF xD2,W 29A6: CLRF 03 29A8: ADDLW 9F 29AA: MOVWF FE9 29AC: MOVLW 03 29AE: ADDWFC 03,W 29B0: MOVWF FEA 29B2: MOVFF FEF,3DB 29B6: MOVFF 3D9,3D7 29BA: MOVFF 3DB,3D6 .................... if(lCRC != rCRC && !IGNORE_CRC) 29BE: MOVF xD6,W 29C0: SUBWF xD4,W 29C2: BNZ 29CA 29C4: MOVF xD7,W 29C6: SUBWF xD5,W 29C8: BZ 2A82 .................... { .................... if(OUTPUT_ERR) .................... fprintf(BASESTREAM,"$ERR16:CRC CHECK FAILED,%LX,%LX\r\n",lCRC,rCRC); 29CA: CLRF xD8 29CC: MOVF xD8,W 29CE: MOVLB 0 29D0: CALL 044C 29D4: MOVLB 3 29D6: INCF xD8,F 29D8: MOVWF 00 29DA: CLRF 18 29DC: BTFSC FF2.7 29DE: BSF 18.7 29E0: BCF FF2.7 29E2: MOVFF FE8,4AF 29E6: MOVLB 0 29E8: CALL 05BA 29EC: BTFSC 18.7 29EE: BSF FF2.7 29F0: MOVLW 18 29F2: MOVLB 3 29F4: SUBWF xD8,W 29F6: BNZ 29CC 29F8: MOVFF 3D5,3D9 29FC: MOVLW 37 29FE: MOVWF xDA 2A00: MOVLB 0 2A02: CALL 2194 2A06: MOVFF 3D4,3D9 2A0A: MOVLW 37 2A0C: MOVLB 3 2A0E: MOVWF xDA 2A10: MOVLB 0 2A12: CALL 2194 2A16: CLRF 18 2A18: BTFSC FF2.7 2A1A: BSF 18.7 2A1C: BCF FF2.7 2A1E: MOVLW 2C 2A20: MOVLB 4 2A22: MOVWF xAF 2A24: MOVLB 0 2A26: CALL 05BA 2A2A: BTFSC 18.7 2A2C: BSF FF2.7 2A2E: MOVFF 3D7,3D9 2A32: MOVLW 37 2A34: MOVLB 3 2A36: MOVWF xDA 2A38: MOVLB 0 2A3A: CALL 2194 2A3E: MOVFF 3D6,3D9 2A42: MOVLW 37 2A44: MOVLB 3 2A46: MOVWF xDA 2A48: MOVLB 0 2A4A: CALL 2194 2A4E: CLRF 18 2A50: BTFSC FF2.7 2A52: BSF 18.7 2A54: BCF FF2.7 2A56: MOVLW 0D 2A58: MOVLB 4 2A5A: MOVWF xAF 2A5C: MOVLB 0 2A5E: CALL 05BA 2A62: BTFSC 18.7 2A64: BSF FF2.7 2A66: CLRF 18 2A68: BTFSC FF2.7 2A6A: BSF 18.7 2A6C: BCF FF2.7 2A6E: MOVLW 0A 2A70: MOVLB 4 2A72: MOVWF xAF 2A74: MOVLB 0 2A76: CALL 05BA 2A7A: BTFSC 18.7 2A7C: BSF FF2.7 .................... comp_bad_crc = TRUE; 2A7E: MOVLB 3 2A80: BSF x05.7 .................... //return; .................... } .................... //-----Parse Data---- .................... //the packet is bytes 2 to length-2 .................... //the frame is byte 3, it should be 0x05 but we don't need it .................... //the number of parameters is byte 4, but we don't use it .................... //the parameter components are five bytes long, the first byte is the DataCompoentID, the next four are the value .................... for(i=4;i<(length-2);i+=5) 2A82: MOVLW 04 2A84: MOVWF xD1 2A86: MOVLW 02 2A88: SUBWF xD2,W 2A8A: SUBWF xD1,W 2A8C: BC 2B20 .................... { .................... parse_comp(compstr[i],make32(compstr[i+1],compstr[i+2],compstr[i+3],compstr[i+4])); 2A8E: CLRF 03 2A90: MOVF xD1,W 2A92: ADDLW 9F 2A94: MOVWF FE9 2A96: MOVLW 03 2A98: ADDWFC 03,W 2A9A: MOVWF FEA 2A9C: MOVFF FEF,3D8 2AA0: MOVLW 01 2AA2: ADDWF xD1,W 2AA4: CLRF 03 2AA6: ADDLW 9F 2AA8: MOVWF FE9 2AAA: MOVLW 03 2AAC: ADDWFC 03,W 2AAE: MOVWF FEA 2AB0: MOVFF FEF,3DA 2AB4: MOVLW 02 2AB6: ADDWF xD1,W 2AB8: CLRF 03 2ABA: ADDLW 9F 2ABC: MOVWF FE9 2ABE: MOVLW 03 2AC0: ADDWFC 03,W 2AC2: MOVWF FEA 2AC4: MOVFF FEF,3DC 2AC8: MOVLW 03 2ACA: ADDWF xD1,W 2ACC: CLRF 03 2ACE: ADDLW 9F 2AD0: MOVWF FE9 2AD2: MOVLW 03 2AD4: ADDWFC 03,W 2AD6: MOVWF FEA 2AD8: MOVFF FEF,3DE 2ADC: MOVLW 04 2ADE: ADDWF xD1,W 2AE0: CLRF 03 2AE2: ADDLW 9F 2AE4: MOVWF FE9 2AE6: MOVLW 03 2AE8: ADDWFC 03,W 2AEA: MOVWF FEA 2AEC: MOVFF FEF,3E0 2AF0: MOVFF 3DA,3E4 2AF4: MOVFF 3DC,3E3 2AF8: MOVFF 3DE,3E2 2AFC: MOVFF 3E0,3E1 2B00: MOVFF 3D8,3E5 2B04: MOVFF 3DA,3E9 2B08: MOVFF 3DC,3E8 2B0C: MOVFF 3DE,3E7 2B10: MOVFF 3E0,3E6 2B14: MOVLB 0 2B16: BRA 23DA .................... } 2B18: MOVLW 05 2B1A: MOVLB 3 2B1C: ADDWF xD1,F 2B1E: BRA 2A86 .................... .................... //-----Adjust for true magnetic declination----- .................... //E-subtract from heading .................... //W-add to heading .................... if(GPS.magVarDir == 'E' && GPS.status =='A') 2B20: MOVF x2B,W 2B22: SUBLW 45 2B24: BNZ 2B68 2B26: MOVF x0D,W 2B28: SUBLW 41 2B2A: BNZ 2B68 .................... { .................... //subtract .................... COMPASS.heading = (COMPASS.heading - GPS.magVar); 2B2C: BSF FD8.1 2B2E: MOVFF 392,49D 2B32: MOVFF 391,49C 2B36: MOVFF 390,49B 2B3A: MOVFF 38F,49A 2B3E: MOVFF 32A,4A1 2B42: MOVFF 329,4A0 2B46: MOVFF 328,49F 2B4A: MOVFF 327,49E 2B4E: MOVLB 0 2B50: CALL 0DEE 2B54: MOVFF 03,392 2B58: MOVFF 02,391 2B5C: MOVFF 01,390 2B60: MOVFF 00,38F .................... .................... } else if(GPS.magVarDir == 'W' && GPS.status =='A') 2B64: BRA 2BAC 2B66: MOVLB 3 2B68: MOVF x2B,W 2B6A: SUBLW 57 2B6C: BNZ 2BAE 2B6E: MOVF x0D,W 2B70: SUBLW 41 2B72: BNZ 2BAE .................... { .................... //add .................... COMPASS.heading = (COMPASS.heading + GPS.magVar); 2B74: BCF FD8.1 2B76: MOVFF 392,49D 2B7A: MOVFF 391,49C 2B7E: MOVFF 390,49B 2B82: MOVFF 38F,49A 2B86: MOVFF 32A,4A1 2B8A: MOVFF 329,4A0 2B8E: MOVFF 328,49F 2B92: MOVFF 327,49E 2B96: MOVLB 0 2B98: CALL 0DEE 2B9C: MOVFF 03,392 2BA0: MOVFF 02,391 2BA4: MOVFF 01,390 2BA8: MOVFF 00,38F 2BAC: MOVLB 3 .................... } .................... .................... //-----DEBUG OUTPUT---- .................... /* .................... //fprintf(BASESTREAM,"\r\nTIMED OUT AFTER:%d Characters\r\n",i); .................... fprintf(BASESTREAM,"$HDG:%f$TEMP:%f$ROLL:%f$PITCH:%f\r\n",COMPASS.heading,COMPASS.temp,COMPASS.roll,COMPASS.pitch); .................... //put out all recieved .................... for(i=0;i<30;i++) .................... { .................... putc(compstr[i]); .................... } .................... */ .................... } 2BAE: MOVLB 0 2BB0: GOTO 6AF8 (RETURN) .................... .................... #include "report_base.c" .................... /* .................... * This function performs synchronization with base station .................... * .................... */ .................... void report_base() .................... { .................... .................... .................... fprintf(BASESTREAM,"$DH:%f$WD:%f$MD:%c$ST:%c$TME:%s$LAT:%1.6f$LON:%1.6f$SOG:%f$HDG:%f$A1:%u$A2:%u\r\n",hCONTROL.desired,WAYPOINTS.distance,PARAMS.mode,GPS.status,GPS.time,GPS.lat,GPS.lon,GPS.sog,COMPASS.heading,ROBO.amps1,ROBO.amps2); * 6476: MOVLB 3 6478: CLRF xD1 647A: MOVF xD1,W 647C: MOVLB 0 647E: CALL 0488 6482: MOVLB 3 6484: INCF xD1,F 6486: MOVWF 00 6488: CLRF 18 648A: BTFSC FF2.7 648C: BSF 18.7 648E: BCF FF2.7 6490: MOVFF FE8,4AF 6494: MOVLB 0 6496: CALL 05BA 649A: BTFSC 18.7 649C: BSF FF2.7 649E: MOVLW 04 64A0: MOVLB 3 64A2: SUBWF xD1,W 64A4: BNZ 647A 64A6: MOVLW 89 64A8: MOVWF FE9 64AA: MOVFF 34,3DF 64AE: MOVFF 33,3DE 64B2: MOVFF 32,3DD 64B6: MOVFF 31,3DC 64BA: MOVLW 02 64BC: MOVWF xE0 64BE: MOVLB 0 64C0: RCALL 628A 64C2: MOVLW 06 64C4: MOVLB 3 64C6: MOVWF xD2 64C8: MOVF xD2,W 64CA: MOVLB 0 64CC: CALL 0488 64D0: MOVLB 3 64D2: INCF xD2,F 64D4: MOVWF 00 64D6: CLRF 18 64D8: BTFSC FF2.7 64DA: BSF 18.7 64DC: BCF FF2.7 64DE: MOVFF FE8,4AF 64E2: MOVLB 0 64E4: CALL 05BA 64E8: BTFSC 18.7 64EA: BSF FF2.7 64EC: MOVLW 0A 64EE: MOVLB 3 64F0: SUBWF xD2,W 64F2: BNZ 64C8 64F4: MOVLW 89 64F6: MOVWF FE9 64F8: MOVFF 264,3DF 64FC: MOVFF 263,3DE 6500: MOVFF 262,3DD 6504: MOVFF 261,3DC 6508: MOVLW 02 650A: MOVWF xE0 650C: MOVLB 0 650E: RCALL 628A 6510: MOVLW 0C 6512: MOVLB 3 6514: MOVWF xD3 6516: MOVF xD3,W 6518: MOVLB 0 651A: CALL 0488 651E: MOVLB 3 6520: INCF xD3,F 6522: MOVWF 00 6524: CLRF 18 6526: BTFSC FF2.7 6528: BSF 18.7 652A: BCF FF2.7 652C: MOVFF FE8,4AF 6530: MOVLB 0 6532: CALL 05BA 6536: BTFSC 18.7 6538: BSF FF2.7 653A: MOVLW 10 653C: MOVLB 3 653E: SUBWF xD3,W 6540: BNZ 6516 6542: CLRF 18 6544: BTFSC FF2.7 6546: BSF 18.7 6548: BCF FF2.7 654A: MOVFF 38D,4AF 654E: MOVLB 0 6550: CALL 05BA 6554: BTFSC 18.7 6556: BSF FF2.7 6558: MOVLW 12 655A: MOVLB 3 655C: MOVWF xD4 655E: MOVF xD4,W 6560: MOVLB 0 6562: CALL 0488 6566: MOVLB 3 6568: INCF xD4,F 656A: MOVWF 00 656C: CLRF 18 656E: BTFSC FF2.7 6570: BSF 18.7 6572: BCF FF2.7 6574: MOVFF FE8,4AF 6578: MOVLB 0 657A: CALL 05BA 657E: BTFSC 18.7 6580: BSF FF2.7 6582: MOVLW 16 6584: MOVLB 3 6586: SUBWF xD4,W 6588: BNZ 655E 658A: CLRF 18 658C: BTFSC FF2.7 658E: BSF 18.7 6590: BCF FF2.7 6592: MOVFF 30D,4AF 6596: MOVLB 0 6598: CALL 05BA 659C: BTFSC 18.7 659E: BSF FF2.7 65A0: MOVLW 18 65A2: MOVLB 3 65A4: MOVWF xD5 65A6: MOVF xD5,W 65A8: MOVLB 0 65AA: CALL 0488 65AE: MOVLB 3 65B0: INCF xD5,F 65B2: MOVWF 00 65B4: CLRF 18 65B6: BTFSC FF2.7 65B8: BSF 18.7 65BA: BCF FF2.7 65BC: MOVFF FE8,4AF 65C0: MOVLB 0 65C2: CALL 05BA 65C6: BTFSC 18.7 65C8: BSF FF2.7 65CA: MOVLW 1D 65CC: MOVLB 3 65CE: SUBWF xD5,W 65D0: BNZ 65A6 65D2: MOVLW 03 65D4: MOVWF FEA 65D6: MOVLW 06 65D8: MOVWF FE9 65DA: MOVLW 00 65DC: IORWF FEF,W 65DE: BZ 6610 65E0: MOVFF FEA,3D7 65E4: MOVFF FE9,3D6 65E8: CLRF 18 65EA: BTFSC FF2.7 65EC: BSF 18.7 65EE: BCF FF2.7 65F0: MOVFF FEF,4AF 65F4: MOVLB 0 65F6: CALL 05BA 65FA: BTFSC 18.7 65FC: BSF FF2.7 65FE: MOVFF 3D7,FEA 6602: MOVFF 3D6,FE9 6606: INCF FE9,F 6608: BTFSC FD8.2 660A: INCF FEA,F 660C: MOVLB 3 660E: BRA 65DA 6610: MOVLW 1F 6612: MOVWF xD8 6614: MOVF xD8,W 6616: MOVLB 0 6618: CALL 0488 661C: MOVLB 3 661E: INCF xD8,F 6620: MOVWF 00 6622: CLRF 18 6624: BTFSC FF2.7 6626: BSF 18.7 6628: BCF FF2.7 662A: MOVFF FE8,4AF 662E: MOVLB 0 6630: CALL 05BA 6634: BTFSC 18.7 6636: BSF FF2.7 6638: MOVLW 24 663A: MOVLB 3 663C: SUBWF xD8,W 663E: BNZ 6614 6640: MOVLW 89 6642: MOVWF FE9 6644: MOVFF 311,3DF 6648: MOVFF 310,3DE 664C: MOVFF 30F,3DD 6650: MOVFF 30E,3DC 6654: MOVLW 06 6656: MOVWF xE0 6658: MOVLB 0 665A: RCALL 628A 665C: MOVLW 29 665E: MOVLB 3 6660: MOVWF xD9 6662: MOVF xD9,W 6664: MOVLB 0 6666: CALL 0488 666A: MOVLB 3 666C: INCF xD9,F 666E: MOVWF 00 6670: CLRF 18 6672: BTFSC FF2.7 6674: BSF 18.7 6676: BCF FF2.7 6678: MOVFF FE8,4AF 667C: MOVLB 0 667E: CALL 05BA 6682: BTFSC 18.7 6684: BSF FF2.7 6686: MOVLW 2E 6688: MOVLB 3 668A: SUBWF xD9,W 668C: BNZ 6662 668E: MOVLW 89 6690: MOVWF FE9 6692: MOVFF 316,3DF 6696: MOVFF 315,3DE 669A: MOVFF 314,3DD 669E: MOVFF 313,3DC 66A2: MOVLW 06 66A4: MOVWF xE0 66A6: MOVLB 0 66A8: RCALL 628A 66AA: MOVLW 33 66AC: MOVLB 3 66AE: MOVWF xDA 66B0: MOVF xDA,W 66B2: MOVLB 0 66B4: CALL 0488 66B8: MOVLB 3 66BA: INCF xDA,F 66BC: MOVWF 00 66BE: CLRF 18 66C0: BTFSC FF2.7 66C2: BSF 18.7 66C4: BCF FF2.7 66C6: MOVFF FE8,4AF 66CA: MOVLB 0 66CC: CALL 05BA 66D0: BTFSC 18.7 66D2: BSF FF2.7 66D4: MOVLW 38 66D6: MOVLB 3 66D8: SUBWF xDA,W 66DA: BNZ 66B0 66DC: MOVLW 89 66DE: MOVWF FE9 66E0: MOVFF 31B,3DF 66E4: MOVFF 31A,3DE 66E8: MOVFF 319,3DD 66EC: MOVFF 318,3DC 66F0: MOVLW 02 66F2: MOVWF xE0 66F4: MOVLB 0 66F6: RCALL 628A 66F8: MOVLW 3A 66FA: MOVLB 3 66FC: MOVWF xDB 66FE: MOVF xDB,W 6700: MOVLB 0 6702: CALL 0488 6706: MOVLB 3 6708: INCF xDB,F 670A: MOVWF 00 670C: CLRF 18 670E: BTFSC FF2.7 6710: BSF 18.7 6712: BCF FF2.7 6714: MOVFF FE8,4AF 6718: MOVLB 0 671A: CALL 05BA 671E: BTFSC 18.7 6720: BSF FF2.7 6722: MOVLW 3F 6724: MOVLB 3 6726: SUBWF xDB,W 6728: BNZ 66FE 672A: MOVLW 89 672C: MOVWF FE9 672E: MOVFF 392,3DF 6732: MOVFF 391,3DE 6736: MOVFF 390,3DD 673A: MOVFF 38F,3DC 673E: MOVLW 02 6740: MOVWF xE0 6742: MOVLB 0 6744: RCALL 628A 6746: MOVLW 41 6748: MOVLB 3 674A: MOVWF xDC 674C: MOVF xDC,W 674E: MOVLB 0 6750: CALL 0488 6754: MOVLB 3 6756: INCF xDC,F 6758: MOVWF 00 675A: CLRF 18 675C: BTFSC FF2.7 675E: BSF 18.7 6760: BCF FF2.7 6762: MOVFF FE8,4AF 6766: MOVLB 0 6768: CALL 05BA 676C: BTFSC 18.7 676E: BSF FF2.7 6770: MOVLW 45 6772: MOVLB 3 6774: SUBWF xDC,W 6776: BNZ 674C 6778: CLRF 18 677A: BTFSC FF2.7 677C: BSF 18.7 677E: BCF FF2.7 6780: MOVFF 2C,4AD 6784: MOVLW 1B 6786: MOVLB 4 6788: MOVWF xAE 678A: MOVLB 0 678C: CALL 0638 6790: BTFSC 18.7 6792: BSF FF2.7 6794: MOVLW 47 6796: MOVLB 3 6798: MOVWF xDD 679A: MOVF xDD,W 679C: MOVLB 0 679E: CALL 0488 67A2: MOVLB 3 67A4: INCF xDD,F 67A6: MOVWF 00 67A8: CLRF 18 67AA: BTFSC FF2.7 67AC: BSF 18.7 67AE: BCF FF2.7 67B0: MOVFF FE8,4AF 67B4: MOVLB 0 67B6: CALL 05BA 67BA: BTFSC 18.7 67BC: BSF FF2.7 67BE: MOVLW 4B 67C0: MOVLB 3 67C2: SUBWF xDD,W 67C4: BNZ 679A 67C6: CLRF 18 67C8: BTFSC FF2.7 67CA: BSF 18.7 67CC: BCF FF2.7 67CE: MOVFF 2D,4AD 67D2: MOVLW 1B 67D4: MOVLB 4 67D6: MOVWF xAE 67D8: MOVLB 0 67DA: CALL 0638 67DE: BTFSC 18.7 67E0: BSF FF2.7 67E2: CLRF 18 67E4: BTFSC FF2.7 67E6: BSF 18.7 67E8: BCF FF2.7 67EA: MOVLW 0D 67EC: MOVLB 4 67EE: MOVWF xAF 67F0: MOVLB 0 67F2: CALL 05BA 67F6: BTFSC 18.7 67F8: BSF FF2.7 67FA: CLRF 18 67FC: BTFSC FF2.7 67FE: BSF 18.7 6800: BCF FF2.7 6802: MOVLW 0A 6804: MOVLB 4 6806: MOVWF xAF 6808: MOVLB 0 680A: CALL 05BA 680E: BTFSC 18.7 6810: BSF FF2.7 .................... //debug .................... //fprintf(BASESTREAM,"$LT:%f$LN:%f$DH:%f$WD:%f$MD:%c$ST:%c$TME:%s$LAT:%1.6f$LON:%1.6f$SOG:%f$HDG:%f$A1:%u$A2:%u\r\n",WAYPOINTS.point[WAYPOINTS.current].lat,WAYPOINTS.point[WAYPOINTS.current].lon,hCONTROL.desired,WAYPOINTS.distance,PARAMS.mode,GPS.status,GPS.time,GPS.lat,GPS.lon,GPS.sog,COMPASS.heading,ROBO.amps1,ROBO.amps2); .................... .................... } 6812: GOTO 6B1A (RETURN) .................... .................... #include "calc_heading.c" .................... /** .................... * This file uses Rhumb Line Navigation to calculate the heading based on the current position and the desired position .................... * Rhumb Line Navigation is used instead of great circle navigation because it provides a constant heading and within .................... * short distances any inefficiencies compared with great circle are negligable. .................... * North West hemishpere is assumed! .................... * .................... **/ .................... void calc_heading() .................... { .................... //---Local Variables--- .................... float tc=0; //true calculated course * 4EAE: MOVLB 3 4EB0: CLRF xD1 4EB2: CLRF xD2 4EB4: CLRF xD3 4EB6: CLRF xD4 .................... float lon1=0; //current longitude in radians 4EB8: CLRF xD5 4EBA: CLRF xD6 4EBC: CLRF xD7 4EBE: CLRF xD8 .................... float lon2=0; //destination longitude in radians 4EC0: CLRF xD9 4EC2: CLRF xDA 4EC4: CLRF xDB 4EC6: CLRF xDC .................... float lat1=0; //current latitude in radians 4EC8: CLRF xDD 4ECA: CLRF xDE 4ECC: CLRF xDF 4ECE: CLRF xE0 .................... float lat2=0; //destination latitude in radians 4ED0: CLRF xE1 4ED2: CLRF xE2 4ED4: CLRF xE3 4ED6: CLRF xE4 .................... float part1=0; //break expression up 4ED8: CLRF xE5 4EDA: CLRF xE6 4EDC: CLRF xE7 4EDE: CLRF xE8 .................... float part2=0; //break expression up 4EE0: CLRF xE9 4EE2: CLRF xEA 4EE4: CLRF xEB 4EE6: CLRF xEC .................... //---Check for Autonomous Mode--- .................... if(PARAMS.mode == 'A') 4EE8: MOVF x8D,W 4EEA: SUBLW 41 4EEC: BTFSS FD8.2 4EEE: BRA 53A6 .................... { .................... //----Initialize Variables-- .................... lon1 = GPS.lon*pi/180; 4EF0: MOVFF 316,495 4EF4: MOVFF 315,494 4EF8: MOVFF 314,493 4EFC: MOVFF 313,492 4F00: MOVLW DB 4F02: MOVLB 4 4F04: MOVWF x99 4F06: MOVLW 0F 4F08: MOVWF x98 4F0A: MOVLW 49 4F0C: MOVWF x97 4F0E: MOVLW 80 4F10: MOVWF x96 4F12: MOVLB 0 4F14: CALL 0CC2 4F18: MOVFF 00,3ED 4F1C: MOVFF 01,3EE 4F20: MOVFF 02,3EF 4F24: MOVFF 03,3F0 4F28: MOVFF 03,496 4F2C: MOVFF 02,495 4F30: MOVFF 01,494 4F34: MOVFF 00,493 4F38: MOVLB 4 4F3A: CLRF x9A 4F3C: CLRF x99 4F3E: MOVLW 34 4F40: MOVWF x98 4F42: MOVLW 86 4F44: MOVWF x97 4F46: MOVLB 0 4F48: CALL 1066 4F4C: MOVFF 03,3D8 4F50: MOVFF 02,3D7 4F54: MOVFF 01,3D6 4F58: MOVFF 00,3D5 .................... lon2 = WAYPOINTS.point[WAYPOINTS.current].lon*pi/180; 4F5C: MOVLB 3 4F5E: CLRF xF2 4F60: MOVFF 260,3F1 4F64: CLRF xF4 4F66: MOVLW 08 4F68: MOVWF xF3 4F6A: MOVLB 0 4F6C: CALL 0CA0 4F70: MOVFF 02,3EE 4F74: MOVFF 01,3ED 4F78: MOVLW 04 4F7A: MOVLB 3 4F7C: ADDWF 01,W 4F7E: MOVWF 01 4F80: MOVLW 00 4F82: ADDWFC 02,W 4F84: MOVWF 03 4F86: MOVF 01,W 4F88: ADDLW 60 4F8A: MOVWF FE9 4F8C: MOVLW 00 4F8E: ADDWFC 03,W 4F90: MOVWF FEA 4F92: MOVFF FEF,3EF 4F96: MOVFF FEC,3F0 4F9A: MOVFF FEC,3F1 4F9E: MOVFF FEC,3F2 4FA2: MOVFF 3F2,495 4FA6: MOVFF 3F1,494 4FAA: MOVFF 3F0,493 4FAE: MOVFF 3EF,492 4FB2: MOVLW DB 4FB4: MOVLB 4 4FB6: MOVWF x99 4FB8: MOVLW 0F 4FBA: MOVWF x98 4FBC: MOVLW 49 4FBE: MOVWF x97 4FC0: MOVLW 80 4FC2: MOVWF x96 4FC4: MOVLB 0 4FC6: CALL 0CC2 4FCA: MOVFF 00,3F3 4FCE: MOVFF 01,3F4 4FD2: MOVFF 02,3F5 4FD6: MOVFF 03,3F6 4FDA: MOVFF 03,496 4FDE: MOVFF 02,495 4FE2: MOVFF 01,494 4FE6: MOVFF 00,493 4FEA: MOVLB 4 4FEC: CLRF x9A 4FEE: CLRF x99 4FF0: MOVLW 34 4FF2: MOVWF x98 4FF4: MOVLW 86 4FF6: MOVWF x97 4FF8: MOVLB 0 4FFA: CALL 1066 4FFE: MOVFF 03,3DC 5002: MOVFF 02,3DB 5006: MOVFF 01,3DA 500A: MOVFF 00,3D9 .................... lat1 = GPS.lat*pi/180; 500E: MOVFF 311,495 5012: MOVFF 310,494 5016: MOVFF 30F,493 501A: MOVFF 30E,492 501E: MOVLW DB 5020: MOVLB 4 5022: MOVWF x99 5024: MOVLW 0F 5026: MOVWF x98 5028: MOVLW 49 502A: MOVWF x97 502C: MOVLW 80 502E: MOVWF x96 5030: MOVLB 0 5032: CALL 0CC2 5036: MOVFF 00,3ED 503A: MOVFF 01,3EE 503E: MOVFF 02,3EF 5042: MOVFF 03,3F0 5046: MOVFF 03,496 504A: MOVFF 02,495 504E: MOVFF 01,494 5052: MOVFF 00,493 5056: MOVLB 4 5058: CLRF x9A 505A: CLRF x99 505C: MOVLW 34 505E: MOVWF x98 5060: MOVLW 86 5062: MOVWF x97 5064: MOVLB 0 5066: CALL 1066 506A: MOVFF 03,3E0 506E: MOVFF 02,3DF 5072: MOVFF 01,3DE 5076: MOVFF 00,3DD .................... lat2 = WAYPOINTS.point[WAYPOINTS.current].lat*pi/180; 507A: MOVLB 3 507C: CLRF xF2 507E: MOVFF 260,3F1 5082: CLRF xF4 5084: MOVLW 08 5086: MOVWF xF3 5088: MOVLB 0 508A: CALL 0CA0 508E: MOVFF 02,3EE 5092: MOVFF 01,3ED 5096: MOVLW 60 5098: MOVLB 3 509A: ADDWF 01,W 509C: MOVWF FE9 509E: MOVLW 00 50A0: ADDWFC 02,W 50A2: MOVWF FEA 50A4: MOVFF FEF,3EF 50A8: MOVFF FEC,3F0 50AC: MOVFF FEC,3F1 50B0: MOVFF FEC,3F2 50B4: MOVFF 3F2,495 50B8: MOVFF 3F1,494 50BC: MOVFF 3F0,493 50C0: MOVFF 3EF,492 50C4: MOVLW DB 50C6: MOVLB 4 50C8: MOVWF x99 50CA: MOVLW 0F 50CC: MOVWF x98 50CE: MOVLW 49 50D0: MOVWF x97 50D2: MOVLW 80 50D4: MOVWF x96 50D6: MOVLB 0 50D8: CALL 0CC2 50DC: MOVFF 00,3F3 50E0: MOVFF 01,3F4 50E4: MOVFF 02,3F5 50E8: MOVFF 03,3F6 50EC: MOVFF 03,496 50F0: MOVFF 02,495 50F4: MOVFF 01,494 50F8: MOVFF 00,493 50FC: MOVLB 4 50FE: CLRF x9A 5100: CLRF x99 5102: MOVLW 34 5104: MOVWF x98 5106: MOVLW 86 5108: MOVWF x97 510A: MOVLB 0 510C: CALL 1066 5110: MOVFF 03,3E4 5114: MOVFF 02,3E3 5118: MOVFF 01,3E2 511C: MOVFF 00,3E1 .................... //---Perform Expression in Parts--- .................... part1=log(tan(lat2/2+pi/4)/tan(lat1/2+pi/4)); 5120: MOVFF 3E4,496 5124: MOVFF 3E3,495 5128: MOVFF 3E2,494 512C: MOVFF 3E1,493 5130: MOVLB 4 5132: CLRF x9A 5134: CLRF x99 5136: CLRF x98 5138: MOVLW 80 513A: MOVWF x97 513C: MOVLB 0 513E: CALL 1066 5142: MOVFF 00,3ED 5146: MOVFF 01,3EE 514A: MOVFF 02,3EF 514E: MOVFF 03,3F0 5152: BCF FD8.1 5154: MOVFF 03,49D 5158: MOVFF 02,49C 515C: MOVFF 01,49B 5160: MOVFF 00,49A 5164: MOVLW DB 5166: MOVLB 4 5168: MOVWF xA1 516A: MOVLW 0F 516C: MOVWF xA0 516E: MOVLW 49 5170: MOVWF x9F 5172: MOVLW 7E 5174: MOVWF x9E 5176: MOVLB 0 5178: CALL 0DEE 517C: MOVFF 00,3F1 5180: MOVFF 01,3F2 5184: MOVFF 02,3F3 5188: MOVFF 03,3F4 518C: MOVFF 03,403 5190: MOVFF 02,402 5194: MOVFF 01,401 5198: MOVFF 00,400 519C: CALL 33B0 51A0: MOVFF 00,3F2 51A4: MOVFF 01,3F3 51A8: MOVFF 02,3F4 51AC: MOVFF 03,3F5 51B0: MOVFF 3E0,496 51B4: MOVFF 3DF,495 51B8: MOVFF 3DE,494 51BC: MOVFF 3DD,493 51C0: MOVLB 4 51C2: CLRF x9A 51C4: CLRF x99 51C6: CLRF x98 51C8: MOVLW 80 51CA: MOVWF x97 51CC: MOVLB 0 51CE: CALL 1066 51D2: MOVFF 00,3F6 51D6: MOVFF 01,3F7 51DA: MOVFF 02,3F8 51DE: MOVFF 03,3F9 51E2: MOVFF FEA,3FB 51E6: MOVFF FE9,3FA 51EA: BCF FD8.1 51EC: MOVFF 03,49D 51F0: MOVFF 02,49C 51F4: MOVFF 01,49B 51F8: MOVFF 00,49A 51FC: MOVLW DB 51FE: MOVLB 4 5200: MOVWF xA1 5202: MOVLW 0F 5204: MOVWF xA0 5206: MOVLW 49 5208: MOVWF x9F 520A: MOVLW 7E 520C: MOVWF x9E 520E: MOVLB 0 5210: CALL 0DEE 5214: MOVFF 3FB,FEA 5218: MOVFF 3FA,FE9 521C: MOVFF 00,3FC 5220: MOVFF 01,3FD 5224: MOVFF 02,3FE 5228: MOVFF 03,3FF 522C: MOVFF 03,403 5230: MOVFF 02,402 5234: MOVFF 01,401 5238: MOVFF 00,400 523C: CALL 33B0 5240: MOVFF 3F5,496 5244: MOVFF 3F4,495 5248: MOVFF 3F3,494 524C: MOVFF 3F2,493 5250: MOVFF 03,49A 5254: MOVFF 02,499 5258: MOVFF 01,498 525C: MOVFF 00,497 5260: CALL 1066 5264: MOVFF 00,3FD 5268: MOVFF 01,3FE 526C: MOVFF 02,3FF 5270: MOVFF 03,400 5274: MOVFF 03,404 5278: MOVFF 02,403 527C: MOVFF 01,402 5280: MOVFF 00,401 5284: CALL 349E 5288: MOVFF 03,3E8 528C: MOVFF 02,3E7 5290: MOVFF 01,3E6 5294: MOVFF 00,3E5 .................... part2=atan2(lon1-lon2,part1); 5298: BSF FD8.1 529A: MOVFF 3D8,49D 529E: MOVFF 3D7,49C 52A2: MOVFF 3D6,49B 52A6: MOVFF 3D5,49A 52AA: MOVFF 3DC,4A1 52AE: MOVFF 3DB,4A0 52B2: MOVFF 3DA,49F 52B6: MOVFF 3D9,49E 52BA: CALL 0DEE 52BE: MOVFF 00,3ED 52C2: MOVFF 01,3EE 52C6: MOVFF 02,3EF 52CA: MOVFF 03,3F0 52CE: MOVFF 03,3F4 52D2: MOVFF 02,3F3 52D6: MOVFF 01,3F2 52DA: MOVFF 00,3F1 52DE: MOVFF 3E8,3F8 52E2: MOVFF 3E7,3F7 52E6: MOVFF 3E6,3F6 52EA: MOVFF 3E5,3F5 52EE: GOTO 46DC 52F2: MOVFF 03,3EC 52F6: MOVFF 02,3EB 52FA: MOVFF 01,3EA 52FE: MOVFF 00,3E9 .................... tc = fmod(part2,2*pi); 5302: MOVFF 3EC,3F0 5306: MOVFF 3EB,3EF 530A: MOVFF 3EA,3EE 530E: MOVFF 3E9,3ED 5312: MOVLW DB 5314: MOVLB 3 5316: MOVWF xF4 5318: MOVLW 0F 531A: MOVWF xF3 531C: MOVLW 49 531E: MOVWF xF2 5320: MOVLW 81 5322: MOVWF xF1 5324: MOVLB 0 5326: BRA 4D44 5328: MOVFF 03,3D4 532C: MOVFF 02,3D3 5330: MOVFF 01,3D2 5334: MOVFF 00,3D1 .................... //---Set Desired Heading to true course in degrees--- .................... hCONTROL.desired = tc*180/pi; 5338: MOVFF 3D4,495 533C: MOVFF 3D3,494 5340: MOVFF 3D2,493 5344: MOVFF 3D1,492 5348: MOVLB 4 534A: CLRF x99 534C: CLRF x98 534E: MOVLW 34 5350: MOVWF x97 5352: MOVLW 86 5354: MOVWF x96 5356: MOVLB 0 5358: CALL 0CC2 535C: MOVFF 00,3ED 5360: MOVFF 01,3EE 5364: MOVFF 02,3EF 5368: MOVFF 03,3F0 536C: MOVFF 03,496 5370: MOVFF 02,495 5374: MOVFF 01,494 5378: MOVFF 00,493 537C: MOVLW DB 537E: MOVLB 4 5380: MOVWF x9A 5382: MOVLW 0F 5384: MOVWF x99 5386: MOVLW 49 5388: MOVWF x98 538A: MOVLW 80 538C: MOVWF x97 538E: MOVLB 0 5390: CALL 1066 5394: MOVFF 03,34 5398: MOVFF 02,33 539C: MOVFF 01,32 53A0: MOVFF 00,31 53A4: MOVLB 3 .................... } .................... } 53A6: MOVLB 0 53A8: GOTO 6B04 (RETURN) .................... .................... #include "calc_distance.c" .................... /** .................... * This file uses Rhumb Line Navigation to calculate the distance based on the current position and the desired position .................... * Rhumb Line Navigation is used instead of great circle navigation because it provides a constant heading and within .................... * short distances any inefficiencies compared with great circle are negligable. .................... * point1 is current location, point 2 is destination .................... **/ .................... void calc_distance() .................... { .................... //---Local Variables--- .................... float q=0; //calcualation parameter * 3AAA: MOVLB 3 3AAC: CLRF xD1 3AAE: CLRF xD2 3AB0: CLRF xD3 3AB2: CLRF xD4 .................... float d=0; //distance between points 3AB4: CLRF xD5 3AB6: CLRF xD6 3AB8: CLRF xD7 3ABA: CLRF xD8 .................... //----Variables to break expression into parts--- .................... float part1=0; 3ABC: CLRF xD9 3ABE: CLRF xDA 3AC0: CLRF xDB 3AC2: CLRF xDC .................... float part2=0; 3AC4: CLRF xDD 3AC6: CLRF xDE 3AC8: CLRF xDF 3ACA: CLRF xE0 .................... float lon1=0; //current longitude in radians 3ACC: CLRF xE1 3ACE: CLRF xE2 3AD0: CLRF xE3 3AD2: CLRF xE4 .................... float lon2=0; //destination longitude in radians 3AD4: CLRF xE5 3AD6: CLRF xE6 3AD8: CLRF xE7 3ADA: CLRF xE8 .................... float lat1=0; //current latitude in radians 3ADC: CLRF xE9 3ADE: CLRF xEA 3AE0: CLRF xEB 3AE2: CLRF xEC .................... float lat2=0; //destination latitude in radians 3AE4: CLRF xED 3AE6: CLRF xEE 3AE8: CLRF xEF 3AEA: CLRF xF0 .................... //---Check for Autonomous Mode--- .................... if(PARAMS.mode == 'A') 3AEC: MOVF x8D,W 3AEE: SUBLW 41 3AF0: BTFSS FD8.2 3AF2: BRA 41A8 .................... { .................... //----Initialize Variables-- .................... lon1 = GPS.lon*pi/180; 3AF4: MOVFF 316,495 3AF8: MOVFF 315,494 3AFC: MOVFF 314,493 3B00: MOVFF 313,492 3B04: MOVLW DB 3B06: MOVLB 4 3B08: MOVWF x99 3B0A: MOVLW 0F 3B0C: MOVWF x98 3B0E: MOVLW 49 3B10: MOVWF x97 3B12: MOVLW 80 3B14: MOVWF x96 3B16: MOVLB 0 3B18: CALL 0CC2 3B1C: MOVFF 00,3F1 3B20: MOVFF 01,3F2 3B24: MOVFF 02,3F3 3B28: MOVFF 03,3F4 3B2C: MOVFF 03,496 3B30: MOVFF 02,495 3B34: MOVFF 01,494 3B38: MOVFF 00,493 3B3C: MOVLB 4 3B3E: CLRF x9A 3B40: CLRF x99 3B42: MOVLW 34 3B44: MOVWF x98 3B46: MOVLW 86 3B48: MOVWF x97 3B4A: MOVLB 0 3B4C: CALL 1066 3B50: MOVFF 03,3E4 3B54: MOVFF 02,3E3 3B58: MOVFF 01,3E2 3B5C: MOVFF 00,3E1 .................... lon2 = WAYPOINTS.point[WAYPOINTS.current].lon*pi/180; 3B60: MOVLB 3 3B62: CLRF xF2 3B64: MOVFF 260,3F1 3B68: CLRF xF4 3B6A: MOVLW 08 3B6C: MOVWF xF3 3B6E: MOVLB 0 3B70: CALL 0CA0 3B74: MOVFF 02,3F2 3B78: MOVFF 01,3F1 3B7C: MOVLW 04 3B7E: MOVLB 3 3B80: ADDWF 01,W 3B82: MOVWF 01 3B84: MOVLW 00 3B86: ADDWFC 02,W 3B88: MOVWF 03 3B8A: MOVF 01,W 3B8C: ADDLW 60 3B8E: MOVWF FE9 3B90: MOVLW 00 3B92: ADDWFC 03,W 3B94: MOVWF FEA 3B96: MOVFF FEF,3F3 3B9A: MOVFF FEC,3F4 3B9E: MOVFF FEC,3F5 3BA2: MOVFF FEC,3F6 3BA6: MOVFF 3F6,495 3BAA: MOVFF 3F5,494 3BAE: MOVFF 3F4,493 3BB2: MOVFF 3F3,492 3BB6: MOVLW DB 3BB8: MOVLB 4 3BBA: MOVWF x99 3BBC: MOVLW 0F 3BBE: MOVWF x98 3BC0: MOVLW 49 3BC2: MOVWF x97 3BC4: MOVLW 80 3BC6: MOVWF x96 3BC8: MOVLB 0 3BCA: CALL 0CC2 3BCE: MOVFF 00,3F7 3BD2: MOVFF 01,3F8 3BD6: MOVFF 02,3F9 3BDA: MOVFF 03,3FA 3BDE: MOVFF 03,496 3BE2: MOVFF 02,495 3BE6: MOVFF 01,494 3BEA: MOVFF 00,493 3BEE: MOVLB 4 3BF0: CLRF x9A 3BF2: CLRF x99 3BF4: MOVLW 34 3BF6: MOVWF x98 3BF8: MOVLW 86 3BFA: MOVWF x97 3BFC: MOVLB 0 3BFE: CALL 1066 3C02: MOVFF 03,3E8 3C06: MOVFF 02,3E7 3C0A: MOVFF 01,3E6 3C0E: MOVFF 00,3E5 .................... lat1 = GPS.lat*pi/180; 3C12: MOVFF 311,495 3C16: MOVFF 310,494 3C1A: MOVFF 30F,493 3C1E: MOVFF 30E,492 3C22: MOVLW DB 3C24: MOVLB 4 3C26: MOVWF x99 3C28: MOVLW 0F 3C2A: MOVWF x98 3C2C: MOVLW 49 3C2E: MOVWF x97 3C30: MOVLW 80 3C32: MOVWF x96 3C34: MOVLB 0 3C36: CALL 0CC2 3C3A: MOVFF 00,3F1 3C3E: MOVFF 01,3F2 3C42: MOVFF 02,3F3 3C46: MOVFF 03,3F4 3C4A: MOVFF 03,496 3C4E: MOVFF 02,495 3C52: MOVFF 01,494 3C56: MOVFF 00,493 3C5A: MOVLB 4 3C5C: CLRF x9A 3C5E: CLRF x99 3C60: MOVLW 34 3C62: MOVWF x98 3C64: MOVLW 86 3C66: MOVWF x97 3C68: MOVLB 0 3C6A: CALL 1066 3C6E: MOVFF 03,3EC 3C72: MOVFF 02,3EB 3C76: MOVFF 01,3EA 3C7A: MOVFF 00,3E9 .................... lat2 = WAYPOINTS.point[WAYPOINTS.current].lat*pi/180; 3C7E: MOVLB 3 3C80: CLRF xF2 3C82: MOVFF 260,3F1 3C86: CLRF xF4 3C88: MOVLW 08 3C8A: MOVWF xF3 3C8C: MOVLB 0 3C8E: CALL 0CA0 3C92: MOVFF 02,3F2 3C96: MOVFF 01,3F1 3C9A: MOVLW 60 3C9C: MOVLB 3 3C9E: ADDWF 01,W 3CA0: MOVWF FE9 3CA2: MOVLW 00 3CA4: ADDWFC 02,W 3CA6: MOVWF FEA 3CA8: MOVFF FEF,3F3 3CAC: MOVFF FEC,3F4 3CB0: MOVFF FEC,3F5 3CB4: MOVFF FEC,3F6 3CB8: MOVFF 3F6,495 3CBC: MOVFF 3F5,494 3CC0: MOVFF 3F4,493 3CC4: MOVFF 3F3,492 3CC8: MOVLW DB 3CCA: MOVLB 4 3CCC: MOVWF x99 3CCE: MOVLW 0F 3CD0: MOVWF x98 3CD2: MOVLW 49 3CD4: MOVWF x97 3CD6: MOVLW 80 3CD8: MOVWF x96 3CDA: MOVLB 0 3CDC: CALL 0CC2 3CE0: MOVFF 00,3F7 3CE4: MOVFF 01,3F8 3CE8: MOVFF 02,3F9 3CEC: MOVFF 03,3FA 3CF0: MOVFF 03,496 3CF4: MOVFF 02,495 3CF8: MOVFF 01,494 3CFC: MOVFF 00,493 3D00: MOVLB 4 3D02: CLRF x9A 3D04: CLRF x99 3D06: MOVLW 34 3D08: MOVWF x98 3D0A: MOVLW 86 3D0C: MOVWF x97 3D0E: MOVLB 0 3D10: CALL 1066 3D14: MOVFF 03,3F0 3D18: MOVFF 02,3EF 3D1C: MOVFF 01,3EE 3D20: MOVFF 00,3ED .................... if (abs(lat2-lat1) < .00000001){ 3D24: BSF FD8.1 3D26: MOVFF 3F0,49D 3D2A: MOVFF 3EF,49C 3D2E: MOVFF 3EE,49B 3D32: MOVFF 3ED,49A 3D36: MOVFF 3EC,4A1 3D3A: MOVFF 3EB,4A0 3D3E: MOVFF 3EA,49F 3D42: MOVFF 3E9,49E 3D46: CALL 0DEE 3D4A: MOVFF 00,3F1 3D4E: MOVFF 01,3F2 3D52: MOVFF 02,3F3 3D56: MOVFF 03,3F4 3D5A: MOVFF 00,00 3D5E: MOVFF 01,01 3D62: MOVFF 02,02 3D66: MOVFF 03,03 3D6A: BCF 01.7 3D6C: MOVFF 00,3F2 3D70: MOVFF 01,3F3 3D74: MOVFF 02,3F4 3D78: MOVFF 03,3F5 3D7C: MOVFF 03,43D 3D80: MOVFF 02,43C 3D84: MOVFF 01,43B 3D88: MOVFF 00,43A 3D8C: MOVLW 77 3D8E: MOVLB 4 3D90: MOVWF x41 3D92: MOVLW CC 3D94: MOVWF x40 3D96: MOVLW 2B 3D98: MOVWF x3F 3D9A: MOVLW 64 3D9C: MOVWF x3E 3D9E: MOVLB 0 3DA0: CALL 2360 3DA4: BNC 3DCC .................... q=cos(lat1); 3DA6: MOVFF 3EC,417 3DAA: MOVFF 3EB,416 3DAE: MOVFF 3EA,415 3DB2: MOVFF 3E9,414 3DB6: CALL 2FD4 3DBA: MOVFF 03,3D4 3DBE: MOVFF 02,3D3 3DC2: MOVFF 01,3D2 3DC6: MOVFF 00,3D1 .................... } else { 3DCA: BRA 3F9E .................... part1=tan(lat2/2+pi/4); 3DCC: MOVFF 3F0,496 3DD0: MOVFF 3EF,495 3DD4: MOVFF 3EE,494 3DD8: MOVFF 3ED,493 3DDC: MOVLB 4 3DDE: CLRF x9A 3DE0: CLRF x99 3DE2: CLRF x98 3DE4: MOVLW 80 3DE6: MOVWF x97 3DE8: MOVLB 0 3DEA: CALL 1066 3DEE: MOVFF 00,3F1 3DF2: MOVFF 01,3F2 3DF6: MOVFF 02,3F3 3DFA: MOVFF 03,3F4 3DFE: BCF FD8.1 3E00: MOVFF 03,49D 3E04: MOVFF 02,49C 3E08: MOVFF 01,49B 3E0C: MOVFF 00,49A 3E10: MOVLW DB 3E12: MOVLB 4 3E14: MOVWF xA1 3E16: MOVLW 0F 3E18: MOVWF xA0 3E1A: MOVLW 49 3E1C: MOVWF x9F 3E1E: MOVLW 7E 3E20: MOVWF x9E 3E22: MOVLB 0 3E24: CALL 0DEE 3E28: MOVFF 00,3F5 3E2C: MOVFF 01,3F6 3E30: MOVFF 02,3F7 3E34: MOVFF 03,3F8 3E38: MOVFF 03,403 3E3C: MOVFF 02,402 3E40: MOVFF 01,401 3E44: MOVFF 00,400 3E48: CALL 33B0 3E4C: MOVFF 03,3DC 3E50: MOVFF 02,3DB 3E54: MOVFF 01,3DA 3E58: MOVFF 00,3D9 .................... part2=log(part1/tan(lat1/2+pi/4)); 3E5C: MOVFF 3EC,496 3E60: MOVFF 3EB,495 3E64: MOVFF 3EA,494 3E68: MOVFF 3E9,493 3E6C: MOVLB 4 3E6E: CLRF x9A 3E70: CLRF x99 3E72: CLRF x98 3E74: MOVLW 80 3E76: MOVWF x97 3E78: MOVLB 0 3E7A: CALL 1066 3E7E: MOVFF 00,3F1 3E82: MOVFF 01,3F2 3E86: MOVFF 02,3F3 3E8A: MOVFF 03,3F4 3E8E: BCF FD8.1 3E90: MOVFF 03,49D 3E94: MOVFF 02,49C 3E98: MOVFF 01,49B 3E9C: MOVFF 00,49A 3EA0: MOVLW DB 3EA2: MOVLB 4 3EA4: MOVWF xA1 3EA6: MOVLW 0F 3EA8: MOVWF xA0 3EAA: MOVLW 49 3EAC: MOVWF x9F 3EAE: MOVLW 7E 3EB0: MOVWF x9E 3EB2: MOVLB 0 3EB4: CALL 0DEE 3EB8: MOVFF 00,3F5 3EBC: MOVFF 01,3F6 3EC0: MOVFF 02,3F7 3EC4: MOVFF 03,3F8 3EC8: MOVFF 03,403 3ECC: MOVFF 02,402 3ED0: MOVFF 01,401 3ED4: MOVFF 00,400 3ED8: CALL 33B0 3EDC: MOVFF 3DC,496 3EE0: MOVFF 3DB,495 3EE4: MOVFF 3DA,494 3EE8: MOVFF 3D9,493 3EEC: MOVFF 03,49A 3EF0: MOVFF 02,499 3EF4: MOVFF 01,498 3EF8: MOVFF 00,497 3EFC: CALL 1066 3F00: MOVFF 00,3F6 3F04: MOVFF 01,3F7 3F08: MOVFF 02,3F8 3F0C: MOVFF 03,3F9 3F10: MOVFF 03,404 3F14: MOVFF 02,403 3F18: MOVFF 01,402 3F1C: MOVFF 00,401 3F20: CALL 349E 3F24: MOVFF 03,3E0 3F28: MOVFF 02,3DF 3F2C: MOVFF 01,3DE 3F30: MOVFF 00,3DD .................... q= (lat2-lat1)/part2; 3F34: BSF FD8.1 3F36: MOVFF 3F0,49D 3F3A: MOVFF 3EF,49C 3F3E: MOVFF 3EE,49B 3F42: MOVFF 3ED,49A 3F46: MOVFF 3EC,4A1 3F4A: MOVFF 3EB,4A0 3F4E: MOVFF 3EA,49F 3F52: MOVFF 3E9,49E 3F56: CALL 0DEE 3F5A: MOVFF 00,3F1 3F5E: MOVFF 01,3F2 3F62: MOVFF 02,3F3 3F66: MOVFF 03,3F4 3F6A: MOVFF 03,496 3F6E: MOVFF 02,495 3F72: MOVFF 01,494 3F76: MOVFF 00,493 3F7A: MOVFF 3E0,49A 3F7E: MOVFF 3DF,499 3F82: MOVFF 3DE,498 3F86: MOVFF 3DD,497 3F8A: CALL 1066 3F8E: MOVFF 03,3D4 3F92: MOVFF 02,3D3 3F96: MOVFF 01,3D2 3F9A: MOVFF 00,3D1 .................... } .................... part1=q*q*(lon2-lon1)*(lon2-lon1); 3F9E: MOVFF 3D4,495 3FA2: MOVFF 3D3,494 3FA6: MOVFF 3D2,493 3FAA: MOVFF 3D1,492 3FAE: MOVFF 3D4,499 3FB2: MOVFF 3D3,498 3FB6: MOVFF 3D2,497 3FBA: MOVFF 3D1,496 3FBE: CALL 0CC2 3FC2: MOVFF 00,3F1 3FC6: MOVFF 01,3F2 3FCA: MOVFF 02,3F3 3FCE: MOVFF 03,3F4 3FD2: BSF FD8.1 3FD4: MOVFF 3E8,49D 3FD8: MOVFF 3E7,49C 3FDC: MOVFF 3E6,49B 3FE0: MOVFF 3E5,49A 3FE4: MOVFF 3E4,4A1 3FE8: MOVFF 3E3,4A0 3FEC: MOVFF 3E2,49F 3FF0: MOVFF 3E1,49E 3FF4: CALL 0DEE 3FF8: MOVFF 3F4,495 3FFC: MOVFF 3F3,494 4000: MOVFF 3F2,493 4004: MOVFF 3F1,492 4008: MOVFF 03,499 400C: MOVFF 02,498 4010: MOVFF 01,497 4014: MOVFF 00,496 4018: CALL 0CC2 401C: MOVFF 00,3F5 4020: MOVFF 01,3F6 4024: MOVFF 02,3F7 4028: MOVFF 03,3F8 402C: BSF FD8.1 402E: MOVFF 3E8,49D 4032: MOVFF 3E7,49C 4036: MOVFF 3E6,49B 403A: MOVFF 3E5,49A 403E: MOVFF 3E4,4A1 4042: MOVFF 3E3,4A0 4046: MOVFF 3E2,49F 404A: MOVFF 3E1,49E 404E: CALL 0DEE 4052: MOVFF 3F8,495 4056: MOVFF 3F7,494 405A: MOVFF 3F6,493 405E: MOVFF 3F5,492 4062: MOVFF 03,499 4066: MOVFF 02,498 406A: MOVFF 01,497 406E: MOVFF 00,496 4072: CALL 0CC2 4076: MOVFF 03,3DC 407A: MOVFF 02,3DB 407E: MOVFF 01,3DA 4082: MOVFF 00,3D9 .................... part2=(lat2-lat1)*(lat2-lat1); 4086: BSF FD8.1 4088: MOVFF 3F0,49D 408C: MOVFF 3EF,49C 4090: MOVFF 3EE,49B 4094: MOVFF 3ED,49A 4098: MOVFF 3EC,4A1 409C: MOVFF 3EB,4A0 40A0: MOVFF 3EA,49F 40A4: MOVFF 3E9,49E 40A8: CALL 0DEE 40AC: MOVFF 00,3F1 40B0: MOVFF 01,3F2 40B4: MOVFF 02,3F3 40B8: MOVFF 03,3F4 40BC: BSF FD8.1 40BE: MOVFF 3F0,49D 40C2: MOVFF 3EF,49C 40C6: MOVFF 3EE,49B 40CA: MOVFF 3ED,49A 40CE: MOVFF 3EC,4A1 40D2: MOVFF 3EB,4A0 40D6: MOVFF 3EA,49F 40DA: MOVFF 3E9,49E 40DE: CALL 0DEE 40E2: MOVFF 3F4,495 40E6: MOVFF 3F3,494 40EA: MOVFF 3F2,493 40EE: MOVFF 3F1,492 40F2: MOVFF 03,499 40F6: MOVFF 02,498 40FA: MOVFF 01,497 40FE: MOVFF 00,496 4102: CALL 0CC2 4106: MOVFF 03,3E0 410A: MOVFF 02,3DF 410E: MOVFF 01,3DE 4112: MOVFF 00,3DD .................... d=sqrt(part2+ part1); 4116: BCF FD8.1 4118: MOVFF 3E0,49D 411C: MOVFF 3DF,49C 4120: MOVFF 3DE,49B 4124: MOVFF 3DD,49A 4128: MOVFF 3DC,4A1 412C: MOVFF 3DB,4A0 4130: MOVFF 3DA,49F 4134: MOVFF 3D9,49E 4138: CALL 0DEE 413C: MOVFF 00,3F1 4140: MOVFF 01,3F2 4144: MOVFF 02,3F3 4148: MOVFF 03,3F4 414C: MOVFF 03,3F8 4150: MOVFF 02,3F7 4154: MOVFF 01,3F6 4158: MOVFF 00,3F5 415C: BRA 397E 415E: MOVFF 03,3D8 4162: MOVFF 02,3D7 4166: MOVFF 01,3D6 416A: MOVFF 00,3D5 .................... //---Assign Calculated Distance--- Nautical Miles .................... WAYPOINTS.distance = d*((180*60)/pi); 416E: MOVFF 3D8,495 4172: MOVFF 3D7,494 4176: MOVFF 3D6,493 417A: MOVFF 3D5,492 417E: MOVLW F2 4180: MOVLB 4 4182: MOVWF x99 4184: MOVLW DB 4186: MOVWF x98 4188: MOVLW 56 418A: MOVWF x97 418C: MOVLW 8A 418E: MOVWF x96 4190: MOVLB 0 4192: CALL 0CC2 4196: MOVFF 03,264 419A: MOVFF 02,263 419E: MOVFF 01,262 41A2: MOVFF 00,261 41A6: MOVLB 3 .................... .................... } .................... .................... } 41A8: MOVLB 0 41AA: GOTO 6B00 (RETURN) .................... .................... #include "check_threshold.c" .................... //---OPTIONS--- .................... #define THRESHOLD_DISTANCE 15 //Proximity to get to waypoint in FEET .................... /** .................... *Checks if we are "close enough" to waypoint to approach next one .................... *If we are at last waypoint, stop! .................... * .................... **/ .................... void check_threshold() .................... { .................... //---Local Variables--- .................... float threshold=0; * 53AC: MOVLB 3 53AE: CLRF xD1 53B0: CLRF xD2 53B2: CLRF xD3 53B4: CLRF xD4 .................... //---Initialize--- .................... threshold =(float)0.000164578834 * (float)THRESHOLD_DISTANCE; //threshold in nautical miles 53B6: MOVLW 9E 53B8: MOVWF xD4 53BA: MOVLW C9 53BC: MOVWF xD3 53BE: MOVLW 21 53C0: MOVWF xD2 53C2: MOVLW 76 53C4: MOVWF xD1 .................... .................... //---Check for Autonomous Mode--- .................... if(PARAMS.mode == 'A') 53C6: MOVF x8D,W 53C8: SUBLW 41 53CA: BTFSS FD8.2 53CC: BRA 551A .................... { .................... //perform check .................... if(WAYPOINTS.distance <= threshold) 53CE: MOVFF 264,43D 53D2: MOVFF 263,43C 53D6: MOVFF 262,43B 53DA: MOVFF 261,43A 53DE: MOVFF 3D4,441 53E2: MOVFF 3D3,440 53E6: MOVFF 3D2,43F 53EA: MOVFF 3D1,43E 53EE: MOVLB 0 53F0: CALL 2360 53F4: BC 53F8 53F6: BNZ 53FC .................... { .................... //go to next waypoint .................... WAYPOINTS.current++; 53F8: MOVLB 2 53FA: INCF x60,F .................... } .................... //are we at the last waypoint? .................... if( WAYPOINTS.point[WAYPOINTS.current].lon == (float)0 || WAYPOINTS.point[WAYPOINTS.current].lat == (float)0) 53FC: MOVLB 3 53FE: CLRF xF2 5400: MOVFF 260,3F1 5404: CLRF xF4 5406: MOVLW 08 5408: MOVWF xF3 540A: MOVLB 0 540C: CALL 0CA0 5410: MOVFF 02,3D6 5414: MOVFF 01,3D5 5418: MOVLW 04 541A: MOVLB 3 541C: ADDWF 01,W 541E: MOVWF 01 5420: MOVLW 00 5422: ADDWFC 02,W 5424: MOVWF 03 5426: MOVF 01,W 5428: ADDLW 60 542A: MOVWF FE9 542C: MOVLW 00 542E: ADDWFC 03,W 5430: MOVWF FEA 5432: MOVFF FEF,3D7 5436: MOVFF FEC,3D8 543A: MOVFF FEC,3D9 543E: MOVFF FEC,3DA 5442: MOVFF 3DA,43D 5446: MOVFF 3D9,43C 544A: MOVFF 3D8,43B 544E: MOVFF 3D7,43A 5452: MOVLB 4 5454: CLRF x41 5456: CLRF x40 5458: CLRF x3F 545A: CLRF x3E 545C: MOVLB 0 545E: CALL 2360 5462: BZ 54C0 5464: MOVLB 3 5466: CLRF xF2 5468: MOVFF 260,3F1 546C: CLRF xF4 546E: MOVLW 08 5470: MOVWF xF3 5472: MOVLB 0 5474: CALL 0CA0 5478: MOVFF 02,3DC 547C: MOVFF 01,3DB 5480: MOVLW 60 5482: MOVLB 3 5484: ADDWF 01,W 5486: MOVWF FE9 5488: MOVLW 00 548A: ADDWFC 02,W 548C: MOVWF FEA 548E: MOVFF FEF,3DD 5492: MOVFF FEC,3DE 5496: MOVFF FEC,3DF 549A: MOVFF FEC,3E0 549E: MOVFF 3E0,43D 54A2: MOVFF 3DF,43C 54A6: MOVFF 3DE,43B 54AA: MOVFF 3DD,43A 54AE: MOVLB 4 54B0: CLRF x41 54B2: CLRF x40 54B4: CLRF x3F 54B6: CLRF x3E 54B8: MOVLB 0 54BA: CALL 2360 54BE: BNZ 5518 .................... { .................... //we're at last waypoint, STOP! .................... PARAMS.mode = 'K'; //kill mode 54C0: MOVLW 4B 54C2: MOVLB 3 54C4: MOVWF x8D .................... //set motors to 0! .................... //set motor 1 command; .................... //for roboteq .................... strcpy(ROBO.M1,"A00"); 54C6: CLRF FEA 54C8: MOVLW 24 54CA: MOVWF FE9 54CC: MOVFF FF2,3D5 54D0: BCF FF2.7 54D2: MOVLW 00 54D4: MOVLB 0 54D6: CALL 02A8 54DA: TBLRD*- 54DC: TBLRD*+ 54DE: MOVF FF5,W 54E0: MOVWF FEE 54E2: IORLW 00 54E4: BNZ 54DC 54E6: MOVLB 3 54E8: BTFSC xD5.7 54EA: BSF FF2.7 .................... //set motor 1 command .................... //for roboteq .................... strcpy(ROBO.M2,"B00"); 54EC: CLRF FEA 54EE: MOVLW 28 54F0: MOVWF FE9 54F2: MOVFF FF2,3D5 54F6: BCF FF2.7 54F8: MOVLW 00 54FA: MOVLB 0 54FC: CALL 02C6 5500: TBLRD*- 5502: TBLRD*+ 5504: MOVF FF5,W 5506: MOVWF FEE 5508: IORLW 00 550A: BNZ 5502 550C: MOVLB 3 550E: BTFSC xD5.7 5510: BSF FF2.7 .................... //OK, update motors .................... update_robo(); 5512: MOVLB 0 5514: CALL 0974 5518: MOVLB 3 .................... } .................... } .................... } 551A: MOVLB 0 551C: GOTO 6B08 (RETURN) .................... .................... #include "control_heading.c" .................... /* .................... Notes: .................... Sign Convention - Clockwise is positive, Counterclockwise is negative .................... COMPASS.heading should be a float .................... */ .................... .................... .................... //------OPTIONS--------- .................... #define hSCALE 10; //increasing this will increase the resolution of the gains, effectively dividing them by this .................... #define hIntSCALE 10; //The integral gain needs to be scaled further than the P and D gains .................... #define iDEFAULT 5; //default integral gain .................... #define pDEFAULT 5; //default proportional gain .................... #define dDEFAULT 5; //default derivitive gain .................... #define desDEFAULT 0; //default desired heading .................... .................... /** .................... * This struct contains all the parameters for heading control .................... **/ .................... /*MOVED TO GLOBALS .................... struct .................... { .................... int1 enabled; //flag that enables / disables heading control .................... float desired; //desired heading .................... float error; //current error .................... float pError; //previous error .................... signed int16 iVal; //integral component .................... signed int16 pVal; //proportional component .................... signed int16 dVal; //derivitive component .................... int iGain; //integral gain .................... int pGain; //proportional gain .................... int dGain; //derivative gain .................... signed int16 PIDtot; //total value of P+I+D components .................... .................... } hCONTROL; .................... */ //MOVED TO GLOBALS .................... .................... .................... /** .................... * This function calculates the error for heading .................... * .................... **/ .................... void hError() .................... { .................... //---local variables--- .................... signed int16 error = 0; 5520: MOVLB 3 5522: CLRF xD3 5524: CLRF xD4 .................... //calculate error .................... error = hCONTROL.desired - COMPASS.heading; 5526: BSF FD8.1 5528: MOVFF 34,49D 552C: MOVFF 33,49C 5530: MOVFF 32,49B 5534: MOVFF 31,49A 5538: MOVFF 392,4A1 553C: MOVFF 391,4A0 5540: MOVFF 390,49F 5544: MOVFF 38F,49E 5548: MOVLB 0 554A: CALL 0DEE 554E: MOVFF 03,43D 5552: MOVFF 02,43C 5556: MOVFF 01,43B 555A: MOVFF 00,43A 555E: CALL 2F98 5562: MOVFF 02,3D4 5566: MOVFF 01,3D3 .................... .................... //A compass returns to 0* after 360, so this needs to be taken into account .................... //also, error can't be more than 180* .................... if(abs(error) > 180) 556A: MOVFF 3D4,03 556E: MOVLB 3 5570: MOVF xD3,W 5572: BTFSS xD4.7 5574: BRA 5586 5576: MOVLW 00 5578: BSF FD8.0 557A: SUBFWB xD3,W 557C: MOVWF 00 557E: MOVLW 00 5580: SUBFWB xD4,W 5582: MOVWF 03 5584: MOVF 00,W 5586: MOVWF xD5 5588: MOVFF 03,3D6 558C: BTFSC 03.7 558E: BRA 55B2 5590: MOVF xD6,F 5592: BNZ 559A 5594: MOVF xD5,W 5596: SUBLW B4 5598: BC 55B2 .................... { .................... //we need to switch direction then .................... //error *= -1; .................... //test if error is positive or negative .................... if(error<0) 559A: BTFSC xD4.7 559C: BRA 55A0 559E: BRA 55AA .................... { .................... //we need to add 180* .................... error +=360; 55A0: MOVLW 68 55A2: ADDWF xD3,F 55A4: MOVLW 01 55A6: ADDWFC xD4,F .................... } else 55A8: BRA 55B2 .................... { .................... //we need to subract 180* .................... error -= 360; 55AA: MOVLW 68 55AC: SUBWF xD3,F 55AE: MOVLW 01 55B0: SUBWFB xD4,F .................... } .................... } .................... //we now have our properly signed error .................... hCONTROL.error = error; 55B2: MOVFF 3D4,417 55B6: MOVFF 3D3,416 55BA: MOVLB 0 55BC: CALL 344E 55C0: MOVFF 03,38 55C4: MOVFF 02,37 55C8: MOVFF 01,36 55CC: MOVFF 00,35 .................... } 55D0: GOTO 591C (RETURN) .................... .................... /** .................... * This function interprets the hCONTROL.PIDtot and applies scaling / conversion and sends it to roboteq .................... * .................... **/ .................... void apply_h_robo() .................... { .................... //------Local Variables------ .................... char hexVal[3] = ""; * 5724: MOVLB 3 5726: CLRF xD3 5728: CLRF xD4 572A: CLRF xD5 .................... char dir[2] = ""; 572C: CLRF xD6 572E: CLRF xD7 .................... char comb[4]=""; 5730: CLRF xD8 5732: CLRF xD9 5734: CLRF xDA 5736: CLRF xDB .................... signed int16 maxPIDtot = 127*hSCALE; 5738: MOVLW F6 573A: MOVWF xDC 573C: MOVLW 04 573E: MOVWF xDD .................... //if PIDtot is larger than max, set to max .................... //a larger max value will increase the resolution of the Gains .................... if(hCONTROL.PIDtot > maxPIDtot) 5740: BTFSS xDD.7 5742: BRA 574A 5744: BTFSS 47.7 5746: BRA 575C 5748: BRA 574E 574A: BTFSC 47.7 574C: BRA 5766 574E: MOVF xDD,W 5750: SUBWF 47,W 5752: BNC 5766 5754: BNZ 575C 5756: MOVF 46,W 5758: SUBWF xDC,W 575A: BC 5766 .................... { .................... hCONTROL.PIDtot = 127*hSCALE; 575C: MOVLW 04 575E: MOVWF 47 5760: MOVLW F6 5762: MOVWF 46 .................... } else if(hCONTROL.PIDtot < -maxPIDtot) 5764: BRA 579C 5766: MOVLW 00 5768: BSF FD8.0 576A: SUBFWB xDC,W 576C: MOVWF 00 576E: MOVLW 00 5770: SUBFWB xDD,W 5772: MOVWF 03 5774: MOVFF 00,01 5778: BTFSS 47.7 577A: BRA 5782 577C: BTFSS 03.7 577E: BRA 5794 5780: BRA 5786 5782: BTFSC 03.7 5784: BRA 579C 5786: MOVF 47,W 5788: SUBWF 03,W 578A: BNC 579C 578C: BNZ 5794 578E: MOVF 01,W 5790: SUBWF 46,W 5792: BC 579C .................... { .................... hCONTROL.PIDtot = -127*hSCALE; 5794: MOVLW FB 5796: MOVWF 47 5798: MOVLW 0A 579A: MOVWF 46 .................... } .................... //divide by scale .................... hCONTROL.PIDtot /= hSCALE; 579C: MOVFF 47,3DF 57A0: MOVFF 46,3DE 57A4: CLRF xE1 57A6: MOVLW 0A 57A8: MOVWF xE0 57AA: MOVLB 0 57AC: RCALL 55D4 57AE: MOVFF 02,47 57B2: MOVFF 01,46 .................... //perform power scaling .................... if(PARAMS.pScale > 0 && PARAMS.pScale < 100) 57B6: MOVLB 3 57B8: MOVF x8E,F 57BA: BZ 5864 57BC: MOVF x8E,W 57BE: SUBLW 63 57C0: BNC 5864 .................... hCONTROL.PIDtot =(int16) ((float)hCONTROL.PIDtot*((float)PARAMS.pScale / 100)) ; 57C2: MOVFF 47,417 57C6: MOVFF 46,416 57CA: MOVLB 0 57CC: CALL 344E 57D0: MOVFF 00,3DE 57D4: MOVFF 01,3DF 57D8: MOVFF 02,3E0 57DC: MOVFF 03,3E1 57E0: MOVLB 4 57E2: CLRF x97 57E4: MOVFF 38E,496 57E8: MOVLB 0 57EA: CALL 0DB8 57EE: MOVFF 00,3E2 57F2: MOVFF 01,3E3 57F6: MOVFF 02,3E4 57FA: MOVFF 03,3E5 57FE: MOVFF 03,496 5802: MOVFF 02,495 5806: MOVFF 01,494 580A: MOVFF 00,493 580E: MOVLB 4 5810: CLRF x9A 5812: CLRF x99 5814: MOVLW 48 5816: MOVWF x98 5818: MOVLW 85 581A: MOVWF x97 581C: MOVLB 0 581E: CALL 1066 5822: MOVFF 3E1,495 5826: MOVFF 3E0,494 582A: MOVFF 3DF,493 582E: MOVFF 3DE,492 5832: MOVFF 03,499 5836: MOVFF 02,498 583A: MOVFF 01,497 583E: MOVFF 00,496 5842: CALL 0CC2 5846: MOVFF 03,43D 584A: MOVFF 02,43C 584E: MOVFF 01,43B 5852: MOVFF 00,43A 5856: CALL 2F98 585A: MOVFF 02,47 585E: MOVFF 01,46 5862: MOVLB 3 .................... //set the direction for roboteq .................... if(hCONTROL.PIDtot >=0) 5864: BTFSC 47.7 5866: BRA 5870 5868: BRA 586A .................... { .................... //clockwise turn .................... dir[0] = 'a'; 586A: MOVLW 61 586C: MOVWF xD6 .................... } else 586E: BRA 5874 .................... { .................... //counterclockwise turn .................... dir[0] = 'A'; 5870: MOVLW 41 5872: MOVWF xD6 .................... } .................... //now convert value of PIDtot to hex string .................... sprintf(hexVal,"%X",abs(hCONTROL.PIDtot)); 5874: MOVFF 47,03 5878: MOVF 46,W 587A: BTFSS 47.7 587C: BRA 588E 587E: MOVLW 00 5880: BSF FD8.0 5882: SUBFWB 46,W 5884: MOVWF 00 5886: MOVLW 00 5888: SUBFWB 47,W 588A: MOVWF 03 588C: MOVF 00,W 588E: MOVWF xDE 5890: MOVFF 03,3DF 5894: MOVLW 03 5896: MOVWF xD0 5898: MOVLW D3 589A: MOVWF xCF 589C: MOVFF 3DE,3E0 58A0: MOVLW 37 58A2: MOVWF xE1 58A4: MOVLB 0 58A6: RCALL 566C .................... //set combination of hex val and dir to roboteq .................... //combine dir and hexVal .................... strcat(comb,dir); 58A8: MOVLW 03 58AA: MOVLB 3 58AC: MOVWF xDF 58AE: MOVLW D8 58B0: MOVWF xDE 58B2: MOVLW 03 58B4: MOVWF xE1 58B6: MOVLW D6 58B8: MOVWF xE0 58BA: MOVLB 0 58BC: RCALL 56B2 .................... strcat(comb,hexVal); 58BE: MOVLW 03 58C0: MOVLB 3 58C2: MOVWF xDF 58C4: MOVLW D8 58C6: MOVWF xDE 58C8: MOVLW 03 58CA: MOVWF xE1 58CC: MOVLW D3 58CE: MOVWF xE0 58D0: MOVLB 0 58D2: RCALL 56B2 .................... //copy command to roboteq M2 .................... strcpy(ROBO.M2,comb); 58D4: CLRF FEA 58D6: MOVLW 28 58D8: MOVWF FE9 58DA: MOVLW 03 58DC: MOVWF FE2 58DE: MOVLW D8 58E0: MOVWF FE1 58E2: MOVF FE7,F 58E4: MOVFF FE6,FEE 58E8: BNZ 58E2 .................... .................... } 58EA: GOTO 5B3E (RETURN) .................... .................... .................... /** .................... * This function performs heading control .................... * .................... **/ .................... void control_heading() .................... { .................... //----Local Variables---- .................... signed int16 maxPIDtot = 127*hSCALE; //used to test if saturation has occured 58EE: MOVLW F6 58F0: MOVLB 3 58F2: MOVWF xD1 58F4: MOVLW 04 58F6: MOVWF xD2 .................... .................... //-----Check if hCONTROL is enabled------ .................... if(!hCONTROL.enabled || PARAMS.mode == 'K') 58F8: BTFSS 30.0 58FA: BRA 5902 58FC: MOVF x8D,W 58FE: SUBLW 4B 5900: BNZ 5908 .................... { //we're not enabled, exit .................... //also clear integral value .................... hCONTROL.iVal = 0; 5902: CLRF 3E 5904: CLRF 3D .................... return; 5906: BRA 5B40 .................... } .................... .................... //----Update Previous Error---- .................... hCONTROL.pError = hCONTROL.error; 5908: MOVFF 38,3C 590C: MOVFF 37,3B 5910: MOVFF 36,3A 5914: MOVFF 35,39 .................... .................... //----Calculate New Error------ .................... hError(); 5918: MOVLB 0 591A: BRA 5520 .................... //----if error is 0, return, a bit optimistic----- .................... if(hCONTROL.error == 0) 591C: MOVFF 38,43D 5920: MOVFF 37,43C 5924: MOVFF 36,43B 5928: MOVFF 35,43A 592C: MOVLB 4 592E: CLRF x41 5930: CLRF x40 5932: CLRF x3F 5934: CLRF x3E 5936: MOVLB 0 5938: CALL 2360 593C: BNZ 5942 .................... { .................... return; 593E: MOVLB 3 5940: BRA 5B40 .................... } .................... //------Proportional----- .................... hCONTROL.pVal = hCONTROL.pGain*hCONTROL.error; 5942: MOVLB 4 5944: CLRF x97 5946: MOVFF 44,496 594A: MOVLB 0 594C: CALL 0DB8 5950: MOVFF 03,495 5954: MOVFF 02,494 5958: MOVFF 01,493 595C: MOVFF 00,492 5960: MOVFF 38,499 5964: MOVFF 37,498 5968: MOVFF 36,497 596C: MOVFF 35,496 5970: CALL 0CC2 5974: MOVFF 03,43D 5978: MOVFF 02,43C 597C: MOVFF 01,43B 5980: MOVFF 00,43A 5984: CALL 2F98 5988: MOVFF 02,40 598C: MOVFF 01,3F .................... //------Derivitive------- .................... hCONTROL.dVal = hCONTROL.dGain*(hCONTROL.error-hCONTROL.pError); 5990: BSF FD8.1 5992: MOVFF 38,49D 5996: MOVFF 37,49C 599A: MOVFF 36,49B 599E: MOVFF 35,49A 59A2: MOVFF 3C,4A1 59A6: MOVFF 3B,4A0 59AA: MOVFF 3A,49F 59AE: MOVFF 39,49E 59B2: CALL 0DEE 59B6: MOVFF 00,3D3 59BA: MOVFF 01,3D4 59BE: MOVFF 02,3D5 59C2: MOVFF 03,3D6 59C6: MOVLB 4 59C8: CLRF x97 59CA: MOVFF 45,496 59CE: MOVLB 0 59D0: CALL 0DB8 59D4: MOVFF 03,495 59D8: MOVFF 02,494 59DC: MOVFF 01,493 59E0: MOVFF 00,492 59E4: MOVFF 3D6,499 59E8: MOVFF 3D5,498 59EC: MOVFF 3D4,497 59F0: MOVFF 3D3,496 59F4: CALL 0CC2 59F8: MOVFF 03,43D 59FC: MOVFF 02,43C 5A00: MOVFF 01,43B 5A04: MOVFF 00,43A 5A08: CALL 2F98 5A0C: MOVFF 02,42 5A10: MOVFF 01,41 .................... //------Integral--------- .................... //we should only perform integration if plant is not saturated .................... if(abs(hCONTROL.PIDtot) < maxPIDtot) 5A14: MOVFF 47,03 5A18: MOVF 46,W 5A1A: BTFSS 47.7 5A1C: BRA 5A2E 5A1E: MOVLW 00 5A20: BSF FD8.0 5A22: SUBFWB 46,W 5A24: MOVWF 00 5A26: MOVLW 00 5A28: SUBFWB 47,W 5A2A: MOVWF 03 5A2C: MOVF 00,W 5A2E: MOVLB 3 5A30: MOVWF xD3 5A32: MOVFF 03,3D4 5A36: BTFSS 03.7 5A38: BRA 5A40 5A3A: BTFSS xD2.7 5A3C: BRA 5A52 5A3E: BRA 5A44 5A40: BTFSC xD2.7 5A42: BRA 5B1A 5A44: MOVF xD4,W 5A46: SUBWF xD2,W 5A48: BNC 5B1A 5A4A: BNZ 5A52 5A4C: MOVF xD1,W 5A4E: SUBWF xD3,W 5A50: BC 5B1A .................... hCONTROL.iVal += (hCONTROL.iGain*hCONTROL.error)/hIntSCALE; 5A52: MOVFF 3E,417 5A56: MOVFF 3D,416 5A5A: MOVLB 0 5A5C: CALL 344E 5A60: MOVFF 00,3D7 5A64: MOVFF 01,3D8 5A68: MOVFF 02,3D9 5A6C: MOVFF 03,3DA 5A70: MOVLB 4 5A72: CLRF x97 5A74: MOVFF 43,496 5A78: MOVLB 0 5A7A: CALL 0DB8 5A7E: MOVFF 03,495 5A82: MOVFF 02,494 5A86: MOVFF 01,493 5A8A: MOVFF 00,492 5A8E: MOVFF 38,499 5A92: MOVFF 37,498 5A96: MOVFF 36,497 5A9A: MOVFF 35,496 5A9E: CALL 0CC2 5AA2: MOVFF 00,3D3 5AA6: MOVFF 01,3D4 5AAA: MOVFF 02,3D5 5AAE: MOVFF 03,3D6 5AB2: MOVFF 03,496 5AB6: MOVFF 02,495 5ABA: MOVFF 01,494 5ABE: MOVFF 00,493 5AC2: MOVLB 4 5AC4: CLRF x9A 5AC6: CLRF x99 5AC8: MOVLW 20 5ACA: MOVWF x98 5ACC: MOVLW 82 5ACE: MOVWF x97 5AD0: MOVLB 0 5AD2: CALL 1066 5AD6: BCF FD8.1 5AD8: MOVFF 3DA,49D 5ADC: MOVFF 3D9,49C 5AE0: MOVFF 3D8,49B 5AE4: MOVFF 3D7,49A 5AE8: MOVFF 03,4A1 5AEC: MOVFF 02,4A0 5AF0: MOVFF 01,49F 5AF4: MOVFF 00,49E 5AF8: CALL 0DEE 5AFC: MOVFF 03,43D 5B00: MOVFF 02,43C 5B04: MOVFF 01,43B 5B08: MOVFF 00,43A 5B0C: CALL 2F98 5B10: MOVFF 02,3E 5B14: MOVFF 01,3D 5B18: MOVLB 3 .................... //also, if integral gain is zero, clear any effect .................... if(hCONTROL.iGain == 0) 5B1A: MOVF 43,F 5B1C: BNZ 5B22 .................... { .................... hCONTROL.iVal = 0; 5B1E: CLRF 3E 5B20: CLRF 3D .................... } .................... //------Total PID-------- .................... hCONTROL.PIDtot = hCONTROL.pVal + hCONTROL.dVal + hCONTROL.iVal; 5B22: MOVF 41,W 5B24: ADDWF 3F,W 5B26: MOVWF xD3 5B28: MOVF 42,W 5B2A: ADDWFC 40,W 5B2C: MOVWF xD4 5B2E: MOVF 3D,W 5B30: ADDWF xD3,W 5B32: MOVWF 46 5B34: MOVF 3E,W 5B36: ADDWFC xD4,W 5B38: MOVWF 47 .................... //------Apply PID-------- .................... apply_h_robo(); 5B3A: MOVLB 0 5B3C: BRA 5724 5B3E: MOVLB 3 .................... } 5B40: MOVLB 0 5B42: GOTO 6B0C (RETURN) .................... .................... #include "control_velocity.c" .................... //------OPTIONS--------- .................... #define vSCALE 5; //increasing this will increase the resolution of the gains, effectively dividing them by this .................... #define vIntSCALE 10; //The integral gain needs to be scaled further than the P and D gains .................... #define vMAX 100; //maximum value to output to motor controller - 0 to 127 - this is because velocity cannot be 100% in order to trun .................... //#define iDEFAULT 5; //default integral gain .................... //#define pDEFAULT 5; //default proportional gain .................... //#define dDEFAULT 5; //default derivitive gain .................... //#define desDEFAULT 0; //default desired heading .................... .................... /** .................... * This struct contains all the parameters for velocity control .................... **/ .................... /** MOVED TO GLOBALS .................... struct .................... { .................... int1 enabled; //flag that enables / disables velocity control .................... float desired; //desired velocity .................... float error; //current error .................... float pError; //previous error .................... signed int16 iVal; //integral component .................... signed int16 pVal; //proportional component .................... signed int16 dVal; //derivitive component .................... int iGain; //integral gain .................... int pGain; //proportional gain .................... int dGain; //derivative gain .................... signed int16 PIDtot; //total value of P+I+D components .................... .................... } vCONTROL; .................... **/ .................... .................... .................... .................... /** .................... * This function calculates the error for velocity .................... * .................... **/ .................... void vError() .................... { .................... .................... //calculate error .................... vCONTROL.error = vCONTROL.desired - GPS.sog; 5B46: BSF FD8.1 5B48: MOVFF 4C,49D 5B4C: MOVFF 4B,49C 5B50: MOVFF 4A,49B 5B54: MOVFF 49,49A 5B58: MOVFF 31B,4A1 5B5C: MOVFF 31A,4A0 5B60: MOVFF 319,49F 5B64: MOVFF 318,49E 5B68: CALL 0DEE 5B6C: MOVFF 03,50 5B70: MOVFF 02,4F 5B74: MOVFF 01,4E 5B78: MOVFF 00,4D .................... //!!Perform some reality checking .................... .................... } 5B7C: GOTO 5D7A (RETURN) .................... .................... /** .................... * This function interprets the hCONTROL.PIDtot and applies scaling / conversion and sends it to roboteq .................... * .................... **/ .................... void apply_v_robo() .................... { .................... //------Local Variables------ .................... char hexVal[3] = ""; //the hex value of PID tot in a string 5B80: MOVLB 3 5B82: CLRF xD3 5B84: CLRF xD4 5B86: CLRF xD5 .................... char dir[2] = ""; //a single letter for forward or reverse 5B88: CLRF xD6 5B8A: CLRF xD7 .................... char comb[4]=""; //this is the variable used to combine hexVal of PIDTOT and Direction 5B8C: CLRF xD8 5B8E: CLRF xD9 5B90: CLRF xDA 5B92: CLRF xDB .................... signed int16 maxPIDtot = vMAX*vSCALE; //maximum value PID total can have 5B94: MOVLW 64 5B96: MOVWF xDC 5B98: CLRF xDD .................... //if PIDtot is larger than max, set to max .................... //a larger vscale value will increase the resolution of the Gains .................... if(vCONTROL.PIDtot > maxPIDtot) 5B9A: BTFSS xDD.7 5B9C: BRA 5BA4 5B9E: BTFSS 5F.7 5BA0: BRA 5BB6 5BA2: BRA 5BA8 5BA4: BTFSC 5F.7 5BA6: BRA 5BBE 5BA8: MOVF xDD,W 5BAA: SUBWF 5F,W 5BAC: BNC 5BBE 5BAE: BNZ 5BB6 5BB0: MOVF 5E,W 5BB2: SUBWF xDC,W 5BB4: BC 5BBE .................... { .................... vCONTROL.PIDtot = vMAX*vSCALE; 5BB6: CLRF 5F 5BB8: MOVLW 64 5BBA: MOVWF 5E .................... } else if(vCONTROL.PIDtot < -maxPIDtot) 5BBC: BRA 5BF4 5BBE: MOVLW 00 5BC0: BSF FD8.0 5BC2: SUBFWB xDC,W 5BC4: MOVWF 00 5BC6: MOVLW 00 5BC8: SUBFWB xDD,W 5BCA: MOVWF 03 5BCC: MOVFF 00,01 5BD0: BTFSS 5F.7 5BD2: BRA 5BDA 5BD4: BTFSS 03.7 5BD6: BRA 5BEC 5BD8: BRA 5BDE 5BDA: BTFSC 03.7 5BDC: BRA 5BF4 5BDE: MOVF 5F,W 5BE0: SUBWF 03,W 5BE2: BNC 5BF4 5BE4: BNZ 5BEC 5BE6: MOVF 01,W 5BE8: SUBWF 5E,W 5BEA: BC 5BF4 .................... { .................... vCONTROL.PIDtot = -vMAX*vSCALE; 5BEC: MOVLW FF 5BEE: MOVWF 5F 5BF0: MOVLW 9C 5BF2: MOVWF 5E .................... } .................... //divide by scale .................... vCONTROL.PIDtot /= vSCALE; 5BF4: MOVFF 5F,3DF 5BF8: MOVFF 5E,3DE 5BFC: CLRF xE1 5BFE: MOVLW 05 5C00: MOVWF xE0 5C02: MOVLB 0 5C04: RCALL 55D4 5C06: MOVFF 02,5F 5C0A: MOVFF 01,5E .................... //perform power scaling .................... if(PARAMS.pScale > 0 && PARAMS.pScale < 100) 5C0E: MOVLB 3 5C10: MOVF x8E,F 5C12: BZ 5CBC 5C14: MOVF x8E,W 5C16: SUBLW 63 5C18: BNC 5CBC .................... vCONTROL.PIDtot =(int16) ((float)vCONTROL.PIDtot*((float)PARAMS.pScale / 100)) ; 5C1A: MOVFF 5F,417 5C1E: MOVFF 5E,416 5C22: MOVLB 0 5C24: CALL 344E 5C28: MOVFF 00,3DE 5C2C: MOVFF 01,3DF 5C30: MOVFF 02,3E0 5C34: MOVFF 03,3E1 5C38: MOVLB 4 5C3A: CLRF x97 5C3C: MOVFF 38E,496 5C40: MOVLB 0 5C42: CALL 0DB8 5C46: MOVFF 00,3E2 5C4A: MOVFF 01,3E3 5C4E: MOVFF 02,3E4 5C52: MOVFF 03,3E5 5C56: MOVFF 03,496 5C5A: MOVFF 02,495 5C5E: MOVFF 01,494 5C62: MOVFF 00,493 5C66: MOVLB 4 5C68: CLRF x9A 5C6A: CLRF x99 5C6C: MOVLW 48 5C6E: MOVWF x98 5C70: MOVLW 85 5C72: MOVWF x97 5C74: MOVLB 0 5C76: CALL 1066 5C7A: MOVFF 3E1,495 5C7E: MOVFF 3E0,494 5C82: MOVFF 3DF,493 5C86: MOVFF 3DE,492 5C8A: MOVFF 03,499 5C8E: MOVFF 02,498 5C92: MOVFF 01,497 5C96: MOVFF 00,496 5C9A: CALL 0CC2 5C9E: MOVFF 03,43D 5CA2: MOVFF 02,43C 5CA6: MOVFF 01,43B 5CAA: MOVFF 00,43A 5CAE: CALL 2F98 5CB2: MOVFF 02,5F 5CB6: MOVFF 01,5E 5CBA: MOVLB 3 .................... //set the direction for roboteq .................... if(vCONTROL.PIDtot >=0) 5CBC: BTFSC 5F.7 5CBE: BRA 5CC8 5CC0: BRA 5CC2 .................... { .................... //forward .................... dir[0] = 'b'; 5CC2: MOVLW 62 5CC4: MOVWF xD6 .................... } else 5CC6: BRA 5CCC .................... { .................... //reverse .................... dir[0] = 'B'; 5CC8: MOVLW 42 5CCA: MOVWF xD6 .................... } .................... //now convert value of PIDtot to hex string .................... sprintf(hexVal,"%X",abs(vCONTROL.PIDtot)); 5CCC: MOVFF 5F,03 5CD0: MOVF 5E,W 5CD2: BTFSS 5F.7 5CD4: BRA 5CE6 5CD6: MOVLW 00 5CD8: BSF FD8.0 5CDA: SUBFWB 5E,W 5CDC: MOVWF 00 5CDE: MOVLW 00 5CE0: SUBFWB 5F,W 5CE2: MOVWF 03 5CE4: MOVF 00,W 5CE6: MOVWF xDE 5CE8: MOVFF 03,3DF 5CEC: MOVLW 03 5CEE: MOVWF xD0 5CF0: MOVLW D3 5CF2: MOVWF xCF 5CF4: MOVFF 3DE,3E0 5CF8: MOVLW 37 5CFA: MOVWF xE1 5CFC: MOVLB 0 5CFE: RCALL 566C .................... //set combination of hex val and dir to roboteq .................... //combine dir and hexVal .................... strcat(comb,dir); 5D00: MOVLW 03 5D02: MOVLB 3 5D04: MOVWF xDF 5D06: MOVLW D8 5D08: MOVWF xDE 5D0A: MOVLW 03 5D0C: MOVWF xE1 5D0E: MOVLW D6 5D10: MOVWF xE0 5D12: MOVLB 0 5D14: RCALL 56B2 .................... strcat(comb,hexVal); 5D16: MOVLW 03 5D18: MOVLB 3 5D1A: MOVWF xDF 5D1C: MOVLW D8 5D1E: MOVWF xDE 5D20: MOVLW 03 5D22: MOVWF xE1 5D24: MOVLW D3 5D26: MOVWF xE0 5D28: MOVLB 0 5D2A: RCALL 56B2 .................... //copy command to roboteq M2 .................... strcpy(ROBO.M2,comb); 5D2C: CLRF FEA 5D2E: MOVLW 28 5D30: MOVWF FE9 5D32: MOVLW 03 5D34: MOVWF FE2 5D36: MOVLW D8 5D38: MOVWF FE1 5D3A: MOVF FE7,F 5D3C: MOVFF FE6,FEE 5D40: BNZ 5D3A .................... .................... } 5D42: GOTO 5F9C (RETURN) .................... .................... .................... /** .................... * This function performs heading control .................... * .................... **/ .................... void control_velocity() .................... { .................... //----Local Variables---- .................... signed int16 maxPIDtot = 127*vSCALE; //used to test if saturation has occured 5D46: MOVLW 7B 5D48: MOVLB 3 5D4A: MOVWF xD1 5D4C: MOVLW 02 5D4E: MOVWF xD2 .................... .................... //-----Check if hCONTROL is enabled------ .................... if(!vCONTROL.enabled || PARAMS.mode == 'K' || GPS.status == 'V') 5D50: BTFSS 48.0 5D52: BRA 5D60 5D54: MOVF x8D,W 5D56: SUBLW 4B 5D58: BZ 5D60 5D5A: MOVF x0D,W 5D5C: SUBLW 56 5D5E: BNZ 5D66 .................... { //we're not enabled, or GPS hasn't locked on .................... //also clear integral value .................... vCONTROL.iVal = 0; 5D60: CLRF 56 5D62: CLRF 55 .................... return; 5D64: BRA 5F9E .................... } .................... .................... //----Update Previous Error---- .................... vCONTROL.pError = vCONTROL.error; 5D66: MOVFF 50,54 5D6A: MOVFF 4F,53 5D6E: MOVFF 4E,52 5D72: MOVFF 4D,51 .................... .................... //----Calculate New Error------ .................... vError(); 5D76: MOVLB 0 5D78: BRA 5B46 .................... //----if error is 0, return, a bit optimistic----- .................... if(vCONTROL.error == 0) 5D7A: MOVFF 50,43D 5D7E: MOVFF 4F,43C 5D82: MOVFF 4E,43B 5D86: MOVFF 4D,43A 5D8A: MOVLB 4 5D8C: CLRF x41 5D8E: CLRF x40 5D90: CLRF x3F 5D92: CLRF x3E 5D94: MOVLB 0 5D96: CALL 2360 5D9A: BNZ 5DA0 .................... { .................... return; 5D9C: MOVLB 3 5D9E: BRA 5F9E .................... } .................... //------Proportional----- .................... vCONTROL.pVal = vCONTROL.pGain*vCONTROL.error; 5DA0: MOVLB 4 5DA2: CLRF x97 5DA4: MOVFF 5C,496 5DA8: MOVLB 0 5DAA: CALL 0DB8 5DAE: MOVFF 03,495 5DB2: MOVFF 02,494 5DB6: MOVFF 01,493 5DBA: MOVFF 00,492 5DBE: MOVFF 50,499 5DC2: MOVFF 4F,498 5DC6: MOVFF 4E,497 5DCA: MOVFF 4D,496 5DCE: CALL 0CC2 5DD2: MOVFF 03,43D 5DD6: MOVFF 02,43C 5DDA: MOVFF 01,43B 5DDE: MOVFF 00,43A 5DE2: CALL 2F98 5DE6: MOVFF 02,58 5DEA: MOVFF 01,57 .................... //------Derivitive------- .................... vCONTROL.dVal = vCONTROL.dGain*(vCONTROL.error-vCONTROL.pError); 5DEE: BSF FD8.1 5DF0: MOVFF 50,49D 5DF4: MOVFF 4F,49C 5DF8: MOVFF 4E,49B 5DFC: MOVFF 4D,49A 5E00: MOVFF 54,4A1 5E04: MOVFF 53,4A0 5E08: MOVFF 52,49F 5E0C: MOVFF 51,49E 5E10: CALL 0DEE 5E14: MOVFF 00,3D3 5E18: MOVFF 01,3D4 5E1C: MOVFF 02,3D5 5E20: MOVFF 03,3D6 5E24: MOVLB 4 5E26: CLRF x97 5E28: MOVFF 5D,496 5E2C: MOVLB 0 5E2E: CALL 0DB8 5E32: MOVFF 03,495 5E36: MOVFF 02,494 5E3A: MOVFF 01,493 5E3E: MOVFF 00,492 5E42: MOVFF 3D6,499 5E46: MOVFF 3D5,498 5E4A: MOVFF 3D4,497 5E4E: MOVFF 3D3,496 5E52: CALL 0CC2 5E56: MOVFF 03,43D 5E5A: MOVFF 02,43C 5E5E: MOVFF 01,43B 5E62: MOVFF 00,43A 5E66: CALL 2F98 5E6A: MOVFF 02,5A 5E6E: MOVFF 01,59 .................... //------Integral--------- .................... //we should only perform integration if plant is not saturated .................... if(abs(vCONTROL.PIDtot) < maxPIDtot) 5E72: MOVFF 5F,03 5E76: MOVF 5E,W 5E78: BTFSS 5F.7 5E7A: BRA 5E8C 5E7C: MOVLW 00 5E7E: BSF FD8.0 5E80: SUBFWB 5E,W 5E82: MOVWF 00 5E84: MOVLW 00 5E86: SUBFWB 5F,W 5E88: MOVWF 03 5E8A: MOVF 00,W 5E8C: MOVLB 3 5E8E: MOVWF xD3 5E90: MOVFF 03,3D4 5E94: BTFSS 03.7 5E96: BRA 5E9E 5E98: BTFSS xD2.7 5E9A: BRA 5EB0 5E9C: BRA 5EA2 5E9E: BTFSC xD2.7 5EA0: BRA 5F78 5EA2: MOVF xD4,W 5EA4: SUBWF xD2,W 5EA6: BNC 5F78 5EA8: BNZ 5EB0 5EAA: MOVF xD1,W 5EAC: SUBWF xD3,W 5EAE: BC 5F78 .................... vCONTROL.iVal += (vCONTROL.iGain*vCONTROL.error)/vIntSCALE; 5EB0: MOVFF 56,417 5EB4: MOVFF 55,416 5EB8: MOVLB 0 5EBA: CALL 344E 5EBE: MOVFF 00,3D7 5EC2: MOVFF 01,3D8 5EC6: MOVFF 02,3D9 5ECA: MOVFF 03,3DA 5ECE: MOVLB 4 5ED0: CLRF x97 5ED2: MOVFF 5B,496 5ED6: MOVLB 0 5ED8: CALL 0DB8 5EDC: MOVFF 03,495 5EE0: MOVFF 02,494 5EE4: MOVFF 01,493 5EE8: MOVFF 00,492 5EEC: MOVFF 50,499 5EF0: MOVFF 4F,498 5EF4: MOVFF 4E,497 5EF8: MOVFF 4D,496 5EFC: CALL 0CC2 5F00: MOVFF 00,3D3 5F04: MOVFF 01,3D4 5F08: MOVFF 02,3D5 5F0C: MOVFF 03,3D6 5F10: MOVFF 03,496 5F14: MOVFF 02,495 5F18: MOVFF 01,494 5F1C: MOVFF 00,493 5F20: MOVLB 4 5F22: CLRF x9A 5F24: CLRF x99 5F26: MOVLW 20 5F28: MOVWF x98 5F2A: MOVLW 82 5F2C: MOVWF x97 5F2E: MOVLB 0 5F30: CALL 1066 5F34: BCF FD8.1 5F36: MOVFF 3DA,49D 5F3A: MOVFF 3D9,49C 5F3E: MOVFF 3D8,49B 5F42: MOVFF 3D7,49A 5F46: MOVFF 03,4A1 5F4A: MOVFF 02,4A0 5F4E: MOVFF 01,49F 5F52: MOVFF 00,49E 5F56: CALL 0DEE 5F5A: MOVFF 03,43D 5F5E: MOVFF 02,43C 5F62: MOVFF 01,43B 5F66: MOVFF 00,43A 5F6A: CALL 2F98 5F6E: MOVFF 02,56 5F72: MOVFF 01,55 5F76: MOVLB 3 .................... //also, if integral gain is zero, clear any effect .................... if(vCONTROL.iGain == 0) 5F78: MOVF 5B,F 5F7A: BNZ 5F80 .................... { .................... vCONTROL.iVal = 0; 5F7C: CLRF 56 5F7E: CLRF 55 .................... } .................... //------Total PID-------- .................... vCONTROL.PIDtot = vCONTROL.pVal + vCONTROL.dVal + vCONTROL.iVal; 5F80: MOVF 59,W 5F82: ADDWF 57,W 5F84: MOVWF xD3 5F86: MOVF 5A,W 5F88: ADDWFC 58,W 5F8A: MOVWF xD4 5F8C: MOVF 55,W 5F8E: ADDWF xD3,W 5F90: MOVWF 5E 5F92: MOVF 56,W 5F94: ADDWFC xD4,W 5F96: MOVWF 5F .................... //------Apply PID-------- .................... apply_v_robo(); 5F98: MOVLB 0 5F9A: BRA 5B80 5F9C: MOVLB 3 .................... } 5F9E: MOVLB 0 5FA0: GOTO 6B10 (RETURN) .................... .................... .................... .................... .................... .................... /** .................... * This function just says hello during initial boot up .................... * .................... **/ .................... void test_ports() .................... { .................... fprintf(BASESTREAM, "HELLO BASESTREAM\r\n"); * 0840: MOVLB 3 0842: CLRF xD1 0844: MOVF xD1,W 0846: MOVLB 0 0848: RCALL 04F2 084A: MOVLB 3 084C: INCF xD1,F 084E: MOVWF 00 0850: CLRF 18 0852: BTFSC FF2.7 0854: BSF 18.7 0856: BCF FF2.7 0858: MOVFF FE8,4AF 085C: MOVLB 0 085E: RCALL 05BA 0860: BTFSC 18.7 0862: BSF FF2.7 0864: MOVLW 12 0866: MOVLB 3 0868: SUBWF xD1,W 086A: BNZ 0844 .................... fprintf(COMPSTREAM, "HELLO COMPSTREAM\r\n"); 086C: CLRF xD1 086E: MOVF xD1,W 0870: MOVLB 0 0872: RCALL 0520 0874: MOVLB 3 0876: INCF xD1,F 0878: MOVWF 00 087A: MOVWF xDA 087C: MOVLB 0 087E: RCALL 0770 0880: MOVLW 12 0882: MOVLB 3 0884: SUBWF xD1,W 0886: BNZ 086E .................... fprintf(GPSSTREAM, "HELLO GPS\r\n"); 0888: CLRF xD1 088A: MOVF xD1,W 088C: MOVLB 0 088E: RCALL 054E 0890: MOVLB 3 0892: INCF xD1,F 0894: MOVWF 00 0896: MOVF 00,W 0898: BTFSS F9E.4 089A: BRA 0898 089C: MOVWF FAD 089E: MOVLW 0B 08A0: SUBWF xD1,W 08A2: BNZ 088A .................... fprintf(ROBOSTREAM, "HELLO ROBOSTREAM\r\n"); 08A4: CLRF xD1 08A6: MOVF xD1,W 08A8: MOVLB 0 08AA: RCALL 0574 08AC: MOVLB 3 08AE: INCF xD1,F 08B0: MOVWF 00 08B2: MOVWF xDE 08B4: MOVLB 0 08B6: RCALL 07C2 08B8: MOVLW 12 08BA: MOVLB 3 08BC: SUBWF xD1,W 08BE: BNZ 08A6 .................... } 08C0: MOVLB 0 08C2: GOTO 6ADE (RETURN) .................... /** .................... * This is the main entry point where the program begins .................... * .................... **/ .................... void main() .................... { * 6816: CLRF FF8 6818: BCF FD0.7 681A: BSF 0D.7 681C: CLRF FEA 681E: CLRF FE9 6820: BCF F94.1 6822: BSF F8B.1 6824: BSF FB8.3 6826: MOVLW 08 6828: MOVWF FAF 682A: MOVLW 02 682C: MOVWF FB0 682E: MOVLW A6 6830: MOVWF FAC 6832: MOVLW 90 6834: MOVWF FAB 6836: BCF F93.5 6838: BSF F8A.5 683A: BCF F93.1 683C: BSF F8A.1 683E: MOVF FC1,W 6840: ANDLW C0 6842: IORLW 0F 6844: MOVWF FC1 6846: MOVLW 07 6848: MOVWF FB4 684A: CLRF 19 684C: CLRF 1A 684E: CLRF 1F 6850: MOVLB 2 6852: CLRF x69 6854: CLRF x6A 6856: CLRF x6B 6858: CLRF x6C 685A: CLRF x6D 685C: CLRF x6E 685E: CLRF x6F 6860: CLRF x70 6862: CLRF x71 6864: CLRF x72 6866: CLRF x73 6868: CLRF x74 686A: CLRF x75 686C: CLRF x76 686E: CLRF x77 6870: CLRF x78 6872: CLRF x79 6874: CLRF x7A 6876: CLRF x7B 6878: CLRF x7C 687A: CLRF x7D 687C: CLRF x7E 687E: CLRF x7F 6880: CLRF x80 6882: CLRF x81 6884: CLRF x82 6886: CLRF x83 6888: CLRF x84 688A: CLRF x85 688C: CLRF x86 688E: CLRF x87 6890: CLRF x88 6892: CLRF x89 6894: CLRF x8A 6896: CLRF x8B 6898: CLRF x8C 689A: CLRF x8D 689C: CLRF x8E 689E: CLRF x8F 68A0: CLRF x90 68A2: CLRF x91 68A4: CLRF x92 68A6: CLRF x93 68A8: CLRF x94 68AA: CLRF x95 68AC: CLRF x96 68AE: CLRF x97 68B0: CLRF x98 68B2: CLRF x99 68B4: CLRF x9A 68B6: CLRF x9B 68B8: CLRF x9C 68BA: CLRF x9D 68BC: CLRF x9E 68BE: CLRF x9F 68C0: CLRF xA0 68C2: CLRF xA1 68C4: CLRF xA2 68C6: CLRF xA3 68C8: CLRF xA4 68CA: CLRF xA5 68CC: CLRF xA6 68CE: CLRF xA7 68D0: CLRF xA8 68D2: CLRF xA9 68D4: CLRF xAA 68D6: CLRF xAB 68D8: CLRF xAC 68DA: CLRF xAD 68DC: CLRF xAE 68DE: CLRF xAF 68E0: CLRF xB0 68E2: CLRF xB1 68E4: CLRF xB2 68E6: CLRF xB3 68E8: CLRF xB4 68EA: CLRF xB5 68EC: CLRF xB6 68EE: CLRF xB7 68F0: CLRF xB8 68F2: CLRF xB9 68F4: CLRF xBA 68F6: CLRF xBB 68F8: CLRF xBC 68FA: CLRF xBD 68FC: CLRF xBE 68FE: CLRF xBF 6900: CLRF xC0 6902: CLRF xC1 6904: CLRF xC2 6906: CLRF xC3 6908: CLRF xC4 690A: CLRF xC5 690C: CLRF xC6 690E: CLRF xC7 6910: CLRF xC8 6912: CLRF xC9 6914: CLRF xCA 6916: CLRF xCB 6918: CLRF xCC 691A: CLRF xCD 691C: CLRF xCE 691E: CLRF xCF 6920: CLRF xD0 6922: CLRF xD1 6924: CLRF xD2 6926: CLRF xD3 6928: CLRF xD4 692A: CLRF xD5 692C: CLRF xD6 692E: CLRF xD7 6930: CLRF xD8 6932: CLRF xD9 6934: CLRF xDA 6936: CLRF xDB 6938: CLRF xDC 693A: CLRF xDD 693C: CLRF xDE 693E: CLRF xDF 6940: CLRF xE0 6942: CLRF xE1 6944: CLRF xE2 6946: CLRF xE3 6948: CLRF xE4 694A: CLRF xE5 694C: CLRF xE6 694E: CLRF xE7 6950: CLRF xE8 6952: CLRF xE9 6954: CLRF xEA 6956: CLRF xEB 6958: CLRF xEC 695A: CLRF xED 695C: CLRF xEE 695E: CLRF xEF 6960: CLRF xF0 6962: CLRF xF1 6964: CLRF xF2 6966: CLRF xF3 6968: CLRF xF4 696A: CLRF xF5 696C: CLRF xF6 696E: CLRF xF7 6970: CLRF xF8 6972: CLRF xF9 6974: CLRF xFA 6976: CLRF xFB 6978: CLRF xFC 697A: CLRF xFD 697C: CLRF xFE 697E: CLRF xFF 6980: MOVLB 3 6982: CLRF x00 6984: CLRF x01 6986: CLRF x02 6988: CLRF x03 698A: CLRF x04 698C: BCF x05.0 698E: BCF x05.1 6990: CLRF x2C 6992: CLRF x2D 6994: CLRF x2E 6996: CLRF x2F 6998: CLRF x30 699A: CLRF x31 699C: CLRF x32 699E: CLRF x33 69A0: CLRF x34 69A2: CLRF x35 69A4: CLRF x36 69A6: CLRF x37 69A8: CLRF x38 69AA: CLRF x39 69AC: CLRF x3A 69AE: CLRF x3B 69B0: CLRF x3C 69B2: CLRF x3D 69B4: CLRF x3E 69B6: CLRF x3F 69B8: CLRF x40 69BA: CLRF x41 69BC: CLRF x42 69BE: CLRF x43 69C0: CLRF x44 69C2: CLRF x45 69C4: CLRF x46 69C6: CLRF x47 69C8: CLRF x48 69CA: CLRF x49 69CC: CLRF x4A 69CE: CLRF x4B 69D0: CLRF x4C 69D2: CLRF x4D 69D4: CLRF x4E 69D6: CLRF x4F 69D8: CLRF x50 69DA: CLRF x51 69DC: CLRF x52 69DE: CLRF x53 69E0: CLRF x54 69E2: CLRF x55 69E4: CLRF x56 69E6: CLRF x57 69E8: CLRF x58 69EA: CLRF x59 69EC: CLRF x5A 69EE: CLRF x5B 69F0: CLRF x5C 69F2: CLRF x5D 69F4: CLRF x5E 69F6: CLRF x5F 69F8: CLRF x60 69FA: CLRF x61 69FC: CLRF x62 69FE: CLRF x63 6A00: CLRF x64 6A02: CLRF x65 6A04: CLRF x66 6A06: CLRF x67 6A08: CLRF x68 6A0A: CLRF x69 6A0C: CLRF x6A 6A0E: CLRF x6B 6A10: CLRF x6C 6A12: CLRF x6D 6A14: CLRF x6E 6A16: CLRF x6F 6A18: CLRF x70 6A1A: CLRF x71 6A1C: CLRF x72 6A1E: CLRF x73 6A20: CLRF x74 6A22: CLRF x75 6A24: CLRF x76 6A26: CLRF x77 6A28: CLRF x78 6A2A: CLRF x79 6A2C: CLRF x7A 6A2E: CLRF x7B 6A30: CLRF x7C 6A32: CLRF x7D 6A34: CLRF x7E 6A36: CLRF x7F 6A38: CLRF x80 6A3A: CLRF x81 6A3C: CLRF x82 6A3E: CLRF x83 6A40: CLRF x84 6A42: CLRF x85 6A44: CLRF x86 6A46: CLRF x87 6A48: CLRF x88 6A4A: CLRF x89 6A4C: CLRF x8A 6A4E: CLRF x8B 6A50: BCF x05.2 6A52: CLRF x8C 6A54: BCF x05.3 6A56: BCF x05.4 6A58: BCF x05.5 6A5A: BCF x05.6 6A5C: BCF x05.7 6A5E: CLRF x9F 6A60: CLRF xA0 6A62: CLRF xA1 6A64: CLRF xA2 6A66: CLRF xA3 6A68: CLRF xA4 6A6A: CLRF xA5 6A6C: CLRF xA6 6A6E: CLRF xA7 6A70: CLRF xA8 6A72: CLRF xA9 6A74: CLRF xAA 6A76: CLRF xAB 6A78: CLRF xAC 6A7A: CLRF xAD 6A7C: CLRF xAE 6A7E: CLRF xAF 6A80: CLRF xB0 6A82: CLRF xB1 6A84: CLRF xB2 6A86: CLRF xB3 6A88: CLRF xB4 6A8A: CLRF xB5 6A8C: CLRF xB6 6A8E: CLRF xB7 6A90: CLRF xB8 6A92: CLRF xB9 6A94: CLRF xBA 6A96: CLRF xBB 6A98: CLRF xBC 6A9A: CLRF xBD 6A9C: CLRF xBE 6A9E: CLRF xBF 6AA0: CLRF xC0 6AA2: CLRF xC1 6AA4: CLRF xC2 6AA6: CLRF xC3 6AA8: CLRF xC4 6AAA: CLRF xC5 6AAC: CLRF xC6 6AAE: CLRF xC7 6AB0: CLRF xC8 6AB2: CLRF xC9 6AB4: CLRF xCA 6AB6: CLRF xCB 6AB8: CLRF xCC 6ABA: CLRF xCD 6ABC: CLRF xCE 6ABE: CLRF xCF 6AC0: CLRF xD0 .................... //wait for capacitors to charge / ics to start etc... .................... delay_ms(500); 6AC2: MOVLW 02 6AC4: MOVWF xD1 6AC6: MOVLW FA 6AC8: MOVWF xD2 6ACA: MOVLB 0 6ACC: GOTO 073E 6AD0: MOVLB 3 6AD2: DECFSZ xD1,F 6AD4: BRA 6AC6 .................... //Setup watch dog timer .................... setup_wdt(WDT_ON); 6AD6: BSF FD1.0 .................... //test ports for hardware debugging .................... test_ports(); 6AD8: MOVLB 0 6ADA: GOTO 0840 .................... //enable interrupts .................... enable_interrupts(INT_RDA); 6ADE: BSF F9D.5 .................... enable_interrupts(GLOBAL); 6AE0: MOVLW C0 6AE2: IORWF FF2,F .................... //make sure we're stopped at startup! - Kill Mode .................... PARAMS.mode = 'K'; 6AE4: MOVLW 4B 6AE6: MOVLB 3 6AE8: MOVWF x8D .................... //Set Current waypoint .................... WAYPOINTS.current=0; 6AEA: MOVLB 2 6AEC: CLRF x60 .................... // Main Loop .................... WHILE(1) .................... { .................... .................... query_base(); 6AEE: MOVLB 0 6AF0: GOTO 1F12 .................... update_compass(); 6AF4: GOTO 2774 .................... parse_gps(); 6AF8: GOTO 2C32 .................... calc_distance(); 6AFC: GOTO 3AAA .................... calc_heading(); 6B00: GOTO 4EAE .................... check_threshold(); 6B04: GOTO 53AC .................... control_heading(); 6B08: GOTO 58EE .................... control_velocity(); 6B0C: GOTO 5D46 .................... update_robo(); 6B10: CALL 0974 .................... query_robo(); 6B14: GOTO 609E .................... report_base(); 6B18: BRA 6476 .................... .................... } 6B1A: BRA 6AF0 .................... .................... } 6B1C: BRA 6B1C Configuration Fuses: Word 1: 0200 HS NOIESO NOFCMEN RESERVED Word 2: 0E19 NOBROWNOUT WDT128 NOWDT BORV25 NOPUT Word 3: 8300 PBADEN CCP2C1 NOLPT1OSC MCLR RESERVED Word 4: 0000 NOSTVREN DEBUG NOLVP NOXINST RESERVED Word 5: C00F NOPROTECT NOCPD NOCPB Word 6: E00F NOWRT NOWRTD NOWRTC NOWRTB Word 7: 400F NOEBTR NOEBTRB