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

ackermann: centralize mode management, resets and checks

Browse Source
This commit is contained in:
chfriedrich98 2025-04-30 09:10:01 +02:00 committed by chfriedrich98
parent cd486b2da6
commit 47a9b552f8
12 changed files with 961 additions and 892 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp
View File

@ -57,49 +57,83 @@ void AckermannActControl::updateActControl()
{
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
// Motor control
if (_rover_throttle_setpoint_sub.updated()) {
rover_throttle_setpoint_s rover_throttle_setpoint{};
_rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint);
_throttle_setpoint = rover_throttle_setpoint.throttle_body_x;
}
if (PX4_ISFINITE(_throttle_setpoint)) {
actuator_motors_s actuator_motors_sub{};
_actuator_motors_sub.copy(&actuator_motors_sub);
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint,
_throttle_setpoint, actuator_motors_sub.control[0], _param_ro_accel_limit.get(),
_param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
} else {
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = 0.f;
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
}
// Servo control
if (_rover_steering_setpoint_sub.updated()) {
rover_steering_setpoint_s rover_steering_setpoint{};
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
_steering_setpoint = rover_steering_setpoint.normalized_steering_angle;
}
if (PX4_ISFINITE(_steering_setpoint)) {
actuator_servos_s actuator_servos_sub{};
_actuator_servos_sub.copy(&actuator_servos_sub);
if (_param_ra_str_rate_limit.get() > FLT_EPSILON
&& _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured
if (fabsf(_servo_setpoint.getState() - actuator_servos_sub.control[0]) > fabsf(
_steering_setpoint -
actuator_servos_sub.control[0])) {
_servo_setpoint.setForcedValue(actuator_servos_sub.control[0]);
}
_servo_setpoint.update(_steering_setpoint, dt);
} else {
_servo_setpoint.setForcedValue(_steering_setpoint);
}
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = _servo_setpoint.getState();
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
} else {
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = 0.f;
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
}
generateActuatorSetpoint();
}
void AckermannActControl::generateActuatorSetpoint()
void AckermannActControl::stopVehicle()
{
// Motor control
rover_throttle_setpoint_s rover_throttle_setpoint{};
_rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint);
actuator_motors_s actuator_motors_sub{};
_actuator_motors_sub.copy(&actuator_motors_sub);
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint,
rover_throttle_setpoint.throttle_body_x, actuator_motors_sub.control[0], _param_ro_accel_limit.get(),
_param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt);
actuator_motors.control[0] = 0.f;
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
// Servo control
rover_steering_setpoint_s rover_steering_setpoint{};
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
actuator_servos_s actuator_servos_sub{};
_actuator_servos_sub.copy(&actuator_servos_sub);
if (_param_ra_str_rate_limit.get() > FLT_EPSILON
&& _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured
if (fabsf(_servo_setpoint.getState() - actuator_servos_sub.control[0]) > fabsf(
rover_steering_setpoint.normalized_steering_angle -
actuator_servos_sub.control[0])) {
_servo_setpoint.setForcedValue(actuator_servos_sub.control[0]);
}
_servo_setpoint.update(rover_steering_setpoint.normalized_steering_angle, _dt);
} else {
_servo_setpoint.setForcedValue(rover_steering_setpoint.normalized_steering_angle);
}
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = _servo_setpoint.getState();
actuator_servos.control[0] = 0.f;
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
}

30
src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp
View File

@ -44,7 +44,6 @@
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/rover_steering_setpoint.h>
@ -64,10 +63,15 @@ public:
~AckermannActControl() = default;
/**
* @brief Update actuator controller.
* @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
*/
void updateActControl();
/**
* @brief Stop the vehicle by sending 0 commands to motors and servos.
*/
void stopVehicle();
protected:
/**
* @brief Update the parameters of the module.
@ -76,11 +80,6 @@ protected:
private:
/**
* @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
*/
void generateActuatorSetpoint();
// uORB subscriptions
uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
@ -88,12 +87,13 @@ private:
uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
// uORB publications
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
// Variables
hrt_abstime _timestamp{0};
float _dt{0.f};
float _throttle_setpoint{NAN};
float _steering_setpoint{NAN};
// Controllers
SlewRate<float> _servo_setpoint{0.f};
@ -101,11 +101,11 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RA_STR_RATE_LIM>) _param_ra_str_rate_limit,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RA_STR_RATE_LIM>) _param_ra_str_rate_limit,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
)
};

139
src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp
View File

