Rev 509 | Rev 511 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 509 | Rev 510 | ||
|---|---|---|---|
| Line 122... | Line 122... | ||
| 122 | u32 FC_I2C_ErrorConter; |
122 | u32 FC_I2C_ErrorConter; |
| 123 | SPI_Version_t FC_Version; |
123 | SPI_Version_t FC_Version; |
| 124 | s16 POI_KameraNick = 0; |
124 | s16 POI_KameraNick = 0; |
| 125 | u8 NC_Wait_for_LED = 0; |
125 | u8 NC_Wait_for_LED = 0; |
| 126 | s16 GyroCompassCorrected = 0; // corrected with the magnetic declination |
126 | s16 GyroCompassCorrected = 0; // corrected with the magnetic declination |
| - | 127 | s16 CompassSetpointCorrected = 0; // The compass setpoint that the FC tries to keep - corrected with the magnetic declination |
|
| - | 128 | s16 CompassSetpoint = 0; // in 0,1° |
|
| Line 127... | Line 129... | ||
| 127 | 129 | ||
| 128 | //-------------------------------------------------------------- |
130 | //-------------------------------------------------------------- |
| 129 | void SSP0_IRQHandler(void) |
131 | void SSP0_IRQHandler(void) |
| 130 | { |
132 | { |
| Line 305... | Line 307... | ||
| 305 | { |
307 | { |
| 306 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
308 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
| 307 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
309 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
| 308 | ToFlightCtrl.CompassHeading = Compass_Heading; |
310 | ToFlightCtrl.CompassHeading = Compass_Heading; |
| 309 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
311 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
| 310 | GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading + FC.FromFC_CompassOffset - GeoMagDec) % 3600; |
312 | //GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading + FC.FromFC_CompassOffset - GeoMagDec) % 3600; |
| - | 313 | GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading + FC.FromFC_CompassOffset + GeoMagDec) % 3600; |
|
| 311 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
314 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
| 312 | ToFlightCtrl.MagVecX = MagVector.X; |
315 | ToFlightCtrl.MagVecX = MagVector.X; |
| 313 | ToFlightCtrl.MagVecY = MagVector.Y; |
316 | ToFlightCtrl.MagVecY = MagVector.Y; |
| 314 | ToFlightCtrl.MagVecZ = MagVector.Z; |
317 | ToFlightCtrl.MagVecZ = MagVector.Z; |
| 315 | // ToFlightCtrl.NCStatus = 0; |
318 | // ToFlightCtrl.NCStatus = 0; |
| Line 592... | Line 595... | ||
| 592 | ClearFCStatusFlags = 0; |
595 | ClearFCStatusFlags = 0; |
| 593 | } |
596 | } |
| 594 | FC.StatusFlags |= FromFlightCtrl.Param.Byte[8]; |
597 | FC.StatusFlags |= FromFlightCtrl.Param.Byte[8]; |
| 595 | if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive) |
598 | if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive) |
| 596 | { |
599 | { |
| - | 600 | // HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; |
|
| 597 | HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; |
601 | HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600; |
| 598 | Compass_Init(); |
602 | Compass_Init(); |
| 599 | FCCalibActive = 10; |
603 | FCCalibActive = 10; |
| 600 | FC_is_Calibrated = 0; |
604 | FC_is_Calibrated = 0; |
| 601 | } |
605 | } |
| 602 | else |
606 | else |
| Line 610... | Line 614... | ||
| 610 | } |
614 | } |
| 611 | } |
615 | } |
| 612 | } |
616 | } |
| 613 | if(FC.StatusFlags & FC_STATUS_START) |
617 | if(FC.StatusFlags & FC_STATUS_START) |
| 614 | { |
618 | { |
| 615 | if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
619 | // if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
| - | 620 | if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600; |
|
| 616 | HeadFreeStartAngle = GyroCompassCorrected; |
621 | else HeadFreeStartAngle = GyroCompassCorrected; |
| 617 | } |
622 | } |
| Line 618... | Line 623... | ||
| 618 | 623 | ||
| 619 | if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
624 | if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
| 620 | { |
625 | { |
| Line 691... | Line 696... | ||
| 691 | Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3]; |
696 | Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3]; |
| 692 | Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4]; |
697 | Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4]; |
| 693 | if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5]; |
698 | if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5]; |
| 694 | if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SingePoint = FromFlightCtrl.Param.Byte[6]; |
699 | if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SingePoint = FromFlightCtrl.Param.Byte[6]; |
| 695 | if(FromFlightCtrl.Param.Byte[7]) FromFC_Store_SingePoint = FromFlightCtrl.Param.Byte[7]; |
700 | if(FromFlightCtrl.Param.Byte[7]) FromFC_Store_SingePoint = FromFlightCtrl.Param.Byte[7]; |
| - | 701 | CompassSetpoint = FromFlightCtrl.Param.sInt[4] * 10; // 8 & 9 |
|
| - | 702 | CompassSetpointCorrected = (3600 + CompassSetpoint + FC.FromFC_CompassOffset + GeoMagDec) % 3600; |
|
| - | 703 | //FromFlightCtrl.Param.Byte[10] |
|
| - | 704 | //FromFlightCtrl.Param.Byte[11] |
|
| 696 | break; |
705 | break; |
| 697 | case SPI_FCCMD_STICK: |
706 | case SPI_FCCMD_STICK: |
| 698 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
707 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
| 699 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
708 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
| 700 | FC.StickRoll = FromFlightCtrl.Param.sByte[2]; |
709 | FC.StickRoll = FromFlightCtrl.Param.sByte[2]; |