forked from Archive/PX4-Autopilot
flight_mode_manager: pass deltas on estimator reset
This commit is contained in:
parent
d79eea0c41
commit
87610957a4
|
@ -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));
|
||||
}
|
||||
|
|
|
@ -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. */
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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{};
|
||||
|
|
|
@ -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());
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
};
|
||||
|
|
|
@ -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));
|
||||
}
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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));
|
||||
}
|
||||
|
|
|
@ -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,
|
||||
|
|
Loading…
Reference in New Issue