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

Tilt-Hex Pos-Ctrl: (WIP) Started code for attitude setpoint generation for omni vehicles

Browse Source
This commit is contained in:
Azarakhsh Keipour 2019-10-02 19:39:28 -04:00
parent a0a0c2280b
commit 8bfdf29055
3 changed files with 28 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,13 +44,19 @@ using namespace matrix;
namespace ControlMath
{
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp, const bool omni)
{
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
bodyzToAttitude(-thr_sp, yaw_sp, att_sp, omni);
att_sp.thrust_body[2] = -thr_sp.length();
if (omni == true) {
att_sp.thrust_body[0] = thr_sp(0);
att_sp.thrust_body[1] = thr_sp(1);
att_sp.thrust_body[2] = thr_sp(2);
}
}
void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp, const bool omni)
{
// zero vector, no direction, set safe level value
if (body_z.norm_squared() < FLT_EPSILON) {
@ -101,6 +107,19 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
att_sp.roll_body = euler(0);
att_sp.pitch_body = euler(1);
att_sp.yaw_body = euler(2);
if (omni == true) {
// Define attitude and thrust for the omni-directional vehicles
Eulerf omni_euler;
omni_euler(0) = 0;
omni_euler(1) = 0;
omni_euler(2) = att_sp.yaw_body;
//Quatf omni_q_sp = omni_euler;
//omni_q_sp.copyTo(att_sp.q_d);
att_sp.roll_body = omni_euler(0);
att_sp.pitch_body = omni_euler(1);
}
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

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

@ -51,14 +51,16 @@ namespace ControlMath
* @param yaw_sp the desired yaw
* @param att_sp attitude setpoint to fill
*/
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp,
const bool omni = true);
/**
* Converts a body z vector and yaw set-point to a desired attitude.
* @param body_z a world frame 3D vector in direction of the desired body z axis
* @param yaw_sp the desired yaw setpoint
* @param att_sp attitude setpoint to fill
*/
void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp,
const bool omni = true);
/**
* Outputs the sum of two vectors but respecting the limits and priority.

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -910,6 +910,8 @@ MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoin
if (_vehicle_land_detected.ground_contact
|| _vehicle_land_detected.maybe_landed) {
// we set the collective thrust to zero, this will help to decide if we are actually landed or not
setpoint.thrust_body[0] = 0.f;
setpoint.thrust_body[1] = 0.f;
setpoint.thrust_body[2] = 0.f;
// go level to avoid corrections but keep the heading we have
Quatf(AxisAngle<float>(0, 0, _states.yaw)).copyTo(setpoint.q_d);