mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-20 06:45:16 +03:00
Added GIMBAL_MIXTILT (16) to gimbal_flags. Setting gimbal_flags=16 will use "mixed" gimbal i.e. SERVO_MIX_TILT from MultiWii. Untested, but should work(tm).
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@239 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
e7bc3cc38e
commit
006e6629f6
3 changed files with 2864 additions and 2854 deletions
|
@ -346,8 +346,13 @@ void mixTable(void)
|
|||
servo[1] = cfg.gimbal_roll_mid + aux[1];
|
||||
|
||||
if (rcOptions[BOXCAMSTAB]) {
|
||||
servo[0] += cfg.gimbal_pitch_gain * angle[PITCH] / 16;
|
||||
servo[1] += cfg.gimbal_roll_gain * angle[ROLL] / 16;
|
||||
if (cfg.gimbal_flags & GIMBAL_MIXTILT) {
|
||||
servo[0] = (-cfg.gimbal_roll_gain) * angle[PITCH] / 16 - cfg.gimbal_roll_gain * angle[ROLL] / 16;
|
||||
servo[1] = (-cfg.gimbal_roll_gain) * angle[PITCH] / 16 - cfg.gimbal_roll_gain * angle[ROLL] / 16;
|
||||
} else {
|
||||
servo[0] += cfg.gimbal_pitch_gain * angle[PITCH] / 16;
|
||||
servo[1] += cfg.gimbal_roll_gain * angle[ROLL] / 16;
|
||||
}
|
||||
}
|
||||
|
||||
servo[0] = constrain(servo[0], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue