Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Mount_Servo: use reference to state

Browse Source
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 // 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: case MAV_MOUNT_MODE_RETRACT:
{ {
_angle_bf_output_deg = _frontend.state[_instance]._retract_angles.get(); _angle_bf_output_deg = _state._retract_angles.get();
break; break;
} }
// move mount to a neutral position, typically pointing forward // move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: case MAV_MOUNT_MODE_NEUTRAL:
{ {
_angle_bf_output_deg = _frontend.state[_instance]._neutral_angles.get(); _angle_bf_output_deg = _state._neutral_angles.get();
break; break;
} }
@ -72,7 +72,7 @@ void AP_Mount_Servo::update()
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:
{ {
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) { 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(); stabilize();
} }
break; break;
@ -91,16 +91,16 @@ void AP_Mount_Servo::update()
} }
// write the results to the servos // 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(_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, _frontend.state[_instance]._tilt_angle_min*0.1f, _frontend.state[_instance]._tilt_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, _frontend.state[_instance]._pan_angle_min*0.1f, _frontend.state[_instance]._pan_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 // set_mode - sets mount's mode
void AP_Mount_Servo::set_mode(enum MAV_MOUNT_MODE mode) void AP_Mount_Servo::set_mode(enum MAV_MOUNT_MODE mode)
{ {
// record the mode change and return success // record the mode change and return success
_frontend.state[_instance]._mode = mode; _state._mode = mode;
} }
// private methods // private methods
@ -126,7 +126,7 @@ void AP_Mount_Servo::status_msg(mavlink_channel_t chan)
void AP_Mount_Servo::stabilize() void AP_Mount_Servo::stabilize()
{ {
// only do the full 3D frame transform if we are doing pan control // 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 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 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. 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); cam.from_euler(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z);
gimbal_target = m * cam; gimbal_target = m * cam;
gimbal_target.to_euler(&_angle_bf_output_deg.x, &_angle_bf_output_deg.y, &_angle_bf_output_deg.z); 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.x = _state._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.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); _angle_bf_output_deg.z = degrees(_angle_bf_output_deg.z);
} else { } else {
// otherwise base mount roll and tilt on the ahrs // 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.x = degrees(_angle_ef_target_rad.x);
_angle_bf_output_deg.y = degrees(_angle_ef_target_rad.y); _angle_bf_output_deg.y = degrees(_angle_ef_target_rad.y);
_angle_bf_output_deg.z = degrees(_angle_ef_target_rad.z); _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); _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); _angle_bf_output_deg.y -= degrees(_frontend._ahrs.pitch);
} }
// lead filter // lead filter
const Vector3f &gyro = _frontend._ahrs.get_gyro(); 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 // 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()); 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 // Compute rate of change of euler pitch angle
float pitch_rate = _frontend._ahrs.cos_pitch() * gyro.y - _frontend._ahrs.sin_roll() * gyro.z; 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: public:
// Constructor // Constructor
AP_Mount_Servo(AP_Mount &frontend, uint8_t instance): AP_Mount_Servo(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance):
AP_Mount_Backend(frontend, instance), AP_Mount_Backend(frontend, state, instance),
_roll_idx(RC_Channel_aux::k_none), _roll_idx(RC_Channel_aux::k_none),
_tilt_idx(RC_Channel_aux::k_none), _tilt_idx(RC_Channel_aux::k_none),
_pan_idx(RC_Channel_aux::k_none), _pan_idx(RC_Channel_aux::k_none),