Subversion Repositories NaviCtrl

Rev

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

Rev 229 Rev 231
Line 123... Line 123...
123 
        sprintf(msg,"\n\r NaviCtrl V%d.%d%c",  VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
 123 
        sprintf(msg,"\n\r NaviCtrl V%d.%d%c",  VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
124 
        UART1_PutString(msg);
 124 
        UART1_PutString(msg);
125 
}
 125 
}
Line 126... Line 126...
126 
 
 126 
 
- 
 
 127 
//----------------------------------------------------------------------------------------------------
- 
 
 128 
#define DEFEKT_G_NICK           0x01
- 
 
 129 
#define DEFEKT_G_ROLL           0x02
- 
 
 130 
#define DEFEKT_G_GIER           0x04
- 
 
 131 
#define DEFEKT_A_NICK           0x08
- 
 
 132 
#define DEFEKT_A_ROLL           0x10
- 
 
 133 
#define DEFEKT_A_Z              0x20
- 
 
 134 
#define DEFEKT_PRESSURE         0x40
- 
 
 135 
#define DEFEKT_CAREFREE_ERR 0x80
- 
 
 136 
 
- 
 
 137 
#define DEFEKT_I2C              0x01
- 
 
 138 
#define DEFEKT_BL_MISSING       0x02
- 
 
 139 
#define DEFEKT_SPI_RX_ERR       0x04
- 
 
 140 
#define DEFEKT_PPM_ERR          0x08
- 
 
 141 
#define DEFEKT_MIXER_ERR    0x10
- 
 
 142 
//----------------------------------------------------------------------------------------------------
- 
 
 143 
#define ERROR_FC_COMMUNICATION          0x01
- 
 
 144 
#define ERROR_MK3_COMMUNICATION         0x02
- 
 
 145 
#define ERROR_FC_INCOMPATIBLE           0x04
- 
 
 146 
#define ERROR_MK3_INCOMPATIBLE          0x08
- 
 
 147 
#define ERROR_GPS_COMMUNICATION         0x10
- 
 
 148 
#define ERROR_COMPASS_VALUE                     0x20
- 
 
 149 
//----------------------------------------------------------------------------------------------------
127 
//----------------------------------------------------------------------------------------------------
 150 
 
128 
void CheckErrors(void)
 151 
