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: clean up speed terminology

Browse Source
This commit is contained in:
chfriedrich98 2025-04-29 15:11:41 +02:00 committed by chfriedrich98
parent 1d5e58b948
commit b727ce86a0
9 changed files with 92 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/RoverThrottleSetpoint.msg
View File

@ -2,5 +2,4 @@
uint64 timestamp # time since system start (microseconds)
float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] (Positiv = forwards, Negativ = backwards)
float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1] (Mecanum only, Positiv = right, Negativ = left)

8
msg/RoverVelocityStatus.msg
View File

@ -1,10 +1,8 @@
uint64 timestamp # time since system start (microseconds)
float32 measured_speed_body_x # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
float32 speed_body_x_setpoint # [m/s] Speed setpoint in body x direction. Positiv: forwards, Negativ: backwards
float32 adjusted_speed_body_x_setpoint # [m/s] Post slew rate speed setpoint in body x direction. Positiv: forwards, Negativ: backwards
float32 measured_speed_body_y # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
float32 speed_body_y_setpoint # [m/s] Speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers)
float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers)
float32 pid_throttle_body_x_integral # Integral of the PID for the closed loop controller of the speed in body x direction
float32 pid_throttle_body_y_integral # Integral of the PID for the closed loop controller of the speed in body y direction
float32 measured_speed_body_y # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left (Mecanum only)
float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Mecanum only)
float32 pid_throttle_body_y_integral # Integral of the PID for the closed loop controller of the speed in body y direction (Mecanum only)

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

@ -158,21 +158,21 @@ void AckermannPosControl::manualPositionMode()
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
const float speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
const float speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
_max_yaw_rate / _param_ro_yaw_p.get());
if (fabsf(yaw_delta) > FLT_EPSILON
|| fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
|| fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
_course_control = false;
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
} else { // Course control if the steering input is zero (keep driving on a straight line)
@ -185,19 +185,19 @@ void AckermannPosControl::manualPositionMode()
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_body_x_setpoint) *
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) *
vector_scaling * _pos_ctl_course_direction;
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, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(speed_body_x_setpoint));
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
}
}
@ -232,20 +232,20 @@ void AckermannPosControl::autoPositionMode()
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
} else { // Regular guidance algorithm
const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp,
distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _curr_wp_type, _waypoint_transition_angle, _prev_waypoint_transition_angle,
_param_ro_speed_limit.get());
const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp,
distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _curr_wp_type, _waypoint_transition_angle, _prev_waypoint_transition_angle,
_param_ro_speed_limit.get());
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(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(speed_body_x_setpoint));
fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = false;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
@ -360,22 +360,22 @@ void AckermannPosControl::goToPositionMode()
const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _param_nav_acc_rad.get()) {
const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ?
_rover_position_setpoint.cruising_speed :
_param_ro_speed_limit.get();
const float speed_body_x_setpoint = math::min(speed_setpoint, max_speed);
speed_setpoint = math::min(speed_setpoint, max_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, _curr_pos_ned,
_curr_pos_ned, fabsf(speed_body_x_setpoint));
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = false;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);

37
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp
View File

