Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 19:57:12 +08:00
Code Issues Actions 14 Packages Projects Releases Wiki Activity

[Position|AutoLine]SmoothVel - Update position and velocity states of the trajectories in case of EKF reset. Until now, only the position reset on XY axis was properly handled. Now, xy, vxy, z and vz are handled

Browse Source
This commit is contained in:
bresch
2019-03-13 11:24:06 +01:00
committed by Daniel Agar
parent 0813f30723
commit a08b1682e3
4 changed files with 73 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+27 -7
View File
@@ -117,19 +117,39 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con
return math::constrain(val, min, max);
}
void FlightTaskAutoLineSmoothVel::_checkEkfResetCounters()
{
// Check if a reset event has happened.
if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
_trajectory[0].setCurrentPosition(_position(0));
_trajectory[1].setCurrentPosition(_position(1));
_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
}
if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
_trajectory[0].setCurrentVelocity(_velocity(0));
_trajectory[1].setCurrentVelocity(_velocity(1));
_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
}
if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
_trajectory[2].setCurrentPosition(_position(2));
_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
}
if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
_trajectory[2].setCurrentVelocity(_velocity(2));
_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
}
}
void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
{
// Interface: A valid position setpoint generates a velocity target using a P controller. If a velocity is specified
// that one is used as a velocity limit.
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
// Check if a reset event has happened.
if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) {
// Reset the XY axes
_trajectory[0].setCurrentPosition(_position(0));
_trajectory[1].setCurrentPosition(_position(1));
_reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
}
_checkEkfResetCounters();
if (PX4_ISFINITE(_position_setpoint(0)) &&
PX4_ISFINITE(_position_setpoint(1))) {
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+10 -2
View File
@@ -69,12 +69,20 @@ protected:
void _setDefaultConstraints() override;
inline float _constrainOneSide(float val, float constrain);
void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
void _generateHeading();
bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
void _updateTrajConstraints();
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
void _generateTrajectory();
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
float _yaw_sp_prev{NAN};
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
float _yaw_sp_prev;
/* counters for estimator local position resets */
struct {
uint8_t xy;
uint8_t vxy;
uint8_t z;
uint8_t vz;
} _reset_counters{0, 0, 0, 0};
};
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+28 -10
View File
@@ -89,6 +89,32 @@ void FlightTaskManualPositionSmoothVel::reset(Axes axes, bool force_z_zero)
_position_setpoint_z_locked = NAN;
}
void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters()
{
// Check if a reset event has happened.
if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
_smoothing[0].setCurrentPosition(_position(0));
_smoothing[1].setCurrentPosition(_position(1));
_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
}
if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
_smoothing[0].setCurrentVelocity(_velocity(0));
_smoothing[1].setCurrentVelocity(_velocity(1));
_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
}
if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
_smoothing[2].setCurrentPosition(_position(2));
_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
}
if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
_smoothing[2].setCurrentVelocity(_velocity(2));
_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
}
}
void FlightTaskManualPositionSmoothVel::_updateSetpoints()
{
/* Get yaw setpont, un-smoothed position setpoints.*/
@@ -111,6 +137,8 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
float jerk[3] = {_jerk_max.get(), _jerk_max.get(), _jerk_max.get()};
_checkEkfResetCounters();
/* Check for position unlock
* During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
* is continuous. We know that the output of the position loop (part of the velocity setpoint) will suddenly become null
@@ -161,16 +189,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
if (_position_lock_xy_active) {
// Check if a reset event has happened.
if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) {
// Reset the XY axes
_smoothing[0].setCurrentPosition(_position(0));
_smoothing[1].setCurrentPosition(_position(1));
_reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
}
}
if (!_position_lock_xy_active) {
_smoothing[0].setCurrentPosition(_position(0));
_smoothing[1].setCurrentPosition(_position(1));
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+8 -1
View File
@@ -70,6 +70,7 @@ private:
* Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states
*/
void reset(Axes axes, bool force_z_zero = false);
void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
matrix::Vector3f _vel_sp_smooth;
@@ -78,5 +79,11 @@ private:
matrix::Vector2f _position_setpoint_xy_locked;
float _position_setpoint_z_locked{NAN};
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
/* counters for estimator local position resets */
struct {
uint8_t xy;
uint8_t vxy;
uint8_t z;
uint8_t vz;
} _reset_counters{0, 0, 0, 0};
};