Skip to content

Commit

Permalink
acro_mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@chfriedrich98
chfriedrich98 committed Aug 26, 2024
1 parent be53497 commit e403480
Show file tree
Hide file tree
Showing 7 changed files with 379 additions and 262 deletions.
20 changes: 20 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_r1_rover_mecanum
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,26 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum}

param set-default SIM_GZ_EN 1 # Gazebo bridge

# Rover parameters
param set-default RM_WHEEL_TRACK 0.3
param set-default RM_MAN_YAW_SCALE 0.1
param set-default RM_YAW_RATE_I 0.1
param set-default RM_YAW_RATE_P 0.5
param set-default RM_MAX_ACCEL 6
param set-default RM_MAX_JERK 30
param set-default RM_MAX_SPEED 7
param set-default RM_HEADING_P 5
param set-default RM_HEADING_I 0.1
param set-default RM_MAX_YAW_RATE 180
param set-default RM_MISS_SPD_DEF 7
param set-default RM_TRANS_DRV_TRN 0.349066
param set-default RM_TRANS_TRN_DRV 0.174533

# Pure pursuit parameters
param set-default PP_LOOKAHD_MAX 30
param set-default PP_LOOKAHD_MIN 2
param set-default PP_LOOKAHD_GAIN 1

# Simulated sensors
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 0
Expand Down
2 changes: 2 additions & 0 deletions src/modules/logger/logged_topics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,8 @@ void LoggedTopics::add_default_topics()
add_optional_topic("rover_ackermann_status", 100);
add_optional_topic("rover_differential_guidance_status", 100);
add_optional_topic("rover_differential_status", 100);
add_optional_topic("rover_mecanum_guidance_status", 100);
add_optional_topic("rover_mecanum_status", 100);
add_topic("rtl_time_estimate", 1000);
add_topic("rtl_status", 2000);
add_optional_topic("sensor_airflow", 100);
Expand Down
215 changes: 110 additions & 105 deletions src/modules/rover_mecanum/RoverMecanum.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ RoverMecanum::RoverMecanum() :
{
updateParams();
_rover_mecanum_status_pub.advertise();
// pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
}

bool RoverMecanum::init()
Expand All @@ -54,14 +54,14 @@ void RoverMecanum::updateParams()
{
ModuleParams::updateParams();

// _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;

// pid_set_parameters(&_pid_yaw_rate,
// _param_rd_p_gain_yaw_rate.get(), // Proportional gain
// _param_rd_i_gain_yaw_rate.get(), // Integral gain
// 0.f, // Derivative gain
// 1.f, // Integral limit
// 1.f); // Output limit
pid_set_parameters(&_pid_yaw_rate,
_param_rm_yaw_rate_p.get(), // Proportional gain
_param_rm_yaw_rate_i.get(), // Integral gain
0.f, // Derivative gain
1.f, // Integral limit
1.f); // Output limit

}

Expand All @@ -73,43 +73,12 @@ void RoverMecanum::Run()
return;
}

// hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
// const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
updateSubscriptions();

// uORB subscriber updates
if (_parameter_update_sub.updated()) {
parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
updateParams();
}

if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
_nav_state = vehicle_status.nav_state;
}

if (_vehicle_angular_velocity_sub.updated()) {
vehicle_angular_velocity_s vehicle_angular_velocity{};
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
}

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);
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_forward_speed = velocity_in_body_frame(0);
}
// Timestamps
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;

// Navigation modes
switch (_nav_state) {
Expand All @@ -126,25 +95,24 @@ void RoverMecanum::Run()
_mecanum_setpoint.closed_loop_yaw_rate = false;
} break;

// case vehicle_status_s::NAVIGATION_STATE_ACRO: {
// manual_control_setpoint_s manual_control_setpoint{};
case vehicle_status_s::NAVIGATION_STATE_ACRO: {
manual_control_setpoint_s manual_control_setpoint{};

// if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// _mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle;
// _mecanum_setpoint.lateral_throttle = manual_control_setpoint.yaw;
// _mecanum_setpoint.yaw_rate = math::interpolate<float>(manual_control_setpoint.roll,
// -1.f, 1.f,
// -_max_yaw_rate, _max_yaw_rate);
// }
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
_mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle;
_mecanum_setpoint.lateral_throttle = manual_control_setpoint.roll;
_mecanum_setpoint.yaw_rate = math::interpolate<float>(manual_control_setpoint.yaw,
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
}

// _mecanum_setpoint.closed_loop_yaw_rate = true;
// } break;
_mecanum_setpoint.closed_loop_yaw_rate = true;
} break;