@ -80,11 +80,11 @@ void AckermannVelControl::updateVelControl()
// Publish position controller status (logging only)
rover_velocity_status_s rover_velocity_status;
rover_velocity_status.timestamp = _timestamp;
rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x;
rover_velocity_status.measured_speed_body_x = _vehicle_speed;
rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState();
rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
rover_velocity_status.measured_speed_body_y = NAN;
rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
rover_velocity_status.pid_throttle_body_y_integral = NAN;
_rover_velocity_status_pub.publish(rover_velocity_status);
}
@ -106,10 +106,10 @@ void AckermannVelControl::updateSubscriptions()
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
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;
}
}
@ -143,9 +143,16 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
_ackermann_velocity_setpoint_sub.copy(&_ackermann_velocity_setpoint);
}
const Vector2f velocity_ned = Vector2f(_ackermann_velocity_setpoint.velocity_ned[0],
_ackermann_velocity_setpoint.velocity_ned[1]);
// Santitize input
if (!velocity_ned.isAllFinite()) {
return;
}
// Attitude Setpoint
if (fabsf(_ackermann_velocity_setpoint.velocity_ned[1]) < FLT_EPSILON
&& fabsf(_ackermann_velocity_setpoint.velocity_ned[0]) < FLT_EPSILON) {
if (velocity_ned.norm() < FLT_EPSILON) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
@ -154,23 +161,21 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
} else {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
const float yaw_setpoint = atan2f(_ackermann_velocity_setpoint.velocity_ned[1],
_ackermann_velocity_setpoint.velocity_ned[0]);
const float yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
rover_attitude_setpoint.yaw_setpoint = _ackermann_velocity_setpoint.backwards ? matrix::wrap_pi(
yaw_setpoint + M_PI_F) : yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
// Throttle Setpoint
const float speed_magnitude = math::min(sqrtf(powf(_ackermann_velocity_setpoint.velocity_ned[0],
2) + powf(_ackermann_velocity_setpoint.velocity_ned[1], 2)), _param_ro_speed_limit.get());
const float speed_body_x_setpoint = _ackermann_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude;
const float speed_magnitude = math::min(velocity_ned.norm(), _param_ro_speed_limit.get());
const float speed_setpoint = _ackermann_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude;
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed,
speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
_param_ro_max_thr_speed.get(), _dt);
rover_throttle_setpoint.throttle_body_y = 0.f;
rover_throttle_setpoint.throttle_body_y = NAN;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
}

3
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp
View File

@ -126,8 +126,7 @@ private:
// Variables
hrt_abstime _timestamp{0};
Quatf _vehicle_attitude_quaternion{};
float _vehicle_speed_body_x{0.f};
float _vehicle_speed_body_y{0.f};
float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
float _vehicle_yaw{0.f};
float _dt{0.f};
bool _prev_param_check_passed{true};

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

@ -97,9 +97,10 @@ void DifferentialPosControl::updateSubscriptions()
}
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
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;
}
}
@ -148,14 +149,14 @@ void DifferentialPosControl::manualPositionMode()
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
const float speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
const float speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
const float bearing_scaling = math::min(_max_yaw_rate / _param_ro_yaw_p.get(),
_param_rd_trans_drv_trn.get() - FLT_EPSILON);
const float bearing_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -bearing_scaling, bearing_scaling);
if (fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Turn on spot
if (fabsf(speed_setpoint) < FLT_EPSILON) { // Turn on spot
_course_control = false;
const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta);
differential_velocity_setpoint_s differential_velocity_setpoint{};
@ -169,7 +170,7 @@ void DifferentialPosControl::manualPositionMode()
const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta);
differential_velocity_setpoint_s differential_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = speed_body_x_setpoint;
differential_velocity_setpoint.speed = speed_setpoint;
differential_velocity_setpoint.bearing = bearing_setpoint;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
@ -183,18 +184,18 @@ void DifferentialPosControl::manualPositionMode()
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_body_x_setpoint) *
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) *
vector_scaling * _pos_ctl_course_direction;
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, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(speed_body_x_setpoint));
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
differential_velocity_setpoint_s differential_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = speed_body_x_setpoint;
differential_velocity_setpoint.bearing = speed_body_x_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
differential_velocity_setpoint.speed = speed_setpoint;
differential_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
bearing_setpoint + M_PI_F);
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
}
@ -238,18 +239,18 @@ void DifferentialPosControl::autoPositionMode()
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
} else {
const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(),
_param_rd_miss_spd_gain.get(), _curr_wp_type);
const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(),
_param_rd_miss_spd_gain.get(), _curr_wp_type);
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(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(speed_body_x_setpoint));
fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
differential_velocity_setpoint_s differential_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = speed_body_x_setpoint;
differential_velocity_setpoint.speed = speed_setpoint;
differential_velocity_setpoint.bearing = bearing_setpoint;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
}
@ -289,21 +290,21 @@ void DifferentialPosControl::goToPositionMode()
const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _param_nav_acc_rad.get()) {
const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ?
_rover_position_setpoint.cruising_speed :
_param_ro_speed_limit.get();
const float speed_body_x_setpoint = math::min(speed_setpoint, max_speed);
speed_setpoint = math::min(speed_setpoint, max_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, _curr_pos_ned,
_curr_pos_ned, fabsf(speed_body_x_setpoint));
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
differential_velocity_setpoint_s differential_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = speed_body_x_setpoint;
differential_velocity_setpoint.speed = speed_setpoint;
differential_velocity_setpoint.bearing = bearing_setpoint;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);

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

