Subversion Repositories NaviCtrl

Rev

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

Rev 480 Rev 482
Line 66... Line 66...
66 
 
 66 
 
67 
u8 CompassCalStateQueue[10];
 67 
u8 CompassCalStateQueue[10];
Line 68... Line 68...
68 
fifo_t CompassCalcStateFiFo;
 68 
fifo_t CompassCalcStateFiFo;
- 
 
 69 
 
69 
 
 70 
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
70 
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
 71 
volatile s16vec_t AccVector;   // current acceleration vector of compass, not supported by any HW version
71 
volatile s16 Compass_Heading;  // is written by mk3mag or ncmag implementation
 72 
volatile s16 Compass_Heading;  // is written by mk3mag or ncmag implementation
72 
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
 73 
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
73 
s16 Hx = 0, Hy = 0;
 74 
s16 Hx = 0, Hy = 0;
Line 81... Line 82...
81 
 
 82 
 
82 
#define COMPASS_NONE    0
 83 
#define COMPASS_NONE    0
83 
#define COMPASS_MK3MAG  1
 84 
#define COMPASS_MK3MAG  1
84 
#define COMPASS_NCMAG   2
 85 
#define COMPASS_NCMAG   2
- 
 
 86 
u8 Compass_Device = COMPASS_NONE;
Line 85... Line 87...
85 
u8 Compass_Device = COMPASS_NONE;
 87 
I2C_TypeDef* Compass_I2CPort = I2C1;
86 
 
 88 
 
87 
void Compass_Init(void)
 89 
