fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: improve hold position logic (#25466)

Browse Source
This commit is contained in:
chfriedrich98 2025-08-22 12:10:16 +02:00 committed by GitHub
parent c1d15d0e09
commit 248f113141
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 128 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp
View File

@ -58,10 +58,8 @@ void AckermannPosControl::updatePosControl()
hrt_abstime timestamp = hrt_absolute_time();
const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (_target_waypoint_ned.isAllFinite()) {
float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm();
if (_arrival_speed > FLT_EPSILON) {
distance_to_target -= _acceptance_radius; // shift target to the edge of the acceptance radius if arrival speed not zero
@ -71,18 +69,13 @@ void AckermannPosControl::updatePosControl()
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
fabsf(_rover_position_setpoint.cruising_speed));
}
speed_setpoint = math::min(speed_setpoint, _cruising_speed);
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned,
_curr_pos_ned, fabsf(speed_setpoint));
if (_param_ro_speed_red.get() > FLT_EPSILON) {
@ -114,6 +107,16 @@ void AckermannPosControl::updatePosControl()
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
_stopped = true;
_target_waypoint_ned = _curr_pos_ned;
}
if (_stopped && _updated_reset_counter != _reset_counter) {
_target_waypoint_ned = _curr_pos_ned;
_reset_counter = _updated_reset_counter;
}
}
}
}
@ -136,21 +139,24 @@ void AckermannPosControl::updateSubscriptions()
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
if (!_global_ned_proj_ref.isInitialized()
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
vehicle_local_position.ref_timestamp);
}
_updated_reset_counter = vehicle_local_position.xy_reset_counter;
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1));
_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
}
if (_rover_position_setpoint_sub.updated()) {
_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
rover_position_setpoint_s rover_position_setpoint;
_rover_position_setpoint_sub.copy(&rover_position_setpoint);
_start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
_arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f;
_cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed :
_param_ro_speed_limit.get();
_target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]);
_stopped = false;
}
}

19
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
View File

@ -41,7 +41,6 @@
#include <matrix/matrix/math.hpp>
#include <lib/pure_pursuit/PurePursuit.hpp>
#include <math.h>
#include <lib/geo/geo.h>
// uORB includes
#include <uORB/Publication.hpp>
@ -80,6 +79,11 @@ public:
*/
bool runSanityChecks();
/**
* @brief Reset position controller.
*/
void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;};
protected:
/**
* @brief Update the parameters of the module.
@ -97,7 +101,6 @@ private:
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)};
rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
@ -108,14 +111,17 @@ private:
Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_pos_ned{};
Vector2f _start_ned{};
Vector2f _target_waypoint_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f};
float _acceptance_radius{0.f}; // Acceptance radius for the waypoint.
float _min_speed{NAN};
// Class Instances
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
float _vehicle_speed{0.f};
float _cruising_speed{NAN};
bool _stopped{false};
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
@ -128,6 +134,7 @@ private:
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
)
};

1
src/modules/rover_ackermann/RoverAckermann.cpp
View File

@ -182,6 +182,7 @@ void RoverAckermann::runSanityChecks()
void RoverAckermann::reset()
{
_ackermann_pos_control.reset();
_ackermann_speed_control.reset();
_ackermann_att_control.reset();
_ackermann_rate_control.reset();

48
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
View File

@ -55,10 +55,9 @@ void DifferentialPosControl::updatePosControl()
hrt_abstime timestamp = hrt_absolute_time();
const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (_target_waypoint_ned.isAllFinite()) {
float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm();
if (_arrival_speed > FLT_EPSILON) {
distance_to_target -=
@ -68,18 +67,13 @@ void DifferentialPosControl::updatePosControl()
if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) {
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
fabsf(_rover_position_setpoint.cruising_speed));
}
speed_setpoint = math::min(speed_setpoint, _cruising_speed);
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned,
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
@ -122,6 +116,16 @@ void DifferentialPosControl::updatePosControl()
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
_stopped = true;
_target_waypoint_ned = _curr_pos_ned;
}
if (_stopped && _updated_reset_counter != _reset_counter) {
_target_waypoint_ned = _curr_pos_ned;
_reset_counter = _updated_reset_counter;
}
}
}
@ -132,24 +136,34 @@ void DifferentialPosControl::updateSubscriptions()
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
}
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
_updated_reset_counter = vehicle_local_position.xy_reset_counter;
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1));
_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
}
if (_rover_position_setpoint_sub.updated()) {
_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
rover_position_setpoint_s rover_position_setpoint;
_rover_position_setpoint_sub.copy(&rover_position_setpoint);
_start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
_arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f;
_cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed :
_param_ro_speed_limit.get();
_target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]);
_stopped = false;
}
}
bool DifferentialPosControl::runSanityChecks()

17
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
View File

@ -38,7 +38,6 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <matrix/matrix/math.hpp>
#include <lib/pure_pursuit/PurePursuit.hpp>
#include <math.h>
@ -87,6 +86,11 @@ public:
*/
bool runSanityChecks();
/**
* @brief Reset position controller.
*/
void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;};
protected:
/**
* @brief Update the parameters of the module.
@ -103,7 +107,6 @@ private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
@ -111,10 +114,17 @@ private:
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
// Variables
Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_pos_ned{};
Vector2f _start_ned{};
Vector2f _target_waypoint_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f};
float _vehicle_speed{0.f};
float _cruising_speed{NAN};
bool _stopped{false};
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */
DrivingState _current_state{DrivingState::DRIVING};
DEFINE_PARAMETERS(
@ -128,6 +138,7 @@ private:
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
)
};

