Rev 417 | Rev 420 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 417 | Rev 419 | ||
---|---|---|---|
Line 101... | Line 101... | ||
101 | s32 Kalman_MaxFusion = 64; |
101 | s32 Kalman_MaxFusion = 64; |
102 | s32 Kalman_Kompass = 32; |
102 | s32 Kalman_Kompass = 32; |
103 | s32 ToFcGpsZ = 0; |
103 | s32 ToFcGpsZ = 0; |
104 | u8 CompassCalState = 0; |
104 | u8 CompassCalState = 0; |
Line 105... | Line 105... | ||
105 | 105 | ||
106 | 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 }; |
106 | u8 SPI_CommandSequence[] = { SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN, SPI_NCCMD_VERSION }; |
107 | u8 SPI_CommandCounter = 0; |
107 | u8 SPI_CommandCounter = 0; |
108 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
108 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
109 | s32 HeadFreeStartAngle = 0; |
109 | s32 HeadFreeStartAngle = 0; |
110 | s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde |
110 | s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde |
Line 268... | Line 268... | ||
268 | // set the pointer to the checksum byte in the tx buffer |
268 | // set the pointer to the checksum byte in the tx buffer |
269 | Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum); |
269 | Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum); |
Line 270... | Line 270... | ||
270 | 270 | ||
271 | ToFlightCtrl.GPSStick.Nick = 0; |
271 | ToFlightCtrl.GPSStick.Nick = 0; |
272 | ToFlightCtrl.GPSStick.Roll = 0; |
272 | ToFlightCtrl.GPSStick.Roll = 0; |
Line 273... | Line 273... | ||
273 | ToFlightCtrl.GPSStick.Yaw = 0; |
273 | // ToFlightCtrl.GPSStick.Yaw = 0; |
274 | 274 | ||
Line 275... | Line 275... | ||
275 | VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0); |
275 | VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0); |
Line 283... | Line 283... | ||
283 | 283 | ||
284 | //------------------------------------------------------ |
284 | //------------------------------------------------------ |
285 | void SPI0_UpdateBuffer(void) |
285 | void SPI0_UpdateBuffer(void) |
286 | { |
286 | { |
287 | static u32 timeout = 0; |
287 | static u32 timeout = 0; |
288 | static u8 counter = 50,hott_index = 0; |
288 | static u8 counter = 50,hott_index = 0, last_error_code = 0; |
289 | s16 tmp; |
289 | s16 tmp; |
Line 290... | Line 290... | ||
290 | s32 i1,i2; |
290 | s32 i1,i2; |
291 | 291 | ||
Line 295... | Line 295... | ||
295 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
295 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
296 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
296 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
297 | ToFlightCtrl.CompassHeading = Compass_Heading; |
297 | ToFlightCtrl.CompassHeading = Compass_Heading; |
298 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
298 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
299 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
299 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
300 | // ToFlightCtrl.MagVecX = MagVector.X; |
300 | ToFlightCtrl.MagVecX = MagVector.X; |
301 | // ToFlightCtrl.MagVecY = MagVector.Y; |
301 | ToFlightCtrl.MagVecY = MagVector.Y; |
302 | ToFlightCtrl.MagVecZ = MagVector.Z; |
302 | ToFlightCtrl.MagVecZ = MagVector.Z; |
303 | ToFlightCtrl.NCStatus = 0; |
303 | // ToFlightCtrl.NCStatus = 0; |
304 | // cycle spi commands |
304 | // cycle spi commands |
- | 305 | if(ErrorCode != last_error_code) ToFlightCtrl.Command = SPI_NCCMD_VERSION; |
|
305 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
306 | else ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
306 | // restart command cycle at the end |
307 | // restart command cycle at the end |
307 | if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
308 | if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
Line -... | Line 309... | ||
- | 309 | ||
- | 310 | last_error_code = ErrorCode; |
|
308 | 311 | ||
309 | #define FLAG_GPS_AID 0x01 |
312 | #define FLAG_GPS_AID 0x01 |
310 | switch (ToFlightCtrl.Command) |
313 | switch (ToFlightCtrl.Command) |
311 | { |
314 | { |
312 | case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt |
315 | case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt |
Line 362... | Line 365... | ||
362 | case SPI_MISC: |
365 | case SPI_MISC: |
363 | ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5; |
366 | ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5; |
364 | ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination; |
367 | ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination; |
365 | ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic; |
368 | ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic; |
366 | ToFlightCtrl.Param.Byte[3] = SpeakHoTT; |
369 | ToFlightCtrl.Param.Byte[3] = SpeakHoTT; |
367 | ToFlightCtrl.Param.Byte[4] = 0; |
370 | ToFlightCtrl.Param.Byte[4] = NaviData.WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1 |
368 | ToFlightCtrl.Param.Byte[5] = 0; |
371 | ToFlightCtrl.Param.Byte[5] = NaviData.WaypointNumber; // number of stored waypoints |
369 | ToFlightCtrl.Param.Byte[6] = 0; |
- | |
370 | ToFlightCtrl.Param.Byte[7] = 0; |
372 | ToFlightCtrl.Param.Int[3] = NaviData.TargetPositionDeviation.Distance / 10; |
371 | ToFlightCtrl.Param.Byte[8] = 0; |
373 | ToFlightCtrl.Param.Byte[8] = NaviData.TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached |
372 | ToFlightCtrl.Param.Byte[9] = 0; |
374 | ToFlightCtrl.Param.Byte[9] = 0; |
373 | ToFlightCtrl.Param.Byte[10] = 0; |
375 | ToFlightCtrl.Param.Byte[10] = 0; |
374 | ToFlightCtrl.Param.Byte[11] = 0; |
376 | ToFlightCtrl.Param.Byte[11] = 0; |
375 | //DebugOut.Analog[16] = SpeakHoTT; |
377 | //DebugOut.Analog[16] = SpeakHoTT; |
376 | SpeakHoTT = 0; |
378 | SpeakHoTT = 0; |
Line 383... | Line 385... | ||
383 | ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s |
385 | ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s |
384 | ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5 |
386 | ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5 |
385 | ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7 |
387 | ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7 |
386 | if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254; |
388 | if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254; |
387 | ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127); |
389 | ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127); |
- | 390 | FC_WP_EventChannel = 0; // the GPS-Routine will set it again |
|
388 | if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp)) |
391 | if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp)) |
389 | { |
392 | { |
390 | ToFlightCtrl.Param.Byte[9] = (u8)tmp; |
393 | ToFlightCtrl.Param.Byte[9] = (u8)tmp; |
391 | } |
394 | } |
392 | else |
395 | else |
Line 580... | Line 583... | ||
580 | CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255); |
583 | CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255); |
581 | CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255); |
584 | CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255); |
582 | CHK_POTI_MM(Parameter.NaviAccCompensation,FromFlightCtrl.Param.Byte[10],0,255); |
585 | CHK_POTI_MM(Parameter.NaviAccCompensation,FromFlightCtrl.Param.Byte[10],0,255); |
583 | CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255); |
586 | CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255); |
584 | break; |
587 | break; |
- | 588 | case SPI_FCCMD_PARAMETER2: |
|
- | 589 | CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255); |
|
585 | 590 | break; |
|
586 | case SPI_FCCMD_STICK: |
591 | case SPI_FCCMD_STICK: |
587 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
592 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
588 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
593 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
589 | FC.StickRoll = FromFlightCtrl.Param.sByte[2]; |
594 | FC.StickRoll = FromFlightCtrl.Param.sByte[2]; |
590 | FC.StickNick = FromFlightCtrl.Param.sByte[3]; |
595 | FC.StickNick = FromFlightCtrl.Param.sByte[3]; |
Line 632... | Line 637... | ||
632 | ServoParams.RollControl = FromFlightCtrl.Param.Byte[6]; |
637 | ServoParams.RollControl = FromFlightCtrl.Param.Byte[6]; |
633 | ServoParams.RollComp = FromFlightCtrl.Param.Byte[7]; |
638 | ServoParams.RollComp = FromFlightCtrl.Param.Byte[7]; |
634 | ServoParams.RollMin = FromFlightCtrl.Param.Byte[8]; |
639 | ServoParams.RollMin = FromFlightCtrl.Param.Byte[8]; |
635 | ServoParams.RollMax = FromFlightCtrl.Param.Byte[9]; |
640 | ServoParams.RollMax = FromFlightCtrl.Param.Byte[9]; |
636 | BL_MinOfMaxPWM = FromFlightCtrl.Param.Byte[10]; |
641 | BL_MinOfMaxPWM = FromFlightCtrl.Param.Byte[10]; |
- | 642 | FC_I2C_ErrorConter = FromFlightCtrl.Param.Byte[11]; |
|
637 | break; |
643 | break; |
Line 638... | Line 644... | ||
638 | 644 | ||
639 | case SPI_FCCMD_VERSION: |
645 | case SPI_FCCMD_VERSION: |
640 | FC_Version.Major = FromFlightCtrl.Param.Byte[0]; |
646 | FC_Version.Major = FromFlightCtrl.Param.Byte[0]; |
Line 680... | Line 686... | ||
680 | if(Parameter.User8 < 100) FC.StatusFlags = 0; |
686 | if(Parameter.User8 < 100) FC.StatusFlags = 0; |
681 | else |
687 | else |
682 | if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START; |
688 | if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START; |
683 | else FC.StatusFlags = FC_STATUS_FLY | FC_STATUS_MOTOR_RUN; |
689 | else FC.StatusFlags = FC_STATUS_FLY | FC_STATUS_MOTOR_RUN; |
684 | BL_MinOfMaxPWM = 255; |
690 | BL_MinOfMaxPWM = 255; |
- | 691 | NaviData.FCStatusFlags = FC.StatusFlags; |
|
685 | */ |
692 | */ |
686 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
693 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 687... | Line 694... | ||
687 | 694 |