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{};