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

AP_Mount: Implement roll control on Solo gimbal

Browse Source
This commit is contained in:
Igor Campos 2020-12-01 17:00:12 -04:00 committed by Peter Barker
parent 9020c79677
commit 174700b7ad
2 changed files with 21 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Mount/SoloGimbal.cpp
View File

@ -122,7 +122,7 @@ void SoloGimbal::send_controls(mavlink_channel_t chan)
} }
case GIMBAL_MODE_STABILIZE: { case GIMBAL_MODE_STABILIZE: {
_ang_vel_dem_rads += get_ang_vel_dem_yaw(quatEst); _ang_vel_dem_rads += get_ang_vel_dem_yaw(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_tilt(quatEst); _ang_vel_dem_rads += get_ang_vel_dem_roll_tilt(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_feedforward(quatEst); _ang_vel_dem_rads += get_ang_vel_dem_feedforward(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_gyro_bias(); _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
float ang_vel_dem_norm = _ang_vel_dem_rads.length(); float ang_vel_dem_norm = _ang_vel_dem_rads.length();
@ -313,7 +313,7 @@ Vector3f SoloGimbal::get_ang_vel_dem_yaw(const Quaternion &quatEst)
return gimbalRateDemVecYaw; return gimbalRateDemVecYaw;
} }
Vector3f SoloGimbal::get_ang_vel_dem_tilt(const Quaternion &quatEst) Vector3f SoloGimbal::get_ang_vel_dem_roll_tilt(const Quaternion &quatEst)
{ {
// Calculate the gimbal 321 Euler angle estimates relative to earth frame // Calculate the gimbal 321 Euler angle estimates relative to earth frame
Vector3f eulerEst = quatEst.to_vector312(); Vector3f eulerEst = quatEst.to_vector312();
@ -340,14 +340,22 @@ Vector3f SoloGimbal::get_ang_vel_dem_tilt(const Quaternion &quatEst)
Vector3f SoloGimbal::get_ang_vel_dem_feedforward(const Quaternion &quatEst) Vector3f SoloGimbal::get_ang_vel_dem_feedforward(const Quaternion &quatEst)
{ {
// quaternion demanded at the previous time step // quaternion demanded at the previous time step
static float lastDem; static float lastDemY;
// calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
float delta = _att_target_euler_rad.y - lastDem; float deltaY = _att_target_euler_rad.y - lastDemY;
lastDem = _att_target_euler_rad.y; lastDemY = _att_target_euler_rad.y;
// quaternion demanded at the previous time step
static float lastDemX;
// calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
float deltaX = _att_target_euler_rad.x - lastDemX;
lastDemX = _att_target_euler_rad.x;
Vector3f gimbalRateDemVecForward; Vector3f gimbalRateDemVecForward;
gimbalRateDemVecForward.y = delta / _measurement.delta_time; gimbalRateDemVecForward.x = deltaX / _measurement.delta_time;
gimbalRateDemVecForward.y = deltaY / _measurement.delta_time;
return gimbalRateDemVecForward; return gimbalRateDemVecForward;
} }
@ -383,6 +391,12 @@ void SoloGimbal::update_target(const Vector3f &newTarget)
_att_target_euler_rad.y = _att_target_euler_rad.y + 0.02f*(newTarget.y - _att_target_euler_rad.y); _att_target_euler_rad.y = _att_target_euler_rad.y + 0.02f*(newTarget.y - _att_target_euler_rad.y);
// Update tilt // Update tilt
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y,radians(-90),radians(0)); _att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y,radians(-90),radians(0));
// For roll angle
// Low-pass filter
_att_target_euler_rad.x = _att_target_euler_rad.x + 0.02f*(newTarget.x - _att_target_euler_rad.x);
// Update roll
_att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x,radians(-30),radians(30));
} }
void SoloGimbal::write_logs() void SoloGimbal::write_logs()

2
libraries/AP_Mount/SoloGimbal.h
View File

@ -83,7 +83,7 @@ private:
void update_joint_angle_est(); void update_joint_angle_est();
Vector3f get_ang_vel_dem_yaw(const Quaternion &quatEst); Vector3f get_ang_vel_dem_yaw(const Quaternion &quatEst);
Vector3f get_ang_vel_dem_tilt(const Quaternion &quatEst); Vector3f get_ang_vel_dem_roll_tilt(const Quaternion &quatEst);
Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst); Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst);
Vector3f get_ang_vel_dem_gyro_bias(); Vector3f get_ang_vel_dem_gyro_bias();
Vector3f get_ang_vel_dem_body_lock(); Vector3f get_ang_vel_dem_body_lock();