Subversion Repositories NaviCtrl

Rev

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

Rev 581 Rev 587
Line 105... Line 105...
105 
u8 CompassCalState = 0;
 105 
u8 CompassCalState = 0;
Line 106... Line 106...
106 
 
 106 
 
107 
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_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 };
108 
u8 SPI_CommandCounter = 0;
 108 
u8 SPI_CommandCounter = 0;
109 
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
 109 
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
- 
 
 110 
s32 HeadFreeStartAngle = 0;       // in 0,1°
110 
s32 HeadFreeStartAngle = 0;
 111 
s32 CompassDirectionAtMotorStart = 0; // in 0,1°
111 
s16 FC_WP_EventChannel = 0, LogFC_WP_EventChannel = 0, FC_WP_EventChannel_Processed = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
 112 
s16 FC_WP_EventChannel = 0, LogFC_WP_EventChannel = 0, FC_WP_EventChannel_Processed = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
112 
u32 ToFC_AltitudeRate = 0;
 113 
u32 ToFC_AltitudeRate = 0;
113 
s32 ToFC_AltitudeSetpoint = 0;
 114 
s32 ToFC_AltitudeSetpoint = 0;
114 
u8  FromFC_VarioCharacter = ' ';
 115 
u8  FromFC_VarioCharacter = ' ';
Line 598... Line 599...
598 
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
 599 
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
599 
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
 600 
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
600 
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
 601 
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
601 
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
 602 
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
602 
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
 603 
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
603 
 
 - 
 
604 
                                FC.RealStatusFlags = FromFlightCtrl.Param.Byte[8];
 604 
                                FC.RealStatusFlags = FromFlightCtrl.Param.Byte[8];
605 
                                if(FC.RealStatusFlags & FC_STATUS_MOTOR_RUN) SimulationFlags = 0; // stop the simulation if the motors would really start
 605 
                                if(FC.RealStatusFlags & FC_STATUS_MOTOR_RUN) SimulationFlags = 0; // stop the simulation if the motors would really start
Line 606... Line 606...
606 
 
 606 
 
607 
                                if(!(SimulationFlags & SIMULATION_ACTIVE))
 607 
                                if(!(SimulationFlags & SIMULATION_ACTIVE))
Line 614... Line 614...
614 
                                }
 614 
                                }
615 
                                FC.StatusFlags |= FC.RealStatusFlags;
 615 
                                FC.StatusFlags |= FC.RealStatusFlags;
616 
                                if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
 616 
                                if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
617 
                                {
 617 
                                {
618 
                                        HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600;
 618 
                                        HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600;
- 
 
 619 
                                        CompassDirectionAtMotorStart = HeadFreeStartAngle;
619 
                                        Compass_Init();
 620 
                                        Compass_Init();
620 
                                        FCCalibActive = 10;
 621 
                                        FCCalibActive = 10;
621 
                                        FC_is_Calibrated = 0;
 622 
                                        FC_is_Calibrated = 0;
622 
                                }
 623 
                                }
623 
                                else
 624 
                                else
Line 633... Line 634...
633 
                                }
 634 
                                }
634 
                                if(FC.StatusFlags & FC_STATUS_START)
 635 
                                if(FC.StatusFlags & FC_STATUS_START)
635 
                                {
 636 
                                {
636 
                                        if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600;
 637 
                                        if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600;
637 
                                        else HeadFreeStartAngle = GyroCompassCorrected;
 638 
                                        else HeadFreeStartAngle = GyroCompassCorrected;
- 
 
 639 
                                        CompassDirectionAtMotorStart = HeadFreeStartAngle;
638 
                                }
 640 
                                }
Line 639... Line 641...
639 
 
 641 
 
640 
                                if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
 642 
                                if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
641 
                                {
 643 
                                {
Line 709... Line 711...
709 
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
 711 
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
710 
                                break;
 712 
                                break;
711 
                        case SPI_FCCMD_PARAMETER2:
 713 
                        case SPI_FCCMD_PARAMETER2:
712 
                                CHK_POTI_MM(Parameter.AutoPhotoDistance,FromFlightCtrl.Param.Byte[0],0,255);
 714 
                                CHK_POTI_MM(Parameter.AutoPhotoDistance,FromFlightCtrl.Param.Byte[0],0,255);
713 
                                if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1];     // will be cleared in the SD-Logging
 715 
                                if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1];     // will be cleared in the SD-Logging
714 
                                Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2];
 - 
 
715 
                                Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3];
 716 
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1]; // 2 & 3
716 
                                Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4];
 717 
                                Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4];
717 
                                if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5];
 718 
                                if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5];
718 
                                if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SinglePoint = FromFlightCtrl.Param.Byte[6];
 719 
                                if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SinglePoint = FromFlightCtrl.Param.Byte[6];
719 
                                if(FromFlightCtrl.Param.Byte[7]) FromFC_Save_SinglePoint = FromFlightCtrl.Param.Byte[7];
 720 
                                if(FromFlightCtrl.Param.Byte[7]) FromFC_Save_SinglePoint = FromFlightCtrl.Param.Byte[7];
