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

AP_Mount: use only radians within servo backend

This commit is contained in:
Peter Barker 2023-03-25 15:04:42 +11:00 committed by Peter Barker
parent db15803017
commit 3b2e92c4d6
2 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -29,24 +29,24 @@ void AP_Mount_Servo::update()
switch (get_mode()) { switch (get_mode()) {
// 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 = _params.retract_angles.get(); _angle_bf_output_rad = _params.retract_angles.get() * DEG_TO_RAD;
// initialise _angle_rad to smooth transition if user changes to RC_TARGETTING // initialise _angle_rad to smooth transition if user changes to RC_TARGETTING
_angle_rad.roll = radians(_angle_bf_output_deg.x); _angle_rad.roll = _angle_bf_output_rad.x;
_angle_rad.pitch = radians(_angle_bf_output_deg.y); _angle_rad.pitch = _angle_bf_output_rad.y;
_angle_rad.yaw = radians(_angle_bf_output_deg.z); _angle_rad.yaw = _angle_bf_output_rad.z;
_angle_rad.yaw_is_ef = false; _angle_rad.yaw_is_ef = false;
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 = _params.neutral_angles.get(); _angle_bf_output_rad = _params.neutral_angles.get() * DEG_TO_RAD;
// initialise _angle_rad to smooth transition if user changes to RC_TARGETTING // initialise _angle_rad to smooth transition if user changes to RC_TARGETTING
_angle_rad.roll = radians(_angle_bf_output_deg.x); _angle_rad.roll = _angle_bf_output_rad.x;
_angle_rad.pitch = radians(_angle_bf_output_deg.y); _angle_rad.pitch = _angle_bf_output_rad.y;
_angle_rad.yaw = radians(_angle_bf_output_deg.z); _angle_rad.yaw = _angle_bf_output_rad.z;
_angle_rad.yaw_is_ef = false; _angle_rad.yaw_is_ef = false;
break; break;
} }
@ -61,7 +61,7 @@ void AP_Mount_Servo::update()
update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad); update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad);
break; break;
} }
// update _angle_bf_output_deg based on angle target // update _angle_bf_output_rad based on angle target
update_angle_outputs(_angle_rad); update_angle_outputs(_angle_rad);
break; break;
} }
@ -75,7 +75,7 @@ void AP_Mount_Servo::update()
} else if (get_rc_angle_target(rc_target)) { } else if (get_rc_angle_target(rc_target)) {
_angle_rad = rc_target; _angle_rad = rc_target;
} }
// update _angle_bf_output_deg based on angle target // update _angle_bf_output_rad based on angle target
update_angle_outputs(_angle_rad); update_angle_outputs(_angle_rad);
break; break;
} }
@ -112,9 +112,9 @@ void AP_Mount_Servo::update()
move_servo(_open_idx, mount_open, 0, 1); move_servo(_open_idx, mount_open, 0, 1);
// write the results to the servos // write the results to the servos
move_servo(_roll_idx, _angle_bf_output_deg.x*10, _params.roll_angle_min*10, _params.roll_angle_max*10); move_servo(_roll_idx, degrees(_angle_bf_output_rad.x)*10, _params.roll_angle_min*10, _params.roll_angle_max*10);
move_servo(_tilt_idx, _angle_bf_output_deg.y*10, _params.pitch_angle_min*10, _params.pitch_angle_max*10); move_servo(_tilt_idx, degrees(_angle_bf_output_rad.y)*10, _params.pitch_angle_min*10, _params.pitch_angle_max*10);
move_servo(_pan_idx, _angle_bf_output_deg.z*10, _params.yaw_angle_min*10, _params.yaw_angle_max*10); move_servo(_pan_idx, degrees(_angle_bf_output_rad.z)*10, _params.yaw_angle_min*10, _params.yaw_angle_max*10);
} }
// returns true if this mount can control its pan (required for multicopters) // returns true if this mount can control its pan (required for multicopters)
@ -126,7 +126,7 @@ bool AP_Mount_Servo::has_pan_control() const
// get attitude as a quaternion. returns true on success // get attitude as a quaternion. returns true on success
bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat) bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat)
{ {
att_quat.from_euler(radians(_angle_bf_output_deg.x), radians(_angle_bf_output_deg.y), radians(_angle_bf_output_deg.z)); att_quat.from_euler(_angle_bf_output_rad);
return true; return true;
} }
@ -141,9 +141,9 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad)
const float yaw_bf_rad = constrain_float(get_bf_yaw_angle(angle_rad), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max)); const float yaw_bf_rad = constrain_float(get_bf_yaw_angle(angle_rad), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
// default output to target earth-frame roll and pitch angles, body-frame yaw // default output to target earth-frame roll and pitch angles, body-frame yaw
_angle_bf_output_deg.x = degrees(angle_rad.roll); _angle_bf_output_rad.x = angle_rad.roll;
_angle_bf_output_deg.y = degrees(angle_rad.pitch); _angle_bf_output_rad.y = angle_rad.pitch;
_angle_bf_output_deg.z = degrees(yaw_bf_rad); _angle_bf_output_rad.z = yaw_bf_rad;
// this is sufficient for self-stabilising brushless gimbals // this is sufficient for self-stabilising brushless gimbals
if (!requires_stabilization) { if (!requires_stabilization) {
@ -159,8 +159,8 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad)
} }
// add roll and pitch lean angle correction // add roll and pitch lean angle correction
_angle_bf_output_deg.x -= degrees(ahrs_angle_rad.x); _angle_bf_output_rad.x -= ahrs_angle_rad.x;
_angle_bf_output_deg.y -= degrees(ahrs_angle_rad.y); _angle_bf_output_rad.y -= ahrs_angle_rad.y;
// lead filter // lead filter
const Vector3f &gyro = ahrs.get_gyro(); const Vector3f &gyro = ahrs.get_gyro();
@ -168,13 +168,13 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad)
if (!is_zero(_params.roll_stb_lead) && fabsf(ahrs.pitch) < M_PI/3.0f) { if (!is_zero(_params.roll_stb_lead) && fabsf(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 + (ahrs.sin_pitch() / ahrs.cos_pitch()) * (gyro.y * ahrs.sin_roll() + gyro.z * ahrs.cos_roll()); float roll_rate = gyro.x + (ahrs.sin_pitch() / ahrs.cos_pitch()) * (gyro.y * ahrs.sin_roll() + gyro.z * ahrs.cos_roll());
_angle_bf_output_deg.x -= degrees(roll_rate) * _params.roll_stb_lead; _angle_bf_output_rad.x -= roll_rate * _params.roll_stb_lead;
} }
if (!is_zero(_params.pitch_stb_lead)) { if (!is_zero(_params.pitch_stb_lead)) {
// Compute rate of change of euler pitch angle // Compute rate of change of euler pitch angle
float pitch_rate = ahrs.cos_pitch() * gyro.y - ahrs.sin_roll() * gyro.z; float pitch_rate = ahrs.cos_pitch() * gyro.y - ahrs.sin_roll() * gyro.z;
_angle_bf_output_deg.y -= degrees(pitch_rate) * _params.pitch_stb_lead; _angle_bf_output_rad.y -= pitch_rate * _params.pitch_stb_lead;
} }
} }

2
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -57,6 +57,6 @@ private:
SRV_Channel::Aux_servo_function_t _open_idx; // SRV_Channel mount open function index SRV_Channel::Aux_servo_function_t _open_idx; // SRV_Channel mount open function index
MountTarget _angle_rad; // angle target MountTarget _angle_rad; // angle target
Vector3f _angle_bf_output_deg; // final body frame output angle in degrees Vector3f _angle_bf_output_rad; // final body frame output angle in radians
}; };
#endif // HAL_MOUNT_SERVO_ENABLED #endif // HAL_MOUNT_SERVO_ENABLED