@ -38,8 +38,6 @@ using namespace time_literals;
AckermannAttControl::AckermannAttControl(ModuleParams *parent) : ModuleParams(parent)
{
_rover_rate_setpoint_pub.advertise();
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_attitude_status_pub.advertise();
updateParams();
}
@ -52,47 +50,42 @@ void AckermannAttControl::updateParams()
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
}
// Set up PID controller
_pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f);
_pid_yaw.setIntegralLimit(0.f);
_pid_yaw.setOutputLimit(_max_yaw_rate);
// Set up slew rate
_adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate);
}
void AckermannAttControl::updateAttControl()
{
updateSubscriptions();
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
if (_vehicle_control_mode_sub.updated()) {
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
}
if (PX4_ISFINITE(_yaw_setpoint)) {
// Calculate yaw rate limit for slew rate
float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) /
_param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate);
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();
}
float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate,
_vehicle_yaw, _yaw_setpoint, dt);
if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
// Estimate forward speed based on throttle
if (_actuator_motors_sub.updated()) {
actuator_motors_s actuator_motors;
_actuator_motors_sub.copy(&actuator_motors);
_estimated_speed_body_x = math::interpolate<float> (actuator_motors.control[0], -1.f, 1.f,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
}
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
if (_vehicle_control_mode.flag_control_manual_enabled) {
generateAttitudeAndThrottleSetpoint();
}
generateRateSetpoint();
} else { // Reset pid and slew rate when attitude control is not active
_pid_yaw.resetIntegral();
_adjusted_yaw_setpoint.setForcedValue(0.f);
} else {
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = 0.f;
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}
// Publish attitude controller status (logging only)
@ -104,82 +97,28 @@ void AckermannAttControl::updateAttControl()
}
void AckermannAttControl::generateAttitudeAndThrottleSetpoint()
void AckermannAttControl::updateSubscriptions()
{
const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
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(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control
_stab_yaw_ctl = false;
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
if (!_stab_yaw_ctl) {
_stab_yaw_setpoint = _vehicle_yaw;
_stab_yaw_ctl = true;
}
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
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();
}
// Estimate forward speed based on throttle
if (_actuator_motors_sub.updated()) {
actuator_motors_s actuator_motors;
_actuator_motors_sub.copy(&actuator_motors);
_estimated_speed_body_x = math::interpolate<float> (actuator_motors.control[0], -1.f, 1.f,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
}
}
void AckermannAttControl::generateRateSetpoint()
{
if (_rover_attitude_setpoint_sub.updated()) {
_rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
_rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint);
_yaw_setpoint = rover_attitude_setpoint.yaw_setpoint;
}
if (_rover_rate_setpoint_sub.updated()) {
_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
}
// Check if a new rate setpoint was already published from somewhere else
if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update
&& _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) {
return;
}
// Calculate yaw rate limit for slew rate
float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) /
_param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate);
float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate,
_vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt);
_last_rate_setpoint_update = _timestamp;
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}
bool AckermannAttControl::runSanityChecks()

48
src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp
View File

@ -47,9 +47,6 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/rover_attitude_status.h>
#include <uORB/topics/rover_attitude_setpoint.h>
@ -69,10 +66,21 @@ public:
~AckermannAttControl() = default;
/**
* @brief Update attitude controller.
* @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
*/
void updateAttControl();
/**
* @brief Reset attitude controller.
*/
void reset() {_pid_yaw.resetIntegral(); _yaw_setpoint = NAN;};
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
protected:
/**
* @brief Update the parameters of the module.
@ -81,48 +89,26 @@ protected:
private:
/**
* @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode).
* @brief Update uORB subscriptions used in attitude controller.
*/
void generateAttitudeAndThrottleSetpoint();
/**
* @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
*/
void generateRateSetpoint();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
void updateSubscriptions();
// uORB subscriptions
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{};
rover_attitude_setpoint_s _rover_attitude_setpoint{};
rover_rate_setpoint_s _rover_rate_setpoint{};
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
// Variables
float _vehicle_yaw{0.f};
hrt_abstime _timestamp{0};
hrt_abstime _last_rate_setpoint_update{0};
float _dt{0.f};
float _max_yaw_rate{0.f};
float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x]
between [0, 0] and [1, _param_ro_max_thr_speed].*/
float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode
bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode
float _yaw_setpoint{NAN};
// Controllers
PID _pid_yaw;

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

