Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-10-06 09:02:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(GotoControl): same EKF reset solution but save some flash

Browse Source
This commit is contained in:
Matthias Grob
2026-09-29 13:46:24 +02:00
parent 049ea8c079
commit bf52da8598
4 changed files with 5 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/HeadingSmoothing.cpp
+1 -2
View File
@@ -40,8 +40,7 @@ HeadingSmoothing::HeadingSmoothing()
void HeadingSmoothing::reset(const float heading, const float heading_rate)
{
const float wrapped_heading = matrix::wrap_pi(heading);
_velocity_smoothing.setCurrentVelocity(wrapped_heading);
_velocity_smoothing.setCurrentVelocity(matrix::wrap_pi(heading));
if (PX4_ISFINITE(heading_rate)) {
_velocity_smoothing.setCurrentAcceleration(heading_rate);
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+1 -1
View File
@@ -721,7 +721,7 @@ void FlightTaskAuto::_ekfResetHandlerVelocityZ(const float delta_vz)
void FlightTaskAuto::_ekfResetHandlerHeading(const float delta_psi)
{
_yaw_setpoint_previous = wrap_pi(_yaw_setpoint_previous + delta_psi);
_heading_smoothing.reset(wrap_pi(_heading_smoothing.getSmoothedHeading() + delta_psi));
_heading_smoothing.reset(_heading_smoothing.getSmoothedHeading() + delta_psi);
if (PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint = wrap_pi(_yaw_setpoint + delta_psi);
src/modules/mc_pos_control/GotoControl/GotoControl.cpp
-15
View File
@@ -140,21 +140,6 @@ void GotoControl::update(const float dt, const Vector3f &position, const Vector3
_vehicle_constraints_pub.publish(vehicle_constraints);
}
void GotoControl::ekfResetHandlerPosition(const Vector3f &position)
{
_position_smoothing.forceSetPosition(position);
}
void GotoControl::ekfResetHandlerVelocity(const Vector3f &velocity)
{
_position_smoothing.forceSetVelocity(velocity);
}
void GotoControl::ekfResetHandlerHeading(const float delta_heading)
{
_heading_smoothing.reset(wrap_pi(_heading_smoothing.getSmoothedHeading() + delta_heading));
}
void GotoControl::resetPositionSmoother(const Vector3f &position, const Vector3f &velocity, const Vector3f &acceleration)
{
if (!position.isAllFinite()) {
src/modules/mc_pos_control/GotoControl/GotoControl.hpp
+3 -3
View File
@@ -92,9 +92,9 @@ public:
void update(const float dt, const matrix::Vector3f &position, const matrix::Vector3f &velocity, const matrix::Vector3f &acceleration,
const float heading);
void ekfResetHandlerPosition(const matrix::Vector3f &position);
void ekfResetHandlerVelocity(const matrix::Vector3f &velocity);
void ekfResetHandlerHeading(const float delta_heading);
void ekfResetHandlerPosition(const matrix::Vector3f &position) { _position_smoothing.forceSetPosition(position); }
void ekfResetHandlerVelocity(const matrix::Vector3f &velocity) { _position_smoothing.forceSetVelocity(velocity); }
void ekfResetHandlerHeading(const float delta_heading) { _heading_smoothing.reset(_heading_smoothing.getSmoothedHeading() + delta_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; }