fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 17:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Omni Pos-Ctrl: Refactoring of the attitude set point generation for omni-directional modes

Browse Source
This commit is contained in:
Azarakhsh Keipour
2020-01-21 18:23:07 -05:00
parent db6b09adf0
commit 8fd875f8c7
2 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+10 -4
View File
@@ -47,10 +47,16 @@ namespace ControlMath
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
vehicle_attitude_setpoint_s &att_sp)
{
if (omni_att_mode > 0) {
thrustToOmniAttitude(thr_sp, yaw_sp, att_sp);
if (omni_att_mode > 2 || omni_att_mode < 0) {
PX4_ERR("omni_att_mode set to unknown value!");
}
} else {
switch (omni_att_mode) {
case 2: // Attitude is set to the fixed zero roll and pitch (used for omnidirectional vehicles)
thrustToZeroTiltAttitude(thr_sp, yaw_sp, att_sp);
break;
default:
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
att_sp.thrust_body[2] = -thr_sp.length();
}
@@ -109,7 +115,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
att_sp.yaw_body = euler(2);
}
void thrustToOmniAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
{
// set Z axis to upward direction
Vector3f body_z = Vector3f(0.f, 0.f, 1.f);
src/modules/mc_pos_control/PositionControl/ControlMath.hpp
+2 -2
View File
@@ -63,12 +63,12 @@ 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 desired attitude for an omni-directional multirotor.
* Converts thrust vector and yaw set-point to a zero-tilt attitude for an omni-directional multirotor.
* @param thr_sp a 3D vector
* @param yaw_sp the desired yaw
* @param att_sp attitude setpoint to fill
*/
void thrustToOmniAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
/**
* Outputs the sum of two vectors but respecting the limits and priority.