1
src/modules/rover_differential/RoverDifferential.cpp
View File

@ -183,6 +183,7 @@ void RoverDifferential::runSanityChecks()
void RoverDifferential::reset()
{
_differential_pos_control.reset();
_differential_speed_control.reset();
_differential_att_control.reset();
_differential_rate_control.reset();

50
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp
View File

@ -57,11 +57,9 @@ void MecanumPosControl::updatePosControl()
hrt_abstime timestamp = hrt_absolute_time();
const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
if (_target_waypoint_ned.isAllFinite()) {
if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm();
if (_arrival_speed > FLT_EPSILON) {
distance_to_target -=
@ -72,18 +70,13 @@ void MecanumPosControl::updatePosControl()
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
fabsf(_rover_position_setpoint.cruising_speed));
}
speed_setpoint = math::min(speed_setpoint, _cruising_speed);
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned,
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
@ -111,6 +104,16 @@ void MecanumPosControl::updatePosControl()
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
_stopped = true;
_target_waypoint_ned = _curr_pos_ned;
}
if (_stopped && _updated_reset_counter != _reset_counter) {
_target_waypoint_ned = _curr_pos_ned;
_reset_counter = _updated_reset_counter;
}
}
}
}
@ -127,24 +130,25 @@ void MecanumPosControl::updateSubscriptions()
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
if (!_global_ned_proj_ref.isInitialized()
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
vehicle_local_position.ref_timestamp);
}
_updated_reset_counter = vehicle_local_position.xy_reset_counter;
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1));
_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
}
if (_rover_position_setpoint_sub.updated()) {
_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
rover_position_setpoint_s rover_position_setpoint;
_rover_position_setpoint_sub.copy(&rover_position_setpoint);
_start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
_yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw;
_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
_arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f;
_cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed :
_param_ro_speed_limit.get();
_target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]);
_stopped = false;
}
}
bool MecanumPosControl::runSanityChecks()

18
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
View File

@ -38,7 +38,6 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <matrix/matrix/math.hpp>
#include <lib/pure_pursuit/PurePursuit.hpp>
#include <lib/geo/geo.h>
@ -80,6 +79,13 @@ public:
*/
bool runSanityChecks();
/**
* @brief Reset position controller.
*/
void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;};
protected:
protected:
/**
* @brief Update the parameters of the module.
@ -96,7 +102,6 @@ private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
@ -107,13 +112,16 @@ private:
Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_pos_ned{};
Vector2f _start_ned{};
Vector2f _target_waypoint_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f};
float _yaw_setpoint{NAN};
// Class Instances
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
float _vehicle_speed{0.f};
float _cruising_speed{NAN};
bool _stopped{false};
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th,

1
src/modules/rover_mecanum/RoverMecanum.cpp
View File

@ -183,6 +183,7 @@ void RoverMecanum::runSanityChecks()
void RoverMecanum::reset()
{
_mecanum_pos_control.reset();
_mecanum_speed_control.reset();
_mecanum_att_control.reset();
_mecanum_rate_control.reset();