Subversion Repositories NaviCtrl

Rev

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

Rev 328 Rev 329
Line 96... Line 96...
96 
s32 Kalman_K = 32;
 96 
s32 Kalman_K = 32;
97 
s32 Kalman_MaxDrift = 5 * 16;
 97 
s32 Kalman_MaxDrift = 5 * 16;
98 
s32 Kalman_MaxFusion = 64;
 98 
s32 Kalman_MaxFusion = 64;
99 
s32 ToFcGpsZ = 0;
 99 
s32 ToFcGpsZ = 0;
Line 100... Line 100...
100 
 
 100 
 
101 
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN};
 101 
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN};
102 
u8 SPI_CommandCounter = 0;
 102 
u8 SPI_CommandCounter = 0;
103 
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
 103 
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
104 
s32 HeadFreeStartAngle = 0;
 104 
s32 HeadFreeStartAngle = 0;
105 
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
 105 
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
106 
u32 ToFC_AltitudeRate = 0;
 106 
u32 ToFC_AltitudeRate = 0;
107 
s32 ToFC_AltitudeSetpoint = 0;
 107 
s32 ToFC_AltitudeSetpoint = 0;
108 
u8  FromFC_VarioCharacter = ' ';
 108 
u8  FromFC_VarioCharacter = ' ';
109 
u8 DisableFC_Sticks = 0;
 109 
u8 DisableFC_Sticks = 0;
- 
 
 110 
u8 NC_GPS_ModeCharacter = ' ';
Line 110... Line 111...
110 
u8 NC_GPS_ModeCharacter = ' ';
 111 
u8 FCCalibActive = 0;
Line 111... Line 112...
111 
 
 112 
 
112 
SPI_Version_t FC_Version;
 113 
SPI_Version_t FC_Version;
Line 272... Line 273...
272 
 
 273 
 
273 
//------------------------------------------------------
 274 
//------------------------------------------------------
274 
void SPI0_UpdateBuffer(void)
 275 
