diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 55c84f53b3..1cbe502137 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -54,7 +54,7 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s sta _trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i)); } - _yaw_sp_prev = _yaw; + _yaw_sp_prev = state_prev.yaw; _updateTrajConstraints(); _initEkfResetCounters(); @@ -72,6 +72,32 @@ void FlightTaskAutoLineSmoothVel::reActivate() _initEkfResetCounters(); } +void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) +{ + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); } + + if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); } + + if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); } + + if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); } + + 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; } + + if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } +} + void FlightTaskAutoLineSmoothVel::_generateSetpoints() { _prepareSetpoints(); diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index c178b7c394..48f666ab6d 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -64,6 +64,7 @@ protected: (ParamFloat) _param_mpc_z_traj_p ); + void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); void _generateSetpoints() override; /**< Generate setpoints along line. */ /** determines when to trigger a takeoff (ignored in flight) */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index b8f269a99e..e3b596e881 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -47,47 +47,6 @@ bool FlightTask::updateInitialize() return true; } -void FlightTask::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) -{ - // If the position setpoint is unknown, set to the current postion - if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); } - - if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); } - - if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); } - - // If the velocity setpoint is unknown, set to the current velocity - if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); } - - if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); } - - 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; } - - // No jerk estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(setpoints.jerk_x)) { setpoints.jerk_x = 0.f; } - - if (!PX4_ISFINITE(setpoints.jerk_y)) { setpoints.jerk_y = 0.f; } - - if (!PX4_ISFINITE(setpoints.jerk_z)) { setpoints.jerk_z = 0.f; } - - if (!PX4_ISFINITE(setpoints.thrust[0])) { setpoints.thrust[0] = 0.f; } - - if (!PX4_ISFINITE(setpoints.thrust[1])) { setpoints.thrust[1] = 0.f; } - - if (!PX4_ISFINITE(setpoints.thrust[2])) { setpoints.thrust[2] = -0.5f; } - - if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } - - if (!PX4_ISFINITE(setpoints.yawspeed)) { setpoints.yawspeed = 0.f; } -} - const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint() { /* fill position setpoint message */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 04f210ba71..443e8fcd4c 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -120,8 +120,6 @@ public: */ const vehicle_local_position_setpoint_s getPositionSetpoint(); - void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - /** * Get vehicle constraints. * The constraints can vary with task. diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 4b04206e30..fb9b063fa0 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -66,6 +66,18 @@ void FlightTaskManualAltitudeSmoothVel::reActivate() _resetPositionLock(); } +void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) +{ + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); } + + // If the velocity setpoint is unknown, set to the current velocity + 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; } +} + void FlightTaskManualAltitudeSmoothVel::_resetPositionLock() { // Always start unlocked diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 3460d9f08f..10477b15ab 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -63,6 +63,7 @@ protected: private: + void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); void _resetPositionLock(); void _initEkfResetCounters(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 8d70a881d1..c73a0eb7ff 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -72,6 +72,30 @@ void FlightTaskManualPositionSmoothVel::reActivate() _resetPositionLock(); } +void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) +{ + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); } + + if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); } + + if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); } + + if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); } + + 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; } +} + void FlightTaskManualPositionSmoothVel::_resetPositionLock() { _position_lock_xy_active = false; diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index b500b66bd3..99a394f396 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -62,6 +62,7 @@ protected: (ParamFloat) _param_mpc_acc_down_max ) private: + void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); void _resetPositionLock(); void _initEkfResetCounters(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ diff --git a/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp b/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp index 6fa73f8d2c..bde203eab3 100644 --- a/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp +++ b/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp @@ -52,6 +52,14 @@ bool FlightTaskTransition::activate(vehicle_local_position_setpoint_s state_prev return FlightTask::activate(state_prev); } +void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) +{ + // If the setpoint is unknown, set to the current estimate + if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); } + + if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } +} + void FlightTaskTransition::updateAccelerationEstimate() { // Estimate the acceleration by filtering the raw derivative of the velocity estimate diff --git a/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp b/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp index 2b6eb0950d..571e8912bb 100644 --- a/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp +++ b/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp @@ -52,6 +52,7 @@ public: bool update() override; private: + void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); void updateAccelerationEstimate(); float _transition_altitude = 0.0f;