fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 08:30:35 +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
9 changed files with 128 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
+31 -17
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()