diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp index c898186ec7..4022464167 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp @@ -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); } diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp index d8933525e3..fb3630c13c 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp @@ -44,7 +44,6 @@ // uORB includes #include #include -#include #include #include #include @@ -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_pub{ORB_ID(actuator_motors)}; - uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _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 _servo_setpoint{0.f}; @@ -101,11 +101,11 @@ private: // Parameters DEFINE_PARAMETERS( - (ParamInt) _param_r_rev, - (ParamFloat) _param_ra_str_rate_limit, - (ParamFloat) _param_ra_max_str_ang, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, + (ParamInt) _param_r_rev, + (ParamFloat) _param_ra_str_rate_limit, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp index 9c07a3ab3b..a866390316 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -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 (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(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 (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() diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp index 93d17928be..2d58bae4ce 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp @@ -47,9 +47,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -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_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _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; diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index 681d775699..a2b5ec0e01 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -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(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); - const float yaw_delta = math::interpolate(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(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; } diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp index fab6c96e30..af2d7581b0 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp @@ -38,26 +38,17 @@ #include // Library includes -#include -#include #include -#include #include -#include #include +#include // uORB includes #include #include #include #include -#include -#include -#include #include -#include -#include -#include #include #include #include @@ -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_pub{ORB_ID(rover_velocity_setpoint)}; - uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; - uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _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) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_yaw_stick_dz, - (ParamFloat) _param_ro_accel_limit, (ParamFloat) _param_ro_decel_limit, (ParamFloat) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_th, (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_p, - (ParamFloat) _param_ra_acc_rad_max, - (ParamFloat) _param_ra_acc_rad_gain, - (ParamFloat) _param_ra_max_str_ang, - (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_nav_acc_rad - + (ParamFloat) _param_ro_yaw_rate_limit ) }; diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp index f27f6127c2..3c74a3ea48 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -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(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(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(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 - (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(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(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(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(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(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(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(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(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(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; } diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp index 8f0891cb48..d5014a4306 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -47,9 +47,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -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_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + uORB::Publication _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 _yaw_rate_setpoint{0.f}; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; DEFINE_PARAMETERS( (ParamFloat) _param_ro_max_thr_speed, diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp index 8d9bf88e8b..2804d0b8a6 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp @@ -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(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(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; } diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp index 74dea1693c..42f437293c 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp @@ -51,10 +51,7 @@ #include #include #include -#include -#include #include -#include #include 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_pub{ORB_ID(rover_throttle_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; uORB::Publication _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 _speed_setpoint; + SlewRate _adjusted_speed_setpoint; DEFINE_PARAMETERS( (ParamFloat) _param_ro_max_thr_speed, diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index d57d6a0d8c..fcc0f059c6 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -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(¶m_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 + (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(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(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_delta = math::interpolate(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(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[]) diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index 0655ba9dda..968f99ed4f 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -40,14 +40,30 @@ #include #include +// Library includes +#include +#include + // uORB includes #include #include +#include #include +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include +#include +#include +#include +#include +#include +#include // 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_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _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) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_ra_acc_rad_max, + (ParamFloat) _param_ra_acc_rad_gain + ) };