Subversion Repositories NaviCtrl

Rev

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

Rev 482 Rev 483
Line 66... Line 66...
66 
 
 66 
 
67 
u8 CompassCalStateQueue[10];
 67 
u8 CompassCalStateQueue[10];
Line 68... Line 68...
68 
fifo_t CompassCalcStateFiFo;
 68 
fifo_t CompassCalcStateFiFo;
69 
 
 - 
 
70 
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
 69 
 
71 
volatile s16vec_t AccVector;   // current acceleration vector of compass, not supported by any HW version
 70 
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
72 
volatile s16 Compass_Heading;  // is written by mk3mag or ncmag implementation
 71 
volatile s16 Compass_Heading;  // is written by mk3mag or ncmag implementation
73 
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
 72 
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
74 
s16 Hx = 0, Hy = 0;
 73 
s16 Hx = 0, Hy = 0;
Line 82... Line 81...
82 
 
 81 
 
83 
#define COMPASS_NONE    0
 82 
#define COMPASS_NONE    0
84 
#define COMPASS_MK3MAG  1
 83 
#define COMPASS_MK3MAG  1
85 
#define COMPASS_NCMAG   2
 84 
#define COMPASS_NCMAG   2
86 
u8 Compass_Device = COMPASS_NONE;
 - 
 
Line 87... Line 85...
87 
I2C_TypeDef* Compass_I2CPort = I2C1;
 85 
u8 Compass_Device = COMPASS_NONE;
88 
 
 86 
 
89 
void Compass_Init(void)
 87 
