Rev 906 | Rev 916 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 906 | Rev 908 | ||
|---|---|---|---|
| Line 212... | Line 212... | ||
| 212 | s32 min_n, max_n, min_r, max_r; |
212 | s32 min_n, max_n, min_r, max_r; |
| 213 | // initiate transmission |
213 | // initiate transmission |
| 214 | tmp = (s16) (FromFC_ServoRollControl - 128) * 4; |
214 | tmp = (s16) (FromFC_ServoRollControl - 128) * 4; |
| 215 | if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) ToGimbalCtrl.Roll = tmp; else ToGimbalCtrl.Roll = -tmp; |
215 | if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) ToGimbalCtrl.Roll = tmp; else ToGimbalCtrl.Roll = -tmp; |
| 216 | // ToGimbalCtrl.Yaw = (s32) SerialChannel.Ch[0] * 3; |
216 | // ToGimbalCtrl.Yaw = (s32) SerialChannel.Ch[0] * 3; |
| 217 | ToGimbalCtrl.Yaw = (s32) (PPM_In[EE_Parameter.GimbalYawChannel]) * -1; // war bis 1.21* -2 -> das Gimbal nimmt aber seltsamerweise manchmal nur +-200 an |
217 | ToGimbalCtrl.Yaw = (s32) (PPM_In[EE_Parameter.GimbalYawChannel]) * -1; // war bis 1.21 * -2 -> das Gimbal nimmt aber seltsamerweise manchmal nur +-200 an |
| Line 218... | Line 218... | ||
| 218 | 218 | ||
| 219 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
219 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
| 220 | // digital switching outputs |
220 | // digital switching outputs |
| 221 | if(PPM_In[EE_Parameter.GimbalOut1Channel] > 10) ToGimbalCtrl.BitCmd |= GIMBAL_CMD_OUT1; |
221 | if(PPM_In[EE_Parameter.GimbalOut1Channel] > 10) ToGimbalCtrl.BitCmd |= GIMBAL_CMD_OUT1; |
| 222 | if(PPM_In[EE_Parameter.GimbalOut2Channel] > 10) ToGimbalCtrl.BitCmd |= GIMBAL_CMD_OUT2; |
222 | if(PPM_In[EE_Parameter.GimbalOut2Channel] > 10) ToGimbalCtrl.BitCmd |= GIMBAL_CMD_OUT2; |
| 223 | // flags will be reset after transmitting |
223 | // flags will be reset after transmitting |
| Line 224... | Line -... | ||
| 224 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 225 | 224 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 226 | if(FromMenuGimbalYaw) ToGimbalCtrl.Yaw = FromMenuGimbalYaw; |
225 | |
| 227 | ToGimbalCtrl.NRY_Speed = 100; |
226 | ToGimbalCtrl.NRY_Speed = 100; |
| 228 | ToGimbalCtrl.Filter = 5; |
227 | ToGimbalCtrl.Filter = 5; |
| 229 | ToGimbalCtrl.Nick = NickGimbal; |
228 | ToGimbalCtrl.Nick = NickGimbal; |
| Line 233... | Line 232... | ||
| 233 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
232 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
| 234 | if(FC.RC_Quality < 50) // Failsafe-Positions |
233 | if(FC.RC_Quality < 50) // Failsafe-Positions |
| 235 | { |
234 | { |
| 236 | if(EE_Parameter.ServoFS_Pos[0]) (s32)((u32)EE_Parameter.ServoFS_Pos[0] * 10) - 1280; //else ToGimbalCtrl.Nick = 0; |
235 | if(EE_Parameter.ServoFS_Pos[0]) (s32)((u32)EE_Parameter.ServoFS_Pos[0] * 10) - 1280; //else ToGimbalCtrl.Nick = 0; |
| 237 | if(EE_Parameter.ServoFS_Pos[1]) (s32)((u32)EE_Parameter.ServoFS_Pos[1] * 10) - 1280; //else ToGimbalCtrl.Roll = 0; |
236 | if(EE_Parameter.ServoFS_Pos[1]) (s32)((u32)EE_Parameter.ServoFS_Pos[1] * 10) - 1280; //else ToGimbalCtrl.Roll = 0; |
| - | 237 | ToGimbalCtrl.Yaw = 0; |
|
| 238 | } |
238 | } |
| - | 239 | if(FromMenuGimbalYaw) ToGimbalCtrl.Yaw = FromMenuGimbalYaw; |
|
| Line 239... | Line 240... | ||
| 239 | 240 | ||
| 240 | min_n = (s32)((u32)EE_Parameter.ServoNickMin * 10) - 1280; |
241 | min_n = (s32)((u32)EE_Parameter.ServoNickMin * 10) - 1280; |
| 241 | max_n = (s32)((u32)EE_Parameter.ServoNickMax * 10) - 1280; |
242 | max_n = (s32)((u32)EE_Parameter.ServoNickMax * 10) - 1280; |
| 242 | if(ToGimbalCtrl.Nick > max_n) ToGimbalCtrl.Nick = max_n; |
243 | if(ToGimbalCtrl.Nick > max_n) ToGimbalCtrl.Nick = max_n; |