forked from Archive/PX4-Autopilot
AutoSmoothVel - Override checkTakeoff with task-specific logic and reactivate z axis with downward velocity to takeoff smoothly
This commit is contained in:
parent
7c7d980cf0
commit
bb055fdaf3
|
@ -57,10 +57,12 @@ bool FlightTaskAutoLineSmoothVel::activate()
|
||||||
|
|
||||||
void FlightTaskAutoLineSmoothVel::reActivate()
|
void FlightTaskAutoLineSmoothVel::reActivate()
|
||||||
{
|
{
|
||||||
// Don't reset during takeoff TODO: Find a proper solution
|
// On ground, reset acceleration and velocity to zero
|
||||||
// The issue here is that with a small increment of velocity setpoint (generated by this flight task), the
|
for (int i = 0; i < 2; ++i) {
|
||||||
// land detector doesn't detect takeoff and without takeoff detection, the
|
_trajectory[i].reset(0.f, 0.f, _position(i));
|
||||||
// flight task is always reset.
|
}
|
||||||
|
|
||||||
|
_trajectory[2].reset(0.f, 0.7f, _position(2));
|
||||||
}
|
}
|
||||||
|
|
||||||
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
|
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
|
||||||
|
@ -143,6 +145,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
|
||||||
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
|
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
|
||||||
|
|
||||||
_checkEkfResetCounters();
|
_checkEkfResetCounters();
|
||||||
|
_want_takeoff = false;
|
||||||
|
|
||||||
if (PX4_ISFINITE(_position_setpoint(0)) &&
|
if (PX4_ISFINITE(_position_setpoint(0)) &&
|
||||||
PX4_ISFINITE(_position_setpoint(1))) {
|
PX4_ISFINITE(_position_setpoint(1))) {
|
||||||
|
@ -191,6 +194,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
|
||||||
_velocity_setpoint(2) = vel_sp_z;
|
_velocity_setpoint(2) = vel_sp_z;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_want_takeoff = _velocity_setpoint(2) < -0.3f;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -67,6 +67,9 @@ protected:
|
||||||
|
|
||||||
void _generateSetpoints() override; /**< Generate setpoints along line. */
|
void _generateSetpoints() override; /**< Generate setpoints along line. */
|
||||||
|
|
||||||
|
/** determines when to trigger a takeoff (ignored in flight) */
|
||||||
|
bool _checkTakeoff() override { return _want_takeoff; };
|
||||||
|
|
||||||
inline float _constrainOneSide(float val, float constrain);
|
inline float _constrainOneSide(float val, float constrain);
|
||||||
void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
|
void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
|
||||||
void _generateHeading();
|
void _generateHeading();
|
||||||
|
@ -75,7 +78,9 @@ protected:
|
||||||
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
|
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
|
||||||
void _generateTrajectory();
|
void _generateTrajectory();
|
||||||
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
|
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
|
||||||
float _yaw_sp_prev;
|
|
||||||
|
float _yaw_sp_prev{NAN};
|
||||||
|
bool _want_takeoff{false};
|
||||||
|
|
||||||
/* counters for estimator local position resets */
|
/* counters for estimator local position resets */
|
||||||
struct {
|
struct {
|
||||||
|
|
Loading…
Reference in New Issue