diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index e2ebb11bd0..ec999cdc0f 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -61,8 +61,8 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint) // If the velocity setpoint is unknown, set to the current velocity if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } - // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } + // If accel setpoint unknown, set to the current accel + if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = _acceleration(i); } } _position_smoothing.reset(accel_prev, vel_prev, pos_prev); diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index fa2943f147..58d0253012 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -153,6 +153,17 @@ void FlightTask::_evaluateVehicleLocalPosition() _velocity(2) = _sub_vehicle_local_position.get().vz; } + // acceleration is calculated as the velocity derivative in EKF2, + // if velocity is available acceleration values are available + if (_sub_vehicle_local_position.get().v_xy_valid) { + _acceleration(0) = _sub_vehicle_local_position.get().ax; + _acceleration(1) = _sub_vehicle_local_position.get().ay; + } + + if (_sub_vehicle_local_position.get().v_z_valid) { + _acceleration(2) = _sub_vehicle_local_position.get().az; + } + // distance to bottom if (_sub_vehicle_local_position.get().dist_bottom_valid && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) { diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 06327d8ca8..080c594d1a 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -201,6 +201,7 @@ protected: /* Current vehicle state */ matrix::Vector3f _position; /**< current vehicle position */ matrix::Vector3f _velocity; /**< current vehicle velocity */ + matrix::Vector3f _acceleration; /**< current vehicle acceleration (derivative of velocity) */ float _yaw{}; /**< current vehicle yaw heading */ float _unaided_yaw{}; diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index a266a5ec1b..663c3a969c 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -42,6 +42,7 @@ #include using namespace time_literals; +using namespace matrix; bool GotoControl::checkForSetpoint(const hrt_abstime &now, const bool enabled) { @@ -64,10 +65,11 @@ bool GotoControl::checkForSetpoint(const hrt_abstime &now, const bool enabled) return need_to_run; } -void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading) +void GotoControl::update(const float dt, const Vector3f &position, const Vector3f &velocity, const Vector3f &acceleration, + const float heading) { if (!_is_initialized) { - resetPositionSmoother(position); + resetPositionSmoother(position, velocity, acceleration); resetHeadingSmoother(heading); _is_initialized = true; } @@ -87,7 +89,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const } if (_need_smoother_reset) { - resetPositionSmoother(position); + resetPositionSmoother(position, velocity, acceleration); } setPositionSmootherLimits(_goto_setpoint); @@ -138,7 +140,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const _vehicle_constraints_pub.publish(vehicle_constraints); } -void GotoControl::resetPositionSmoother(const matrix::Vector3f &position) +void GotoControl::resetPositionSmoother(const Vector3f &position, const Vector3f &velocity, const Vector3f &acceleration) { if (!position.isAllFinite()) { // TODO: error messaging @@ -146,9 +148,7 @@ void GotoControl::resetPositionSmoother(const matrix::Vector3f &position) return; } - const Vector3f initial_acceleration{}; - const Vector3f initial_velocity{}; - _position_smoothing.reset(initial_acceleration, initial_velocity, position); + _position_smoothing.reset(acceleration, velocity, position); _need_smoother_reset = false; } diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp index f9ff08f9ca..4ae7aa9ec8 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp @@ -64,9 +64,11 @@ public: /** * @brief resets the position smoother at the current position with zero velocity and acceleration. * - * @param position [m] (NED) local vehicle position + * @param position [m] vehicle position state NED local frame + * @param velocity [m/s] vehicle velocity state + * @param acceleration [m/s^2] vehicle acceleration state */ - void resetPositionSmoother(const matrix::Vector3f &position); + void resetPositionSmoother(const matrix::Vector3f &position, const matrix::Vector3f &velocity, const matrix::Vector3f &acceleration); /** * @brief resets the heading smoother at the current heading with zero heading rate and acceleration. @@ -80,12 +82,15 @@ public: * loops to track. * * @param[in] dt [s] time since last control update - * @param[in] position [m] (NED) local vehicle position + * @param[in] position [m] vehicle position state NED local frame + * @param[in] velocity [m/s] vehicle velocity state + * @param[in] acceleration [m/s^2] vehicle acceleration state * @param[in] heading [rad] (from North) vehicle heading * @param[in] goto_setpoint struct containing current go-to setpoints * @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints */ - void update(const float dt, const matrix::Vector3f &position, const float heading); + void update(const float dt, const matrix::Vector3f &position, const matrix::Vector3f &velocity, const matrix::Vector3f &acceleration, + const float heading); // Setting all parameters from the outside saves 300bytes flash void setParamMpcAccHor(const float param_mpc_acc_hor) { _param_mpc_acc_hor = param_mpc_acc_hor; } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 0d6b57c68b..fe5f138050 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -433,7 +433,7 @@ void MulticopterPositionControl::Run() && !_trajectory_setpoint_sub.updated(); if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample, goto_setpoint_enable)) { - _goto_control.update(dt, states.position, states.yaw); + _goto_control.update(dt, states.position, states.velocity, states.acceleration, states.yaw); } _trajectory_setpoint_sub.update(&_setpoint);