From 0153e1b1263a940f3053d8b79d92c28a9e3b94c5 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 12 Sep 2019 13:54:35 +0200 Subject: [PATCH] Matrix - Explicitly cast array[3] to Vector3f Use .xy() for Vector2 -> Vector3 assignment --- .../FlightTaskManualPositionSmoothVel.cpp | 8 ++++---- .../tasks/Utility/ManualVelocitySmoothingXYTest.cpp | 8 ++++---- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 1315b39f9f..9b172d1a3c 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -211,10 +211,10 @@ void FlightTaskManualPositionSmoothVel::_setOutputState() void FlightTaskManualPositionSmoothVel::_setOutputStateXY() { - _jerk_setpoint = _smoothing_xy.getCurrentJerk(); - _acceleration_setpoint = _smoothing_xy.getCurrentAcceleration(); - _velocity_setpoint = _smoothing_xy.getCurrentVelocity(); - _position_setpoint = _smoothing_xy.getCurrentPosition(); + _jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk(); + _acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration(); + _velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity(); + _position_setpoint.xy() = _smoothing_xy.getCurrentPosition(); } void FlightTaskManualPositionSmoothVel::_setOutputStateZ() diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXYTest.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXYTest.cpp index 0726ff9da1..b2eb242ff4 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXYTest.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXYTest.cpp @@ -74,10 +74,10 @@ TEST_F(ManualVelocitySmoothingXYTest, getCurrentState) Vector3f a_end; Vector3f v_end; Vector3f x_end; - j_end = _smoothing.getCurrentJerk(); - a_end = _smoothing.getCurrentAcceleration(); - v_end = _smoothing.getCurrentVelocity(); - x_end = _smoothing.getCurrentPosition(); + j_end.xy() = _smoothing.getCurrentJerk(); + a_end.xy() = _smoothing.getCurrentAcceleration(); + v_end.xy() = _smoothing.getCurrentVelocity(); + x_end.xy() = _smoothing.getCurrentPosition(); // THEN: the returned values should match the input EXPECT_EQ(j_end, Vector3f(0.f, 0.f, 0.f)); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 8ee2980749..96a534b587 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -673,7 +673,7 @@ MulticopterPositionControl::run() // Inform FlightTask about the input and output of the velocity controller // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) - _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), local_pos_sp.thrust); + _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), Vector3f(local_pos_sp.thrust)); // 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.