diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp index 4022464167..b448d09396 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp @@ -77,12 +77,6 @@ void AckermannActControl::updateActControl() 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 @@ -114,15 +108,7 @@ void AckermannActControl::updateActControl() 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); } - - } void AckermannActControl::stopVehicle() diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp index a866390316..55f4e733c3 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -81,11 +81,6 @@ void AckermannAttControl::updateAttControl() 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); - } 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) diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index a2b5ec0e01..026406c57d 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -57,53 +57,49 @@ void AckermannPosControl::updatePosControl() hrt_abstime timestamp = hrt_absolute_time(); 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; - if (PX4_ISFINITE(distance_to_target) && distance_to_target > _acceptance_radius) { + if (target_waypoint_ned.isAllFinite()) { + float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); - 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 (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)); + 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 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); } - - 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 (_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); @@ -130,6 +126,12 @@ void AckermannPosControl::updateSubscriptions() _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); } + 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; + } + } bool AckermannPosControl::runSanityChecks() diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp index 3c74a3ea48..30cb26510b 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -64,47 +64,49 @@ void AckermannRateControl::updateRateControl() _timestamp = hrt_absolute_time(); const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - 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 (PX4_ISFINITE(_yaw_rate_setpoint)) { + if (fabsf(_estimated_speed) > FLT_EPSILON) { + // 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); + 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); } - _adjusted_yaw_rate_setpoint.update(constrained_yaw_rate, dt); + // 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 { - _adjusted_yaw_rate_setpoint.setForcedValue(constrained_yaw_rate); + _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); } - - // 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); } diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp index 2804d0b8a6..dffbf87758 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp @@ -87,12 +87,6 @@ void AckermannVelControl::updateVelControl() 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) diff --git a/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp b/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp index f2810f06f4..cdedb950c2 100644 --- a/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp +++ b/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp @@ -52,27 +52,6 @@ void ManualMode::updateParams() _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } -void ManualMode::manualControl(const int nav_state) -{ - switch (nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: - manual(); - break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: - acro(); - break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: - stab(); - break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: - position(); - break; - } -} - void ManualMode::manual() { manual_control_setpoint_s manual_control_setpoint{}; @@ -121,20 +100,24 @@ void ManualMode::stab() 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 + if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { _stab_yaw_setpoint = NAN; - const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta); + + // Rate control + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = hrt_absolute_time(); + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + // Set uncontrolled setpoint invalid rover_attitude_setpoint_s rover_attitude_setpoint{}; rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + rover_attitude_setpoint.yaw_setpoint = NAN; _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + } else { // Heading control if (!PX4_ISFINITE(_stab_yaw_setpoint)) { _stab_yaw_setpoint = _vehicle_yaw; } @@ -173,30 +156,44 @@ void ManualMode::position() 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 + if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON + || fabsf(speed_setpoint) < FLT_EPSILON) { _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; + + // Speed control + rover_velocity_setpoint_s rover_velocity_setpoint{}; + rover_velocity_setpoint.timestamp = hrt_absolute_time(); + rover_velocity_setpoint.speed = speed_setpoint; + rover_velocity_setpoint.bearing = NAN; + rover_velocity_setpoint.yaw = NAN; + _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + + // Rate control + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = hrt_absolute_time(); + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + // Set uncontrolled setpoints invalid + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = NAN; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + 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.position_ned[0] = NAN; + rover_position_setpoint.position_ned[1] = NAN; 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.cruising_speed = NAN; 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) + } else { // Course control 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; diff --git a/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp b/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp index 8079b326ac..4f6c4a0506 100644 --- a/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp +++ b/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp @@ -67,23 +67,6 @@ public: ManualMode(ModuleParams *parent); ~ManualMode() = default; - /** - * @brief Generate and publish roverSetpoints from manualControlSetpoints. - */ - void manualControl(int nav_state); - - /** - * @brief Reset manual mode variables. - */ - void reset(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: /** * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. */ @@ -104,6 +87,18 @@ private: */ void position(); + /** + * @brief Reset manual mode variables. + */ + void reset(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index 236ecb6af2..f22676e3fa 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -66,13 +66,18 @@ void RoverAckermann::Run() 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) { + // Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated at 2 Hz) + if (_vehicle_control_mode.flag_control_manual_enabled != vehicle_control_mode.flag_control_manual_enabled || + _vehicle_control_mode.flag_control_auto_enabled != vehicle_control_mode.flag_control_auto_enabled || + _vehicle_control_mode.flag_control_offboard_enabled != vehicle_control_mode.flag_control_offboard_enabled || + _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.flag_control_allocation_enabled != vehicle_control_mode.flag_control_allocation_enabled) { _vehicle_control_mode = vehicle_control_mode; runSanityChecks(); + reset(); } else { _vehicle_control_mode = vehicle_control_mode; @@ -80,20 +85,13 @@ void RoverAckermann::Run() } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - - // 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) { + + _was_armed = true; + // Generate setpoints if (_vehicle_control_mode.flag_control_manual_enabled) { - _manual_mode.manualControl(_nav_state); + manualControl(); } else if (_vehicle_control_mode.flag_control_auto_enabled) { _auto_mode.autoControl(); @@ -112,6 +110,22 @@ void RoverAckermann::Run() } +void RoverAckermann::manualControl() +{ + if (_vehicle_control_mode.flag_control_position_enabled) { + _manual_mode.position(); + + } else if (_vehicle_control_mode.flag_control_attitude_enabled) { + _manual_mode.stab(); + + } else if (_vehicle_control_mode.flag_control_rates_enabled) { + _manual_mode.acro(); + + } else if (_vehicle_control_mode.flag_control_allocation_enabled) { + _manual_mode.manual(); + } +} + void RoverAckermann::updateControllers() { if (_vehicle_control_mode.flag_control_position_enabled) { diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index c9f8370afa..a0232b4b45 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -48,7 +48,6 @@ #include #include #include -#include // Local includes #include "AckermannActControl/AckermannActControl.hpp" @@ -90,6 +89,11 @@ protected: private: void Run() override; + /** + * @brief Generate and publish roverSetpoints from manualControlSetpoints. + */ + void manualControl(); + /** * @brief Update the active controllers. */ @@ -112,7 +116,6 @@ private: // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; vehicle_control_mode_s _vehicle_control_mode{}; // Class instances @@ -126,7 +129,6 @@ private: OffboardMode _offboard_mode{this}; // Variables - 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 };