Subversion Repositories NaviCtrl

Rev

Rev 488 | Rev 492 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

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