Subversion Repositories NaviCtrl

Rev

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

Rev 268 Rev 292
Line 56... Line 56...
56 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 56 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57 
#include "91x_lib.h"
 57 
#include "91x_lib.h"
58 
#include "compass.h"
 58 
#include "compass.h"
59 
#include "mk3mag.h"
 59 
#include "mk3mag.h"
60 
#include "ncmag.h"
 60 
#include "ncmag.h"
- 
 
 61 
#include "spi_slave.h"
- 
 
 62 
#include "mymath.h"
61 
#include "uart1.h"
 63 
#include "uart1.h"
62 
#include "fifo.h"
 64 
#include "fifo.h"
63 
#include "led.h"
 65 
#include "led.h"
- 
 
 66 
#include "main.h"
Line 64... Line 67...
64 
 
 67 
 
65 
 
 68 
 
Line 106... Line 109...
106 
        }
 109 
        }
107 
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
 110 
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
108 
}
 111 
}
Line 109... Line 112...
109 
 
 112 
 
110 
 
 113 
 
111 
void Compass_UpdateHeading(void)
 114 
void Compass_CalcHeading(void)
112 
{
 115 
{
113 
        switch(Compass_Device)
 116 
        if((UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) || Compass_CalState)
- 
 
 117 
        {
- 
 
 118 
                Compass_Heading = -1;
- 
 
 119 
        }
114 
        {
 120 
        else // fc attitude is avialable and no compass calibration active
- 
 
 121 
        {
115 
                case COMPASS_MK3MAG:
 122 
                // calculate attitude correction
- 
 
 123 
                // a float implementation takes too long
- 
 
 124 
                s16 tmp, Hx, Hy;
- 
 
 125 
                s32 sinnick, cosnick, sinroll, cosroll;
- 
 
 126 
               
116 
                        MK3MAG_UpdateCompass();
 127 
                tmp = FromFlightCtrl.AngleNick/10; // in deg
- 
 
 128 
                sinnick = (s32)c_sin_8192(tmp);
- 
 
 129 
                cosnick = (s32)c_cos_8192(tmp);
117 
                        break;
 130 
                tmp = FromFlightCtrl.AngleRoll/10; // in deg
- 
 
 131 
                sinroll = (s32)c_sin_8192(tmp);
- 
 
 132 
                cosroll = (s32)c_cos_8192(tmp);
- 
 
 133 
                // tbd. compensation signs and oriantation has to be fixed
118 
                case COMPASS_NCMAG:
 134 
                Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);
- 
 
 135 
                Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);             
- 
 
 136 
                // calculate heading
119 
                        NCMAG_UpdateCompass();
 137 
                tmp = (s16)(c_tan2_546(Hy, Hx)/546L);
120 
                        break;
 138 
                if (tmp > 0) tmp = 360 - tmp;
121 
                default:
 139 
                else tmp = -tmp;
122 
                        break;
 140 
                Compass_Heading = tmp;
Line 123... Line 141...
123 
        }
 141 
        }
124 
}
 142 
}
- 
 
 143 
 
- 
 
 144 
void Compass_Update(void)
- 
 
 145 
{
125 
 
 146 
        // check for new cal state
126 
void Compass_UpdateMagVector(void)
 147 
        Compass_UpdateCalState();
127 
{
 148 
        // initiate new compass communication
128 
        switch(Compass_Device)
 149 
        switch(Compass_Device)
129 
        {
 150 
        {
130 
                case COMPASS_MK3MAG:
 151 
                case COMPASS_MK3MAG:
131 
                        MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT);
 152 
                        MK3MAG_Update();
132 
                        break;
 153 
                        break;
133 
                case COMPASS_NCMAG:
 154 
                case COMPASS_NCMAG:
134 
                        NCMAG_UpdateCompass();
 155 
                        NCMAG_Update();
135 
                default:
 156 
                default: