Subversion Repositories NaviCtrl

Rev

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

Rev 338 Rev 342
Line 99... Line 99...
99 
s32 Kalman_MaxDrift = 5 * 16;
 99 
s32 Kalman_MaxDrift = 5 * 16;
100 
s32 Kalman_MaxFusion = 64;
 100 
s32 Kalman_MaxFusion = 64;
101 
s32 Kalman_Kompass = 32;
 101 
s32 Kalman_Kompass = 32;
102 
s32 ToFcGpsZ = 0;
 102 
s32 ToFcGpsZ = 0;
Line 103... Line 103...
103 
 
 103 
 
104 
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN};
 104 
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN };
105 
u8 SPI_CommandCounter = 0;
 105 
u8 SPI_CommandCounter = 0;
106 
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
 106 
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
107 
s32 HeadFreeStartAngle = 0;
 107 
s32 HeadFreeStartAngle = 0;
108 
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
 108 
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
Line 290... Line 290...
290 
        if (SPI_RxBuffer_Request)
 290 
        if (SPI_RxBuffer_Request)
291 
        {
 291 
        {
292 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
 292 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
293 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
 293 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
294 
                ToFlightCtrl.CompassHeading = Compass_Heading;
 294 
                ToFlightCtrl.CompassHeading = Compass_Heading;
- 
 
 295 
//ToFlightCtrl.CompassHeading += 360 + ((s32) FC.Poti[7] - 100);
- 
 
 296 
//ToFlightCtrl.CompassHeading %= 360;
295 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
 297 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
296 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
 298 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
297 
//              ToFlightCtrl.MagVecX = MagVector.X;
 299 
//              ToFlightCtrl.MagVecX = MagVector.X;
298 
//              ToFlightCtrl.MagVecY = MagVector.Y;
 300 
//              ToFlightCtrl.MagVecY = MagVector.Y;
299 
                ToFlightCtrl.MagVecZ = MagVector.Z;
 301 
                ToFlightCtrl.MagVecZ = MagVector.Z;
Line 345... Line 347...
345 
                                ToFlightCtrl.Param.Byte[5] = DebugOut.StatusGreen;
 347 
                                ToFlightCtrl.Param.Byte[5] = DebugOut.StatusGreen;
346 
                                ToFlightCtrl.Param.Byte[6] = DebugOut.StatusRed;
 348 
                                ToFlightCtrl.Param.Byte[6] = DebugOut.StatusRed;
347 
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;
 349 
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;
348 
                                ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter;
 350 
                                ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter;
349 
                                ToFlightCtrl.Param.Byte[9] = SerialLinkOkay;
 351 
                                ToFlightCtrl.Param.Byte[9] = SerialLinkOkay;
- 
 
 352 
                                ToFlightCtrl.Param.Byte[10] = 0;
- 
 
 353 
                                ToFlightCtrl.Param.Byte[11] = 0;
- 
 
 354 
                                break;
- 
 
 355 
                        case SPI_MISC:
- 
 
 356 
                                ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5;
- 
 
 357 
                                ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination;
- 
 
 358 
                                ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic;
- 
 
 359 
                                ToFlightCtrl.Param.Byte[3] = 0;
- 
 
 360 
                                ToFlightCtrl.Param.Byte[4] = 0;
- 
 
 361 
                                ToFlightCtrl.Param.Byte[5] = 0;
- 
 
 362 
                                ToFlightCtrl.Param.Byte[6] = 0;
- 
 
 363 
                                ToFlightCtrl.Param.Byte[7] = 0;
- 
 
 364 
                                ToFlightCtrl.Param.Byte[8] = 0;
- 
 
 365 
                                ToFlightCtrl.Param.Byte[9] = 0;
- 
 
 366 
                                ToFlightCtrl.Param.Byte[10] = 0;
- 
 
 367 
                                ToFlightCtrl.Param.Byte[11] = 0;
350 
                                break;
 368 
                                break;
Line 351... Line 369...
351 
 
 369 
 
352 
                        case SPI_NCCMD_GPSINFO:
 370 
                        case SPI_NCCMD_GPSINFO:
353 
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
 371 
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;