void Compass_Init(void)
88 
{
 90 
{
89 
        switch(Compass_Device)
 91 
        switch(Compass_Device)
90 
        {
 92 
        {
91 
                case COMPASS_NONE:
 93 
                case COMPASS_NONE:
92 
                        UART1_PutString("\r\n Looking for compass");
 94 
                        UART1_PutString("\r\n Looking for compass");
93 
                        if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
 - 
 
94 
                        else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
 - 
 
95 
 
 - 
 
96 
/*                      if(Version_HW > 11)
 - 
 
97 
                        {
 - 
 
98 
                                if(      NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
 - 
 
99 
                                else if( MK3MAG_Init()) Compass_Device = COMPASS_MK3MAG;
 - 
 
100 
                        }
 - 
 
101 
                        else
 - 
 
102 
                        {
 - 
 
103 
                                if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
 - 
 
104 
                                else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
 - 
 
105 
                        }
 95 
                        if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
Line 106... Line 96...
106 
*/
 96 
                        else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
107 
                        break;
 - 
 
108 
 
 97 
                        break;
109 
                case COMPASS_NCMAG:
 98 
 
110 
                        UART1_PutString("\r\n Re-Init compass");
 99 
                case COMPASS_NCMAG:
Line 111... Line 100...
111 
                        if( NCMAG_Init() )      Compass_Device = COMPASS_NCMAG;
 100 
                        if( NCMAG_Init() )      Compass_Device = COMPASS_NCMAG;
- 
 
 101 
                        else                            Compass_Device = COMPASS_NONE;
- 
 
 102 
                        break;
- 
 
 103 
 
- 
 
 104 
                case COMPASS_MK3MAG:
112 
                        else                            Compass_Device = COMPASS_NONE;
 105 
                        if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
113 
                        break;
 106 
                        else                            Compass_Device = COMPASS_NONE;
114 
 
 107 
                        break;
Line 115... Line 108...
115 
                case COMPASS_MK3MAG:
 108 
 
116 
                default:
 109 
                default:
Line 117... Line 110...
117 
                        // nothing to do
 110 
                        // nothing to do
Line -... Line 111...
- 
 
 111 
                        break;
- 
 
 112 
 
- 
 
 113 
        }
- 
 
 114 
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
118 
                        break;
 115 
 
Line -... Line 116...
- 
 
 116 
        if(Compass_Device == COMPASS_MK3MAG) UART_VersionInfo.Flags |= NC_VERSION_FLAG_MK3MAG_PRESENT; else UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_MK3MAG_PRESENT;
- 
 
 117 
 
- 
 
 118 
        MagVector.X = 0;
- 
 
 119 
        MagVector.Y = 0;
- 
 
 120 
        MagVector.Z = 0;
- 
 
 121 
        Compass_Heading = -1;
- 
 
 122 
}
- 
 
 123 
 
- 
 
 124 
 
- 
 
 125 
void Compass_Check(void)
- 
 
 126 
{
- 
 
 127 
        switch(Compass_Device)
- 
 
 128 
        {
- 
 
 129 
                case COMPASS_NCMAG:
- 
 
 130 
                        NCMAG_CheckOrientation();
- 
 
 131 
                        break;
119 
 
 132 
 
120 
        }
 133 
                case COMPASS_MK3MAG:
121 
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
 134 
                case COMPASS_NONE:
122 
 
 135 
                default:
123 
        if(Compass_Device == COMPASS_MK3MAG) UART_VersionInfo.Flags |= NC_VERSION_FLAG_MK3MAG_PRESENT; else UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_MK3MAG_PRESENT;
 136 
                        // nothing to do
Line 165... Line 178...
165 
        // initiate new compass communication
 178 
        // initiate new compass communication
166 
        switch(Compass_Device)
 179 
        switch(Compass_Device)
167 
        {
 180 
        {
168 
                case COMPASS_MK3MAG:
 181 
                case COMPASS_MK3MAG:
169 
                        MK3MAG_Update();
 182 
                        MK3MAG_Update();
170 
                        DebugOut.Analog[24] = MagVector.X;
 - 
 
171 
                        DebugOut.Analog[25] = MagVector.Y;
 - 
 
172 
                        DebugOut.Analog[26] = MagVector.Z;
 - 
 
173 
                        break;
 183 
                        break;
174 
                case COMPASS_NCMAG:
 184 
                case COMPASS_NCMAG:
175 
                        if(check_value_counter == 2000)
 - 
 
176 
                         {
 - 
 
177 
                          UART1_PutString("\n\r Init Mag.-Sensor");
 - 
 
178 
                          InitNC_MagnetSensor(); // 2 seconds no change of the compass value
 - 
 
179 
                         }
 - 
 
180 
                        else NCMAG_Update(0);
 185 
                        NCMAG_Update(0);
181 
                        DebugOut.Analog[24] = MagRawVector.X;
 - 
 
182 
                        DebugOut.Analog[25] = MagRawVector.Y;
 - 
 
183 
                        DebugOut.Analog[26] = MagRawVector.Z;
 186 
                        break;
184 
                default:
 187 
                default:
185 
                        break;
 188 
                        break;
186 
        }
 189 
        }
187 
    if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normally changing
 - 
 
Line -... Line 190...
- 
 
 190 
 
- 
 
 191 
        DebugOut.Analog[24] = MagVector.X;
- 
 
 192 
        DebugOut.Analog[25] = MagVector.Y;
- 
 
 193 
        DebugOut.Analog[26] = MagVector.Z;
- 
 
 194 
 
- 
 
 195 
    if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normaly changing
188 
 
 196 
 
- 
 
 197 
        if(check_value_counter == 2000)
- 
 
 198 
        {
- 
 
 199 
                UART1_PutString("\n\r Init Mag.-Sensor");
- 
 
 200 
                Compass_Init(); // 2 seconds no change of the compass value
- 
 
 201 
        }
189 
        if(check_value_counter > 5000)
 202 
        else if(check_value_counter > 5000)
190 
         {
 203 
        {
191 
          Compass_Heading = -1; // values didn't change for 5 seconds -> probably a compass-fault
 204 
                Compass_Heading = -1; // values didn't change for 5 seconds -> probably a compass-fault
192 
         }
 205 
        }
Line 193... Line 206...
193 
         else check_value_counter++;
 206 
        else check_value_counter++;
194 
 
 207 
 
195 
        old.X = MagVector.X;
 208 
        old.X = MagVector.X;
196 
        old.Y = MagVector.Y;
 209 
        old.Y = MagVector.Y;