// case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
// case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
// _mecanum_setpoint = _rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed,
// _nav_state);
// break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_mecanum_setpoint = _rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed,
_nav_state);
break;

default: // Unimplemented nav states will stop the rover
_mecanum_setpoint.forward_throttle = 0.f;
Expand All @@ -153,26 +121,27 @@ void RoverMecanum::Run()
break;
}

// float speed_diff_normalized = _mecanum_setpoint.yaw_rate;

// // Closed loop yaw rate control
// if (_mecanum_setpoint.closed_loop_yaw_rate) {
// if (fabsf(_mecanum_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD) {
// speed_diff_normalized = 0.f;
// pid_reset_integral(&_pid_yaw_rate);

// } else {
// const float speed_diff = _mecanum_setpoint.yaw_rate * _param_rd_wheel_track.get(); // Feedforward
// speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_speed.get(),
// _param_rd_max_speed.get(), -1.f, 1.f);
// speed_diff_normalized = math::constrain(speed_diff_normalized +
// pid_calculate(&_pid_yaw_rate, _mecanum_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt),
// -1.f, 1.f); // Feedback
// }

// } else {
// pid_reset_integral(&_pid_yaw_rate);
// }
float speed_diff_normalized = _mecanum_setpoint.yaw_rate;

// Closed loop yaw rate control
if (_mecanum_setpoint.closed_loop_yaw_rate) {
if (fabsf(_mecanum_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD
|| fabsf(_mecanum_setpoint.yaw_rate) < FLT_EPSILON) {
speed_diff_normalized = 0.f;
pid_reset_integral(&_pid_yaw_rate);

} else {
const float speed_diff = _mecanum_setpoint.yaw_rate * _param_rm_wheel_track.get(); // Feedforward
speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rm_max_speed.get(),
_param_rm_max_speed.get(), -1.f, 1.f);
speed_diff_normalized = math::constrain(speed_diff_normalized +
pid_calculate(&_pid_yaw_rate, _mecanum_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt),
-1.f, 1.f); // Feedback
}

} else {
pid_reset_integral(&_pid_yaw_rate);
}

// Publish rover mecanum status (logging)
rover_mecanum_status_s rover_mecanum_status{};
Expand All @@ -187,46 +156,82 @@ void RoverMecanum::Run()
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
computeMotorCommands(_mecanum_setpoint.forward_throttle, _mecanum_setpoint.lateral_throttle,
_mecanum_setpoint.yaw_rate).copyTo(actuator_motors.control);
speed_diff_normalized).copyTo(actuator_motors.control);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);

}

matrix::Vector4f RoverMecanum::computeMotorCommands(float linear_velocity_x, float linear_velocity_y,
const float yaw_rate)
void RoverMecanum::updateSubscriptions()
{
const float wb = _param_rm_wheel_base.get(); // TODO: This function should only use normalized values
if (_parameter_update_sub.updated()) {
parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
updateParams();
}

if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
_nav_state = vehicle_status.nav_state;
}

if (_vehicle_angular_velocity_sub.updated()) {
vehicle_angular_velocity_s vehicle_angular_velocity{};
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
}

// Prioritize ratio between forward and lateral velocity
float combined_velocity = fabsf(linear_velocity_x) + fabsf(linear_velocity_y);
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 (combined_velocity > 1.f) {
linear_velocity_x /= combined_velocity;
linear_velocity_y /= combined_velocity;
combined_velocity = 1.f;
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_forward_speed = velocity_in_body_frame(0);
}
}

