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; |