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 | } |