720 
                                CompassSetpoint = FromFlightCtrl.Param.sInt[4] * 10; // 8 & 9
 721 
                                CompassSetpoint = FromFlightCtrl.Param.sInt[4] * 10; // 8 & 9
721 
                                CompassSetpointCorrected = (3600 + CompassSetpoint + FC.FromFC_CompassOffset + GeoMagDec) % 3600;
 722 
                                CompassSetpointCorrected = (3600 + CompassSetpoint + FC.FromFC_CompassOffset + GeoMagDec) % 3600;
722 
                                CHK_POTI_MM(Parameter.AutoPhotoAltitudes,FromFlightCtrl.Param.Byte[10],0,255);
 723 
                                FC.StatusFlags3 = FromFlightCtrl.Param.Byte[10];
723 
                                Parameter.SingleWpSpeed = FromFlightCtrl.Param.Byte[11];
 724 
                                Parameter.SingleWpSpeed = FromFlightCtrl.Param.Byte[11];
724 
                                break;
 725 
                                break;
725 
                        case SPI_FCCMD_STICK:
 726 
                        case SPI_FCCMD_STICK:
726 
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
 727 
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
727 
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
 728 
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
Line 746... Line 747...
746 
                                           CompassCalState = FromFlightCtrl.Param.Byte[0];
 747 
                                           CompassCalState = FromFlightCtrl.Param.Byte[0];
747 
                                           Compass_SetCalState(CompassCalState);
 748 
                                           Compass_SetCalState(CompassCalState);
748 
                                         }
 749 
                                         }
749 
//                                       else CompassCalState = 0;
 750 
//                                       else CompassCalState = 0;
750 
                                }
 751 
                                }
751 
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
 752 
                                HoverGas = FromFlightCtrl.Param.Byte[1];
752 
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
 753 
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
753 
                                FC.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
 754 
                                FC.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
754 
                                if(!(SimulationFlags & SIMULATION_ACTIVE)) NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // in 5cm
 755 
                                if(!(SimulationFlags & SIMULATION_ACTIVE)) NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // in 5cm
755 
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[6],0,255);
 756 
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[6];
756 
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[7],0,255);
 757 
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[7];
- 
 
 758 
                                DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present
- 
 
 759 
                                DebugOut.StatusGreen |= AMPEL_BL; // status of BL Present
- 
 
 760 
                                if(FC.Error[0] || FC.Error[1] /* || FC.Error[2] || FC.Error[3] || FC.Error[4]*/) DebugOut.StatusRed |= AMPEL_FC;
- 
 
 761 
                                else DebugOut.StatusRed &= ~AMPEL_FC;
757 
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[8],0,255);
 762 
                                FC_I2C_ErrorConter = FromFlightCtrl.Param.Byte[8];
758 
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
 763 
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
759 
                                NaviData.RC_Quality = FC.RC_Quality;
 764 
                                NaviData.RC_Quality = FC.RC_Quality;
760 
                                NC_Wait_for_LED = FromFlightCtrl.Param.Byte[10];
 765 
                                NC_Wait_for_LED = FromFlightCtrl.Param.Byte[10];
761 
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
 766 
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
762 
                                if(LoggingGasCnt == 0) LoggingGasFilter = 0;
 767 
                                if(LoggingGasCnt == 0) LoggingGasFilter = 0;
763 
                                LoggingGasFilter += (u32) FromFlightCtrl.Param.Byte[11];
 768 
                                LoggingGasFilter += (u32) FromFlightCtrl.Param.Byte[11];
764 
                                LoggingGasCnt++;
 769 
                                LoggingGasCnt++;
765 
                                break;
 770 
                                break;
Line 766... Line -...
766 
 
 - 
 
767 
                        case SPI_FCCMD_SERVOS:
 - 
 
768 
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[0];
 - 
 
769 
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
 - 
 
770 
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[3];
 - 
 
771 
                                FC_I2C_ErrorConter = FromFlightCtrl.Param.Byte[4];
 - 
 
772 
                                FC.StatusFlags3 = FromFlightCtrl.Param.Byte[5];
 - 
 
773 
                                Parameter.DescendRange = FromFlightCtrl.Param.Byte[6];
 - 
 
774 
                                Parameter.MaximumAltitude = FromFlightCtrl.Param.Byte[7];
 - 
 
775 
                                FlugMinutenGesamt = FromFlightCtrl.Param.Int[4]; // 8 & 9
 - 
 
776 
                                Parameter.CamOrientation = FromFlightCtrl.Param.Byte[10];
 - 
 
777 
                                UART_VersionInfo.BL_Firmware = FromFlightCtrl.Param.Byte[11];
 - 
 
778 
                                break;
 - 
 
779 
 
 771 
 
780 
                        case SPI_FCCMD_VERSION:
 772 
                        case SPI_FCCMD_VERSION:  // slow!
781 
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
 773 
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
782 
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
 774 
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
783 
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
 775 
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
784 
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
 776 
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
- 
 
 777 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
- 
 
 778 
                                Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[5];
- 
 
 779 
                                Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[6];
