From cb484c5ac749565a9562048921dd15f15d8a8be5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 6 Apr 2022 17:38:37 +0200 Subject: [PATCH] PositionControl: publish NAN jerk because the controller does not read or write jerk --- src/modules/mc_pos_control/PositionControl/PositionControl.cpp | 1 + .../mc_pos_control/PositionControl/PositionControlTest.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8dd2c521e7..6b94db7672 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -246,6 +246,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s local_position_setpoint.vy = _vel_sp(1); local_position_setpoint.vz = _vel_sp(2); _acc_sp.copyTo(local_position_setpoint.acceleration); + nans<3, 1>().copyTo(local_position_setpoint.jerk); _thr_sp.copyTo(local_position_setpoint.thrust); } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 5e625d0878..96698c509d 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -52,7 +52,7 @@ TEST(PositionControlTest, EmptySetpoint) EXPECT_FLOAT_EQ(output_setpoint.vy, 0.f); EXPECT_FLOAT_EQ(output_setpoint.vz, 0.f); EXPECT_EQ(Vector3f(output_setpoint.acceleration), Vector3f(0.f, 0.f, 0.f)); - EXPECT_EQ(Vector3f(output_setpoint.jerk), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(output_setpoint.jerk), Vector3f(NAN, NAN, NAN)); EXPECT_EQ(Vector3f(output_setpoint.thrust), Vector3f(0, 0, 0)); vehicle_attitude_setpoint_s attitude{};