diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index b5468880587c..5409e40d841a 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -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{}; @@ -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[]) diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index cea9300a1b3d..3617b5b80ba6 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -59,6 +59,7 @@ // Local includes #include "RoverAckermannGuidance/RoverAckermannGuidance.hpp" +using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint; using namespace time_literals; @@ -83,12 +84,32 @@ class RoverAckermann : public ModuleBase, 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)}; @@ -100,12 +121,12 @@ class RoverAckermann : public ModuleBase, public ModuleParams, uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; uORB::Publication _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 _steering_with_rate_limit{0.f};