fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 08:41:42 +08:00
Code Issues 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-20 14:35:26 -04: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};
};