Rev 289 | Rev 312 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 289 | Rev 292 | ||
|---|---|---|---|
| Line 53... | Line 53... | ||
| 53 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
53 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 54 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
54 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 55 | // + POSSIBILITY OF SUCH DAMAGE. |
55 | // + POSSIBILITY OF SUCH DAMAGE. |
| 56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 57 | #include <math.h> |
57 | #include <math.h> |
| - | 58 | #include <stdio.h> |
|
| 58 | #include <string.h> |
59 | #include <string.h> |
| 59 | #include "91x_lib.h" |
60 | #include "91x_lib.h" |
| 60 | #include "main.h" |
- | |
| 61 | #include "ncmag.h" |
61 | #include "ncmag.h" |
| 62 | #include "i2c.h" |
62 | #include "i2c.h" |
| 63 | #include "timer1.h" |
63 | #include "timer1.h" |
| 64 | #include "led.h" |
64 | #include "led.h" |
| 65 | #include "spi_slave.h" |
- | |
| 66 | #include "uart1.h" |
65 | #include "uart1.h" |
| 67 | #include "eeprom.h" |
66 | #include "eeprom.h" |
| 68 | #include "mymath.h" |
67 | #include "mymath.h" |
| - | 68 | #include "main.h" |
|
| Line 69... | Line 69... | ||
| 69 | 69 | ||
| 70 | u8 NCMAG_Present = 0; |
70 | u8 NCMAG_Present = 0; |
| Line 71... | Line 71... | ||
| 71 | u8 NCMAG_IsCalibrated = 0; |
71 | u8 NCMAG_IsCalibrated = 0; |
| Line 323... | Line 323... | ||
| 323 | 323 | ||
| 324 | case 5: |
324 | case 5: |
| 325 | // Save values |
325 | // Save values |
| 326 | if(Compass_CalState != OldCalState) // avoid continously writing of eeprom! |
326 | if(Compass_CalState != OldCalState) // avoid continously writing of eeprom! |
| 327 | { |
327 | { |
| 328 | #define MIN_CALIBRATION 256 |
328 | #define MIN_CALIBRATION 256 |
| 329 | Calibration.MagX.Range = Xmax - Xmin; |
329 | Calibration.MagX.Range = Xmax - Xmin; |
| 330 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
330 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
| 331 | Calibration.MagY.Range = Ymax - Ymin; |
331 | Calibration.MagY.Range = Ymax - Ymin; |
| 332 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
332 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
| Line 393... | Line 393... | ||
| 393 | { |
393 | { |
| 394 | // update MagVector from MagRaw Vector by Scaling |
394 | // update MagVector from MagRaw Vector by Scaling |
| 395 | MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range); |
395 | MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range); |
| 396 | MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range); |
396 | MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range); |
| 397 | MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range); |
397 | MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range); |
| 398 | - | ||
| 399 | if(UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) |
- | |
| 400 | { |
- | |
| 401 | Compass_Heading = -1; |
398 | Compass_CalcHeading(); |
| 402 | } |
- | |
| 403 | else // fc attitude is avialable |
- | |
| 404 | { |
- | |
| 405 | // calculate attitude correction |
- | |
| 406 | // a float implementation takes too long for an ISR call! |
- | |
| 407 | s16 tmp, Hx, Hy; |
- | |
| 408 | s32 sinnick, cosnick, sinroll, cosroll; |
- | |
| 409 | - | ||
| 410 | tmp = FromFlightCtrl.AngleNick/10; // in deg |
- | |
| 411 | sinnick = (s32)c_sin_8192(tmp); |
- | |
| 412 | cosnick = (s32)c_cos_8192(tmp); |
- | |
| 413 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
- | |
| 414 | sinroll = (s32)c_sin_8192(tmp); |
- | |
| 415 | cosroll = (s32)c_cos_8192(tmp); |
- | |
| 416 | // tbd. compensation signs and oriantation has to be fixed |
- | |
| 417 | Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
- | |
| 418 | Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
- | |
| 419 | // calculate heading |
- | |
| 420 | tmp = (s16)(c_tan2_546(Hy, Hx)/546L); |
- | |
| 421 | if (tmp > 0) tmp = 360 - tmp; |
- | |
| 422 | else tmp = -tmp; |
- | |
| 423 | Compass_Heading = tmp; |
- | |
| 424 | } |
- | |
| 425 | } |
399 | } |
| 426 | } |
400 | } |
| 427 | // rx data handler for acceleration raw data |
401 | // rx data handler for acceleration raw data |
| 428 | void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
402 | void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
| 429 | { // if number of byte are matching |
403 | { // if number of byte are matching |
| Line 586... | Line 560... | ||
| 586 | I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
560 | I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
| 587 | } |
561 | } |
| 588 | } |
562 | } |
| Line 589... | Line 563... | ||
| 589 | 563 | ||
| 590 | // -------------------------------------------------------- |
564 | // -------------------------------------------------------- |
| 591 | void NCMAG_UpdateCompass(void) |
565 | void NCMAG_Update(void) |
| 592 | { |
566 | { |
| Line 593... | Line 567... | ||
| 593 | static u32 TimerCompassUpdate = 0; |
567 | static u32 TimerUpdate = 0; |
| 594 | 568 | ||
| 595 | if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present ) |
569 | if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present ) |
| 596 | { |
570 | { |
| 597 | Compass_Heading = -1; |
571 | Compass_Heading = -1; |
| Line 598... | Line 572... | ||
| 598 | return; |
572 | return; |
| 599 | } |
573 | } |
| 600 | 574 | ||
| 601 | if(CheckDelay(TimerCompassUpdate)) |
575 | if(CheckDelay(TimerUpdate)) |
| 602 | { |
576 | { |
| 603 | // check for new calibration state |
577 | // check for new calibration state |
| 604 | Compass_UpdateCalState(); |
578 | Compass_UpdateCalState(); |
| 605 | if(Compass_CalState) NCMAG_Calibrate(); |
579 | if(Compass_CalState) NCMAG_Calibrate(); |
| 606 | NCMAG_GetMagVector(); //Get new data; |
580 | NCMAG_GetMagVector(); //Get new data; |
| Line 607... | Line 581... | ||
| 607 | TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz |
581 | TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz |
| 608 | } |
582 | } |