Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Mount_Servo: use reference to state

This commit is contained in:
Randy Mackay 2015-01-19 10:40:27 +09:00 committed by Andrew Tridgell
parent acbcf3c54e
commit 32ea258594
2 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -40,14 +40,14 @@ void AP_Mount_Servo::update()
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT:
{
_angle_bf_output_deg = _frontend.state[_instance]._retract_angles.get();
_angle_bf_output_deg = _state._retract_angles.get();
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
_angle_bf_output_deg = _frontend.state[_instance]._neutral_angles.get();
_angle_bf_output_deg = _state._neutral_angles.get();
break;
}
@ -72,7 +72,7 @@ void AP_Mount_Servo::update()
case MAV_MOUNT_MODE_GPS_POINT:
{
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_frontend.state[_instance]._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control);
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control);
stabilize();
}
break;
@ -91,16 +91,16 @@ void AP_Mount_Servo::update()
}
// write the results to the servos
move_servo(_roll_idx, _angle_bf_output_deg.x*10, _frontend.state[_instance]._roll_angle_min*0.1f, _frontend.state[_instance]._roll_angle_max*0.1f);
move_servo(_tilt_idx, _angle_bf_output_deg.y*10, _frontend.state[_instance]._tilt_angle_min*0.1f, _frontend.state[_instance]._tilt_angle_max*0.1f);
move_servo(_pan_idx, _angle_bf_output_deg.z*10, _frontend.state[_instance]._pan_angle_min*0.1f, _frontend.state[_instance]._pan_angle_max*0.1f);
move_servo(_roll_idx, _angle_bf_output_deg.x*10, _state._roll_angle_min*0.1f, _state._roll_angle_max*0.1f);
move_servo(_tilt_idx, _angle_bf_output_deg.y*10, _state._tilt_angle_min*0.1f, _state._tilt_angle_max*0.1f);
move_servo(_pan_idx, _angle_bf_output_deg.z*10, _state._pan_angle_min*0.1f, _state._pan_angle_max*0.1f);
}
// set_mode - sets mount's mode
void AP_Mount_Servo::set_mode(enum MAV_MOUNT_MODE mode)
{
// record the mode change and return success
_frontend.state[_instance]._mode = mode;
_state._mode = mode;
}
// private methods
@ -126,7 +126,7 @@ void AP_Mount_Servo::status_msg(mavlink_channel_t chan)
void AP_Mount_Servo::stabilize()
{
// only do the full 3D frame transform if we are doing pan control
if (_frontend.state[_instance]._stab_pan) {
if (_state._stab_pan) {
Matrix3f m; ///< holds 3 x 3 matrix, var is used as temp in calcs
Matrix3f cam; ///< Rotation matrix earth to camera. Desired camera from input.
Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
@ -135,8 +135,8 @@ void AP_Mount_Servo::stabilize()
cam.from_euler(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z);
gimbal_target = m * cam;
gimbal_target.to_euler(&_angle_bf_output_deg.x, &_angle_bf_output_deg.y, &_angle_bf_output_deg.z);
_angle_bf_output_deg.x = _frontend.state[_instance]._stab_roll ? degrees(_angle_bf_output_deg.x) : degrees(_angle_ef_target_rad.x);
_angle_bf_output_deg.y = _frontend.state[_instance]._stab_tilt ? degrees(_angle_bf_output_deg.y) : degrees(_angle_ef_target_rad.y);
_angle_bf_output_deg.x = _state._stab_roll ? degrees(_angle_bf_output_deg.x) : degrees(_angle_ef_target_rad.x);
_angle_bf_output_deg.y = _state._stab_tilt ? degrees(_angle_bf_output_deg.y) : degrees(_angle_ef_target_rad.y);
_angle_bf_output_deg.z = degrees(_angle_bf_output_deg.z);
} else {
// otherwise base mount roll and tilt on the ahrs
@ -144,26 +144,26 @@ void AP_Mount_Servo::stabilize()
_angle_bf_output_deg.x = degrees(_angle_ef_target_rad.x);
_angle_bf_output_deg.y = degrees(_angle_ef_target_rad.y);
_angle_bf_output_deg.z = degrees(_angle_ef_target_rad.z);
if (_frontend.state[_instance]._stab_roll) {
if (_state._stab_roll) {
_angle_bf_output_deg.x -= degrees(_frontend._ahrs.roll);
}
if (_frontend.state[_instance]._stab_tilt) {
if (_state._stab_tilt) {
_angle_bf_output_deg.y -= degrees(_frontend._ahrs.pitch);
}
// lead filter
const Vector3f &gyro = _frontend._ahrs.get_gyro();
if (_frontend.state[_instance]._stab_roll && _frontend.state[_instance]._roll_stb_lead != 0.0f && fabsf(_frontend._ahrs.pitch) < M_PI/3.0f) {
if (_state._stab_roll && _state._roll_stb_lead != 0.0f && fabsf(_frontend._ahrs.pitch) < M_PI/3.0f) {
// Compute rate of change of euler roll angle
float roll_rate = gyro.x + (_frontend._ahrs.sin_pitch() / _frontend._ahrs.cos_pitch()) * (gyro.y * _frontend._ahrs.sin_roll() + gyro.z * _frontend._ahrs.cos_roll());
_angle_bf_output_deg.x -= degrees(roll_rate) * _frontend.state[_instance]._roll_stb_lead;
_angle_bf_output_deg.x -= degrees(roll_rate) * _state._roll_stb_lead;
}
if (_frontend.state[_instance]._stab_tilt && _frontend.state[_instance]._pitch_stb_lead != 0.0f) {
if (_state._stab_tilt && _state._pitch_stb_lead != 0.0f) {
// Compute rate of change of euler pitch angle
float pitch_rate = _frontend._ahrs.cos_pitch() * gyro.y - _frontend._ahrs.sin_roll() * gyro.z;
_angle_bf_output_deg.y -= degrees(pitch_rate) * _frontend.state[_instance]._pitch_stb_lead;
_angle_bf_output_deg.y -= degrees(pitch_rate) * _state._pitch_stb_lead;
}
}
}

4
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -19,8 +19,8 @@ class AP_Mount_Servo : public AP_Mount_Backend
{
public:
// Constructor
AP_Mount_Servo(AP_Mount &frontend, uint8_t instance):
AP_Mount_Backend(frontend, instance),
AP_Mount_Servo(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance):
AP_Mount_Backend(frontend, state, instance),
_roll_idx(RC_Channel_aux::k_none),
_tilt_idx(RC_Channel_aux::k_none),
_pan_idx(RC_Channel_aux::k_none),