@ -38,7 +38,6 @@ using namespace time_literals;
AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(parent)
{
_pure_pursuit_status_pub.advertise();
_rover_position_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
updateParams();
@ -49,38 +48,66 @@ void AckermannPosControl::updateParams()
ModuleParams::updateParams();
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON
&& _param_ra_max_str_ang.get() > FLT_EPSILON) {
_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
}
}
void AckermannPosControl::updatePosControl()
{
updateSubscriptions();
if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
// Generate Position Setpoint
if (_vehicle_control_mode.flag_control_offboard_enabled) {
generatePositionSetpoint();
hrt_abstime timestamp = hrt_absolute_time();
} else if (_vehicle_control_mode.flag_control_manual_enabled) {
manualPositionMode();
const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
float distance_to_target = target_waypoint_ned.isAllFinite() ? (target_waypoint_ned - _curr_pos_ned).norm() : NAN;
} else if (_vehicle_control_mode.flag_control_auto_enabled) {
autoPositionMode();
if (PX4_ISFINITE(distance_to_target) && distance_to_target > _acceptance_radius) {
float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
0.f;
const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target;
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance, 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));
}
// Generate Velocity Setpoint
generateVelocitySetpoint();
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,
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
bearing_setpoint + M_PI_F);
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else {
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
void AckermannPosControl::updateSubscriptions()
{
if (_vehicle_control_mode_sub.updated()) {
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
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]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
}
if (_position_controller_status_sub.updated()) {
position_controller_status_s position_controller_status{};
_position_controller_status_sub.copy(&position_controller_status);
_acceptance_radius = position_controller_status.acceptance_radius;
}
if (_vehicle_attitude_sub.updated()) {
@ -105,292 +132,13 @@ void AckermannPosControl::updateSubscriptions()
}
void AckermannPosControl::generatePositionSetpoint()
{
if (_offboard_control_mode_sub.updated()) {
_offboard_control_mode_sub.copy(&_offboard_control_mode);
}
if (!_offboard_control_mode.position) {
return;
}
trajectory_setpoint_s trajectory_setpoint{};
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
// Translate trajectory setpoint to rover position setpoint
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
rover_position_setpoint.cruising_speed = _param_ro_speed_limit.get();
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
void AckermannPosControl::manualPositionMode()
{
updateSubscriptions();
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
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_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
_course_control = false;
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta);
const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint));
const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _param_pp_lookahd_max.get() *
pos_ctl_course_direction;
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
rover_position_setpoint.start_ned[0] = NAN;
rover_position_setpoint.start_ned[1] = NAN;
rover_position_setpoint.arrival_speed = NAN;
rover_position_setpoint.cruising_speed = speed_setpoint;
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
} else { // Course control if the steering input is zero (keep driving on a straight line)
if (!_course_control) {
_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
_pos_ctl_start_position_ned = _curr_pos_ned;
_course_control = true;
}
// 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_setpoint) *
vector_scaling * _pos_ctl_course_direction;
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
rover_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0);
rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1);
rover_position_setpoint.arrival_speed = NAN;
rover_position_setpoint.cruising_speed = speed_setpoint;
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
}
void AckermannPosControl::autoPositionMode()
{
updateSubscriptions();
if (_position_setpoint_triplet_sub.updated()) {
updateWaypointsAndAcceptanceRadius();
}
// Distances to waypoints
const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2));
const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
rover_position_setpoint.arrival_speed = autoArrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type,
_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.cruising_speed = autoCruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp,
distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle,
_prev_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
void AckermannPosControl::updateWaypointsAndAcceptanceRadius()
{
position_setpoint_triplet_s position_setpoint_triplet{};
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
_curr_wp_type = position_setpoint_triplet.current.type;
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
_curr_pos_ned, _global_ned_proj_ref);
_prev_waypoint_transition_angle = _waypoint_transition_angle;
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
// Update acceptance radius
_prev_acceptance_radius = _acceptance_radius;
if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
_acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get());
} else {
_acceptance_radius = _param_nav_acc_rad.get();
}
// Waypoint cruising speed
_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
}
float AckermannPosControl::updateAcceptanceRadius(const float waypoint_transition_angle,
const float default_acceptance_radius, const float acceptance_radius_gain,
const float acceptance_radius_max, const float wheel_base, const float max_steer_angle)
{
// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
float acceptance_radius = default_acceptance_radius;
if (PX4_ISFINITE(_waypoint_transition_angle)) {
const float theta = waypoint_transition_angle / 2.f;
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
const float acceptance_radius_temp_scaled = acceptance_radius_gain *
acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
acceptance_radius_max);
}
// Publish updated acceptance radius
position_controller_status_s pos_ctrl_status{};
pos_ctrl_status.acceptance_radius = acceptance_radius;
pos_ctrl_status.timestamp = hrt_absolute_time();
_position_controller_status_pub.publish(pos_ctrl_status);
return acceptance_radius;
}
float AckermannPosControl::autoArrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad,
const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate)
{
if (!PX4_ISFINITE(waypoint_transition_angle)
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
return 0.f; // Stop at the waypoint
} else {
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
const float cornering_speed = max_yaw_rate * turning_circle;
return math::constrain(cornering_speed, miss_speed_min, cruising_speed); // Slow down for cornering
}
}
float AckermannPosControl::autoCruisingSpeed(const float cruising_speed, const float miss_speed_min,
const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, const float prev_acc_rad,
const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate)
{
// Catch improper values
if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) {
return cruising_speed;
}
// Cornering slow down effect
if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) {
const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f);
const float cornering_speed = max_yaw_rate * turning_circle;
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
}
if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) {
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
const float cornering_speed = max_yaw_rate * turning_circle;
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
}
return cruising_speed; // Fallthrough
}
void AckermannPosControl::generateVelocitySetpoint()
{
hrt_abstime timestamp = hrt_absolute_time();
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]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
}
if (_position_controller_status_sub.updated()) {
position_controller_status_s position_controller_status{};
_position_controller_status_sub.copy(&position_controller_status);
_acceptance_radius = position_controller_status.acceptance_radius;
}
const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _param_nav_acc_rad.get()) {
float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
0.f;
const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target;
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance, 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));
}
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,
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(
yaw_setpoint + M_PI_F);
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else {
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
bool AckermannPosControl::runSanityChecks()
{
bool ret = true;
if (_param_ro_max_thr_speed.get() < FLT_EPSILON) {
ret = false;
}
if (_param_ra_wheel_base.get() < FLT_EPSILON) {
ret = false;
}
if (_param_ra_max_str_ang.get() < FLT_EPSILON) {
ret = false;
}
if (_param_ro_speed_limit.get() < FLT_EPSILON) {
ret = false;
}
if (_param_ro_yaw_p.get() < FLT_EPSILON) {
ret = false;
}
_prev_param_check_passed = ret;
return ret;
}

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

@ -38,26 +38,17 @@
#include <px4_platform_common/events.h>
// Library includes
#include <lib/rover_control/RoverControl.hpp>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <lib/pure_pursuit/PurePursuit.hpp>
#include <lib/geo/geo.h>
#include <math.h>
#include <lib/geo/geo.h>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/pure_pursuit_status.h>
@ -78,19 +69,15 @@ public:
~AckermannPosControl() = default;
/**
* @brief Update position control
* @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint.
*/
void updatePosControl();
/**
* @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
void manualPositionMode();
/**
* @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet.
*/
void autoPositionMode();
bool runSanityChecks();
protected:
/**
@ -104,135 +91,35 @@ private:
*/
void updateSubscriptions();
/**
* @brief Generate and publish roverPositionSetpoint from position of trajectorySetpoint.
*/
void generatePositionSetpoint();
/**
* @brief Update global/NED waypoint coordinates and acceptance radius.
*/
void updateWaypointsAndAcceptanceRadius();
/**
* @brief Publish the acceptance radius for current waypoint based on the angle between a line segment
* from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle.
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param default_acceptance_radius Default acceptance radius for waypoints [m].
* @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-].
* @param acceptance_radius_max Maximum value for the acceptance radius [m].
* @param wheel_base Rover wheelbase [m].
* @param max_steer_angle Rover maximum steer angle [rad].
* @return Updated acceptance radius [m].
*/
float updateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius,
float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle);
/**
* @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner.
* @param cruising_speed Cruising speed [m/s].
* @param miss_speed_min Minimum speed setpoint [m/s].
* @param acc_rad Acceptance radius of the current waypoint [m].
* @param curr_wp_type Type of the current waypoint.
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
* @return Speed setpoint [m/s].
*/
float autoArrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type,
float waypoint_transition_angle, float max_yaw_rate);
/**
* @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner.
* @param cruising_speed Cruising speed [m/s].
* @param miss_speed_min Minimum speed setpoint [m/s].
* @param distance_to_prev_wp Distance to the previous waypoint [m].
* @param distance_to_curr_wp Distance to the current waypoint [m].
* @param acc_rad Acceptance radius of the current waypoint [m].
* @param prev_acc_rad Acceptance radius of the previous waypoint [m].
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
* @return Speed setpoint [m/s].
*/
float autoCruisingSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp,
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float waypoint_transition_angle,
float prev_waypoint_transition_angle, float max_yaw_rate);
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
/**
* @brief Generate RoverVelocitySetpoint from RoverPositionSetpoint
*/
void generateVelocitySetpoint();
// uORB subscriptions
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
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{};
vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{};
// uORB publications
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
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_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_pos_ned{};
Vector2f _pos_ctl_course_direction{};
Vector2f _pos_ctl_start_position_ned{};
Vector2f _start_ned{};
float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f};
float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode.
bool _prev_param_check_passed{true};
// Waypoint variables
Vector2f _curr_wp_ned{};
Vector2f _prev_wp_ned{};
Vector2f _next_wp_ned{};
float _acceptance_radius{0.5f};
float _prev_acceptance_radius{0.5f};
float _cruising_speed{0.f};
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
float _acceptance_radius{0.f}; // Acceptance radius for the waypoint.
// Class Instances
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(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::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit
)
};