// Prioritize yaw rate
const float total_velocity = combined_velocity + fabsf(2 * yaw_rate * wb);
matrix::Vector4f RoverMecanum::computeMotorCommands(float forward_speed, float lateral_speed, float speed_diff)
{
// Prioritize ratio between forward and lateral speed over either magnitude
float combined_speed = fabsf(forward_speed) + fabsf(lateral_speed);

if (total_velocity > 1.f) {
const float excess_velocity = fabsf(total_velocity - 1.f);
linear_velocity_x -= sign(linear_velocity_x) * 0.5f * excess_velocity;
linear_velocity_y -= sign(linear_velocity_y) * 0.5f * excess_velocity;
if (combined_speed > 1.f) {
forward_speed /= combined_speed;
lateral_speed /= combined_speed;
combined_speed = 1.f;
}

// Define input vector and matrix data
float input_data[3] = {linear_velocity_x, linear_velocity_y, yaw_rate};
Matrix<float, 3, 1> input(input_data);
float m_data[12] = {1, -1, -2.f * wb, 1, 1, 2.f * wb, 1, 1, -2.f * wb, 1, -1, 2.f * wb};
Matrix<float, 4, 3> m(m_data);
// // Prioritize yaw rate over forward and lateral speed
// const float total_speed = combined_speed + fabsf(speed_diff);

// if (total_speed > 1.f) {
// const float excess_velocity = fabsf(total_speed - 1.f);
// forward_speed -= sign(forward_speed) * 0.5f * excess_velocity;
// lateral_speed -= sign(lateral_speed) * 0.5f * excess_velocity;
// }

// Perform matrix-vector multiplication
auto result = m * input; // result is Matrix<float, 4, 1>
// Prioritize lateral movement over yaw rate
const float total_speed = combined_speed + fabsf(speed_diff);

if (total_speed > 1.f) {
const float excess_velocity = fabsf(total_speed - 1.f);
speed_diff -= sign(speed_diff) * excess_velocity;
}

// Initialize Vector4f with the scaled results
Vector4f motor_commands(result(0, 0), result(1, 0), result(2, 0), result(3, 0));
// Calculate motor commands
const float input_data[3] = {forward_speed, lateral_speed, speed_diff};
const Matrix<float, 3, 1> input(input_data);
const float m_data[12] = {1, -1, -1, 1, 1, 1, 1, 1, -1, 1, -1, 1};
const Matrix<float, 4, 3> m(m_data);
const Vector4f motor_commands = m * input;

return motor_commands;
}
Expand Down
25 changes: 17 additions & 8 deletions src/modules/rover_mecanum/RoverMecanum.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,20 +81,26 @@ class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
bool init();

/**
* @brief Computes motor commands for mecanum drive.
* @brief Turn normalized speed setpoints into normalized motor commands.
*
* @param forward_speed Linear velocity along the x-axis.
* @param speed_diff Speed difference between left and right wheels.
* @return matrix::Vector2f Motor velocities for the right and left motors.
* @param forward_speed Normalized linear speed in body forward direction [-1, 1].
* @param lateral_speed Normalized linear speed in body lateral direction [-1, 1].
* @param speed_diff Normalized speed difference between left and right wheels [-1, 1].
* @return matrix::Vector4f: Normalized motor inputs [-1, 1]
*/
matrix::Vector4f computeMotorCommands(float linear_velocity_x, float linear_velocity_y, float yaw_rate);
matrix::Vector4f computeMotorCommands(float forward_speed, float lateral_speed, float speed_diff);

protected:
void updateParams() override;

private:
void Run() override;

/**
* @brief Update uORB subscriptions.
*/
void updateSubscriptions();

// uORB Subscriptions
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
Expand All @@ -108,7 +114,7 @@ class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
uORB::Publication<rover_mecanum_status_s> _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)};

// Instances
// RoverMecanumGuidance _rover_mecanum_guidance{this};
RoverMecanumGuidance _rover_mecanum_guidance{this};

// Variables
float _vehicle_body_yaw_rate{0.f};
Expand All @@ -125,9 +131,12 @@ class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control

DEFINE_PARAMETERS(
(ParamFloat<px4::params::RM_WHEEL_BASE>) _param_rm_wheel_base,
(ParamFloat<px4::params::RM_WHEEL_RADIUS>) _param_rm_wheel_radius,
(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
(ParamFloat<px4::params::RM_MAN_YAW_SCALE>) _param_rm_man_yaw_scale,
(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
(ParamFloat<px4::params::RM_YAW_RATE_P>) _param_rm_yaw_rate_p,
(ParamFloat<px4::params::RM_YAW_RATE_I>) _param_rm_yaw_rate_i,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};
Loading

0 comments on commit e403480

Please sign in to comment.