void CheckErrors(void)
- 
 
 152 
{
- 
 
 153 
 UART_VersionInfo.HardwareError[0] = 0;
- 
 
 154 
 
- 
 
 155 
 
- 
 
 156 
  if((MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) || (CheckDelay(I2C1_Timeout)) || (MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) || (I2C_Heading.Heading < 0)) DebugOut.Status[1] |= 0x08;
- 
 
 157 
  else DebugOut.Status[1] &= ~0x08; // MK3Mag green status
- 
 
 158 
 
- 
 
 159 
  if((FC_ErrorCode[1] & DEFEKT_I2C) || (FC_ErrorCode[1] & DEFEKT_BL_MISSING)) DebugOut.Status[1] |= 0x02;
- 
 
 160 
  else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status
129 
{
 161 
 
130 
        if(CheckDelay(SPI0_Timeout))
 162 
        if(CheckDelay(SPI0_Timeout))
131 
        {
 163 
        {
132 
                LED_RED_ON;
 164 
                LED_RED_ON;
133 
                sprintf(ErrorMSG,"no FC communication ");
 165 
                sprintf(ErrorMSG,"no FC communication ");
134 
                ErrorCode = 3;
 166 
                ErrorCode = 3;
- 
 
 167 
                StopNavigation = 1;
- 
 
 168 
                DebugOut.Status[0] &= ~0x01; // status of FC Present
- 
 
 169 
                DebugOut.Status[0] &= ~0x02; // status of BL Present
135 
                StopNavigation = 1;
 170 
                UART_VersionInfo.HardwareError[0] |= ERROR_FC_COMMUNICATION;
136 
        }
 171 
        }
137 
        else if(CheckDelay(I2C1_Timeout))
 172 
        else if(CheckDelay(I2C1_Timeout))
138 
        {
 173 
        {
139 
                LED_RED_ON;
 174 
                LED_RED_ON;
140 
                sprintf(ErrorMSG,"no MK3Mag communication ");
 175 
                sprintf(ErrorMSG,"no MK3Mag communication ");
141 
                //Reset I2CBus
 176 
                //Reset I2CBus
142 
                I2C1_Deinit();
 177 
                I2C1_Deinit();
143 
                I2C1_Init();
 178 
                I2C1_Init();
144 
                ErrorCode = 4;
 179 
                ErrorCode = 4;
- 
 
 180 
                StopNavigation = 1;
- 
 
 181 
                UART_VersionInfo.HardwareError[0] |= ERROR_MK3_COMMUNICATION;
145 
                StopNavigation = 1;
 182 
        DebugOut.Status[1] |= 0x08;
146 
        }
 183 
        }
147 
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
 184 
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
148 
        {
 185 
        {
149 
                LED_RED_ON;
 186 
                LED_RED_ON;
150 
                sprintf(ErrorMSG,"FC not compatible ");
 187 
                sprintf(ErrorMSG,"FC not compatible ");
151 
                ErrorCode = 1;
 188 
                ErrorCode = 1;
- 
 
 189 
                StopNavigation = 1;
152 
                StopNavigation = 1;
 190 
                UART_VersionInfo.HardwareError[0] |= ERROR_FC_INCOMPATIBLE;
- 
 
 191 
        }
- 
 
 192 
 
- 
 
 193 
        else if(FC_ErrorCode[0] & DEFEKT_G_NICK)
- 
 
 194 
        {
- 
 
 195 
                LED_RED_ON;
- 
 
 196 
                sprintf(ErrorMSG,"ERR: FC Nick Gyro");
- 
 
 197 
                ErrorCode = 10;
- 
 
 198 
    }
- 
 
 199 
        else if(FC_ErrorCode[0] & DEFEKT_G_ROLL)
- 
 
 200 
        {
- 
 
 201 
                LED_RED_ON;
- 
 
 202 
                sprintf(ErrorMSG,"ERR: FC Roll Gyro");
- 
 
 203 
                ErrorCode = 11;
- 
 
 204 
    }
- 
 
 205 
        else if(FC_ErrorCode[0] & DEFEKT_G_GIER)
- 
 
 206 
        {
- 
 
 207 
                LED_RED_ON;
- 
 
 208 
                sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
- 
 
 209 
                ErrorCode = 12;
- 
 
 210 
    }
- 
 
 211 
        else if(FC_ErrorCode[0] & DEFEKT_A_NICK)
- 
 
 212 
        {
- 
 
 213 
                LED_RED_ON;
- 
 
 214 
                sprintf(ErrorMSG,"ERR: FC Nick ACC");
- 
 
 215 
                ErrorCode = 13;
- 
 
 216 
    }
- 
 
 217 
        else if(FC_ErrorCode[0] & DEFEKT_A_ROLL)
- 
 
 218 
        {
- 
 
 219 
                LED_RED_ON;
- 
 
 220 
                sprintf(ErrorMSG,"ERR: FC Roll ACC");
- 
 
 221 
                ErrorCode = 14;
- 
 
 222 
    }
- 
 
 223 
        else if(FC_ErrorCode[0] & DEFEKT_A_Z)
- 
 
 224 
        {
- 
 
 225 
                LED_RED_ON;
- 
 
 226 
                sprintf(ErrorMSG,"ERR: FC Z-ACC");
- 
 
 227 
                ErrorCode = 15;
- 
 
 228 
    }
- 
 
 229 
        else if(FC_ErrorCode[0] & DEFEKT_PRESSURE)
- 
 
 230 
        {
- 
 
 231 
                LED_RED_ON;
- 
 
 232 
                sprintf(ErrorMSG,"ERR: Pressure sensor");
- 
 
 233 
                ErrorCode = 16;
- 
 
 234 
    }
- 
 
 235 
        else if(FC_ErrorCode[1] & DEFEKT_I2C)
- 
 
 236 
        {
- 
 
 237 
                LED_RED_ON;
- 
 
 238 
                sprintf(ErrorMSG,"ERR: FC I2C");
- 
 
 239 
                ErrorCode = 17;
- 
 
 240 
    }
- 
 
 241 
        else if(FC_ErrorCode[1] & DEFEKT_BL_MISSING)
- 
 
 242 
        {
- 
 
 243 
                LED_RED_ON;
- 
 
 244 
                sprintf(ErrorMSG,"ERR: Bl Missing");
- 
 
 245 
                ErrorCode = 18;
- 
 
 246 
    }
- 
 
 247 
        else if(FC_ErrorCode[1] & DEFEKT_MIXER_ERR)
- 
 
 248 
        {
- 
 
 249 
                LED_RED_ON;
- 
 
 250 
                sprintf(ErrorMSG,"Mixer Error");
- 
 
 251 
                ErrorCode = 19;
153 
        }
 252 
    }
154 
        else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
 253 
        else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
155 
        {
 254 
        {
156 
                sprintf(ErrorMSG,"MK3Mag not compatible ");
 255 
                sprintf(ErrorMSG,"MK3Mag not compatible ");
157 
                LED_RED_ON;
 256 
                LED_RED_ON;
158 
                ErrorCode = 2;
 257 
                ErrorCode = 2;
- 
 
 258 
                StopNavigation = 1;
159 
                StopNavigation = 1;
 259 
                UART_VersionInfo.HardwareError[0] |= ERROR_MK3_INCOMPATIBLE;
160 
        }
 260 
        }
161 
        else if(CheckDelay(UBX_Timeout))
 261 
        else if(CheckDelay(UBX_Timeout))
162 
        {
 262 
        {
163 
                LED_RED_ON;
 263 
                LED_RED_ON;
164 
                sprintf(ErrorMSG,"no GPS communication ");
 264 
                sprintf(ErrorMSG,"no GPS communication ");
165 
                ErrorCode = 5;
 265 
                ErrorCode = 5;
- 
 
 266 
                StopNavigation = 1;
166 
                StopNavigation = 1;
 267 
                UART_VersionInfo.HardwareError[0] |= ERROR_GPS_COMMUNICATION;
167 
        }
 268 
        }
168 
        else if(I2C_Heading.Heading < 0)
 269 
        else if(I2C_Heading.Heading < 0)
169 
        {
 270 
        {
170 
                LED_RED_ON;
 271 
                LED_RED_ON;
171 
                sprintf(ErrorMSG,"bad compass value ");
 272 
                sprintf(ErrorMSG,"bad compass value ");
172 
                ErrorCode = 6;
 273 
                ErrorCode = 6;
- 
 
 274 
                StopNavigation = 1;
173 
                StopNavigation = 1;
 275 
                UART_VersionInfo.HardwareError[0] |= ERROR_COMPASS_VALUE;
174 
        }
 276 
        }
175 
        else if(FC.Flags & FCFLAG_SPI_RX_ERR)
 277 
        else if(FC.Flags & FCFLAG_SPI_RX_ERR)
176 
        {
 278 
        {
177 
                LED_RED_ON;
 279 
                LED_RED_ON;
178 
                sprintf(ErrorMSG,"FC spi rx error ");
 280 
                sprintf(ErrorMSG,"FC spi rx error ");
179 
                ErrorCode = 8;
 281 
                ErrorCode = 8;
180 
                StopNavigation = 1;
 282 
                StopNavigation = 1;
- 
 
 283 
        }
- 
 
 284 
        else if(FC_ErrorCode[0] & DEFEKT_CAREFREE_ERR)
- 
 
 285 
        {
- 
 
 286 
                LED_RED_ON;
- 
 
 287 
                sprintf(ErrorMSG,"FC: Carefree Error");
- 
 
 288 
                ErrorCode = 20;
181 
        }
 289 
    }
182 
        else if(FC.RC_Quality < 100)
 290 
        else if(FC.RC_Quality < 100)
183 
        {
 291 
        {
184 
                LED_RED_ON;
 292 
                LED_RED_ON;
185 
                sprintf(ErrorMSG,"RC Signal lost ");
 293 
                sprintf(ErrorMSG,"RC Signal lost ");
Line 190... Line 298...
190 
                sprintf(ErrorMSG,"No Error               ");
 298 
                sprintf(ErrorMSG,"No Error               ");
191 
                ErrorCode = 0;
 299 
                ErrorCode = 0;
192 
                StopNavigation = 0;
 300 
                StopNavigation = 0;
193 
                LED_RED_OFF;
 301 
                LED_RED_OFF;
194 
        }
 302 
        }
- 
 
 303 
 if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1] || UART_VersionInfo.HardwareError[0]) DebugOut.Status[1] |= 0x04;
- 
 
 304 
 else DebugOut.Status[1] &= ~0x04;
195 
}
 305 
}
Line 196... Line 306...
196 
 
 306 
 
197 
// the handler will be cyclic called by the timer 1 ISR
 307 
// the handler will be cyclic called by the timer 1 ISR
198 
// used is for critical timing parts that normaly would handled
 308 
// used is for critical timing parts that normaly would handled
Line 255... Line 365...
255 
        TimerCheckError = SetDelay(3000);
 365 
        TimerCheckError = SetDelay(3000);
256 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 366 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
257 
        UART1_PutString("\n\r Version information:");
 367 
        UART1_PutString("\n\r Version information:");
Line 258... Line 368...
258 
 
 368 
 
- 
 
 369 
        GetNaviCtrlVersion();
- 
 
 370 
        DebugOut.Status[0] = 0x04 | 0x08; // NC and MK3Mag
Line 259... Line 371...
259 
        GetNaviCtrlVersion();
 371 
        DebugOut.Status[1] = 0x00;
260 
 
 372 
 
261 
        I2C1_GetMK3MagVersion();
 373 
        I2C1_GetMK3MagVersion();
262 
        if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
 374 
        if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)