yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

ControlMath: refactor thrustToAttitude calculation

Browse Source
This commit is contained in:
Matthias Grob
2019-12-07 15:47:07 +01:00
parent 1a5b06d0cf
commit a4a9d50a97
2 changed files with 36 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
* Copyright (C) 2018 - 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -42,46 +41,46 @@
#include <mathlib/mathlib.h>
using namespace matrix;
namespace ControlMath
{
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
{
att_sp.yaw_body = yaw_sp;
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
att_sp.thrust_body[2] = -thr_sp.length();
}
// desired body_z axis = -normalize(thrust_vector)
Vector3f body_x, body_y, body_z;
if (thr_sp.length() > 0.00001f) {
body_z = -thr_sp.normalized();
} else {
// no thrust, set Z axis to safe value
body_z = Vector3f(0.f, 0.f, 1.f);
void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
{
// zero vector, no direction, set safe level value
if (body_z.norm_squared() < FLT_EPSILON) {
body_z(2) = 1.f;
}
body_z.normalize();
// vector of desired yaw direction in XY plane, rotated by PI/2
Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f);
Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
if (fabsf(body_z(2)) > 0.000001f) {
// desired body_x axis, orthogonal to body_z
body_x = y_C % body_z;
// desired body_x axis, orthogonal to body_z
Vector3f body_x = y_C % body_z;
// keep nose to front while inverted upside down
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
// keep nose to front while inverted upside down
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
if (fabsf(body_z(2)) < 0.000001f) {
// desired thrust is in XY plane, set X downside to construct correct matrix,
// but yaw component will not be used actually
body_x.zero();
body_x(2) = 1.0f;
}
body_x.normalize();
// desired body_y axis
body_y = body_z % body_x;
Vector3f body_y = body_z % body_x;
Dcmf R_sp;
@@ -92,7 +91,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitu
R_sp(i, 2) = body_z(i);
}
//copy quaternion setpoint to attitude setpoint topic
// copy quaternion setpoint to attitude setpoint topic
Quatf q_sp = R_sp;
q_sp.copyTo(att_sp.q_d);
att_sp.q_d_valid = true;
@@ -101,7 +100,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitu
Eulerf euler = R_sp;
att_sp.roll_body = euler(0);
att_sp.pitch_body = euler(1);
att_sp.thrust_body[2] = -thr_sp.length();
att_sp.yaw_body = euler(2);
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

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

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
* Copyright (C) 2018 - 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -47,12 +47,18 @@ namespace ControlMath
{
/**
* Converts thrust vector and yaw set-point to a desired attitude.
* @param thr_sp a 3D vector
* @param thr_sp desired 3D thrust vector
* @param yaw_sp the desired yaw
* @return vehicle_attitude_setpoints_s structure
* @param att_sp attitude setpoint to fill
*/
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp,
vehicle_attitude_setpoint_s &attitude_setpoint);
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
/**
* 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);
/**
* Outputs the sum of two vectors but respecting the limits and priority.