Device = 16F877
Config XT_OSC , WDT_OFF , PWRTE_ON , BODEN_OFF , LVP_OFF , CP_OFF , DEBUG_OFF

'Include "PROTON_4.INC"        ' Demo on the Proton Development Board using a 16F877
Xtal 4

' ================================Настройка LCD ===============================================

Declare LCD_Type=0         ' Тип ЖКИ - буквенно-цифровой
Declare LCD_DTPin PORTB.4         ' Порт данных ЖКИ
Declare LCD_ENPin PORTB.3         ' Управление цепью E
Declare LCD_RSPin PORTB.2         ' Управление цепью RS
Declare LCD_Interface 4           ' 4-х битная разрядность шины данных
Declare LCD_CommandUs 2000        ' Задержка перед посылкой команды
Declare LCD_DataUs 50             ' Задержка перед посылкой данных
Declare LCD_Lines 2               ' Количество строк ЖКИ

TRISD = %00000000
TRISB = %00000000
TRISC = %00000000

' -----[ Pins/Constants/Variables ]-------------------------------------------
Symbol SDA            = PORTC.3                    'P0 transceives to/from SDA
Symbol SCL            = PORTC.2                    'P1 sends clock pulses
Symbol Pi = 3.14159265359

Dim X              As      Word 'x sensor measured value
Dim Y              As      Word 'y sensor measured value
Dim Z              As      Word 'z sensor measured value
Dim brads          As      Word 'angle in brads
Dim degr           As      Word 'angle in degrees
Dim Xc As Float
Dim Yc As Float
Dim ATAN2 As Float
Dim azimuth As Float 'value calculated using X ATN -Y

Dim SCALEFACTOR    As     4081 'SCALEFACTOR = 65536 * (255/4095) = 4081
Dim SCALEOFFSET    As    2048 'The output range of the HMC5883L is -2048 to 2047; therefore to get a 0 (LOWRANGE)to 4095 range
'to 4095 (HIGHRANGE) range 2048 must be added to the sensor value.
Dim ATNFACTOR     As     127  'the ATN function input range is -127 TO 127. In order to get the positive scaled range of
'0 to 255 into the range of -127 to 127, 127 must be subtracted.
Dim LOWRANGE       As     0    'see SCALEOFFSET
Dim HIGHRANGE      As     4095 'see SCALEOFFSE

Dim ATANResult As Float
Dim ATANValue As ATANResult
Dim ATANTempFloat1 As Float
Dim ATANTempFloat2 As Float
Dim ATANFlags As Byte
Dim ATAN_SignBit As ATANFlags.0
Dim ATAN_FlagBit As ATANFlags.1
Dim s As Float 'z sensor measured value

main:
Cls
ATANValue = 12
GoSub ARCTAN         ' Calculate the ArcTangent of ATANValue
Print 1,1, Dec ATANResult
DelayMS 1000
GoTo main

'-------------------------------------------------------------------------------------
' Floating Point ARCTAN
' Returns the arctangent value of a floating point variable
'
' Input     : ATANValue holds the floating point value to calculate
' Output    : ATANResult holds the result of the ArcTan
'
ARCTAN:
ATANFlags = 0
ATANTempFloat1 = ATANValue

If ATANValue < 0 Then
ATAN_SignBit = 1
ATANTempFloat1 = -ATANTempFloat1
EndIf

If ATANTempFloat1 > 1.0 Then
ATANTempFloat1 = 1.0 / ATANTempFloat1
ATAN_FlagBit = 1
EndIf

ATANResult = 0.17630401 * ATANTempFloat1 * ATANTempFloat1
ATANResult = ATANResult + 5.6710795
ATANResult = ATANResult * ATANTempFloat1 * ATANTempFloat1
ATANResult = ATANResult + 22.376096
ATANResult = ATANResult * ATANTempFloat1 * ATANTempFloat1
ATANResult = ATANResult + 19.818457

ATANTempFloat2 = 1.0 * ATANTempFloat1 * ATANTempFloat1
ATANTempFloat2 = ATANTempFloat2 + 11.368190
ATANTempFloat2 = ATANTempFloat2 * ATANTempFloat1 * ATANTempFloat1
ATANTempFloat2 = ATANTempFloat2 + 28.982246
ATANTempFloat2 = ATANTempFloat2 * ATANTempFloat1 * ATANTempFloat1
ATANTempFloat2 = ATANTempFloat2 + 19.818457

ATANResult = ATANResult * ATANTempFloat1 / ATANTempFloat2

If ATAN_FlagBit = 1 Then ATANResult = 1.57079632 - ATANResult
If ATAN_SignBit = 1 Then ATANResult = -ATANResult
Return

'-------------------------------------------------------------------------------------
' The Main Program Starts Here