109,7 → 109,6 |
s32 Kalman_MaxDrift = 5 * 16; |
s32 Kalman_MaxFusion = 64; |
s32 Kalman_Kompass = 32; |
s32 ToFcGpsZ = 0; |
u8 CompassCalState = 0; |
u8 RequestConfigFromFC = 0; |
u8 SendOemName = 0; |
156,8 → 155,8 |
u8 CamCtrlCharacter =' '; |
|
s16 NickServoValue; |
s16 FromFC_ServoRollControl; |
s16 FromFC_ServoNickControl; |
s16 FromFC_ServoRollControl = 128; |
s16 FromFC_ServoNickControl = 128; |
|
//-------------------------------------------------------------- |
void SSP0_IRQHandler(void) |
453,6 → 452,11 |
// if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.OrientationAngle * 15)) % 360; // the FC uses the uncorrected comnpass value |
if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.CamOrientation * 15)) % 360; // the FC uses the uncorrected comnpass value |
else CAM_Orientation.Azimuth = -1; |
if(NC_GPS_ModeCharacter == 'W' && WP_Yaw_WithoutCF) CAM_Orientation.UpdateMask |= FORCE_AZIMUTH_ROTATION; // allows Yawing without CareFree (Yawing at Waypoint flight) |
// ++++++++++++++++++++++++++++++++++++ |
// + update direction for simulation |
if((CAM_Orientation.UpdateMask & FORCE_AZIMUTH_ROTATION) || ((CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH) && (NaviData.FCStatusFlags2 & FC_STATUS2_CAREFREE))) SimulatedDirection = CAM_Orientation.Azimuth; |
// ++++++++++++++++++++++++++++++++++++ |
if(CAM_Orientation.UpdateMask & FORCE_AZIMUTH_ROTATION) ToFlightCtrl.Param.Byte[4] |= KM_BIT_YAW; // allows Yawing without CareFree (Yawing at Coming Home) |
CAM_Orientation.UpdateMask &= ~(CAM_UPDATE_AZIMUTH | FORCE_AZIMUTH_ROTATION); |
} |
517,6 → 521,8 |
ToFlightCtrl.Param.Byte[15] = Partner.StatusFlags2; |
ToFlightCtrl.Param.Byte[16] = Partner.StatusFlags3; |
ToFlightCtrl.Param.Byte[17] = ToFcBaudrateIndex; |
ToFlightCtrl.Param.Byte[18] = NaviData.WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1 |
ToFlightCtrl.Param.Byte[19] = NaviData.WaypointNumber; // number of stored waypoints |
// if(AbsoluteFlyingAltitude > 255) ToFlightCtrl.Param.Byte[11] = 0; // then the limitation of the FC doesn't work |
// else ToFlightCtrl.Param.Byte[11] = AbsoluteFlyingAltitude; |
break; |
524,9 → 530,9 |
ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5; |
ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination; |
ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic; |
ToFlightCtrl.Param.Byte[3] = 0; |
ToFlightCtrl.Param.Byte[4] = NaviData.WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1 |
ToFlightCtrl.Param.Byte[5] = NaviData.WaypointNumber; // number of stored waypoints |
//ToFlightCtrl.Param.Byte[3] |
//ToFlightCtrl.Param.Byte[4] |
//ToFlightCtrl.Param.Byte[5] |
ToFlightCtrl.Param.Int[3] = NaviData.TargetPositionDeviation.Distance_dm / 10; |
ToFlightCtrl.Param.Byte[8] = NaviData.TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached |
ToFlightCtrl.Param.Byte[9] = ToFC_MaxWpListIndex; |
915,9 → 921,16 |
Parameter.User7 = FromFlightCtrl.Param.Byte[18]; |
Parameter.User8 = FromFlightCtrl.Param.Byte[19]; |
|
FromFC_ServoRollControl = FromFlightCtrl.Param.Byte[20]; |
FromFC_ServoNickControl = FromFlightCtrl.Param.Byte[21]; |
|
if((FC.RC_Quality > 100) || !(FC.StatusFlags3 & FC_STATUS3_NOT_CALIBRATED)) |
{ |
FromFC_ServoRollControl = FromFlightCtrl.Param.Byte[20]; |
FromFC_ServoNickControl = FromFlightCtrl.Param.Byte[21]; |
} |
else |
{ |
FromFC_ServoRollControl = 128; // Mittelstellung |
FromFC_ServoNickControl = 128; // Mittelstellung |
} |
break; |
case SPI_FCCMD_STICK2: |
FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
1005,8 → 1018,8 |
Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11]; |
break; |
case SPI_FCCMD_SLOW3: // slow! |
ServoParams.NickControl = FromFlightCtrl.Param.Byte[0]; |
ServoParams.RollControl = FromFlightCtrl.Param.Byte[1]; |
// ServoParams.NickControl = FromFlightCtrl.Param.Byte[0]; |
// ServoParams.RollControl = FromFlightCtrl.Param.Byte[1]; |
Parameter.DescendRange = FromFlightCtrl.Param.Byte[2]; |
Parameter.MaximumAltitude = FromFlightCtrl.Param.Byte[3]; |
ServoParams.CompInvert = FromFlightCtrl.Param.Byte[4]; |