aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Allow mount control when no AHRS exists

Browse Source 
Allow mount control on non stabilized axes
This commit is contained in:
Amilcar Lucas 2012-07-10 23:55:59 +02:00
parent a643d2aa6b
commit 74ef712cd1
1 changed files with 7 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Mount/AP_Mount.cpp
View File

@ -148,16 +148,7 @@ void AP_Mount::update_mount_position()
G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle*100); G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle*100);
G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle*100); G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle*100);
G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle*100); G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle*100);
if (_ahrs){ stabilize();
stabilize();
} else {
if (g_rc_function[RC_Channel_aux::k_mount_roll])
_roll_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]);
if (g_rc_function[RC_Channel_aux::k_mount_pitch])
_pitch_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_pitch]);
if (g_rc_function[RC_Channel_aux::k_mount_yaw])
_yaw_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_yaw]);
}
break; break;
} }
@ -351,8 +342,8 @@ AP_Mount::stabilize()
cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle); cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle);
gimbal_target = m * cam; gimbal_target = m * cam;
gimbal_target.to_euler(&_roll_angle, &_pitch_angle, &_yaw_angle); gimbal_target.to_euler(&_roll_angle, &_pitch_angle, &_yaw_angle);
_roll_angle = degrees(_roll_angle); _roll_angle = _stab_roll?degrees(_roll_angle):degrees(_roll_control_angle);
_pitch_angle = degrees(_pitch_angle); _pitch_angle = _stab_pitch?degrees(_pitch_angle):degrees(_pitch_control_angle);
_yaw_angle = degrees(_yaw_angle); _yaw_angle = degrees(_yaw_angle);
} else { } else {
// otherwise base mount roll and pitch on the ahrs // otherwise base mount roll and pitch on the ahrs
@ -367,6 +358,10 @@ AP_Mount::stabilize()
_pitch_angle -= degrees(_ahrs->pitch); _pitch_angle -= degrees(_ahrs->pitch);
} }
} }
} else {
_roll_angle = degrees(_roll_control_angle);
_pitch_angle = degrees(_pitch_control_angle);
_yaw_angle = degrees(_yaw_control_angle);
} }
} }