204
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp
View File

@ -37,8 +37,6 @@ using namespace time_literals;
AckermannRateControl::AckermannRateControl(ModuleParams *parent) : ModuleParams(parent)
{
_rover_rate_setpoint_pub.advertise();
_rover_throttle_setpoint_pub.advertise();
_rover_steering_setpoint_pub.advertise();
_rover_rate_status_pub.advertise();
updateParams();
@ -48,22 +46,80 @@ void AckermannRateControl::updateParams()
{
ModuleParams::updateParams();
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
// Set up PID controller
_pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f);
_pid_yaw_rate.setIntegralLimit(1.f);
_pid_yaw_rate.setOutputLimit(1.f);
_yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F);
// Set up slew rate
_adjusted_yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F);
}
void AckermannRateControl::updateRateControl()
{
updateSubscriptions();
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
if (_vehicle_control_mode_sub.updated()) {
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
if (fabsf(_estimated_speed) > FLT_EPSILON && PX4_ISFINITE(_yaw_rate_setpoint)) {
// Set up feasible yaw rate setpoint
float steering_setpoint{0.f};
float max_possible_yaw_rate = fabsf(_estimated_speed) * tanf(_param_ra_max_str_ang.get()) /
_param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate);
float constrained_yaw_rate = math::constrain(_yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit);
if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured
if (fabsf(_adjusted_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate -
_vehicle_yaw_rate)) {
_adjusted_yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate);
}
_adjusted_yaw_rate_setpoint.update(constrained_yaw_rate, dt);
} else {
_adjusted_yaw_rate_setpoint.setForcedValue(constrained_yaw_rate);
}
// Feed forward
steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed);
// Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability)
if (_estimated_speed > FLT_EPSILON) {
_pid_yaw_rate.setSetpoint(_adjusted_yaw_rate_setpoint.getState());
steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, dt);
}
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = _timestamp;
rover_steering_setpoint.normalized_steering_angle = math::interpolate<float>(steering_setpoint,
-_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
} else {
_pid_yaw_rate.resetIntegral();
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = _timestamp;
rover_steering_setpoint.normalized_steering_angle = 0.f;
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
}
// Publish rate controller status (logging only)
rover_rate_status_s rover_rate_status;
rover_rate_status.timestamp = _timestamp;
rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate;
rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState();
rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
_rover_rate_status_pub.publish(rover_rate_status);
}
void AckermannRateControl::updateSubscriptions()
{
if (_vehicle_angular_velocity_sub.updated()) {
vehicle_angular_velocity_s vehicle_angular_velocity{};
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
@ -71,107 +127,20 @@ void AckermannRateControl::updateRateControl()
vehicle_angular_velocity.xyz[2] : 0.f;
}
if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
// Estimate forward speed based on throttle
if (_actuator_motors_sub.updated()) {
actuator_motors_s actuator_motors;
_actuator_motors_sub.copy(&actuator_motors);
_estimated_speed_body_x = math::interpolate<float>(actuator_motors.control[0], -1.f, 1.f,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
_estimated_speed_body_x = fabsf(_estimated_speed_body_x) > _param_ro_speed_th.get() ? _estimated_speed_body_x : 0.f;
}
if (_vehicle_control_mode.flag_control_manual_enabled) {
generateRateAndThrottleSetpoint();
}
generateSteeringSetpoint();
} else { // Reset controller and slew rate when rate control is not active
_pid_yaw_rate.resetIntegral();
_yaw_rate_setpoint.setForcedValue(0.f);
// Estimate forward speed based on throttle
if (_actuator_motors_sub.updated()) {
actuator_motors_s actuator_motors;
_actuator_motors_sub.copy(&actuator_motors);
_estimated_speed = math::interpolate<float>(actuator_motors.control[0], -1.f, 1.f,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
_estimated_speed = fabsf(_estimated_speed) > _param_ro_speed_th.get() ? _estimated_speed : 0.f;
}
// Publish rate controller status (logging only)
rover_rate_status_s rover_rate_status;
rover_rate_status.timestamp = _timestamp;
rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate;
rover_rate_status.adjusted_yaw_rate_setpoint = _yaw_rate_setpoint.getState();
rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
_rover_rate_status_pub.publish(rover_rate_status);
}
void AckermannRateControl::generateRateAndThrottleSetpoint()
{
const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled;
if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * math::interpolate<float>
(manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}
}
}
void AckermannRateControl::generateSteeringSetpoint()
{
if (_rover_rate_setpoint_sub.updated()) {
_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
rover_rate_setpoint_s rover_rate_setpoint{};
_rover_rate_setpoint_sub.copy(&rover_rate_setpoint);
_yaw_rate_setpoint = rover_rate_setpoint.yaw_rate_setpoint;
}
float steering_setpoint{0.f};
if (fabsf(_estimated_speed_body_x) > FLT_EPSILON) {
// Set up feasible yaw rate setpoint
float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) /
_param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate);
float constrained_yaw_rate = math::constrain(_rover_rate_setpoint.yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit);
if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured
if (fabsf(_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate -
_vehicle_yaw_rate)) {
_yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate);
}
_yaw_rate_setpoint.update(constrained_yaw_rate, _dt);
} else {
_yaw_rate_setpoint.setForcedValue(constrained_yaw_rate);
}
// Feed forward
steering_setpoint = atanf(_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed_body_x);
// Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability)
if (_estimated_speed_body_x > FLT_EPSILON) {
_pid_yaw_rate.setSetpoint(_yaw_rate_setpoint.getState());
steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, _dt);
}
} else {
_pid_yaw_rate.resetIntegral();
}
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = _timestamp;
rover_steering_setpoint.normalized_steering_angle = math::interpolate<float>(steering_setpoint,
-_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
}
bool AckermannRateControl::runSanityChecks()
@ -180,44 +149,31 @@ bool AckermannRateControl::runSanityChecks()
if (_param_ro_max_thr_speed.get() < FLT_EPSILON) {
ret = false;
if (_prev_param_check_passed) {
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_max_thr_speed"), events::Log::Error,
"Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get());
}
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_max_thr_speed"), events::Log::Error,
"Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get());
}
if (_param_ra_wheel_base.get() < FLT_EPSILON) {
ret = false;
if (_prev_param_check_passed) {
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_wheel_base"), events::Log::Error,
"Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get());
}
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_wheel_base"), events::Log::Error,
"Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get());
}
if (_param_ra_max_str_ang.get() < FLT_EPSILON) {
ret = false;
if (_prev_param_check_passed) {
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_max_str_ang"), events::Log::Error,
"Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get());
}
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_max_str_ang"), events::Log::Error,
"Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get());
}
if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
ret = false;
if (_prev_param_check_passed) {
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get());
}
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get());
}
_prev_param_check_passed = ret;
return ret;
}

