Subversion Repositories NaviCtrl

Rev

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

Rev 331 Rev 338
Line 136... Line 136...
136 
{
 136 
{
137 
    static s32 no_error_delay = 0;
 137 
    static s32 no_error_delay = 0;
138 
        s32 newErrorCode = 0;
 138 
        s32 newErrorCode = 0;
139 
        UART_VersionInfo.HardwareError[0] = 0;
 139 
        UART_VersionInfo.HardwareError[0] = 0;
Line 140... Line 140...
140 
 
 140 
 
141 
        if(CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08;
 141 
        if(CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.StatusRed |= AMPEL_COMPASS;
Line 142... Line 142...
142 
        else DebugOut.Status[1] &= ~0x08; // MK3Mag green status
 142 
        else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status
143 
 
 143 
 
- 
 
 144 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed |= AMPEL_BL;
- 
 
 145 
        else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status
- 
 
 146 
 
Line 144... Line 147...
144 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] |= 0x02;
 147 
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed |= AMPEL_NC;
145 
        else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status
 148 
        else DebugOut.StatusRed &= ~AMPEL_NC;
146 
 
 149 
 
147 
        if(CheckDelay(SPI0_Timeout))
 150 
        if(CheckDelay(SPI0_Timeout))
148 
        {
 151 
        {
149 
                LED_RED_ON;
 152 
                LED_RED_ON;
150 
                sprintf(ErrorMSG,"no FC communication ");
 153 
                sprintf(ErrorMSG,"no FC communication ");
151 
                newErrorCode = 3;
 154 
                newErrorCode = 3;
152 
                StopNavigation = 1;
 155 
                StopNavigation = 1;
153 
                DebugOut.Status[0] &= ~0x01; // status of FC Present
 156 
                DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present
154 
                DebugOut.Status[0] &= ~0x02; // status of BL Present
 157 
                DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present
155 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;
 158 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;
156 
        }
 159 
        }
Line 162... Line 165...
162 
                I2C1_Deinit();
 165 
                I2C1_Deinit();
163 
                I2C1_Init();
 166 
                I2C1_Init();
164 
                newErrorCode = 4;
 167 
                newErrorCode = 4;
165 
                StopNavigation = 1;
 168 
                StopNavigation = 1;
166 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
 169 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
167 
                DebugOut.Status[1] |= 0x08;
 170 
                DebugOut.StatusRed |= AMPEL_COMPASS;
168 
        }
 171 
        }
169 
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
 172 
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
170 
        {
 173 
        {
171 
                LED_RED_ON;
 174 
                LED_RED_ON;
172 
#ifndef FOLLOW_ME
 175 
#ifndef FOLLOW_ME
Line 175... Line 178...
175 
                sprintf(ErrorMSG,"! FollowMe only ! ");
 178 
                sprintf(ErrorMSG,"! FollowMe only ! ");
176 
#endif
 179 
#endif
177 
                newErrorCode = 1;
 180 
                newErrorCode = 1;
178 
                StopNavigation = 1;
 181 
                StopNavigation = 1;
179 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE;
 182 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE;
- 
 
 183 
                DebugOut.StatusRed |= AMPEL_NC;
180 
        }
 184 
        }
Line 181... Line 185...
181 
 
 185 
 
182 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
 186 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
183 
        {
 187 
        {
Line 288... Line 292...
288 
        else if(ErrorDisturbedEarthMagnetField)
 292 
        else if(ErrorDisturbedEarthMagnetField)
289 
        {
 293 
        {
290 
         LED_RED_ON;
 294 
         LED_RED_ON;
291 
         sprintf(ErrorMSG,"Magnet error    ");
 295 
         sprintf(ErrorMSG,"Magnet error    ");
292 
         newErrorCode = 22;
 296 
         newErrorCode = 22;
- 
 
 297 
         DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC;
- 
 
 298 
         UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
- 
 
 299 
        }
- 
 
 300 
        else if(BL_MinOfMaxPWM == 40 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
- 
 
 301 
        {
- 
 
 302 
         LED_RED_ON;
- 
 
 303 
         sprintf(ErrorMSG,"ERR:Motor restart  ");
- 
 
 304 
         newErrorCode = 23;
293 
         DebugOut.Status[1] |= 0x08;
 305 
         DebugOut.StatusRed |= AMPEL_BL;
- 
 
 306 
        }
- 
 
 307 
        else if(BL_MinOfMaxPWM != 255 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
- 
 
 308 
        {
- 
 
 309 
         LED_RED_ON;
- 
 
 310 
         sprintf(ErrorMSG,"ERR:BL Limitation   ");
- 
 
 311 
         newErrorCode = 24;
- 
 
 312 
         DebugOut.StatusRed |= AMPEL_BL;
294 
        }
 313 
        }
295 
        else // no error occured
 314 
        else // no error occured
296 
        {
 315 
        {
297 
                StopNavigation = 0;
 316 
                StopNavigation = 0;
298 
                LED_RED_OFF;
 317 
                LED_RED_OFF;
Line 301... Line 320...
301 
                 {
 320 
                 {
302 
                  sprintf(ErrorMSG,"No Error               ");
 321 
                  sprintf(ErrorMSG,"No Error               ");
303 
                  ErrorCode = 0;
 322 
                  ErrorCode = 0;
304 
                 }
 323 
                 }
305 
        }
 324 
        }
306 
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x04;
 - 
 
307 
        else DebugOut.Status[1] &= ~0x04;
 - 
 
Line 308... Line 325...
308 
 
 325 
 
309 
    if(newErrorCode)
 326 
    if(newErrorCode)
310 
         {
 327 
         {
311 
          if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 5; // delay the errors if the motors are running
 328 
          if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 5; // delay the errors if the motors are running
Line 385... Line 402...
385 
        TimerCheckError = SetDelay(3000);
 402 
        TimerCheckError = SetDelay(3000);
386 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 403 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
387 
        UART1_PutString("\n\r Version information:");
 404 
        UART1_PutString("\n\r Version information:");
Line 388... Line 405...
388 
 
 405 
 
389 
        GetNaviCtrlVersion();
 406 
        GetNaviCtrlVersion();
390 
        DebugOut.Status[0] = 0x04 | 0x08; // NC and MK3Mag
 407 
        DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag
Line 391... Line 408...
391 
        DebugOut.Status[1] = 0x00;
 408 
        DebugOut.StatusRed = 0x00;
Line 392... Line 409...
392 
 
 409 
 
393 
        Compass_Init();
 410 
        Compass_Init();