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