Subversion Repositories NaviCtrl

Rev

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

Rev 473 Rev 475
Line 307... Line 307...
307 
        {
 307 
        {
308 
                // 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
309 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
 309 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
310 
                ToFlightCtrl.CompassHeading = Compass_Heading;
 310 
                ToFlightCtrl.CompassHeading = Compass_Heading;
311 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
 311 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
312 
GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading - GeoMagDec) % 3600;
 312 
GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading + FC.FromFC_CompassOffset - GeoMagDec) % 3600;
313 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
 313 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
314 
                ToFlightCtrl.MagVecX = MagVector.X;
 314 
                ToFlightCtrl.MagVecX = MagVector.X;
315 
                ToFlightCtrl.MagVecY = MagVector.Y;
 315 
                ToFlightCtrl.MagVecY = MagVector.Y;
316 
                ToFlightCtrl.MagVecZ = MagVector.Z;
 316 
                ToFlightCtrl.MagVecZ = MagVector.Z;
317 
//              ToFlightCtrl.NCStatus = 0;
 317 
//              ToFlightCtrl.NCStatus = 0;
Line 616... Line 616...
616 
                                }
 616 
                                }
617 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
 617 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
618 
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[10];
 618 
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[10];
619 
                                DebugOut.Analog[5] = FC.StatusFlags;
 619 
                                DebugOut.Analog[5] = FC.StatusFlags;
620 
                                NaviData.FCStatusFlags = FC.StatusFlags;
 620 
                                NaviData.FCStatusFlags = FC.StatusFlags;
621 
if(FC.StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF) NaviData.FCStatusFlags &= ~FC_STATUS_FLY;
 621 
                                if(FC.StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF) NaviData.FCStatusFlags &= ~FC_STATUS_FLY;
622 
                                FC.StatusFlags2 = FromFlightCtrl.Param.Byte[11];
 622 
                                FC.StatusFlags2 = FromFlightCtrl.Param.Byte[11];
623 
                                NaviData.FCStatusFlags2 = (NaviData.FCStatusFlags2 & (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)) | (FC.StatusFlags2 & (0xff - (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)));
 623 
                                NaviData.FCStatusFlags2 = (NaviData.FCStatusFlags2 & (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)) | (FC.StatusFlags2 & (0xff - (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)));
Line 624... Line 624...
624 
 
 624 
 
625 
                                if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT1_ACTIVE;
 625 
                                if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT1_ACTIVE;
Line 735... Line 735...
735 
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
 735 
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
736 
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
 736 
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
737 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
 737 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
738 
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[5];
 738 
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[5];
739 
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[6];
 739 
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[6];
740 
//      frei            FC.Error[2]                     |= FromFlightCtrl.Param.Byte[7];
 740 
                                if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188)
- 
 
 741 
                                {
- 
 
 742 
                                 FC.FromFC_DisableDeclination = 1;
- 
 
 743 
                                 FC.FromFC_CompassOffset = 10 * (signed char) ((unsigned char) FromFlightCtrl.Param.Byte[7] - 128);
- 
 
 744 
                                 GeoMagDec = 0;
- 
 
 745 
                                }
- 
 
 746 
                                else
- 
 
 747 
                                {
- 
 
 748 
                                 FC.FromFC_DisableDeclination = 0;
- 
 
 749 
                                 FC.FromFC_CompassOffset = 10 * (signed char) FromFlightCtrl.Param.Byte[7];
- 
 
 750 
                                }
741 
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
 751 
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
742 
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
 752 
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
743 
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
 753 
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
744 
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
 754 
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
745 
                                DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present
 755 
                                DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present