| 103,6 → 103,8 |
|---|
| s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
| s32 HeadFreeStartAngle = 0; |
| s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde |
| u32 ToFC_AltitudeSpeed = 0; |
| s32 ToFC_AltitudeSetpoint = 0; |
| |
| SPI_Version_t FC_Version; |
| |
| 280,9 → 282,9 |
|---|
| ToFlightCtrl.CompassHeading = Compass_Heading; |
| DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
| if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
| ToFlightCtrl.MagVecX = MagVector.X; |
| ToFlightCtrl.MagVecY = MagVector.Y; |
| ToFlightCtrl.MagVecZ = MagVector.Z; |
| // ToFlightCtrl.MagVecX = MagVector.X; |
| // ToFlightCtrl.MagVecY = MagVector.Y; |
| // ToFlightCtrl.MagVecZ = MagVector.Z; |
| ToFlightCtrl.NCStatus = 0; |
| // cycle spi commands |
| ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| 300,6 → 302,7 |
|---|
| ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
| ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
| ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
| //ToFlightCtrl.Param.Byte[7] = |
| if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH) |
| { |
| ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth; |
| 331,9 → 334,17 |
|---|
| ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5 |
| ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7 |
| ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110); |
| ToFlightCtrl.Param.Byte[9] = (u8) ToFC_AltitudeSpeed; |
| ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint; |
| break; |
| default: |
| break; |
| // 0 = 0,1 |
| // 1 = 2,3 |
| // 2 = 4,5 |
| // 3 = 6,7 |
| // 4 = 8,9 |
| // 5 = 10,11 |
| } |
| VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
| |