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]; |