void Compass_Init(void)
90 
{
 88 
{
91 
        switch(Compass_Device)
 89 
        switch(Compass_Device)
92 
        {
 90 
        {
93 
                case COMPASS_NONE:
 91 
                case COMPASS_NONE:
94 
                        UART1_PutString("\r\n Looking for compass");
 92 
                        UART1_PutString("\r\n Looking for compass");
- 
 
 93 
                        if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
- 
 
 94 
                        else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
- 
 
 95 
 
- 
 
 96 
/*                      if(Version_HW > 11)
- 
 
 97 
                        {
- 
 
 98 
                                if(      NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
- 
 
 99 
                                else if( MK3MAG_Init()) Compass_Device = COMPASS_MK3MAG;
- 
 
 100 
                        }
- 
 
 101 
                        else
- 
 
 102 
                        {
- 
 
 103 
                                if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
- 
 
 104 
                                else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
95 
                        if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
 105 
                        }
Line 96... Line 106...
96 
                        else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
 106 
*/
- 
 
 107 
                        break;
97 
                        break;
 108 
 
98 
 
 109 
                case COMPASS_NCMAG:
99 
                case COMPASS_NCMAG:
 110 
                        UART1_PutString("\r\n Re-Init compass");
Line 100... Line 111...
100 
                        if( NCMAG_Init() )      Compass_Device = COMPASS_NCMAG;
 111 
                        if( NCMAG_Init() )      Compass_Device = COMPASS_NCMAG;
101 
                        else                            Compass_Device = COMPASS_NONE;
 - 
 
102 
                        break;
 - 
 
103 
 
 - 
 
104 
                case COMPASS_MK3MAG:
 - 
 
105 
                        if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
 112 
                        else                            Compass_Device = COMPASS_NONE;
106 
                        else                            Compass_Device = COMPASS_NONE;
 113 
                        break;
107 
                        break;
 114 
 
Line 108... Line 115...
108 
 
 115 
                case COMPASS_MK3MAG:
109 
                default:
 116 
                default:
Line 110... Line 117...
110 
                        // nothing to do
 117 
                        // nothing to do
Line 111... Line -...
111 
                        break;
 - 
 
112 
 
 - 
 
113 
        }
 - 
 
114 
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
 - 
 
115 
 
 118 
                        break;
Line 116... Line -...
116 
        if(Compass_Device == COMPASS_MK3MAG) UART_VersionInfo.Flags |= NC_VERSION_FLAG_MK3MAG_PRESENT; else UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_MK3MAG_PRESENT;
 - 
 
117 
 
 - 
 
118 
        MagVector.X = 0;
 - 
 
119 
        MagVector.Y = 0;
 - 
 
120 
        MagVector.Z = 0;
 - 
 
121 
        Compass_Heading = -1;
 - 
 
122 
}
 - 
 
123 
 
 - 
 
124 
 
 - 
 
125 
void Compass_Check(void)
 - 
 
126 
{
 - 
 
127 
        switch(Compass_Device)
 - 
 
128 
        {
 - 
 
129 
                case COMPASS_NCMAG:
 - 
 
130 
                        NCMAG_CheckOrientation();
 - 
 
131 
                        break;
 - 
 
132 
 
 119 
 
133 
                case COMPASS_MK3MAG:
 120 
        }
134 
                case COMPASS_NONE:
 121 
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
135 
                default:
 122 
 
136 
                        // nothing to do
 123 
        if(Compass_Device == COMPASS_MK3MAG) UART_VersionInfo.Flags |= NC_VERSION_FLAG_MK3MAG_PRESENT; else UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_MK3MAG_PRESENT;
Line 178... Line 165...
178 
        // initiate new compass communication
 165 
        // initiate new compass communication
179 
        switch(Compass_Device)
 166 
        switch(Compass_Device)
180 
        {
 167 
        {
181 
                case COMPASS_MK3MAG:
 168 
                case COMPASS_MK3MAG:
182 
                        MK3MAG_Update();
 169 
                        MK3MAG_Update();
- 
 
 170 
                        DebugOut.Analog[24] = MagVector.X;
- 
 
 171 
                        DebugOut.Analog[25] = MagVector.Y;
- 
 
 172 
                        DebugOut.Analog[26] = MagVector.Z;
183 
                        break;
 173 
                        break;
184 
                case COMPASS_NCMAG:
 174 
                case COMPASS_NCMAG:
- 
 
 175 
                        if(check_value_counter == 2000)
- 
 
 176 
                         {
- 
 
 177 
                          UART1_PutString("\n\r Init Mag.-Sensor");
- 
 
 178 
                          InitNC_MagnetSensor(); // 2 seconds no change of the compass value
- 
 
 179 
                         }
185 
                        NCMAG_Update(0);
 180 
                        else NCMAG_Update(0);
- 
 
 181 
                        DebugOut.Analog[24] = MagRawVector.X;
- 
 
 182 
                        DebugOut.Analog[25] = MagRawVector.Y;
186 
                        break;
 183 
                        DebugOut.Analog[26] = MagRawVector.Z;
187 
                default:
 184 
                default:
188 
                        break;
 185 
                        break;
189 
        }
 186 
        }
- 
 
 187 
    if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normally changing
Line 190... Line -...
190 
 
 - 
 
191 
        DebugOut.Analog[24] = MagVector.X;
 - 
 
192 
        DebugOut.Analog[25] = MagVector.Y;
 - 
 
193 
        DebugOut.Analog[26] = MagVector.Z;
 - 
 
194 
 
 - 
 
195 
    if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normaly changing
 - 
 
196 
 
 188 
 
197 
        if(check_value_counter == 2000)
 - 
 
198 
        {
 - 
 
199 
                UART1_PutString("\n\r Init Mag.-Sensor");
 - 
 
200 
                Compass_Init(); // 2 seconds no change of the compass value
 - 
 
201 
        }
 - 
 
202 
        else if(check_value_counter > 5000)
 189 
        if(check_value_counter > 5000)
203 
        {
 190 
         {
204 
                Compass_Heading = -1; // values didn't change for 5 seconds -> probably a compass-fault
 191 
          Compass_Heading = -1; // values didn't change for 5 seconds -> probably a compass-fault
205 
        }
 192 
         }
Line 206... Line 193...
206 
        else check_value_counter++;
 193 
         else check_value_counter++;
207 
 
 194 
 
208 
        old.X = MagVector.X;
 195 
        old.X = MagVector.X;
209 
        old.Y = MagVector.Y;
 196 
        old.Y = MagVector.Y;