From 9c25b987bc29a03f2a1c884d0e21db4df331d79a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 22 Oct 2019 16:41:52 +0200 Subject: [PATCH] vehicle_local_position_setpoint: make acceleration and jerk arrays --- msg/vehicle_local_position_setpoint.msg | 8 ++------ .../FlightTaskAutoLineSmoothVel.cpp | 11 ++++------- src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt | 2 +- src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp | 12 +++--------- .../FlightTaskManualAltitudeSmoothVel.cpp | 5 ++--- .../FlightTaskManualPositionSmoothVel.cpp | 12 +++++------- src/modules/mavlink/mavlink_messages.cpp | 12 ++++++------ .../PositionControl/PositionControl.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 9 files changed, 25 insertions(+), 41 deletions(-) diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg index dc60f20384..418cf5052b 100644 --- a/msg/vehicle_local_position_setpoint.msg +++ b/msg/vehicle_local_position_setpoint.msg @@ -11,12 +11,8 @@ float32 yawspeed # in radians/sec float32 vx # in meters/sec float32 vy # in meters/sec float32 vz # in meters/sec -float32 acc_x # in meters/(sec*sec) -float32 acc_y # in meters/(sec*sec) -float32 acc_z # in meters/(sec*sec) -float32 jerk_x # in meters/(sec*sec*sec) -float32 jerk_y # in meters/(sec*sec*sec) -float32 jerk_z # in meters/(sec*sec*sec) +float32[3] acceleration # in meters/sec^2 +float32[3] jerk # in meters/sec^3 float32[3] thrust # normalized thrust vector in NED # TOPICS vehicle_local_position_setpoint trajectory_setpoint diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 45a1b9b633..b61a2f0155 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -46,12 +46,11 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s las bool ret = FlightTaskAutoMapper2::activate(last_setpoint); checkSetpoints(last_setpoint); - const Vector3f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y, last_setpoint.acc_z); const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz); const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z); for (int i = 0; i < 3; ++i) { - _trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i)); + _trajectory[i].reset(last_setpoint.acceleration[i], vel_prev(i), pos_prev(i)); } _yaw_sp_prev = last_setpoint.yaw; @@ -87,11 +86,9 @@ void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); } // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; } - - if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; } - - if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; } + for (int i = 0; i < 3; i++) { + if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; } + } if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } } diff --git a/src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt b/src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt index 25a472bc6b..01303035eb 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt +++ b/src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ px4_add_library(FlightTask - FlightTask.cpp + FlightTask.cpp ) target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index afaaf8cb26..f5a9b64060 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -4,7 +4,7 @@ constexpr uint64_t FlightTask::_timeout; // First index of empty_setpoint corresponds to time-stamp and requires a finite number. -const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}}; +const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}}; const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; @@ -92,14 +92,8 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint() vehicle_local_position_setpoint.vy = _velocity_setpoint(1); vehicle_local_position_setpoint.vz = _velocity_setpoint(2); - vehicle_local_position_setpoint.acc_x = _acceleration_setpoint(0); - vehicle_local_position_setpoint.acc_y = _acceleration_setpoint(1); - vehicle_local_position_setpoint.acc_z = _acceleration_setpoint(2); - - vehicle_local_position_setpoint.jerk_x = _jerk_setpoint(0); - vehicle_local_position_setpoint.jerk_y = _jerk_setpoint(1); - vehicle_local_position_setpoint.jerk_z = _jerk_setpoint(2); - + _acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration); + _jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk); _thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust); vehicle_local_position_setpoint.yaw = _yaw_setpoint; vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint; diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index fb19e80d93..7bc4c90a92 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -48,7 +48,7 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint // Check if the previous FlightTask provided setpoints checkSetpoints(last_setpoint); - _smoothing.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z); + _smoothing.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z); return ret; } @@ -69,7 +69,7 @@ void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_se if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); } // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; } + if (!PX4_ISFINITE(setpoints.acceleration[2])) { setpoints.acceleration[2] = 0.f; } } void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ() @@ -91,7 +91,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() _smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2)); _smoothing.setCurrentPositionEstimate(_position(2)); - // Get yaw setpoint, un-smoothed position setpoints FlightTaskManualAltitude::_updateSetpoints(); diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 3e9b7394d8..b526a27694 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -44,12 +44,12 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint // Check if the previous FlightTask provided setpoints checkSetpoints(last_setpoint); - const Vector2f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y); + const Vector2f accel_prev(last_setpoint.acceleration[0], last_setpoint.acceleration[1]); const Vector2f vel_prev(last_setpoint.vx, last_setpoint.vy); const Vector2f pos_prev(last_setpoint.x, last_setpoint.y); _smoothing_xy.reset(accel_prev, vel_prev, pos_prev); - _smoothing_z.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z); + _smoothing_z.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z); return ret; } @@ -79,11 +79,9 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); } // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; } - - if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; } - - if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; } + for (int i = 0; i < 3; i++) { + if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; } + } } void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY() diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 61a55b8b22..f691649a10 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3259,9 +3259,9 @@ protected: msg.vz = lpos_sp.vz; // acceleration - msg.afx = lpos_sp.acc_x; - msg.afy = lpos_sp.acc_y; - msg.afz = lpos_sp.acc_z; + msg.afx = lpos_sp.acceleration[0]; + msg.afy = lpos_sp.acceleration[1]; + msg.afz = lpos_sp.acceleration[2]; // yaw msg.yaw = lpos_sp.yaw; @@ -3343,9 +3343,9 @@ protected: msg.vx = pos_sp.vx; msg.vy = pos_sp.vy; msg.vz = pos_sp.vz; - msg.afx = pos_sp.acc_x; - msg.afy = pos_sp.acc_y; - msg.afz = pos_sp.acc_z; + msg.afx = pos_sp.acceleration[0]; + msg.afy = pos_sp.acceleration[1]; + msg.afz = pos_sp.acceleration[2]; mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 5e2b57fa5a..8dfd8d5c07 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -69,7 +69,7 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); - _acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z); + _acc_sp = Vector3f(setpoint.acceleration); _thr_sp = Vector3f(setpoint.thrust); _yaw_sp = setpoint.yaw; _yawspeed_sp = setpoint.yawspeed; 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 cdf9d889b6..b7fca6969e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1027,7 +1027,7 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin setpoint.x = setpoint.y = setpoint.z = NAN; setpoint.vx = setpoint.vy = setpoint.vz = NAN; setpoint.yaw = setpoint.yawspeed = NAN; - setpoint.acc_x = setpoint.acc_y = setpoint.acc_z = NAN; + setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN; setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; }