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

(WIP) Omni Pos-Ctrl: Added slow attitude change OMNI_ATT_MODE = 6 (disabled for now)

Browse Source
This commit is contained in:
Azarakhsh Keipour 2020-07-28 18:59:07 -04:00
parent a7920aa09f
commit 1ccd452de6
2 changed files with 63 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -42,6 +42,8 @@
using namespace matrix;
//#pragma GCC optimize("O0")
namespace ControlMath
{
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode,
@ -50,7 +52,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
{
// Print an error if the omni_att_mode parameter is out of range
if (omni_att_mode > 6 || omni_att_mode < 0) {
if (omni_att_mode > 5 || omni_att_mode < 0) {
PX4_ERR("OMNI_ATT_MODE parameter set to unknown value!");
}
@ -73,18 +75,24 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
break;
}
case 6: { // Attitude is changed very slowly given the rate
float tilt_angle_rate = 0.05, tilt_dir_rate = 0.05;
thrustToSlowAttitude(thr_sp, yaw_sp, att, tilt_angle_rate, tilt_dir_rate, omni_proj_axes, att_sp);
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) {
if (omni_att_mode == 5 || omni_att_mode == 6) {
// 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));
omni_att_tilt_dir = wrap_2pi(atan2f(thr_sp(1), thr_sp(0)));
// Set the roll angle
omni_att_roll = att_sp.roll_body;
@ -415,6 +423,41 @@ void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp,
att_sp.thrust_body[2] = thr_sp.dot(body_z);
}
void thrustToSlowAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
const float tilt_angle_rate, const float tilt_dir_rate, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
{
// Calculate the desired tilt angle and direction
float des_tilt_angle = asinf(Vector2f(thr_sp(0), thr_sp(1)).norm() / thr_sp.norm());
float des_tilt_dir = atan2f(thr_sp(1), thr_sp(0));
// Calculate the current z axis in up direction
Vector3f curr_z;
matrix::Dcmf R_body = att;
for (int i = 0; i < 3; i++) {
curr_z(i) = -R_body(i, 2);
}
// Calculate current tilt angle and direction
float curr_tilt_angle = asinf(Vector2f(curr_z(0), curr_z(1)).norm() / curr_z.norm());
float curr_tilt_dir = atan2f(curr_z(1), curr_z(0));
// Calculate the error
float err_tilt_angle = des_tilt_angle - curr_tilt_angle;
float err_tilt_dir = wrap_pi(des_tilt_dir - curr_tilt_dir);
// Calculate the changes to tilt angle and direction
float d_tilt_angle = math::min(tilt_angle_rate, err_tilt_angle);
float d_tilt_dir = math::sign(err_tilt_dir) * math::min(tilt_dir_rate, std::abs(err_tilt_dir));
// Calculate the commanded tilt angle and direction
float cmd_tilt_angle = curr_tilt_angle + d_tilt_angle;
float cmd_tilt_dir = wrap_2pi(curr_tilt_dir + d_tilt_dir);
// Calculate the attitude and thrust
thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, cmd_tilt_angle, cmd_tilt_dir, omni_proj_axes, att_sp);
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
{
if (Vector2f(v0 + v1).norm() <= max) {

21
src/modules/mc_pos_control/PositionControl/ControlMath.hpp
View File

@ -70,7 +70,7 @@ void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const
void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
/**
* Converts thrust vector and yaw set-point to a zero-tilt attitude for an omni-directional multirotor.
* Converts inertial thrust vector and yaw set-point to a zero-tilt attitude and body thrust vector for an omni-directional multirotor.
* @param thr_sp a 3D vector
* @param yaw_sp the desired yaw
* @param att current attitude of the robot
@ -81,7 +81,7 @@ void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp
int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
/**
* Converts thrust vector and yaw set-point to a minimum-tilt attitude for an omni-directional multirotor.
* Converts inertial thrust vector and yaw set-point to a minimum-tilt attitude and body thrust vector for an omni-directional multirotor.
* @param thr_sp a 3D vector
* @param yaw_sp the desired yaw
* @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
@ -92,7 +92,7 @@ void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp
void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust,
const matrix::Quatf &att, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
/**
* Converts thrust vector and yaw set-point to a desired-tilt attitude for an omni-directional multirotor.
* Converts inertial thrust vector and yaw set-point to a desired-tilt attitude and body thrust vector for an omni-directional multirotor.
* @param thr_sp a 3D vector
* @param yaw_sp the desired yaw
* @param att current attitude of the robot
@ -105,7 +105,7 @@ void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_s
const float tilt_angle, const float tilt_dir, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
/**
* Converts thrust vector and yaw set-point to a desired given attitude for an omni-directional multirotor.
* Converts inertial thrust vector and yaw set-point to a desired given attitude and body thrust vector for an omni-directional multirotor.
* @param thr_sp a 3D vector
* @param yaw_sp the desired yaw
* @param att current attitude of the robot
@ -117,6 +117,19 @@ void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_s
void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
const float roll_angle, const float pitch_angle, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
/**
* Converts inertial thrust vector and yaw set-point to a slow-changing attitude and body thrust vector for an omni-directional multirotor.
* @param thr_sp a 3D vector
* @param yaw_sp the desired yaw
* @param att current attitude of the robot
* @param tilt_angle_rate rate for the tilt angle change
* @param tilt_dir_rate rate for the tilt direction change
* @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
* @param att_sp attitude setpoint to fill
*/
void thrustToSlowAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
const float tilt_angle_rate, const float tilt_dir_rate, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
/**
* Outputs the sum of two vectors but respecting the limits and priority.
* The sum of two vectors are constraint such that v0 has priority over v1.