dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

flight_mode_manager: pass deltas on estimator reset

This commit is contained in:
Daniel Agar 2021-07-09 12:31:03 -04:00
parent d79eea0c41
commit 87610957a4
10 changed files with 32 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -87,24 +87,24 @@ void FlightTaskAutoLineSmoothVel::reActivate()
* Those functions are called by the base FlightTask in
* case of an EKF reset event
*/
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY()
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy)
{
_trajectory[0].setCurrentPosition(_position(0));
_trajectory[1].setCurrentPosition(_position(1));
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY()
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy)
{
_trajectory[0].setCurrentVelocity(_velocity(0));
_trajectory[1].setCurrentVelocity(_velocity(1));
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ()
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ(float delta_z)
{
_trajectory[2].setCurrentPosition(_position(2));
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ()
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz)
{
_trajectory[2].setCurrentVelocity(_velocity(2));
}

8
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
View File

@ -55,10 +55,10 @@ public:
protected:
/** Reset position or velocity setpoints in case of EKF reset event */
void _ekfResetHandlerPositionXY() override;
void _ekfResetHandlerVelocityXY() override;
void _ekfResetHandlerPositionZ() override;
void _ekfResetHandlerVelocityZ() override;
void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
void _ekfResetHandlerPositionZ(float delta_z) override;
void _ekfResetHandlerVelocityZ(float delta_vz) override;
void _ekfResetHandlerHeading(float delta_psi) override;
void _generateSetpoints() override; /**< Generate setpoints along line. */

8
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
View File

@ -49,22 +49,22 @@ void FlightTask::_checkEkfResetCounters()
{
// Check if a reset event has happened
if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
_ekfResetHandlerPositionXY();
_ekfResetHandlerPositionXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_xy});
_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
}
if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
_ekfResetHandlerVelocityXY();
_ekfResetHandlerVelocityXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_vxy});
_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
}
if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
_ekfResetHandlerPositionZ();
_ekfResetHandlerPositionZ(_sub_vehicle_local_position.get().delta_z);
_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
}
if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
_ekfResetHandlerVelocityZ();
_ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz);
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
}

8
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
View File

@ -208,10 +208,10 @@ protected:
* TODO: add the delta values to all the handlers
*/
void _checkEkfResetCounters();
virtual void _ekfResetHandlerPositionXY() {};
virtual void _ekfResetHandlerVelocityXY() {};
virtual void _ekfResetHandlerPositionZ() {};
virtual void _ekfResetHandlerVelocityZ() {};
virtual void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) {};
virtual void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) {};
virtual void _ekfResetHandlerPositionZ(float delta_z) {};
virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
virtual void _ekfResetHandlerHeading(float delta_psi) {};
map_projection_reference_s _global_local_proj_ref{};

4
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
View File

@ -77,12 +77,12 @@ bool FlightTaskManualAcceleration::update()
return ret;
}
void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY()
void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy)
{
_stick_acceleration_xy.resetPosition();
}
void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY()
void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy)
{
_stick_acceleration_xy.resetVelocity(_velocity.xy());
}

4
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp
View File

@ -56,6 +56,6 @@ private:
StickAccelerationXY _stick_acceleration_xy;
StickYaw _stick_yaw;
void _ekfResetHandlerPositionXY() override;
void _ekfResetHandlerVelocityXY() override;
void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
};

4
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
View File

@ -69,12 +69,12 @@ void FlightTaskManualAltitudeSmoothVel::reActivate()
_smoothing.reset(0.f, 0.f, _position(2));
}
void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ(float delta_z)
{
_smoothing.setCurrentPosition(_position(2));
}
void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerVelocityZ()
void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz)
{
_smoothing.setCurrentVelocity(_velocity(2));
}

4
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
View File

@ -55,8 +55,8 @@ protected:
virtual void _updateSetpoints() override;
/** Reset position or velocity setpoints in case of EKF reset event */
void _ekfResetHandlerPositionZ() override;
void _ekfResetHandlerVelocityZ() override;
void _ekfResetHandlerPositionZ(float delta_z) override;
void _ekfResetHandlerVelocityZ(float delta_vz) override;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,

8
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@ -73,22 +73,22 @@ void FlightTaskManualPositionSmoothVel::reActivate()
_smoothing_z.reset(0.f, 0.f, _position(2));
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY()
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy)
{
_smoothing_xy.setCurrentPosition(_position.xy());
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY()
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy)
{
_smoothing_xy.setCurrentVelocity(_velocity.xy());
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ()
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ(float delta_z)
{
_smoothing_z.setCurrentPosition(_position(2));
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ()
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz)
{
_smoothing_z.setCurrentVelocity(_velocity(2));
}

8
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
View File

@ -61,10 +61,10 @@ protected:
virtual void _updateSetpoints() override;
/** Reset position or velocity setpoints in case of EKF reset event */
void _ekfResetHandlerPositionXY() override;
void _ekfResetHandlerVelocityXY() override;
void _ekfResetHandlerPositionZ() override;
void _ekfResetHandlerVelocityZ() override;
void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
void _ekfResetHandlerPositionZ(float delta_z) override;
void _ekfResetHandlerVelocityZ(float delta_vz) override;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,