Skip to content

Commit

Permalink
ackermann: refactor main files
Browse files Browse the repository at this point in the history
  • Loading branch information
@chfriedrich98
chfriedrich98 committed Aug 8, 2024
1 parent cd231d0 commit 1af295f
Show file tree
Hide file tree
Showing 2 changed files with 105 additions and 66 deletions.
146 changes: 82 additions & 64 deletions src/modules/rover_ackermann/RoverAckermann.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,32 +73,15 @@ void RoverAckermann::Run()
return;
}

// uORB subscriber updates
if (_parameter_update_sub.updated()) {
updateParams();
}

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

if (_local_position_sub.updated()) {
vehicle_local_position_s local_position{};
_local_position_sub.copy(&local_position);
const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
_actual_speed = rover_velocity.norm();
}
updateSubscriptions();

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

// Generate motor setpoints
if (_armed) {
// Navigation modes
switch (_nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
manual_control_setpoint_s manual_control_setpoint{};
Expand All @@ -118,62 +101,97 @@ void RoverAckermann::Run()
default: // Unimplemented nav states will stop the rover
_motor_setpoint.steering = 0.f;
_motor_setpoint.throttle = 0.f;
_throttle_with_accel_limit.setForcedValue(0.f);
_steering_with_rate_limit.setForcedValue(0.f);
break;
}

// Sanitize actuator commands
if (!PX4_ISFINITE(_motor_setpoint.steering)) {
_motor_setpoint.steering = 0.f;
}
} else { // Reset on disarm
_motor_setpoint.steering = 0.f;
_motor_setpoint.throttle = 0.f;
_throttle_with_accel_limit.setForcedValue(0.f);
_steering_with_rate_limit.setForcedValue(0.f);
}

if (!PX4_ISFINITE(_motor_setpoint.throttle)) {
_motor_setpoint.throttle = 0.f;
}
publishMotorSetpoints(applySlewRates(_motor_setpoint, dt));
}

// Acceleration slew rate
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
&& fabsf(_motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
_throttle_with_accel_limit.update(_motor_setpoint.throttle, dt);
void RoverAckermann::updateSubscriptions()
{
if (_parameter_update_sub.updated()) {
updateParams();
}

} else {
_throttle_with_accel_limit.setForcedValue(_motor_setpoint.throttle);
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
_vehicle_status_sub.copy(&vehicle_status);
_nav_state = vehicle_status.nav_state;
_armed = vehicle_status.arming_state == 2;
}

// Steering slew rate
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
_steering_with_rate_limit.update(_motor_setpoint.steering, dt);
if (_local_position_sub.updated()) {
vehicle_local_position_s local_position{};
_local_position_sub.copy(&local_position);
const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
_actual_speed = rover_velocity.norm();
}
}
motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt)
{
// Sanitize actuator commands
if (!PX4_ISFINITE(motor_setpoint.steering)) {
motor_setpoint.steering = 0.f;
}

} else {
_steering_with_rate_limit.setForcedValue(_motor_setpoint.steering);
}
if (!PX4_ISFINITE(motor_setpoint.throttle)) {
motor_setpoint.throttle = 0.f;
}

// Publish rover ackermann status (logging)
rover_ackermann_status_s rover_ackermann_status{};
rover_ackermann_status.timestamp = _timestamp;
rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
rover_ackermann_status.actual_speed = _actual_speed;
_rover_ackermann_status_pub.publish(rover_ackermann_status);

// Publish to motor
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);

// Publish to servo
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
// Acceleration slew rate
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
&& fabsf(motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
_throttle_with_accel_limit.update(motor_setpoint.throttle, dt);

} else { // Reset on disarm
_motor_setpoint.steering = 0.f;
_motor_setpoint.throttle = 0.f;
_throttle_with_accel_limit.setForcedValue(0.f);
_steering_with_rate_limit.setForcedValue(0.f);
} else {
_throttle_with_accel_limit.setForcedValue(motor_setpoint.throttle);
}

// Steering slew rate
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
_steering_with_rate_limit.update(motor_setpoint.steering, dt);

} else {
_steering_with_rate_limit.setForcedValue(motor_setpoint.steering);
}

motor_setpoint_struct motor_setpoint_temp{};
motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
return motor_setpoint_temp;
}

void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates)
{
// Publish rover Ackermann status (logging)
rover_ackermann_status_s rover_ackermann_status{};
rover_ackermann_status.timestamp = _timestamp;
rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
rover_ackermann_status.actual_speed = _actual_speed;
_rover_ackermann_status_pub.publish(rover_ackermann_status);

// Publish to motor
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = motor_setpoint_with_slew_rates.throttle;
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);

// Publish to servo
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering;
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
}

int RoverAckermann::task_spawn(int argc, char *argv[])
Expand Down
25 changes: 23 additions & 2 deletions src/modules/rover_ackermann/RoverAckermann.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@

// Local includes
#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint;

using namespace time_literals;

Expand All @@ -83,12 +84,32 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,

bool init();


protected:
void updateParams() override;

private:
void Run() override;

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

/**
* @brief Apply slew rates to motor setpoints.
* @param motor_setpoint Normalized steering and throttle setpoints.
* @param dt Time since last update [s].
* @return Motor setpoint with applied slew rates.
*/
motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt);

/**
* @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus.
* @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates.
*/
void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates);

// uORB subscriptions
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
Expand All @@ -100,12 +121,12 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::Publication<rover_ackermann_status_s> _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)};

// Instances
// Class instances
RoverAckermannGuidance _ackermann_guidance{this};

// Variables
int _nav_state{0};
RoverAckermannGuidance::motor_setpoint _motor_setpoint;
motor_setpoint_struct _motor_setpoint;
hrt_abstime _timestamp{0};
float _actual_speed{0.f};
SlewRate<float> _steering_with_rate_limit{0.f};
Expand Down

0 comments on commit 1af295f

Please sign in to comment.