Rev 462 | Rev 471 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 462 | Rev 463 | ||
---|---|---|---|
Line 121... | Line 121... | ||
121 | u8 BL_MinOfMaxPWM; // indication if all BL-controllers run on full power |
121 | u8 BL_MinOfMaxPWM; // indication if all BL-controllers run on full power |
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 |
|
Line 126... | Line 127... | ||
126 | 127 | ||
127 | //-------------------------------------------------------------- |
128 | //-------------------------------------------------------------- |
128 | void SSP0_IRQHandler(void) |
129 | void SSP0_IRQHandler(void) |
129 | { |
130 | { |
Line 304... | Line 305... | ||
304 | { |
305 | { |
305 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
306 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
306 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
307 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
307 | ToFlightCtrl.CompassHeading = Compass_Heading; |
308 | ToFlightCtrl.CompassHeading = Compass_Heading; |
308 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
309 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
- | 310 | GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading - GeoMagDec) % 3600; |
|
309 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
311 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
310 | ToFlightCtrl.MagVecX = MagVector.X; |
312 | ToFlightCtrl.MagVecX = MagVector.X; |
311 | ToFlightCtrl.MagVecY = MagVector.Y; |
313 | ToFlightCtrl.MagVecY = MagVector.Y; |
312 | ToFlightCtrl.MagVecZ = MagVector.Z; |
314 | ToFlightCtrl.MagVecZ = MagVector.Z; |
313 | // ToFlightCtrl.NCStatus = 0; |
315 | // ToFlightCtrl.NCStatus = 0; |
Line 453... | Line 455... | ||
453 | //53.28 5788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
455 | //53.28 5788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
454 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
456 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
455 | ToFlightCtrl.Param.Byte[0] = 3+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden |
457 | ToFlightCtrl.Param.Byte[0] = 3+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden |
456 | ToFlightCtrl.Param.Byte[1] = 9-1; // how many |
458 | ToFlightCtrl.Param.Byte[1] = 9-1; // how many |
457 | //----------------------------- |
459 | //----------------------------- |
458 | ToFlightCtrl.Param.Byte[2] = FromFlightCtrl.GyroHeading / 20;//NaviData.HomePositionDeviation.Bearing / 2; |
460 | ToFlightCtrl.Param.Byte[2] = GyroCompassCorrected / 20;//NaviData.HomePositionDeviation.Bearing / 2; |
459 | i1 = GPSData.Speed_Ground; // in cm/sec |
461 | i1 = GPSData.Speed_Ground; // in cm/sec |
460 | i1 *= 36; |
462 | i1 *= 36; |
461 | i1 /= 1000; |
463 | i1 /= 1000; |
462 | ToFlightCtrl.Param.Byte[3] = i1 % 256; |
464 | ToFlightCtrl.Param.Byte[3] = i1 % 256; |
463 | ToFlightCtrl.Param.Byte[4] = i1 / 256; |
465 | ToFlightCtrl.Param.Byte[4] = i1 / 256; |
Line 573... | Line 575... | ||
573 | ClearFCStatusFlags = 0; |
575 | ClearFCStatusFlags = 0; |
574 | } |
576 | } |
575 | FC.StatusFlags |= FromFlightCtrl.Param.Byte[8]; |
577 | FC.StatusFlags |= FromFlightCtrl.Param.Byte[8]; |
576 | if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive) |
578 | if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive) |
577 | { |
579 | { |
578 | HeadFreeStartAngle = Compass_Heading * 10; |
580 | HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; |
579 | Compass_Init(); |
581 | Compass_Init(); |
580 | FCCalibActive = 10; |
582 | FCCalibActive = 10; |
581 | FC_is_Calibrated = 0; |
583 | FC_is_Calibrated = 0; |
582 | } |
584 | } |
583 | else |
585 | else |
584 | { |
586 | { |
585 | if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1; |
587 | if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1; |
586 | } |
588 | } |
587 | if(FC.StatusFlags & FC_STATUS_START) |
589 | if(FC.StatusFlags & FC_STATUS_START) |
588 | { |
590 | { |
589 | if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else |
591 | if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
590 | HeadFreeStartAngle = FromFlightCtrl.GyroHeading; |
592 | HeadFreeStartAngle = GyroCompassCorrected; |
591 | } |
593 | } |
Line 592... | Line 594... | ||
592 | 594 | ||
593 | if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
595 | if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
594 | { |
596 | { |
Line 597... | Line 599... | ||
597 | if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK)) // nur bei ausreichender Distance -> 20m |
599 | if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK)) // nur bei ausreichender Distance -> 20m |
598 | { |
600 | { |
599 | HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 - Parameter.OrientationAngle * 150) % 3600; // in 0.1° |
601 | HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 - Parameter.OrientationAngle * 150) % 3600; // in 0.1° |
600 | } |
602 | } |
601 | else // Ansonsten die aktuelle Richtung übernehmen |
603 | else // Ansonsten die aktuelle Richtung übernehmen |
602 | HeadFreeStartAngle = (3600 + FromFlightCtrl.GyroHeading /*+ Parameter.OrientationAngle * 150*/) % 3600; // in 0.1° |
604 | HeadFreeStartAngle = GyroCompassCorrected; // in 0.1° |
603 | } |
605 | } |
604 | } |
606 | } |
Line 605... | Line 607... | ||
605 | 607 | ||
606 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
608 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
Line 736... | Line 738... | ||
736 | else DebugOut.StatusRed &= ~AMPEL_FC; |
738 | else DebugOut.StatusRed &= ~AMPEL_FC; |
737 | break; |
739 | break; |
738 | default: |
740 | default: |
739 | break; |
741 | break; |
740 | } |
742 | } |
741 | - | ||
742 | DebugOut.Analog[0] = FromFlightCtrl.AngleNick; |
743 | DebugOut.Analog[0] = FromFlightCtrl.AngleNick; |
743 | DebugOut.Analog[1] = FromFlightCtrl.AngleRoll; |
744 | DebugOut.Analog[1] = FromFlightCtrl.AngleRoll; |
744 | DebugOut.Analog[2] = FromFlightCtrl.AccNick; |
745 | DebugOut.Analog[2] = FromFlightCtrl.AccNick; |
745 | DebugOut.Analog[3] = FromFlightCtrl.AccRoll; |
746 | DebugOut.Analog[3] = FromFlightCtrl.AccRoll; |
746 | DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg |
747 | DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg |