Subversion Repositories NaviCtrl

Rev

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

Rev 171 Rev 180
Line 287... Line 287...
287 
}
 287 
}
Line 288... Line 288...
288 
 
 288 
 
289 
//------------------------------------------------------
 289 
//------------------------------------------------------
290 
void SPI0_UpdateBuffer(void)
 290 
void SPI0_UpdateBuffer(void)
- 
 
 291 
{
- 
 
 292 
        static u32 timeout = 0;
- 
 
 293 
        static u8 counter = 50;
291 
{
 294 
         
292 
        if (SPI_RxBuffer_Request)
 295 
        if (SPI_RxBuffer_Request)
293 
        {
 296 
        {
294 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
 297 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
Line 413... Line 416...
413 
                }
 416 
                }
Line 414... Line 417...
414 
 
 417 
 
415 
                // every time we got new data from the FC via SPI call the navigation routine
 418 
                // every time we got new data from the FC via SPI call the navigation routine
416 
                GPS_Navigation();
 419 
                GPS_Navigation();
- 
 
 420 
                ClearMKFlags = 1;
- 
 
 421 
               
- 
 
 422 
                if(counter)
- 
 
 423 
                {
- 
 
 424 
                        counter--;                                       // count down to enable servo
- 
 
 425 
                        if(!counter) TIMER2_Init();  // enable Servo Output    
Line 417... Line 426...
417 
                ClearMKFlags = 1;
 426 
                }
- 
 
 427 
 
Line 418... Line 428...
418 
 
 428 
                SPI_RxBuffer_Request = 0;
419 
                SPI_RxBuffer_Request = 0;
 429 
                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
420 
 
 430 
 
421 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
 431 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
422 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
 432 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
423 
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
 433 
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
424 
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
 434 
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
425 
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
 435 
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
426 
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
 436 
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
- 
 
 437 
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
- 
 
 438 
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
- 
 
 439 
        }       // EOF if(SPI_RxBuffer_Request)
- 
 
 440 
        else // no new SPI data
- 
 
 441 
        {
- 
 
 442 
                if(CheckDelay(timeout) && (counter == 0))
- 
 
 443 
                {
- 
 
 444 
                        TIMER2_Deinit();  // disable Servo Output
427 
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
 445 
                        counter = 50;     // reset counter for enabling Servo Output
Line 428... Line 446...
428 
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
 446 
                }
429 
        }       // EOF if(SPI_RxBuffer_Request)
 447 
        }
430 
}
 448 
}
Line 452... Line 470...
452 
                        SPI0_UpdateBuffer();
 470 
                        SPI0_UpdateBuffer();
453 
                        if (FC_Version.Major != 0xFF)  break;
 471 
                        if (FC_Version.Major != 0xFF)  break;
454 
                }while (!CheckDelay(timeout));
 472 
                }while (!CheckDelay(timeout));
455 
                UART1_PutString(".");
 473 
                UART1_PutString(".");
456 
                repeat++;
 474 
                repeat++;
457 
        }while((FC_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms = 3s
 475 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
458 
        // if we got it
 476 
        // if we got it
459 
        if (FC_Version.Major != 0xFF)
 477 
        if (FC_Version.Major != 0xFF)
460 
        {
 478 
        {
461 
                sprintf(msg, "\n\r FlightCtrl V%d.%d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
 479 
                sprintf(msg, "\n\r FlightCtrl V%d.%d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
462 
                UART1_PutString(msg);
 480 
                UART1_PutString(msg);