Compass 2 Click

  1 #include <stdint.h>
  2 #include "compass2_hw.h"
  3 
  4 sbit COMPASS2_CS at GPIOD_ODR.B13;
  5 
  6 void system_setup( bus_mode_t mode, uint8_t addr );
  7 
  8 float mRes;                  // scale resolutions per LSB for the sensors
  9 uint8_t asax, asay, asaz;
 10 float adjusted_ASAX, adjusted_ASAY, adjusted_ASAZ;
 11 float heading, adjusted_MX, adjusted_MY, adjusted_MZ, magbias[3];
 12 int16_t mx, my, mz;
 13 char text[20] = { 0 };
 14 
 15 void main()
 16 {
 17   // Local Declarations
 18         uint8_t address = 0x0F;
 19         bus_mode_t my_mode = I2C;
 20         float heading = 0;
 21         char uart_text[5] = { 0 };
 22 
 23         system_setup( my_mode, address );
 24 
 25         while(1)
 26         {
 27             compass2_get_all_values( &mx, &my, &mz );
 28             heading = compass2_get_compass_heading( mx, my, mz );
 29 
 30             if( heading < 0 )
 31                 heading += 360;
 32 
 33             UART1_Write_Text( "Heading: " );
 34             FloatToStr( heading, text );
 35             UART1_Write_Text( text );
 36             UART1_Write_Text( " Direction: " );
 37 
 38             if( heading >= 330 || heading <= 30 )
 39             {
 40                 uart_text[0] = 'N';
 41                 uart_text[1] = 'n';
 42             }
 43             else if( heading >= 300 && heading <= 330 )
 44             {
 45                 uart_text[0] = 'N';
 46                 uart_text[1] = 'W';
 47                 uart_text[2] = 'n';
 48             }
 49             else if( heading >= 240 && heading <= 300)
 50             {
 51                 uart_text[0] = 'W';
 52                 uart_text[1] = 'n';
 53             }
 54             else if( heading >= 210 && heading <= 240 )
 55             {
 56                 uart_text[0] = 'S';
 57                 uart_text[1] = 'W';
 58                 uart_text[2] = 'n';
 59             }
 60             else if( heading <= 210 && heading >= 150 )
 61             {
 62                 uart_text[0] = 'S';
 63                 uart_text[1] = 'n';
 64             }
 65             else if( heading <= 150 && heading >= 120 )
 66             {
 67                 uart_text[0] = 'S';
 68                 uart_text[1] = 'E';
 69                 uart_text[2] = 'n';
 70             }
 71             else if( heading <= 120 && heading >= 60 )
 72             {
 73                 uart_text[0] = 'E';
 74                 uart_text[1] = 'n';
 75             }
 76             else if( heading <= 60 && heading >= 30 )
 77             {
 78                 uart_text[0] = 'N';
 79                 uart_text[1] = 'E';
 80                 uart_text[2] = 'n';
 81             }
 82 
 83             UART1_Write_Text( uart_text );
 84             UART1_Write_Text( "rn" );
 85 
 86             Delay_ms(100);
 87         }
 88 
 89 }
 90 
 91 void system_setup( bus_mode_t mode, uint8_t addr )
 92 {
 93         // GPIOs
 94         GPIO_Digital_Output( &GPIOB_BASE, _GPIO_PINMASK_13 );
 95 
 96         // UART
 97         UART1_Init( 9600 );
 98         UART1_Write_Text( "UART Initializedrn" );
 99 
100         // I2C
101         I2C1_Init_Advanced( 100000, &_GPIO_MODULE_I2C1_PB67 );
102         UART1_Write_Text( "I2C Initializedrn" );
103 
104         // Compass 2
105         UART1_Write_Text( "Getting Device ID..." );
106         compass2_hw_init( addr, mode );
107         UART1_Write_Text( "Compass Initializedrn" );
108 
109         // Compass 2 setup
110         mRes = compass2_set_scale_factor( RES_16 );
111         magbias[0] = +470;
112         magbias[1] = +120;
113         magbias[2] = +125;
114 
115         compass2_get_self_test( &mx, &my, &mz );
116         UART1_Write_Text( "x y z Values: " );
117         LongWordToStr( mx, text );
118         UART1_Write_Text( text );
119         UART1_Write_Text( "t" );
120         LongWordToStr( my, text );
121         UART1_Write_Text( text );
122         UART1_Write_Text( "t" );
123         LongWordToStr( mz, text );
124         UART1_Write_Text( text );
125         UART1_Write_Text( "rn" );
126 
127         compass2_get_adjustment( &asax, &asay, &asaz );
128         adjusted_ASAX = ( (float)asax - 128 ) / 256 + 1;
129         adjusted_ASAY = ( (float)asay - 128 ) / 256 + 1;
130         adjusted_ASAZ = ( (float)asaz - 128 ) / 256 + 1;
131 
132         compass2_set_mode( MODE_CONT_1 );
133         compass2_set_scale_factor( RES_16 );
134 
135         UART1_Write_Text( "Compass2 Setup Completed..rn" );
136 }