124,6 → 124,8 |
s16 POI_KameraNick = 0; |
u8 NC_Wait_for_LED = 0; |
s16 GyroCompassCorrected = 0; // corrected with the magnetic declination |
s16 CompassSetpointCorrected = 0; // The compass setpoint that the FC tries to keep - corrected with the magnetic declination |
s16 CompassSetpoint = 0; // in 0,1° |
|
//-------------------------------------------------------------- |
void SSP0_IRQHandler(void) |
307,7 → 309,8 |
VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
ToFlightCtrl.CompassHeading = Compass_Heading; |
DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading + FC.FromFC_CompassOffset - GeoMagDec) % 3600; |
//GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading + FC.FromFC_CompassOffset - GeoMagDec) % 3600; |
GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading + FC.FromFC_CompassOffset + GeoMagDec) % 3600; |
if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
ToFlightCtrl.MagVecX = MagVector.X; |
ToFlightCtrl.MagVecY = MagVector.Y; |
594,7 → 597,8 |
FC.StatusFlags |= FromFlightCtrl.Param.Byte[8]; |
if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive) |
{ |
HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; |
// HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; |
HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600; |
Compass_Init(); |
FCCalibActive = 10; |
FC_is_Calibrated = 0; |
612,8 → 616,9 |
} |
if(FC.StatusFlags & FC_STATUS_START) |
{ |
if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
HeadFreeStartAngle = GyroCompassCorrected; |
// if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600; |
else HeadFreeStartAngle = GyroCompassCorrected; |
} |
|
if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
693,6 → 698,10 |
if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5]; |
if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SingePoint = FromFlightCtrl.Param.Byte[6]; |
if(FromFlightCtrl.Param.Byte[7]) FromFC_Store_SingePoint = FromFlightCtrl.Param.Byte[7]; |
CompassSetpoint = FromFlightCtrl.Param.sInt[4] * 10; // 8 & 9 |
CompassSetpointCorrected = (3600 + CompassSetpoint + FC.FromFC_CompassOffset + GeoMagDec) % 3600; |
//FromFlightCtrl.Param.Byte[10] |
//FromFlightCtrl.Param.Byte[11] |
break; |
case SPI_FCCMD_STICK: |
FC.StickGas = FromFlightCtrl.Param.sByte[0]; |