void SPI0_UpdateBuffer(void)
275 
{
 276 
{
276 
        static u32 timeout = 0;
 277 
        static u32 timeout = 0;
277 
        static u8 counter = 50;
 278 
        static u8 counter = 50,hott_index = 0;
278 
        static u8 CompassCalState = 0;
 - 
 
279 
        static u8 FCCalibActive = 0;
 279 
        static u8 CompassCalState = 0;
- 
 
 280 
        s16 tmp;
Line 280... Line 281...
280 
        s16 tmp;
 281 
        s32 i1,i2;
281 
 
 282 
 
282 
        if (SPI_RxBuffer_Request)
 283 
        if (SPI_RxBuffer_Request)
283 
        {
 284 
        {
Line 295... Line 296...
295 
                // restart command cycle at the end
 296 
                // restart command cycle at the end
296 
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
 297 
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
297 
#define FLAG_GPS_AID 0x01
 298 
#define FLAG_GPS_AID 0x01
298 
                switch (ToFlightCtrl.Command)
 299 
                switch (ToFlightCtrl.Command)
299 
                {
 300 
                {
300 
                        case  SPI_NCCMD_KALMAN:
 301 
                        case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt
301 
                                CalcHeadFree();
 302 
                                CalcHeadFree();
302 
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
 303 
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
303 
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
 304 
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
304 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
 305 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
305 
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
 306 
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
Line 366... Line 367...
366 
                                        ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
 367 
                                        ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
367 
                                }
 368 
                                }
368 
//                              DebugOut.Analog[25] = (s16)ToFlightCtrl.Param.Byte[9];
 369 
//                              DebugOut.Analog[25] = (s16)ToFlightCtrl.Param.Byte[9];
369 
//                              DebugOut.Analog[20] = ToFlightCtrl.Param.sInt[5];
 370 
//                              DebugOut.Analog[20] = ToFlightCtrl.Param.sInt[5];
370 
                                break;
 371 
                                break;
- 
 
 372 
                        case SPI_NCCMD_HOTT_INFO:
- 
 
 373 
                                switch(hott_index++)
- 
 
 374 
                                {
- 
 
 375 
/*
- 
 
 376 
typedef struct
- 
 
 377 
{
- 
 
 378 
  unsigned char StartByte;  //0         // 0x7C
- 
 
 379 
  unsigned char Packet_ID;  //1         // 0x89  - Vario ID
- 
 
 380 
  unsigned char WarnBeep;   //2         // Anzahl der Töne 0..36
- 
 
 381 
  unsigned char Heading;        //3     // 1 = 2°
- 
 
 382 
  unsigned int Speed;           //4+5   // in km/h
- 
 
 383 
  unsigned char Lat_North;      //6    
- 
 
 384 
  unsigned char Lat_G;      //7
- 
 
 385 
  unsigned char Lat_M;      //8
- 
 
 386 
  unsigned char Lat_Sek1;       //9    
- 
 
 387 
  unsigned char Lat_Sek2;       //10    
- 
 
 388 
  unsigned char Lon_East;       //11    
- 
 
 389 
  unsigned char Lon_G;      //12
- 
 
 390 
  unsigned char Lon_M;      //13
- 
 
 391 
  unsigned char Lon_Sek1;       //14    
- 
 
 392 
  unsigned char Lon_Sek2;       //15    
- 
 
 393 
  unsigned int Distance;        //16+17    // 9000 = 0m
- 
 
 394 
  unsigned int Altitude;        //18+19    // 500 = 0m
- 
 
 395 
  unsigned int m_sec;           //20+21    // 3000 = 0
- 
 
 396 
  unsigned int m_3sec;          //22+23    // 3000 = 0
- 
 
 397 
  unsigned int m_10sec;         //24+25    // 3000 = 0
- 
 
 398 
  unsigned char NullByte;       // 0x00
- 
 
 399 
  unsigned char NullByte1;      // 0x00
- 
 
 400 
  unsigned char EndByte;                // 0x7D
- 
 
 401 
} GPSPacket_t;
- 
 
 402 
*/
- 
 
 403 
                                case 0:
- 
 
 404 
                                        //Dezimalgrad            --> Grad mit Dezimalminuten     --> Grad, Minuten, Sekunden
- 
 
 405 
                                        //53.285788 7.4847269    --> N53° 17.14728 E7° 29.08362  --> N53° 17' 8.837" E7° 29' 5.017"
- 
 
 406 
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
- 
 
 407 
                                        ToFlightCtrl.Param.Byte[0] = 3; // index
- 
 
 408 
                                        ToFlightCtrl.Param.Byte[1] = 9-1;       // how many
- 
 
 409 
                                        //-----------------------------
- 
 
 410 
                                        ToFlightCtrl.Param.Byte[2] = NaviData.HomePositionDeviation.Bearing / 2;
- 
 
 411 
                                        i1 = GPSData.Speed_Ground; // in cm/sec
- 
 
 412 
                                        i1 *= 36;
- 
 
 413 
                                        i1 /= 1000;
- 
 
 414 
                                        ToFlightCtrl.Param.Byte[3] = i1 % 256;
- 
 
 415 
                                        ToFlightCtrl.Param.Byte[4] = i1 / 256;
- 
 
 416 
                                        //-----------------------------
- 
 
 417 
                                        if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = S
- 
 
 418 
                                        else ToFlightCtrl.Param.Byte[5]  = 0; // 1 = S
- 
 
 419 
                                        i1 = abs(GPSData.Position.Latitude)/10000000L;
- 
 
 420 
                                        i2 = abs(GPSData.Position.Latitude)%10000000L;
- 
 
 421 
                                        i1 *= 100;
- 
 
 422 
                                        i1 += i2 / 100000;
- 
 
 423 
                                        i2  = i2 % 100000;
- 
 
 424 
                                        i2 /= 10;
- 
 
 425 
                                        ToFlightCtrl.Param.Byte[6]  = i1 % 256;
- 
 
 426 
                                        ToFlightCtrl.Param.Byte[7]  = i1 / 256;
- 
 
 427 
                                        ToFlightCtrl.Param.Byte[8]  = i2 % 256;
- 
 
 428 
                                        ToFlightCtrl.Param.Byte[9]  = i2 / 256;
371 
 
 429 
 
- 
 
 430 
                                        break;
- 
 
 431 
                                case 1:
- 
 
 432 
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
- 
 
 433 
                                        ToFlightCtrl.Param.Byte[0] = 11;        // index
- 
 
 434 
                                        ToFlightCtrl.Param.Byte[1] = 8-1;       // how many
- 
 
 435 
                                        //-----------------------------
- 
 
 436 
                                        if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = E
- 
 
 437 
                                        else ToFlightCtrl.Param.Byte[2]  = 0; // 1 = S
- 
 
 438 
                                        i1 = abs(GPSData.Position.Longitude)/10000000L;
- 
 
 439 
                                        i2 = abs(GPSData.Position.Longitude)%10000000L;
- 
 
 440 
                                        i1 *= 100;
- 
 
 441 
                                        i1 += i2 / 100000;
- 
 
 442 
                                        i2  = i2 % 100000;
- 
 
 443 
                                        i2 /= 10;
- 
 
 444 
                                        ToFlightCtrl.Param.Byte[3]  = i1 % 256;
- 
 
 445 
                                        ToFlightCtrl.Param.Byte[4]  = i1 / 256;
- 
 
 446 
                                        ToFlightCtrl.Param.Byte[5]  = i2 % 256;
- 
 
 447 
                                        ToFlightCtrl.Param.Byte[6]  = i2 / 256;
- 
 
 448 
                                        //-----------------------------
- 
 
 449 
                                        i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m
- 
 
 450 
                                        ToFlightCtrl.Param.Byte[7]  = i1 % 256;
- 
 
 451 
                                        ToFlightCtrl.Param.Byte[8]  = i1 / 256;
- 
 
 452 
                                        hott_index = 0;
- 
 
 453 
                                        break;
- 
 
 454 
                                   default:
- 
 
 455 
                                                ToFlightCtrl.Param.Byte[0] = 255;
- 
 
 456 
                                                hott_index = 0;
- 
 
 457 
                                                break;
- 
 
 458 
                                }
- 
 
 459 
                                break;
372 
                        default:
 460 
                        default:
373 
                                break;
 461 
                                break;
374 
// 0 = 0,1
 462 
// 0 = 0,1
375 
// 1 = 2,3
 463 
// 1 = 2,3
376 
// 2 = 4,5
 464 
// 2 = 4,5
Line 378... Line 466...
378 
// 4 = 8,9
 466 
// 4 = 8,9
379 
// 5 = 10,11
 467 
// 5 = 10,11
380 
                }
 468 
                }
381 
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
 469 
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
Line 382... Line -...
382 
 
 - 
 
383 
 
 470 
 
384 
                switch(FromFlightCtrl.Command)
 471 
                switch(FromFlightCtrl.Command)
385 
                {
 472 
                {
386 
                        case SPI_FCCMD_USER:
 473 
                        case SPI_FCCMD_USER:
387 
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
 474 
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
Line 406... Line 493...
406 
                                }
 493 
                                }
407 
                                else
 494 
                                else
408 
                                {
 495 
                                {
409 
                                        FCCalibActive = 0;
 496 
                                        FCCalibActive = 0;
410 
                                }
 497 
                                }
411 
 if(FC.StatusFlags & FC_STATUS_START)
 498 
                                if(FC.StatusFlags & FC_STATUS_START)
- 
 
 499 
                             {
412 
     { if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else HeadFreeStartAngle = FromFlightCtrl.GyroHeading; }
 500 
                                   if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else
- 
 
 501 
                                   HeadFreeStartAngle = FromFlightCtrl.GyroHeading;
- 
 
 502 
                                 }
Line 413... Line 503...
413 
 
 503 
 
414 
 if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
 504 
                                 if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
415 
  {
 505 
                                  {
416 
   if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
 506 
                                   if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
417 
    {
 507 
                                    {
418 
     if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))   // nur bei ausreichender Distance -> 20m
 508 
                                         if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))       // nur bei ausreichender Distance -> 20m
419 
     {
 509 
                                         {
420 
       HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
 510 
                                          HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
421 
     }
 511 
                                         }
422 
         else                                                                                            // Ansonsten die aktuelle Richtung übernehmen
 512 
                                         else                                                                                            // Ansonsten die aktuelle Richtung übernehmen
423 
       HeadFreeStartAngle = (3600 + FromFlightCtrl.GyroHeading /*+ Parameter.OrientationAngle * 150*/) % 3600; // in 0.1°        
513 
                                          HeadFreeStartAngle = (3600 + FromFlightCtrl.GyroHeading /*+ Parameter.OrientationAngle * 150*/) % 3600; // in 0.1°     
424 
    }
 514 
                                        }
Line 425... Line 515...
425 
  }
 515 
                                }
Line 426... Line 516...
426 
 
 516 
 
427 
//DebugOut.Analog[16] = HeadFreeStartAngle;
 517 
//DebugOut.Analog[16] = HeadFreeStartAngle;
Line 594... Line 684...
594 
                        SPI0_UpdateBuffer();
 684 
                        SPI0_UpdateBuffer();
595 
                        if (FC_Version.Major != 0xFF)  break;
 685 
                        if (FC_Version.Major != 0xFF)  break;
596 
                }while (!CheckDelay(timeout));
 686 
                }while (!CheckDelay(timeout));
597 
                UART1_PutString(".");
 687 
                UART1_PutString(".");
598 
                repeat++;
 688 
                repeat++;
- 
 
 689 
                FCCalibActive = 1;
599 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
 690 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
600 
        // if we got it
 691 
        // if we got it
601 
        if (FC_Version.Major != 0xFF)
 692 
        if (FC_Version.Major != 0xFF)
602 
        {
 693 
        {
603 
                sprintf(msg, " FC 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);
 694 
                sprintf(msg, " FC 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);