19,6 → 19,7 |
FromGimbalCtrl_t FromGimbalCtrl; |
ToGimbalCtrl_t ToGimbalCtrl; |
|
s32 FromMenuGimbalYaw = 0,FromMenuServoNickControl = 0, MenuNickGimbalOffset = 0; |
u16 CamCtrlTimeout = 25000; |
u16 LaserCtrlTimeout = 5000; |
u16 GimbalCtrlTimeout = 8000; |
189,8 → 190,8 |
tmp = ((signed int)NickServoValue - 8192) / 7; |
if(EE_Parameter.ServoCompInvert & SERVO_NICK_INV) NickGimbal = -tmp; else NickGimbal = tmp; |
NickGimbal += CAM_Orientation.Elevation; // Add Poi Nick |
NickGimbal += MenuNickGimbalOffset / 50; |
|
|
if((old_nick != NickGimbal) && (timing < GIMBAL_DATARATE/2)) timing = 0; // Schneller, wenn sich die Daten ändern |
|
if(timing) |
210,6 → 211,7 |
if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) ToGimbalCtrl.Roll = -tmp; else ToGimbalCtrl.Roll = tmp; |
// ToGimbalCtrl.Yaw = (s32) SerialChannel.Ch[0] * 3; |
ToGimbalCtrl.Yaw = (s32) (PPM_In[EE_Parameter.GimbalYawChannel]) * 2; |
if(FromMenuGimbalYaw) ToGimbalCtrl.Yaw = FromMenuGimbalYaw; |
ToGimbalCtrl.NRY_Speed = 100; |
ToGimbalCtrl.Filter = 5; |
ToGimbalCtrl.Nick = NickGimbal; |
216,7 → 218,7 |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
// + limit angles (0 = -128° 255 = +127°) |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
if(FC.RC_Quality < 150) // Failsafe-Positions |
if(FC.RC_Quality < 50) // Failsafe-Positions |
{ |
if(EE_Parameter.ServoFS_Pos[0]) ToGimbalCtrl.Nick = (s32)((u32)EE_Parameter.ServoFS_Pos[0] * 10) - 1280; |
if(EE_Parameter.ServoFS_Pos[1]) ToGimbalCtrl.Roll = (s32)((u32)EE_Parameter.ServoFS_Pos[1] * 10) - 1280; |
262,6 → 264,8 |
LIMIT_MIN_MAX(NickServoValue,min, max); |
} |
else NickServoValue = (int16_t)FromFC_ServoNickControl * (MULTIPLYER*16); // direct poti control |
MenuNickGimbalOffset += FromMenuServoNickControl; |
LIMIT_MIN_MAX(MenuNickGimbalOffset,-50000,50000); |
} |
|
|