- 
 
 780 
                                CHK_POTI_MM(Parameter.AutoPhotoAltitudes,FromFlightCtrl.Param.Byte[7],0,255);
- 
 
 781 
                                UART_VersionInfo.BL_Firmware = FromFlightCtrl.Param.Byte[8];
- 
 
 782 
// 9
- 
 
 783 
                                FlugMinutenGesamt = FromFlightCtrl.Param.Int[5]; // 10 & 11
- 
 
 784 
                                break;
- 
 
 785 
                        case SPI_FCCMD_NEUTRAL:  // slow!
785 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
 786 
                                FC.AdNeutralNick = FromFlightCtrl.Param.Int[0];
786 
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[5];
 787 
                                FC.AdNeutralRoll = FromFlightCtrl.Param.Int[1];
- 
 
 788 
                                FC.AdNeutralYaw = FromFlightCtrl.Param.Int[2];
- 
 
 789 
                                Parameter.Driftkomp = FromFlightCtrl.Param.Byte[6];    
- 
 
 790 
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[7];
- 
 
 791 
                                Parameter.ReceiverType = FromFlightCtrl.Param.Byte[8];
- 
 
 792 
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[9],0,255);
- 
 
 793 
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[10],0,255);
- 
 
 794 
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[11],0,255);
- 
 
 795 
                                break;
- 
 
 796 
                        case SPI_FCCMD_SLOW2:   // slow!
- 
 
 797 
                                FC.BoatNeutralNick = FromFlightCtrl.Param.Int[0];  // 0 & 1
- 
 
 798 
                                FC.BoatNeutralRoll = FromFlightCtrl.Param.Int[1];  // 2 & 3
- 
 
 799 
                                FC.BoatNeutralYaw = FromFlightCtrl.Param.Int[2];   // 4 & 5
787 
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[6];
 800 
                                Parameter.CamOrientation = FromFlightCtrl.Param.Byte[6];
788 
                                if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188)
 801 
                                if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188)
789 
                                {
 802 
                                {
790 
                                        FC.FromFC_DisableDeclination = 1;
 803 
                                        FC.FromFC_DisableDeclination = 1;
791 
                                        FC.FromFC_CompassOffset = 10 * (s8) (FromFlightCtrl.Param.Byte[7] - 128);
 804 
                                        FC.FromFC_CompassOffset = 10 * (s8) (FromFlightCtrl.Param.Byte[7] - 128);
Line 798... Line 811...
798 
                                }
 811 
                                }
799 
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
 812 
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
800 
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
 813 
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
801 
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
 814 
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
802 
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
 815 
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
803 
                                DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present
 - 
 
804 
                                DebugOut.StatusGreen |= AMPEL_BL; // status of BL Present
 - 
 
805 
                                if(FC.Error[0] || FC.Error[1] /* || FC.Error[2] || FC.Error[3] || FC.Error[4]*/) DebugOut.StatusRed |= AMPEL_FC;
 - 
 
806 
                                else DebugOut.StatusRed &= ~AMPEL_FC;
 - 
 
807 
                                break;
 816 
                                break;
808 
                        case SPI_FCCMD_NEUTRAL:
 817 
                        case SPI_FCCMD_SLOW3:  // slow!
809 
                                FC.AdNeutralNick = FromFlightCtrl.Param.Int[0];
 818 
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[0];
810 
                                FC.AdNeutralRoll = FromFlightCtrl.Param.Int[1];
 819 
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[1];
811 
                                FC.AdNeutralYaw = FromFlightCtrl.Param.Int[2];
 - 
 
812 
                                Parameter.Driftkomp = FromFlightCtrl.Param.Byte[6];    
 820 
                                Parameter.DescendRange = FromFlightCtrl.Param.Byte[2];
813 
                                HoverGas = FromFlightCtrl.Param.Byte[7];
 821 
                                Parameter.MaximumAltitude = FromFlightCtrl.Param.Byte[3];
- 
 
 822 
// 4
- 
 
 823 
// 5
- 
 
 824 
// 6
- 
 
 825 
// 7
- 
 
 826 
// 8
- 
 
 827 
// 9
- 
 
 828 
// 10
- 
 
 829 
// 11
814 
                                break;
 830 
                                break;
- 
 
 831 
 
815 
                        default:
 832 
                        default:
816 
                                break;
 833 
                                break;
817 
                }
 834 
                }
818 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
 835 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
819 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
 836 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
Line 874... Line 891...
874 
                FCCalibActive = 1;
 891 
                FCCalibActive = 1;
875 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
 892 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
876 
        // if we got it
 893 
        // if we got it
877 
        if (FC_Version.Major != 0xFF)
 894 
        if (FC_Version.Major != 0xFF)
878 
        {
 895 
        {
879 
                sprintf(msg, " FC V%d.%02d%c HW:%d.%02d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
 896 
                sprintf(msg, " FC V%d.%02d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
880 
                UART1_PutString(msg);
 897 
                UART1_PutString(msg);
881 
        }
 898 
        }
882 
        else UART1_PutString("\n\r not found!");
 899 
        else UART1_PutString("\n\r not found!");
883 
}
 900 
}