47
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp
View File

@ -47,9 +47,6 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_rate_status.h>
@ -69,10 +66,21 @@ public:
~AckermannRateControl() = default;
/**
* @brief Update rate controller.
* @brief Generate and publish roverSteeringSetpoint from roverRateSetpoint.
*/
void updateRateControl();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
/**
* @brief Reset rate controller.
*/
void reset() {_pid_yaw_rate.resetIntegral(); _yaw_rate_setpoint = NAN;};
protected:
/**
* @brief Update the parameters of the module.
@ -80,50 +88,31 @@ protected:
void updateParams() override;
private:
/**
* @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode).
* @brief Update uORB subscriptions used in rate controller.
*/
void generateRateAndThrottleSetpoint();
/**
* @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.
*/
void generateSteeringSetpoint();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
void updateSubscriptions();
// uORB subscriptions
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
vehicle_control_mode_s _vehicle_control_mode{};
rover_rate_setpoint_s _rover_rate_setpoint{};
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
uORB::Publication<rover_rate_status_s> _rover_rate_status_pub{ORB_ID(rover_rate_status)};
uORB::Publication<rover_rate_status_s> _rover_rate_status_pub{ORB_ID(rover_rate_status)};
// Variables
float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x]
float _estimated_speed{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed]
between [0, 0] and [1, _param_ro_max_thr_speed].*/
float _max_yaw_rate{0.f};
float _vehicle_yaw_rate{0.f};
float _yaw_rate_setpoint{NAN};
hrt_abstime _timestamp{0};
float _dt{0.f}; // Time since last update [s].
bool _prev_param_check_passed{true};
// Controllers
PID _pid_yaw_rate;
SlewRate<float> _yaw_rate_setpoint{0.f};
SlewRate<float> _adjusted_yaw_rate_setpoint{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,

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

@ -38,50 +38,68 @@ using namespace time_literals;
AckermannVelControl::AckermannVelControl(ModuleParams *parent) : ModuleParams(parent)
{
_rover_throttle_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
updateParams();
}
void AckermannVelControl::updateParams()
{
ModuleParams::updateParams();
// Set up PID controller
_pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
_pid_speed.setIntegralLimit(1.f);
_pid_speed.setOutputLimit(1.f);
// Set up slew rate
if (_param_ro_accel_limit.get() > FLT_EPSILON) {
_speed_setpoint.setSlewRate(_param_ro_accel_limit.get());
_adjusted_speed_setpoint.setSlewRate(_param_ro_accel_limit.get());
}
}
void AckermannVelControl::updateVelControl()
{
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
updateSubscriptions();
if ((_vehicle_control_mode.flag_control_velocity_enabled) && _vehicle_control_mode.flag_armed && runSanityChecks()) {
if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Velocity Control
generateVelocitySetpoint();
}
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
generateAttitudeAndThrottleSetpoint();
// Attitude Setpoint
if (PX4_ISFINITE(_bearing_setpoint)) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _bearing_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
} else { // Reset controller and slew rate when position control is not active
_pid_speed.resetIntegral();
_speed_setpoint.setForcedValue(0.f);
// Throttle Setpoint
if (PX4_ISFINITE(_speed_setpoint)) {
const float speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed,
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 = NAN;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
} else {
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = 0.f;
rover_throttle_setpoint.throttle_body_y = NAN;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
}
// 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;
rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState();
rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState();
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;
@ -91,10 +109,6 @@ void AckermannVelControl::updateVelControl()
void AckermannVelControl::updateSubscriptions()
{
if (_vehicle_control_mode_sub.updated()) {
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
}
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
@ -105,68 +119,18 @@ void AckermannVelControl::updateSubscriptions()
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
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;
}
}
void AckermannVelControl::generateVelocitySetpoint()
{
trajectory_setpoint_s trajectory_setpoint{};
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
if (_offboard_control_mode_sub.updated()) {
_offboard_control_mode_sub.copy(&_offboard_control_mode);
}
const bool offboard_vel_control = _offboard_control_mode.velocity && !_offboard_control_mode.position;
const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) {
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = velocity_in_local_frame.norm();
rover_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
{
if (_rover_velocity_setpoint_sub.updated()) {
_rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint;
_rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint);
_speed_setpoint = rover_velocity_setpoint.speed;
_bearing_setpoint = rover_velocity_setpoint.bearing;
}
// Attitude Setpoint
if (fabsf(_rover_velocity_setpoint.speed) < FLT_EPSILON) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else if (PX4_ISFINITE(_rover_velocity_setpoint.bearing)) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.bearing;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
// Throttle Setpoint
const float speed_setpoint = math::constrain(_rover_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.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_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 = NAN;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
}
bool AckermannVelControl::runSanityChecks()
@ -179,14 +143,10 @@ bool AckermannVelControl::runSanityChecks()
if (_param_ro_speed_limit.get() < FLT_EPSILON) {
ret = false;
if (_prev_param_check_passed) {
events::send<float>(events::ID("ackermann_vel_control_conf_invalid_speed_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get());
}
events::send<float>(events::ID("ackermann_vel_control_conf_invalid_speed_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get());
}
_prev_param_check_passed = ret;
return ret;
}

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

@ -51,10 +51,7 @@
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_velocity_status.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
using namespace matrix;
@ -73,10 +70,21 @@ public:
~AckermannVelControl() = default;
/**
* @brief Update velocity controller.
* @brief Generate and publish roverAttitudeSetpoint and RoverThrottleSetpoint from roverVelocitySetpoint.
*/
void updateVelControl();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
/**
* @brief Reset velocity controller.
*/
void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _bearing_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);};
protected:
/**
* @brief Update the parameters of the module.
@ -85,55 +93,32 @@ protected:
private:
/**
* @brief Update uORB subscriptions used in velocity controller.
* @brief Update uORB subscriptions used in position controller.
*/
void updateSubscriptions();
/**
* @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint.
*/
void generateVelocitySetpoint();
/**
* @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint
* from roverVelocitySetpoint.
*/
void generateAttitudeAndThrottleSetpoint();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
// uORB subscriptions
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{};
// uORB publications
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
rover_velocity_setpoint_s _rover_velocity_setpoint{};
// Variables
hrt_abstime _timestamp{0};
Quatf _vehicle_attitude_quaternion{};
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};
float _speed_setpoint{NAN};
float _bearing_setpoint{NAN};
// Controllers
PID _pid_speed;
SlewRate<float> _speed_setpoint;
SlewRate<float> _adjusted_speed_setpoint;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,

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

@ -39,8 +39,6 @@ RoverAckermann::RoverAckermann() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
{
_rover_throttle_setpoint_pub.advertise();
_rover_steering_setpoint_pub.advertise();
updateParams();
}
@ -53,49 +51,472 @@ bool RoverAckermann::init()
void RoverAckermann::updateParams()
{
ModuleParams::updateParams();
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON
&& _param_ra_max_str_ang.get() > FLT_EPSILON) {
_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
}
}
void RoverAckermann::Run()
{
if (_parameter_update_sub.updated()) {
parameter_update_s param_update{};
_parameter_update_sub.copy(&param_update);
updateParams();
runSanityChecks();
}
if (_vehicle_control_mode_sub.updated()) {
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
vehicle_control_mode_s vehicle_control_mode{};
_vehicle_control_mode_sub.copy(&vehicle_control_mode);
// Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated and not on changes)
if (vehicle_control_mode.flag_control_position_enabled != _vehicle_control_mode.flag_control_position_enabled ||
vehicle_control_mode.flag_control_velocity_enabled != _vehicle_control_mode.flag_control_velocity_enabled ||
vehicle_control_mode.flag_control_attitude_enabled != _vehicle_control_mode.flag_control_attitude_enabled ||
vehicle_control_mode.flag_control_rates_enabled != _vehicle_control_mode.flag_control_rates_enabled) {
_vehicle_control_mode = vehicle_control_mode;
runSanityChecks();
} else {
_vehicle_control_mode = vehicle_control_mode;
}
}
const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled
&& !_vehicle_control_mode.flag_control_rates_enabled;
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (full_manual_mode_enabled) { // Manual mode
generateSteeringAndThrottleSetpoint();
// Reset all controllers if the navigation state changes
if (vehicle_status.nav_state != _nav_state) { reset();}
_nav_state = vehicle_status.nav_state;
}
if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) {
// Generate setpoints
if (_vehicle_control_mode.flag_control_manual_enabled) {
manualControl();
} else if (_vehicle_control_mode.flag_control_auto_enabled) {
autoPositionMode();
} else if (_vehicle_control_mode.flag_control_offboard_enabled) {
offboardControl();
}
updateControllers();
} else if (_was_armed) { // Reset all controllers and stop the vehicle
reset();
_ackermann_act_control.stopVehicle();
_was_armed = false;
}
_ackermann_pos_control.updatePosControl();
_ackermann_vel_control.updateVelControl();
_ackermann_att_control.updateAttControl();
_ackermann_rate_control.updateRateControl();
_ackermann_act_control.updateActControl();
}
void RoverAckermann::generateSteeringAndThrottleSetpoint()
void RoverAckermann::manualControl()
{
switch (_nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
manualManualMode();
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
manualAcroMode();
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
manualStabMode();
break;
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
manualPositionMode();
break;
}
}
void RoverAckermann::manualManualMode()
{
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = hrt_absolute_time();
rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll;
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = hrt_absolute_time();
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
}
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = hrt_absolute_time();
rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll;
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = hrt_absolute_time();
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
void RoverAckermann::manualAcroMode()
{
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = hrt_absolute_time();
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * math::interpolate<float>
(manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}
void RoverAckermann::manualStabMode()
{
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
}
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = hrt_absolute_time();
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
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(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control
_stab_yaw_setpoint = NAN;
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = hrt_absolute_time();
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
_stab_yaw_setpoint = _vehicle_yaw;
}
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = hrt_absolute_time();
rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
void RoverAckermann::manualPositionMode()
{
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_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);
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);
}
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
}
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
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_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
_pos_ctl_course_direction = Vector2f(NAN, NAN);
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta);
const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint));
const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _param_pp_lookahd_max.get() *
pos_ctl_course_direction;
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
rover_position_setpoint.start_ned[0] = NAN;
rover_position_setpoint.start_ned[1] = NAN;
rover_position_setpoint.arrival_speed = NAN;
rover_position_setpoint.cruising_speed = speed_setpoint;
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
} else { // Course control if the steering input is zero (keep driving on a straight line)
if (!_pos_ctl_course_direction.isAllFinite()) {
_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
_pos_ctl_start_position_ned = _curr_pos_ned;
}
// 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_setpoint) *
vector_scaling * _pos_ctl_course_direction;
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
rover_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0);
rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1);
rover_position_setpoint.arrival_speed = NAN;
rover_position_setpoint.cruising_speed = speed_setpoint;
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
}
void RoverAckermann::autoPositionMode()
{
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_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);
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);
}
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
}
if (_position_setpoint_triplet_sub.updated()) {
autoUpdateWaypointsAndAcceptanceRadius();
}
// Distances to waypoints
const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2));
const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
rover_position_setpoint.arrival_speed = autoArrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type,
_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.cruising_speed = autoCruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp,
distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle,
_prev_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
void RoverAckermann::autoUpdateWaypointsAndAcceptanceRadius()
{
position_setpoint_triplet_s position_setpoint_triplet{};
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
_curr_wp_type = position_setpoint_triplet.current.type;
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
_curr_pos_ned, _global_ned_proj_ref);
_prev_waypoint_transition_angle = _waypoint_transition_angle;
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
// Update acceptance radius
_prev_acceptance_radius = _acceptance_radius;
if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
_acceptance_radius = autoUpdateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get());
} else {
_acceptance_radius = _param_nav_acc_rad.get();
}
// Waypoint cruising speed
_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
}
float RoverAckermann::autoUpdateAcceptanceRadius(const float waypoint_transition_angle,
const float default_acceptance_radius, const float acceptance_radius_gain,
const float acceptance_radius_max, const float wheel_base, const float max_steer_angle)
{
// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
float acceptance_radius = default_acceptance_radius;
if (PX4_ISFINITE(_waypoint_transition_angle)) {
const float theta = waypoint_transition_angle / 2.f;
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
const float acceptance_radius_temp_scaled = acceptance_radius_gain *
acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
acceptance_radius_max);
}
// Publish updated acceptance radius
position_controller_status_s pos_ctrl_status{};
pos_ctrl_status.acceptance_radius = acceptance_radius;
pos_ctrl_status.timestamp = hrt_absolute_time();
_position_controller_status_pub.publish(pos_ctrl_status);
return acceptance_radius;
}
float RoverAckermann::autoArrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad,
const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate)
{
if (!PX4_ISFINITE(waypoint_transition_angle)
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
return 0.f; // Stop at the waypoint
} else {
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
const float cornering_speed = max_yaw_rate * turning_circle;
return math::constrain(cornering_speed, miss_speed_min, cruising_speed); // Slow down for cornering
}
}
float RoverAckermann::autoCruisingSpeed(const float cruising_speed, const float miss_speed_min,
const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, const float prev_acc_rad,
const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate)
{
// Catch improper values
if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) {
return cruising_speed;
}
// Cornering slow down effect
if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) {
const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f);
const float cornering_speed = max_yaw_rate * turning_circle;
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
}
if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) {
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
const float cornering_speed = max_yaw_rate * turning_circle;
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
}
return cruising_speed; // Fallthrough
}
void RoverAckermann::offboardControl()
{
offboard_control_mode_s offboard_control_mode{};
_offboard_control_mode_sub.copy(&offboard_control_mode);
trajectory_setpoint_s trajectory_setpoint{};
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
if (offboard_control_mode.position) {
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
rover_position_setpoint.start_ned[0] = NAN;
rover_position_setpoint.start_ned[1] = NAN;
rover_position_setpoint.cruising_speed = NAN;
rover_position_setpoint.arrival_speed = NAN;
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
} else if (offboard_control_mode.velocity) {
const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = hrt_absolute_time();
rover_velocity_setpoint.speed = velocity_ned.norm();
rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
void RoverAckermann::updateControllers()
{
if (_vehicle_control_mode.flag_control_position_enabled) {
_ackermann_pos_control.updatePosControl();
}
if (_vehicle_control_mode.flag_control_velocity_enabled) {
_ackermann_vel_control.updateVelControl();
}
if (_vehicle_control_mode.flag_control_attitude_enabled) {
_ackermann_att_control.updateAttControl();
}
if (_vehicle_control_mode.flag_control_rates_enabled) {
_ackermann_rate_control.updateRateControl();
}
if (_vehicle_control_mode.flag_control_allocation_enabled) {
_ackermann_act_control.updateActControl();
}
}
void RoverAckermann::runSanityChecks()
{
if (_vehicle_control_mode.flag_control_rates_enabled && !_ackermann_rate_control.runSanityChecks()) {
_sanity_checks_passed = false;
return;
}
if (_vehicle_control_mode.flag_control_attitude_enabled && !_ackermann_att_control.runSanityChecks()) {
_sanity_checks_passed = false;
return;
}
if (_vehicle_control_mode.flag_control_velocity_enabled && !_ackermann_vel_control.runSanityChecks()) {
_sanity_checks_passed = false;
return;
}
if (_vehicle_control_mode.flag_control_position_enabled && !_ackermann_pos_control.runSanityChecks()) {
_sanity_checks_passed = false;
return;
}
_sanity_checks_passed = true;
}
void RoverAckermann::reset()
{
_ackermann_vel_control.reset();
_ackermann_att_control.reset();
_ackermann_rate_control.reset();
_stab_yaw_setpoint = NAN;
_pos_ctl_course_direction = Vector2f(NAN, NAN);
_pos_ctl_start_position_ned = Vector2f(NAN, NAN);
_curr_pos_ned = Vector2f(NAN, NAN);
}
int RoverAckermann::task_spawn(int argc, char *argv[])

178
src/modules/rover_ackermann/RoverAckermann.hpp
View File

@ -40,14 +40,30 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
// Library includes
#include <math.h>
#include <lib/rover_control/RoverControl.hpp>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_controller_status.h>
// Local includes
#include "AckermannActControl/AckermannActControl.hpp"
@ -87,19 +103,128 @@ private:
void Run() override;
/**
* @brief Generate and publish roverSteeringSetpoint and roverThrottleSetpoint from manualControlSetpoint (Manual Mode).
* @brief Handle manual control
*/
void generateSteeringAndThrottleSetpoint();
void manualControl();
/**
* @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
*/
void manualManualMode();
/**
* @brief Generate and publish roverThrottleSetpoint and RoverRateSetpoint from manualControlSetpoint.
*/
void manualAcroMode();
/**
* @brief Generate and publish roverThrottleSetpoint and RoverAttitudeSetpoint from manualControlSetpoint.
*/
void manualStabMode();
/**
* @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
*/
void manualPositionMode();
/**
* @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet.
*/
void autoPositionMode();
/**
* @brief Update global/NED waypoint coordinates and acceptance radius.
*/
void autoUpdateWaypointsAndAcceptanceRadius();
/**
* @brief Publish the acceptance radius for current waypoint based on the angle between a line segment
* from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle.
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param default_acceptance_radius Default acceptance radius for waypoints [m].
* @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-].
* @param acceptance_radius_max Maximum value for the acceptance radius [m].
* @param wheel_base Rover wheelbase [m].
* @param max_steer_angle Rover maximum steer angle [rad].
* @return Updated acceptance radius [m].
*/
float autoUpdateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius,
float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle);
/**
* @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner.
* @param cruising_speed Cruising speed [m/s].
* @param miss_speed_min Minimum speed setpoint [m/s].
* @param acc_rad Acceptance radius of the current waypoint [m].
* @param curr_wp_type Type of the current waypoint.
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
* @return Speed setpoint [m/s].
*/
float autoArrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type,
float waypoint_transition_angle, float max_yaw_rate);
/**
* @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner.
* @param cruising_speed Cruising speed [m/s].
* @param miss_speed_min Minimum speed setpoint [m/s].
* @param distance_to_prev_wp Distance to the previous waypoint [m].
* @param distance_to_curr_wp Distance to the current waypoint [m].
* @param acc_rad Acceptance radius of the current waypoint [m].
* @param prev_acc_rad Acceptance radius of the previous waypoint [m].
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
* @return Speed setpoint [m/s].
*/
float autoCruisingSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp,
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float waypoint_transition_angle,
float prev_waypoint_transition_angle, float max_yaw_rate);
/**
* @brief Translate trajectorySetpoint to roverSetpoints and publish them
*/
void offboardControl();
/**
* @brief Update the controllers
*/
void updateControllers();
/**
* @brief Check proper parameter setup for the controllers
*
* Modifies:
*
* - _sanity_checks_passed: true if checks for all active controllers pass
*/
void runSanityChecks();
/**
* @brief Reset controllers and manual mode variables.
*/
void reset();
// uORB subscriptions
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
vehicle_control_mode_s _vehicle_control_mode{};
// uORB publications
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
// Class instances
AckermannActControl _ackermann_act_control{this};
@ -108,4 +233,43 @@ private:
AckermannVelControl _ackermann_vel_control{this};
AckermannPosControl _ackermann_pos_control{this};
// Variables
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
Quatf _vehicle_attitude_quaternion{};
float _max_yaw_rate{NAN};
float _vehicle_yaw{NAN};
float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
int _nav_state{0}; // Navigation state of the vehicle
bool _sanity_checks_passed{true}; // True if checks for all active controllers pass
bool _was_armed{false}; // True if the vehicle was armed before the last reset
// Auto Mode Variables
Vector2f _curr_wp_ned{};
Vector2f _prev_wp_ned{};
Vector2f _next_wp_ned{};
float _acceptance_radius{0.5f};
float _prev_acceptance_radius{0.5f};
float _cruising_speed{0.f};
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
// Manual Mode Variables
Vector2f _pos_ctl_course_direction{NAN, NAN};
Vector2f _pos_ctl_start_position_ned{NAN, NAN};
Vector2f _curr_pos_ned{NAN, NAN};
float _stab_yaw_setpoint{NAN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_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::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain
)
};