@ -163,7 +163,7 @@ private:
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<differential_velocity_setpoint_s> _differential_velocity_setpoint_pub{ORB_ID(differential_velocity_setpoint)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
// Variables
hrt_abstime _timestamp{0};
@ -171,7 +171,7 @@ private:
Vector2f _curr_pos_ned{};
Vector2f _pos_ctl_course_direction{};
Vector2f _pos_ctl_start_position_ned{};
float _vehicle_speed_body_x{0.f};
float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f};
float _dt{0.f};

36
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp
View File

@ -76,14 +76,14 @@ void DifferentialVelControl::updateVelControl()
_speed_setpoint.setForcedValue(0.f);
}
// Publish position controller status (logging only)
// Publish velocity controller status (logging only)
rover_velocity_status_s rover_velocity_status;
rover_velocity_status.timestamp = _timestamp;
rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x;
rover_velocity_status.measured_speed_body_x = _vehicle_speed;
rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState();
rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
rover_velocity_status.measured_speed_body_y = NAN;
rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
rover_velocity_status.pid_throttle_body_y_integral = NAN;
_rover_velocity_status_pub.publish(rover_velocity_status);
}
@ -103,10 +103,10 @@ void DifferentialVelControl::updateSubscriptions()
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
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;
}
}
@ -155,31 +155,31 @@ void DifferentialVelControl::generateAttitudeAndThrottleSetpoint()
_current_state = DrivingState::DRIVING;
}
float speed_body_x_setpoint = 0.f;
float speed_setpoint = 0.f;
if (_current_state == DrivingState::DRIVING) {
speed_body_x_setpoint = math::constrain(_differential_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
speed_setpoint = math::constrain(_differential_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
const float speed_body_x_setpoint_normalized = math::interpolate<float>(speed_body_x_setpoint,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
const float speed_setpoint_normalized = math::interpolate<float>(speed_setpoint,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
if (_rover_steering_setpoint_sub.updated()) {
_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
}
if (fabsf(speed_body_x_setpoint_normalized) > 1.f - fabsf(
if (fabsf(speed_setpoint_normalized) > 1.f - fabsf(
_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels
speed_body_x_setpoint = math::interpolate<float>(sign(speed_body_x_setpoint_normalized) * (1.f - fabsf(
_rover_steering_setpoint.normalized_speed_diff)), -1.f, 1.f,
- _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
speed_setpoint = math::interpolate<float>(sign(speed_setpoint_normalized) * (1.f - fabsf(
_rover_steering_setpoint.normalized_speed_diff)), -1.f, 1.f,
- _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
}
}
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed,
speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
_param_ro_max_thr_speed.get(), _dt);
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);

3
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp
View File

@ -137,8 +137,7 @@ private:
// Variables
hrt_abstime _timestamp{0};
Quatf _vehicle_attitude_quaternion{};
float _vehicle_speed_body_x{0.f};
float _vehicle_speed_body_y{0.f};
float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
float _vehicle_yaw{0.f};
float _dt{0.f};
bool _prev_param_check_passed{false};