fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 04:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_local_position_setpoint: make acceleration and jerk arrays

Browse Source
This commit is contained in:
Matthias Grob
2019-10-24 14:58:57 +02:00
parent 0062390c46
commit 9c25b987bc
9 changed files with 25 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_local_position_setpoint.msg
+2 -6
View File
@@ -11,12 +11,8 @@ float32 yawspeed # in radians/sec
float32 vx # in meters/sec float32 vx # in meters/sec
float32 vy # in meters/sec float32 vy # in meters/sec
float32 vz # in meters/sec float32 vz # in meters/sec
float32 acc_x # in meters/(sec*sec) float32[3] acceleration # in meters/sec^2
float32 acc_y # in meters/(sec*sec) float32[3] jerk # in meters/sec^3
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] thrust # normalized thrust vector in NED float32[3] thrust # normalized thrust vector in NED
# TOPICS vehicle_local_position_setpoint trajectory_setpoint # TOPICS vehicle_local_position_setpoint trajectory_setpoint
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+4 -7
View File
@@ -46,12 +46,11 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s las
bool ret = FlightTaskAutoMapper2::activate(last_setpoint); bool ret = FlightTaskAutoMapper2::activate(last_setpoint);
checkSetpoints(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 vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z); const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z);
for (int i = 0; i < 3; ++i) { 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; _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); } if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
// No acceleration estimate available, set to zero if the setpoint is NAN // No acceleration estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; } for (int i = 0; i < 3; i++) {
if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; } }
if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
} }
src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################ ############################################################################
px4_add_library(FlightTask px4_add_library(FlightTask
FlightTask.cpp FlightTask.cpp
) )
target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+3 -9
View File
@@ -4,7 +4,7 @@
constexpr uint64_t FlightTask::_timeout; constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number. // 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 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, {}}; 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.vy = _velocity_setpoint(1);
vehicle_local_position_setpoint.vz = _velocity_setpoint(2); vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
vehicle_local_position_setpoint.acc_x = _acceleration_setpoint(0); _acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
vehicle_local_position_setpoint.acc_y = _acceleration_setpoint(1); _jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
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);
_thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust); _thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
vehicle_local_position_setpoint.yaw = _yaw_setpoint; vehicle_local_position_setpoint.yaw = _yaw_setpoint;
vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint; vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
+2 -3
View File
@@ -48,7 +48,7 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint
// Check if the previous FlightTask provided setpoints // Check if the previous FlightTask provided setpoints
checkSetpoints(last_setpoint); 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; return ret;
} }
@@ -69,7 +69,7 @@ void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_se
if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); } if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
// No acceleration estimate available, set to zero if the setpoint is NAN // 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() void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
@@ -91,7 +91,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
_smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2)); _smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2));
_smoothing.setCurrentPositionEstimate(_position(2)); _smoothing.setCurrentPositionEstimate(_position(2));
// Get yaw setpoint, un-smoothed position setpoints // Get yaw setpoint, un-smoothed position setpoints
FlightTaskManualAltitude::_updateSetpoints(); FlightTaskManualAltitude::_updateSetpoints();
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+5 -7
View File
@@ -44,12 +44,12 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint
// Check if the previous FlightTask provided setpoints // Check if the previous FlightTask provided setpoints
checkSetpoints(last_setpoint); 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 vel_prev(last_setpoint.vx, last_setpoint.vy);
const Vector2f pos_prev(last_setpoint.x, last_setpoint.y); const Vector2f pos_prev(last_setpoint.x, last_setpoint.y);
_smoothing_xy.reset(accel_prev, vel_prev, pos_prev); _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; return ret;
} }
@@ -79,11 +79,9 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se
if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); } if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
// No acceleration estimate available, set to zero if the setpoint is NAN // No acceleration estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; } for (int i = 0; i < 3; i++) {
if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; } }
if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
} }
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY() void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY()
src/modules/mavlink/mavlink_messages.cpp
+6 -6
View File
@@ -3259,9 +3259,9 @@ protected:
msg.vz = lpos_sp.vz; msg.vz = lpos_sp.vz;
// acceleration // acceleration
msg.afx = lpos_sp.acc_x; msg.afx = lpos_sp.acceleration[0];
msg.afy = lpos_sp.acc_y; msg.afy = lpos_sp.acceleration[1];
msg.afz = lpos_sp.acc_z; msg.afz = lpos_sp.acceleration[2];
// yaw // yaw
msg.yaw = lpos_sp.yaw; msg.yaw = lpos_sp.yaw;
@@ -3343,9 +3343,9 @@ protected:
msg.vx = pos_sp.vx; msg.vx = pos_sp.vx;
msg.vy = pos_sp.vy; msg.vy = pos_sp.vy;
msg.vz = pos_sp.vz; msg.vz = pos_sp.vz;
msg.afx = pos_sp.acc_x; msg.afx = pos_sp.acceleration[0];
msg.afy = pos_sp.acc_y; msg.afy = pos_sp.acceleration[1];
msg.afz = pos_sp.acc_z; msg.afz = pos_sp.acceleration[2];
mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg);
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+1 -1
View File
@@ -69,7 +69,7 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); _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); _thr_sp = Vector3f(setpoint.thrust);
_yaw_sp = setpoint.yaw; _yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed; _yawspeed_sp = setpoint.yawspeed;
src/modules/mc_pos_control/mc_pos_control_main.cpp
+1 -1
View File
@@ -1027,7 +1027,7 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
setpoint.x = setpoint.y = setpoint.z = NAN; setpoint.x = setpoint.y = setpoint.z = NAN;
setpoint.vx = setpoint.vy = setpoint.vz = NAN; setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.yaw = setpoint.yawspeed = 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; setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
} }