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: The attitude setpoint is generated based on the MC_OMNI_MODE parameter now

Browse Source
This commit is contained in:
Azarakhsh Keipour 2019-12-28 14:47:27 -05:00
parent 1d8894c930
commit db6b09adf0
6 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,9 +44,10 @@ using namespace matrix;
namespace ControlMath
{
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp, const bool omni)
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
vehicle_attitude_setpoint_s &att_sp)
{
if (omni) {
if (omni_att_mode > 0) {
thrustToOmniAttitude(thr_sp, yaw_sp, att_sp);
} else {

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

@ -50,9 +50,10 @@ namespace ControlMath
* @param thr_sp desired 3D thrust vector
* @param yaw_sp the desired yaw
* @param att_sp attitude setpoint to fill
* @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-daisy-chain 2-zero-tilt
*/
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp,
const bool omni = true);
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
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

7
src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
View File

@ -45,8 +45,9 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
* reason: thrust pointing full upward */
Vector3f thr{0.f, 0.f, -1.f};
float yaw = 0.f;
int omni_att_mode = 0;
vehicle_attitude_setpoint_s att{};
thrustToAttitude(thr, yaw, att);
thrustToAttitude(thr, yaw, omni_att_mode, att);
EXPECT_FLOAT_EQ(att.roll_body, 0.f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
@ -55,7 +56,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
/* expected: same as before but with 90 yaw
* reason: only yaw changed */
yaw = M_PI_2_F;
thrustToAttitude(thr, yaw, att);
thrustToAttitude(thr, yaw, omni_att_mode, att);
EXPECT_FLOAT_EQ(att.roll_body, 0.f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
@ -65,7 +66,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
* reason: thrust points straight down and order Euler
* order is: 1. roll, 2. pitch, 3. yaw */
thr = Vector3f(0.f, 0.f, 1.f);
thrustToAttitude(thr, yaw, att);
thrustToAttitude(thr, yaw, omni_att_mode, att);
EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);

4
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -361,8 +361,8 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
_thr_sp.copyTo(local_position_setpoint.thrust);
}
void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
void PositionControl::getAttitudeSetpoint(const int omni_att_mode, vehicle_attitude_setpoint_s &attitude_setpoint) const
{
ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, omni_att_mode, attitude_setpoint);
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}

3
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
View File

@ -169,9 +169,10 @@ public:
* Get the controllers output attitude setpoint
* This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
* It needs to be executed by the attitude controller to achieve velocity and position tracking.
* @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-daisy-chain 2-zero-tilt
* @param attitude_setpoint reference to struct to fill up
*/
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
void getAttitudeSetpoint(const int omni_att_mode, vehicle_attitude_setpoint_s &attitude_setpoint) const;
private:
/**

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

@ -656,7 +656,7 @@ MulticopterPositionControl::Run()
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint.timestamp = time_stamp_now;
_control.getAttitudeSetpoint(attitude_setpoint);
_control.getAttitudeSetpoint(_param_mc_omni_mode.get(), attitude_setpoint);
// Part of landing logic: if ground-contact/maybe landed was detected, turn off
// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.