Subversion Repositories FlightCtrl

Rev

Rev 1179 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1179 Rev 1199
Line 25... Line 25...
25 
#include "timer0.h"
 25 
#include "timer0.h"
26 
#include "rc.h"
 26 
#include "rc.h"
27 
#include "eeprom.h"
 27 
#include "eeprom.h"
28 
#include "printf_P.h"
 28 
#include "printf_P.h"
Line -... Line 29...
- 
 
 29 
 
- 
 
 30 
 
- 
 
 31 
// for compatibility reasons gcc3.x <-> gcc4.x
- 
 
 32 
#ifndef SPCR
- 
 
 33 
#define SPCR   SPCR0
- 
 
 34 
#endif
- 
 
 35 
#ifndef SPIE
- 
 
 36 
#define SPIE   SPIE0
- 
 
 37 
#endif
- 
 
 38 
#ifndef SPE
- 
 
 39 
#define SPE    SPE0
- 
 
 40 
#endif
- 
 
 41 
#ifndef DORD
- 
 
 42 
#define DORD   DORD0
- 
 
 43 
#endif
- 
 
 44 
#ifndef MSTR
- 
 
 45 
#define MSTR   MSTR0
- 
 
 46 
#endif
- 
 
 47 
#ifndef CPOL
- 
 
 48 
#define CPOL   CPOL0
- 
 
 49 
#endif
- 
 
 50 
#ifndef CPHA
- 
 
 51 
#define CPHA   CPHA0
- 
 
 52 
#endif
- 
 
 53 
#ifndef SPR1
- 
 
 54 
#define SPR1   SPR01
- 
 
 55 
#endif
- 
 
 56 
#ifndef SPR0
- 
 
 57 
#define SPR0   SPR00
- 
 
 58 
#endif
- 
 
 59 
 
- 
 
 60 
#ifndef SPDR
- 
 
 61 
#define SPDR   SPDR0
- 
 
 62 
#endif
- 
 
 63 
 
- 
 
 64 
#ifndef SPSR
- 
 
 65 
#define SPSR   SPSR0
- 
 
 66 
#endif
- 
 
 67 
#ifndef SPIF
- 
 
 68 
#define SPIF   SPIF0
- 
 
 69 
#endif
- 
 
 70 
#ifndef WCOL
- 
 
 71 
#define WCOL   WCOL0
- 
 
 72 
#endif
- 
 
 73 
#ifndef SPI2X
- 
 
 74 
#define SPI2X  SPI2X0
- 
 
 75 
#endif
- 
 
 76 
// -------------------------
- 
 
 77 
 
29 
 
 78 
 
Line 30... Line 79...
30 
#define MAX_AXIS_VALUE          500
 79 
#define MAX_AXIS_VALUE          500
31 
 
 80 
 
Line 84... Line 133...
84 
        #define MM3_SS_PIN     PC4
 133 
        #define MM3_SS_PIN     PC4
85 
        #define MM3_RESET_PORT PORTC
 134 
        #define MM3_RESET_PORT PORTC
86 
        #define MM3_RESET_DDR  DDRC
 135 
        #define MM3_RESET_DDR  DDRC
87 
        #define MM3_RESET_PIN  PC5
 136 
        #define MM3_RESET_PIN  PC5
88 
#endif
 137 
#endif
89 
       
 138 
 
90 
#define MM3_SS_ON      MM3_SS_PORT    &= ~(1<<MM3_SS_PIN);
 139 
#define MM3_SS_ON      MM3_SS_PORT    &= ~(1<<MM3_SS_PIN);
91 
#define MM3_SS_OFF     MM3_SS_PORT    |=  (1<<MM3_SS_PIN);
 140 
#define MM3_SS_OFF     MM3_SS_PORT    |=  (1<<MM3_SS_PIN);
92 
#define MM3_RESET_ON   MM3_RESET_PORT |=  (1<<MM3_RESET_PIN);
 141 
#define MM3_RESET_ON   MM3_RESET_PORT |=  (1<<MM3_RESET_PIN);
93 
#define MM3_RESET_OFF  MM3_RESET_PORT  &= ~(1<<MM3_RESET_PIN);
 142 
#define MM3_RESET_OFF  MM3_RESET_PORT  &= ~(1<<MM3_RESET_PIN);
Line 414... Line 463...
414 
void MM3_Heading(void)
 463 
void MM3_Heading(void)
415 
{
 464 
{
416 
        int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw;
 465 
        int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw;
417 
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
 466 
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
418 
        int16_t angle;
 467 
        int16_t angle;
419 
        uint16_t div_factor;
 - 
 
420 
        int16_t heading;
 468 
        int16_t heading;
Line 421... Line 469...
421 
 
 469 
 
422 
        if (MM3_Timeout)
 470 
        if (MM3_Timeout)
423 
        {
 471 
        {
Line 447... Line 495...
447 
                Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
 495 
                Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
Line 448... Line 496...
448 
 
 496 
 
Line 449... Line -...
449 
 
 - 
 
450 
                // tilt compensation
 - 
 
451 
 
 - 
 
452 
                // calibration factor for transforming Gyro Integrals to angular degrees
 497 
 
453 
                div_factor = (uint16_t)ParamSet.UserParam3 * 8;
 498 
                // tilt compensation
454 
 
 499 
 
455 
                // calculate sinus cosinus of nick and tilt angle
 500 
                // calculate sinus cosinus of nick and tilt angle
Line 456... Line 501...
456 
                angle = (IntegralNick/div_factor);
 501 
                angle = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR);
457 
                sin_nick = (int32_t)(c_sin_8192(angle));
 502 
                sin_nick = (int32_t)(c_sin_8192(angle));
458 
                cos_nick = (int32_t)(c_cos_8192(angle));
 503 
                cos_nick = (int32_t)(c_cos_8192(angle));
Line 459... Line 504...
459 
 
 504 
 
460 
                angle = (IntegralRoll/div_factor);
 505 
                angle = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR);