Rev 254 | Rev 257 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 254 | Rev 256 | ||
---|---|---|---|
Line 62... | Line 62... | ||
62 | #include "timer1.h" |
62 | #include "timer1.h" |
63 | #include "led.h" |
63 | #include "led.h" |
64 | #include "spi_slave.h" |
64 | #include "spi_slave.h" |
65 | #include "uart1.h" |
65 | #include "uart1.h" |
66 | #include "eeprom.h" |
66 | #include "eeprom.h" |
- | 67 | #include "mymath.h" |
|
Line 67... | Line 68... | ||
67 | 68 | ||
68 | u8 NCMAG_Present = 0; |
69 | u8 NCMAG_Present = 0; |
Line 69... | Line 70... | ||
69 | u8 NCMAG_IsCalibrated = 0; |
70 | u8 NCMAG_IsCalibrated = 0; |
Line 74... | Line 75... | ||
74 | u8 NCMAG_MagType = MAG_TYPE_NONE; |
75 | u8 NCMAG_MagType = MAG_TYPE_NONE; |
Line 75... | Line 76... | ||
75 | 76 | ||
76 | #define CALIBRATION_VERSION 1 |
77 | #define CALIBRATION_VERSION 1 |
Line -... | Line 78... | ||
- | 78 | #define EEPROM_ADR_MAG_CALIBRATION 50 |
|
- | 79 | ||
- | 80 | #define NCMAG_MIN_RAWVALUE -2047 |
|
- | 81 | #define NCMAG_MAX_RAWVALUE 2047 |
|
77 | #define EEPROM_ADR_MAG_CALIBRATION 50 |
82 | #define NCMAG_INVALID_DATA -4096 |
78 | 83 | ||
79 | typedef struct |
84 | typedef struct |
80 | { |
85 | { |
81 | s16 Range; |
86 | s16 Range; |
Line 82... | Line 87... | ||
82 | s16 Offset; |
87 | s16 Offset; |
83 | } Scaling_t; |
88 | } __attribute__((packed)) Scaling_t; |
84 | 89 | ||
85 | typedef struct |
90 | typedef struct |
86 | { |
91 | { |
87 | Scaling_t MagX; |
92 | Scaling_t MagX; |
88 | Scaling_t MagY; |
93 | Scaling_t MagY; |
89 | Scaling_t MagZ; |
94 | Scaling_t MagZ; |
Line 90... | Line 95... | ||
90 | u8 Version; |
95 | u8 Version; |
Line 91... | Line 96... | ||
91 | u8 crc; |
96 | u8 crc; |
92 | } Calibration_t; |
97 | } __attribute__((packed)) Calibration_t; |
Line 241... | Line 246... | ||
241 | u8 i, crc = 0xAA; |
246 | u8 i, crc = 0xAA; |
242 | EEPROM_Result_t eres; |
247 | EEPROM_Result_t eres; |
243 | u8 *pBuff = (u8*)&Calibration; |
248 | u8 *pBuff = (u8*)&Calibration; |
Line 244... | Line 249... | ||
244 | 249 | ||
245 | Calibration.Version = CALIBRATION_VERSION; |
250 | Calibration.Version = CALIBRATION_VERSION; |
246 | for(i = 0; i<sizeof(Calibration)-1; i++) |
251 | for(i = 0; i<(sizeof(Calibration)-1); i++) |
247 | { |
252 | { |
248 | crc += pBuff[i]; |
253 | crc += pBuff[i]; |
249 | } |
254 | } |
250 | Calibration.crc = ~crc; |
255 | Calibration.crc = ~crc; |
251 | eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)); |
- | |
252 | DebugOut.Analog[25] = eres; |
256 | eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)); |
253 | if(EEPROM_SUCCESS == eres) i = 1; |
257 | if(EEPROM_SUCCESS == eres) i = 1; |
254 | else i = 0; |
258 | else i = 0; |
255 | return(i); |
259 | return(i); |
Line 260... | Line 264... | ||
260 | u8 i, crc = 0xAA; |
264 | u8 i, crc = 0xAA; |
261 | u8 *pBuff = (u8*)&Calibration; |
265 | u8 *pBuff = (u8*)&Calibration; |
Line 262... | Line 266... | ||
262 | 266 | ||
263 | if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration))) |
267 | if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration))) |
264 | { |
268 | { |
265 | for(i = 0; i<sizeof(Calibration)-1; i++) |
269 | for(i = 0; i<(sizeof(Calibration)-1); i++) |
266 | { |
270 | { |
267 | crc += pBuff[i]; |
271 | crc += pBuff[i]; |
268 | } |
272 | } |
269 | crc = ~crc; |
273 | crc = ~crc; |
270 | if(Calibration.crc != crc) return(0); // crc mismatch |
274 | if(Calibration.crc != crc) return(0); // crc mismatch |
- | 275 | if(Calibration.Version == CALIBRATION_VERSION) |
|
- | 276 | { |
|
- | 277 | //#ifdef DEBUG |
|
- | 278 | u8 msg[50]; |
|
- | 279 | UART1_PutString("\r\n"); |
|
- | 280 | sprintf(msg, "XRange = %d, XOffset = %d \r\n", Calibration.MagX.Range, Calibration.MagX.Offset); |
|
- | 281 | UART1_PutString(msg); |
|
- | 282 | sprintf(msg, "YRange = %d, YOffset = %d \r\n", Calibration.MagY.Range, Calibration.MagY.Offset); |
|
- | 283 | UART1_PutString(msg); |
|
- | 284 | sprintf(msg, "ZRange = %d, ZOffset = %d \r\n", Calibration.MagZ.Range, Calibration.MagZ.Offset); |
|
- | 285 | UART1_PutString(msg); |
|
- | 286 | //#endif |
|
- | 287 | return(1); |
|
271 | if(Calibration.Version == CALIBRATION_VERSION) return(1); |
288 | } |
272 | } |
289 | } |
273 | return(0); |
290 | return(0); |
Line 274... | Line 291... | ||
274 | } |
291 | } |
275 | 292 | ||
276 | 293 | ||
- | 294 | void NCMAG_Calibrate(void) |
|
277 | void NCMAG_Calibrate(void) |
295 | { |
Line -... | Line 296... | ||
- | 296 | static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
|
- | 297 | static s16 X = 0, Y = 0, Z = 0; |
|
- | 298 | static u8 OldCalState = 0; |
|
- | 299 | ||
278 | { |
300 | X = (4*X + MagRawVector.X + 3)/5; |
279 | static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
301 | Y = (4*Y + MagRawVector.Y + 3)/5; |
280 | static u8 OldCalState = 0; |
302 | Z = (4*Z + MagRawVector.Z + 3)/5; |
281 | 303 | ||
282 | switch(Compass_CalState) |
304 | switch(Compass_CalState) |
Line 293... | Line 315... | ||
293 | Zmax = -10000; |
315 | Zmax = -10000; |
294 | break; |
316 | break; |
Line 295... | Line 317... | ||
295 | 317 | ||
296 | case 2: // 2nd step of calibration |
318 | case 2: // 2nd step of calibration |
297 | // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane |
319 | // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane |
298 | if(MagRawVector.X < Xmin) Xmin = MagRawVector.X; |
320 | if(X < Xmin) Xmin = X; |
299 | if(MagRawVector.X > Xmax) Xmax = MagRawVector.X; |
321 | else if(X > Xmax) Xmax = X; |
300 | if(MagRawVector.Y < Ymin) Ymin = MagRawVector.Y; |
322 | if(Y < Ymin) Ymin = Y; |
301 | if(MagRawVector.Y > Ymax) Ymax = MagRawVector.Y; |
323 | else if(Y > Ymax) Ymax = Y; |
Line 302... | Line 324... | ||
302 | break; |
324 | break; |
303 | 325 | ||
304 | case 3: // 3rd step of calibration |
326 | case 3: // 3rd step of calibration |
Line 305... | Line 327... | ||
305 | // used to change the orientation of the MK3MAG vertical to the horizontal plane |
327 | // used to change the orientation of the MK3MAG vertical to the horizontal plane |
306 | break; |
328 | break; |
307 | 329 | ||
308 | case 4: |
330 | case 4: |
309 | // find Min and Max of the Z-Sensor |
331 | // find Min and Max of the Z-Sensor |
Line 310... | Line 332... | ||
310 | if(MagRawVector.Z < Zmin) Zmin = MagRawVector.Z; |
332 | if(Z < Zmin) Zmin = Z; |
311 | if(MagRawVector.Z > Zmax) Zmax = MagRawVector.Z; |
333 | else if(Z > Zmax) Zmax = Z; |
312 | break; |
334 | break; |
Line 319... | Line 341... | ||
319 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
341 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
320 | Calibration.MagY.Range = Ymax - Ymin; |
342 | Calibration.MagY.Range = Ymax - Ymin; |
321 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
343 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
322 | Calibration.MagZ.Range = Zmax - Zmin; |
344 | Calibration.MagZ.Range = Zmax - Zmin; |
323 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
345 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
324 | if(1)//if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150)) |
346 | if((Calibration.MagX.Range > 512) && (Calibration.MagY.Range > 512) && (Calibration.MagZ.Range > 512)) |
325 | { |
347 | { |
326 | NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
348 | NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
327 | } |
349 | } |
328 | else |
350 | else |
329 | { |
351 | { |
Line 352... | Line 374... | ||
352 | // rx data handler for magnetic sensor raw data |
374 | // rx data handler for magnetic sensor raw data |
353 | void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
375 | void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
354 | { // if number of bytes are matching |
376 | { // if number of bytes are matching |
355 | if(RxBufferSize == sizeof(MagRawVector) ) |
377 | if(RxBufferSize == sizeof(MagRawVector) ) |
356 | { // byte order from big to little endian |
378 | { // byte order from big to little endian |
- | 379 | s16 raw; |
|
357 | MagRawVector.X = pRxBuffer[0]<<8; |
380 | raw = pRxBuffer[0]<<8; |
358 | MagRawVector.X+= pRxBuffer[1]; |
381 | raw+= pRxBuffer[1]; |
- | 382 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw; |
|
359 | MagRawVector.Y = pRxBuffer[2]<<8; |
383 | raw = pRxBuffer[2]<<8; |
360 | MagRawVector.Y+= pRxBuffer[3]; |
384 | raw+= pRxBuffer[3]; |
- | 385 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.Y = raw; |
|
361 | MagRawVector.Z = pRxBuffer[4]<<8; |
386 | raw = pRxBuffer[4]<<8; |
362 | MagRawVector.Z+= pRxBuffer[5]; |
387 | raw+= pRxBuffer[5]; |
- | 388 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.Z = raw; |
|
363 | } |
389 | } |
364 | if(Compass_CalState || !NCMAG_IsCalibrated) |
390 | if(Compass_CalState || !NCMAG_IsCalibrated) |
365 | { // direct output the raw data |
391 | { // direct output the raw data |
366 | memcpy((u8*)&MagVector,(u8*)&MagRawVector, sizeof(MagVector)); |
392 | memcpy((u8*)&MagVector,(u8*)&MagRawVector, sizeof(MagVector)); |
367 | Compass_Heading = -1; |
393 | Compass_Heading = -1; |
Line 370... | Line 396... | ||
370 | { |
396 | { |
371 | // update MagVector from MagRaw Vector by Scaling |
397 | // update MagVector from MagRaw Vector by Scaling |
372 | MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range); |
398 | MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range); |
373 | MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range); |
399 | MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range); |
374 | MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range); |
400 | MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range); |
- | 401 | ||
- | 402 | if(UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) |
|
- | 403 | { |
|
- | 404 | Compass_Heading = -1; |
|
- | 405 | } |
|
- | 406 | else // fc attitude is avialable |
|
- | 407 | { |
|
375 | // calculate attitude correction |
408 | // calculate attitude correction |
- | 409 | // a float implementation takes too long for an ISR call! |
|
- | 410 | s16 tmp, Hx, Hy; |
|
376 | double Hx, Hy, Cx, Cy, Cz, nick_rad, roll_rad; |
411 | s32 sinnick, cosnick, sinroll, cosroll; |
- | 412 | ||
377 | Cx = (double)MagVector.X; |
413 | tmp = FromFlightCtrl.AngleNick/10; // in deg |
378 | Cy = (double)MagVector.Y; |
414 | sinnick = (s32)c_sin_8192(tmp); |
379 | Cz = (double)MagVector.Z; |
415 | cosnick = (s32)c_cos_8192(tmp); |
380 | nick_rad = ((double)FromFlightCtrl.AngleNick * M_PI) / 1800.0; |
416 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
- | 417 | sinroll = (s32)c_sin_8192(tmp); |
|
- | 418 | cosroll = (s32)c_cos_8192(tmp); |
|
381 | roll_rad = ((double)FromFlightCtrl.AngleRoll * M_PI) / 1800.0; |
419 | // tbd. compensation signs and oriantation has to be fixed |
382 | - | ||
383 | Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
420 | Hx = (s16)((MagVector.X * cosnick - MagVector.Z * sinnick)/8192L); |
384 | Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
421 | Hy = (s16)((MagVector.Y * cosroll + MagVector.Z * sinroll)/8192L); |
385 | 422 | ||
386 | //DebugOut.Analog[23] = (s16)Hx; |
423 | DebugOut.Analog[23] = (s16)Hx; |
387 | //DebugOut.Analog[24] = (s16)Hy; |
424 | DebugOut.Analog[24] = (s16)Hy; |
388 | 425 | ||
389 | // calculate heading |
426 | // calculate heading |
390 | Compass_Heading = (s16)((180.0 * atan2(Hy, Hx)) / M_PI); |
427 | tmp = (s16)(c_tan2_546(Hy, Hx)/546L); |
- | 428 | if (tmp <= 0) tmp = -tmp; |
|
- | 429 | else tmp = 360 - tmp; |
|
- | 430 | Compass_Heading = tmp; |
|
- | 431 | } |
|
391 | } |
432 | } |
392 | } |
433 | } |
393 | // rx data handler for acceleration raw data |
434 | // rx data handler for acceleration raw data |
394 | void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
435 | void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
395 | { // if number of byte are matching |
436 | { // if number of byte are matching |
Line 715... | Line 756... | ||
715 | } |
756 | } |
716 | } |
757 | } |
717 | else |
758 | else |
718 | { |
759 | { |
719 | UART1_PutString("\n\r Not compatible!"); |
760 | UART1_PutString("\n\r Not compatible!"); |
- | 761 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE; |
|
720 | LED_RED_ON; |
762 | LED_RED_ON; |
721 | } |
763 | } |
722 | } |
764 | } |
723 | else // nothing found |
765 | else // nothing found |
724 | { |
766 | { |