dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

PositionControl: fix attitude setpoint timestamp 

The plot of the attitude setpoint in the log did not show any values
because the message timestamp that the position control module sets
was overwritten by the PositionControl attitude generation.
This commit is contained in:
Matthias Grob 2019-11-04 19:11:54 +01:00
parent c6cc9f0902
commit b6de83117e
4 changed files with 7 additions and 9 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,8 @@
using namespace matrix;
namespace ControlMath
{
vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp)
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
{
vehicle_attitude_setpoint_s att_sp = {};
att_sp.yaw_body = yaw_sp;
// desired body_z axis = -normalize(thrust_vector)
@ -103,8 +102,6 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float
att_sp.roll_body = euler(0);
att_sp.pitch_body = euler(1);
att_sp.thrust_body[2] = -thr_sp.length();
return att_sp;
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

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

@ -51,7 +51,8 @@ namespace ControlMath
* @param yaw_sp the desired yaw
* @return vehicle_attitude_setpoints_s structure
*/
vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp);
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp,
vehicle_attitude_setpoint_s &attitude_setpoint);
/**
* Outputs the sum of two vectors but respecting the limits and priority.

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

@ -46,7 +46,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
Vector3f thr{0.0f, 0.0f, -1.0f};
float yaw = 0.0f;
vehicle_attitude_setpoint_s att{};
att = thrustToAttitude(thr, yaw);
thrustToAttitude(thr, yaw, att);
EXPECT_EQ(att.roll_body, 0);
EXPECT_EQ(att.pitch_body, 0);
EXPECT_EQ(att.yaw_body, 0);
@ -55,7 +55,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
/* expected: same as before but with 90 yaw
* reason: only yaw changed */
yaw = M_PI_2_F;
att = thrustToAttitude(thr, yaw);
thrustToAttitude(thr, yaw, att);
EXPECT_EQ(att.roll_body, 0);
EXPECT_EQ(att.pitch_body, 0);
EXPECT_EQ(att.yaw_body, M_PI_2_F);
@ -65,7 +65,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
* reason: thrust points straight down and order Euler
* order is: 1. roll, 2. pitch, 3. yaw */
thr = Vector3f(0.0f, 0.0f, 1.0f);
att = thrustToAttitude(thr, yaw);
thrustToAttitude(thr, yaw, att);
EXPECT_NEAR(att.roll_body, -M_PI_F, 1e-4);
EXPECT_EQ(att.pitch_body, 0);
EXPECT_EQ(att.yaw_body, M_PI_2_F);

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

@ -367,7 +367,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
{
attitude_setpoint = ControlMath::thrustToAttitude(_thr_sp, _yaw_sp);
ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}