Rev 480 | Rev 483 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 480 | Rev 482 | ||
---|---|---|---|
Line 54... | Line 54... | ||
54 | // + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de. |
54 | // + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de. |
55 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
55 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 56... | Line 56... | ||
56 | 56 | ||
57 | #include <string.h> |
57 | #include <string.h> |
- | 58 | #include <math.h> |
|
58 | #include <math.h> |
59 | #include <stdlib.h> |
59 | #include "91x_lib.h" |
60 | #include "91x_lib.h" |
60 | #include "led.h" |
61 | #include "led.h" |
61 | #include "gps.h" |
62 | #include "gps.h" |
62 | #include "uart1.h" |
63 | #include "uart1.h" |
63 | #include "spi_slave.h" |
64 | #include "spi_slave.h" |
64 | #include "compass.h" |
65 | #include "compass.h" |
65 | #include "timer1.h" |
66 | #include "timer1.h" |
66 | #include "timer2.h" |
67 | #include "timer2.h" |
67 | #include "config.h" |
68 | #include "config.h" |
68 | #include "main.h" |
- | |
69 | #include "compass.h" |
69 | #include "main.h" |
70 | #include "params.h" |
- | |
71 | #include "stdlib.h" |
70 | #include "params.h" |
72 | #include "settings.h" |
- | |
Line 73... | Line 71... | ||
73 | #include "ncmag.h" |
71 | #include "settings.h" |
74 | 72 | ||
75 | #define SPI_RXSYNCBYTE1 0xAA |
73 | #define SPI_RXSYNCBYTE1 0xAA |
76 | #define SPI_RXSYNCBYTE2 0x83 |
74 | #define SPI_RXSYNCBYTE2 0x83 |
Line 584... | Line 582... | ||
584 | FCCalibActive = 10; |
582 | FCCalibActive = 10; |
585 | FC_is_Calibrated = 0; |
583 | FC_is_Calibrated = 0; |
586 | } |
584 | } |
587 | else |
585 | else |
588 | { |
586 | { |
589 | if(FCCalibActive) |
587 | if(FCCalibActive) |
590 | { |
588 | { |
591 | if(--FCCalibActive == 0) |
589 | if(--FCCalibActive == 0) |
592 | { |
590 | { |
593 | FC_is_Calibrated = 1; |
591 | FC_is_Calibrated = 1; |
594 | ExtCompassOrientation = GetExtCompassOrientation(); |
592 | Compass_Check(); |
595 | if(ExtCompassOrientation != Calibration.Version / 16) NCMAG_IsCalibrated = 0; |
- | |
596 | } |
593 | } |
597 | } |
594 | } |
598 | } |
595 | } |
599 | if(FC.StatusFlags & FC_STATUS_START) |
596 | if(FC.StatusFlags & FC_STATUS_START) |
600 | { |
597 | { |
601 | if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
598 | if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
602 | HeadFreeStartAngle = GyroCompassCorrected; |
599 | HeadFreeStartAngle = GyroCompassCorrected; |
603 | } |
600 | } |
Line 604... | Line 601... | ||
604 | 601 | ||
605 | if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
602 | if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
606 | { |
603 | { |
607 | if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen |
604 | if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen |
608 | { |
605 | { |
609 | if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK)) // nur bei ausreichender Distance -> 20m |
606 | if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK)) // nur bei ausreichender Distance -> 20m |
610 | { |
607 | { |
611 | HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 - Parameter.OrientationAngle * 150) % 3600; // in 0.1° |
608 | HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 - Parameter.OrientationAngle * 150) % 3600; // in 0.1° |
612 | } |
609 | } |
613 | else // Ansonsten die aktuelle Richtung übernehmen |
610 | else // Ansonsten die aktuelle Richtung übernehmen |
614 | HeadFreeStartAngle = GyroCompassCorrected; // in 0.1° |
611 | HeadFreeStartAngle = GyroCompassCorrected; // in 0.1° |
615 | } |
612 | } |
- | 613 | } |
|
616 | } |
614 | |
617 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
615 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
618 | FC.BAT_Voltage = FromFlightCtrl.Param.Byte[10]; |
616 | FC.BAT_Voltage = FromFlightCtrl.Param.Byte[10]; |
619 | DebugOut.Analog[7] = FC.BAT_Voltage; |
617 | DebugOut.Analog[7] = FC.BAT_Voltage; |
620 | DebugOut.Analog[5] = FC.StatusFlags; |
618 | DebugOut.Analog[5] = FC.StatusFlags; |
Line 670... | Line 668... | ||
670 | break; |
668 | break; |
671 | case SPI_FCCMD_PARAMETER2: |
669 | case SPI_FCCMD_PARAMETER2: |
672 | CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255); |
670 | CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255); |
673 | if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1]; // will be cleared in the SD-Logging |
671 | if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1]; // will be cleared in the SD-Logging |
674 | Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2]; |
672 | Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2]; |
675 | Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3]; |
673 | Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3]; |
676 | break; |
674 | break; |
677 | case SPI_FCCMD_STICK: |
675 | case SPI_FCCMD_STICK: |
678 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
676 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
679 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
677 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
680 | FC.StickRoll = FromFlightCtrl.Param.sByte[2]; |
678 | FC.StickRoll = FromFlightCtrl.Param.sByte[2]; |
Line 739... | Line 737... | ||
739 | FC_Version.Hardware = FromFlightCtrl.Param.Byte[4]; |
737 | FC_Version.Hardware = FromFlightCtrl.Param.Byte[4]; |
740 | FC.Error[0] |= FromFlightCtrl.Param.Byte[5]; |
738 | FC.Error[0] |= FromFlightCtrl.Param.Byte[5]; |
741 | FC.Error[1] |= FromFlightCtrl.Param.Byte[6]; |
739 | FC.Error[1] |= FromFlightCtrl.Param.Byte[6]; |
742 | if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188) |
740 | if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188) |
743 | { |
741 | { |
744 | FC.FromFC_DisableDeclination = 1; |
742 | FC.FromFC_DisableDeclination = 1; |
745 | FC.FromFC_CompassOffset = 10 * (signed char) ((unsigned char) FromFlightCtrl.Param.Byte[7] - 128); |
743 | FC.FromFC_CompassOffset = 10 * (s8) (FromFlightCtrl.Param.Byte[7] - 128); |
746 | GeoMagDec = 0; |
744 | GeoMagDec = 0; |
747 | } |
745 | } |
748 | else |
746 | else |
749 | { |
747 | { |
750 | FC.FromFC_DisableDeclination = 0; |
748 | FC.FromFC_DisableDeclination = 0; |
751 | FC.FromFC_CompassOffset = 10 * (signed char) FromFlightCtrl.Param.Byte[7]; |
749 | FC.FromFC_CompassOffset = 10 * FromFlightCtrl.Param.sByte[7]; |
752 | } |
750 | } |
753 | Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8]; |
751 | Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8]; |
754 | Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9]; |
752 | Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9]; |
755 | Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10]; |
753 | Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10]; |
756 | Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11]; |
754 | Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11]; |
Line 816... | Line 814... | ||
816 | SPI0_UpdateBuffer(); |
814 | SPI0_UpdateBuffer(); |
817 | if (FC_Version.Major != 0xFF) break; |
815 | if (FC_Version.Major != 0xFF) break; |
818 | }while (!CheckDelay(timeout)); |
816 | }while (!CheckDelay(timeout)); |
819 | UART1_PutString("."); |
817 | UART1_PutString("."); |
820 | repeat++; |
818 | repeat++; |
821 | // FCCalibActive = 1; |
819 | FCCalibActive = 1; |
822 | }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s |
820 | }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s |
823 | // if we got it |
821 | // if we got it |
824 | if (FC_Version.Major != 0xFF) |
822 | if (FC_Version.Major != 0xFF) |
825 | { |
823 | { |
826 | sprintf(msg, " FC V%d.%d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10); |
824 | sprintf(msg, " FC V%d.%d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10); |
827 | UART1_PutString(msg); |
825 | UART1_PutString(msg); |
828 | } |
826 | } |
829 | else UART1_PutString("\n\r not found!"); |
827 | else UART1_PutString("\n\r not found!"); |
830 | } |
828 | } |
Line -... | Line 829... | ||
- | 829 | ||
- | 830 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 831 | // + extended Current measurement -> 200 = 20A 201 = 21A 255 = 75A (20+55) |
|
- | 832 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 833 | u16 BL3_Current(u8 who) // in 0,1A |
|
- | 834 | { |
|
- | 835 | if(Motor[who].Current <= 200) return((u16) Motor[who].Current); |
|
- | 836 | else |
|
- | 837 | { |
|
- | 838 | if(Motor_Version[who] & MOTOR_STATE_BL30) return(200 + 10 * (u16) (Motor[who].Current - 200)); |
|
- | 839 | else return((u16) Motor[who].Current); |
|
- | 840 | } |