fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 13:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Auto traj - Add Trajectory logging

Browse Source 
- move the update after the integration: a new computed jerk has an impact at the next epoch only
- add jerk reduction in case of too large integration time: when a jerk of "min_jerk" during dt is too much
- add jerk reduction if the integration time is larger than the predicted one and that integrating that jerk would lead to an acceleration overshoot
- rename some variables
This commit is contained in:
bresch
2018-10-25 15:29:45 +02:00
committed by Roman Bapst
parent 2847ce20b8
commit 7205e8f359
9 changed files with 140 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+6 -1
View File
@@ -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}};
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, vehicle_constraints_s::GEAR_KEEP, {}};
const vehicle_trajectory_waypoint_s FlightTask::empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
@@ -71,6 +71,10 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
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);
_thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
vehicle_local_position_setpoint.yaw = _yaw_setpoint;
vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
@@ -83,6 +87,7 @@ void FlightTask::_resetSetpoints()
_position_setpoint.setAll(NAN);
_velocity_setpoint.setAll(NAN);
_acceleration_setpoint.setAll(NAN);
_jerk_setpoint.setAll(NAN);
_thrust_setpoint.setAll(NAN);
_yaw_setpoint = _yawspeed_setpoint = NAN;
_desired_waypoint = FlightTask::empty_trajectory_waypoint;