Subversion Repositories NaviCtrl

Rev

Rev 253 | Rev 261 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 253 Rev 254
Line 57... Line 57...
57 
#include "91x_lib.h"
 57 
#include "91x_lib.h"
58 
#include "compass.h"
 58 
#include "compass.h"
59 
#include "mk3mag.h"
 59 
#include "mk3mag.h"
60 
#include "ncmag.h"
 60 
#include "ncmag.h"
61 
#include "uart1.h"
 61 
#include "uart1.h"
- 
 
 62 
#include "fifo.h"
Line -... Line 63...
- 
 
 63 
 
62 
 
 64 
 
63 
volatile s16vec_t MagVector;
 65 
u8 CompassCalStateQueue[10];
- 
 
 66 
fifo_t CompassCalcStateFiFo;
- 
 
 67 
 
- 
 
 68 
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
- 
 
 69 
volatile s16 Compass_Heading;   // is written by mk3mag or ncmag implementation
Line 64... Line 70...
64 
volatile s16 CompassHeading;
 70 
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
65 
 
 71 
 
66 
#define COMPASS_NONE    0
 72 
#define COMPASS_NONE    0
67 
#define COMPASS_MK3MAG  1
 73 
#define COMPASS_MK3MAG  1
Line 68... Line 74...
68 
#define COMPASS_NCMAG   2
 74 
#define COMPASS_NCMAG   2
69 
u8 CompassDevice = COMPASS_NONE;
 75 
u8 Compass_Device = COMPASS_NONE;
70 
 
 76 
 
71 
void Compass_Init(void)
 77 
void Compass_Init(void)
72 
{
 78 
{
73 
        UART1_PutString("\r\n Looking for compass");
 79 
        UART1_PutString("\r\n Looking for compass");
- 
 
 80 
        if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
74 
        if(     MK3MAG_Init() ) CompassDevice = COMPASS_MK3MAG;
 81 
        else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
Line 75... Line 82...
75 
        else if( NCMAG_Init() ) CompassDevice = COMPASS_NCMAG;
 82 
        else                                    Compass_Device = COMPASS_NONE;
76 
        else                                    CompassDevice = COMPASS_NONE;
 83 
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
77 
}
 84 
}
78 
 
 85 
 
79 
 
 86 
 
80 
void Compass_UpdateHeading(void)
 87 
void Compass_UpdateHeading(void)
81 
{
 88 
{
82 
        switch(CompassDevice)
 89 
        switch(Compass_Device)
Line 92... Line 99...
92 
        }
 99 
        }
93 
}
 100 
}
Line 94... Line 101...
94 
 
 101 
 
95 
void Compass_UpdateMagVector(void)
 102 
void Compass_UpdateMagVector(void)
96 
{
 103 
{
97 
        switch(CompassDevice)
 104 
        switch(Compass_Device)
98 
        {
 105 
        {
99 
                case COMPASS_MK3MAG:
 106 
                case COMPASS_MK3MAG:
100 
                        MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT);
 107 
                        MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT);
101 
                        break;
 108 
                        break;
102 
                case COMPASS_NCMAG:
 109 
                case COMPASS_NCMAG:
103 
                        NCMAG_UpdateCompass();
 110 
                        NCMAG_UpdateCompass();
104 
                default:
 111 
                default:
105 
                        break;
 112 
                        break;
106 
        }
 113 
        }
- 
 
 114 
}
- 
 
 115 
 
- 
 
 116 
// put cal state into fifo
- 
 
 117 
void Compass_SetCalState(u8 CalState)
- 
 
 118 
{
- 
 
 119 
        fifo_put(&CompassCalcStateFiFo, CalState);     
- 
 
 120 
}
- 
 
 121 
 
- 
 
 122 
// get cal state from fifo
- 
 
 123 
void Compass_UpdateCalState()
- 
 
 124 
{
- 
 
 125 
        fifo_get(&CompassCalcStateFiFo, (u8*)&Compass_CalState);       
- 
 
 126 
}