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