Subversion Repositories NaviCtrl

Rev

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

Rev 284 Rev 285
Line 101... Line 101...
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_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_AltitudeSpeed = 0;
- 
 
 107 
s32 ToFC_AltitudeSetpoint = 0;
Line 106... Line 108...
106 
 
 108 
 
Line 107... Line 109...
107 
SPI_Version_t FC_Version;
 109 
SPI_Version_t FC_Version;
108 
 
 110 
 
Line 278... Line 280...
278 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
 280 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
279 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
 281 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
280 
                ToFlightCtrl.CompassHeading = Compass_Heading;
 282 
                ToFlightCtrl.CompassHeading = Compass_Heading;
281 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
 283 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
282 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
 284 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
283 
                ToFlightCtrl.MagVecX = MagVector.X;
 285 
//              ToFlightCtrl.MagVecX = MagVector.X;
284 
                ToFlightCtrl.MagVecY = MagVector.Y;
 286 
//              ToFlightCtrl.MagVecY = MagVector.Y;
285 
                ToFlightCtrl.MagVecZ = MagVector.Z;
 287 
//              ToFlightCtrl.MagVecZ = MagVector.Z;
286 
                ToFlightCtrl.NCStatus = 0;
 288 
                ToFlightCtrl.NCStatus = 0;
287 
                // cycle spi commands
 289 
                // cycle spi commands
288 
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
 290 
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
289 
                // restart command cycle at the end
 291 
                // restart command cycle at the end
290 
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
 292 
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
Line 298... Line 300...
298 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
 300 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
299 
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
 301 
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
300 
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
 302 
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
301 
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
 303 
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
302 
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
 304 
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
- 
 
 305 
                //ToFlightCtrl.Param.Byte[7] =
303 
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
 306 
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
304 
                                {
 307 
                                {
305 
                                        ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
 308 
                                        ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
306 
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
 309 
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
307 
                                }
 310 
                                }
Line 329... Line 332...
329 
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
 332 
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
330 
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
 333 
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
331 
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
 334 
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
332 
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
 335 
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
333 
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110);
 336 
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110);
- 
 
 337 
                                ToFlightCtrl.Param.Byte[9] = (u8) ToFC_AltitudeSpeed;
- 
 
 338 
                                ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint;
334 
                                break;
 339 
                                break;
335 
                        default:
 340 
                        default:
336 
                                break;
 341 
                                break;
- 
 
 342 
// 0 = 0,1
- 
 
 343 
// 1 = 2,3
- 
 
 344 
// 2 = 4,5
- 
 
 345 
// 3 = 6,7
- 
 
 346 
// 4 = 8,9
- 
 
 347 
// 5 = 10,11
337 
                }
 348 
                }
338 
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
 349 
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
Line 339... Line 350...
339 
 
 350