fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Omni Pos-Ctrl: Wind estimation is only OMNI_ATT_MODE = 5 now for both tilt and roll/pitch estimation

Browse Source
This commit is contained in:
Azarakhsh Keipour 2020-07-26 01:48:17 -04:00
parent 45af173da1
commit a7920aa09f
1 changed files with 15 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
View File

@ -73,36 +73,25 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
break;
}
case 5: { // Estimate the optimal tilt angle and direction to counteract the wind (used for omnidirectional vehicles)
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
att_sp.thrust_body[2] = -thr_sp.length();
// Calculate the tilt angle
omni_att_tilt_angle = asinf(Vector2f(thr_sp(0), thr_sp(1)).norm() / thr_sp.norm());
// Calculate the tilt direction
omni_att_tilt_dir = atan2f(thr_sp(1), thr_sp(0));
break;
}
case 6: { // Estimate the optimal roll and pitch to counteract the wind (used for omnidirectional vehicles)
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
att_sp.thrust_body[2] = -thr_sp.length();
// Set the roll angle
omni_att_roll = att_sp.roll_body;
// Set the pitch angle
omni_att_pitch = att_sp.pitch_body;
break;
}
default: // Attitude is calculated from the desired thrust direction
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
att_sp.thrust_body[2] = -thr_sp.length();
}
// Estimate the optimal tilt angle and direction to counteract the wind
if (omni_att_mode == 5) {
// Calculate the tilt angle
omni_att_tilt_angle = asinf(Vector2f(thr_sp(0), thr_sp(1)).norm() / thr_sp.norm());
// Calculate the tilt direction
omni_att_tilt_dir = atan2f(thr_sp(1), thr_sp(0));
// Set the roll angle
omni_att_roll = att_sp.roll_body;
// Set the pitch angle
omni_att_pitch = att_sp.pitch_body;
}
}
void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)