diff --git a/src/modules/differential_drive/DifferentialDrive.cpp b/src/modules/differential_drive/DifferentialDrive.cpp index c37b3ad0c1..70553ffeb3 100644 --- a/src/modules/differential_drive/DifferentialDrive.cpp +++ b/src/modules/differential_drive/DifferentialDrive.cpp @@ -82,7 +82,6 @@ void DifferentialDrive::Run() vehicle_control_mode_s vehicle_control_mode{}; if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { - _differential_drive_kinematics.setArmed(vehicle_control_mode.flag_armed); _manual_driving = vehicle_control_mode.flag_control_manual_enabled; _mission_driving = vehicle_control_mode.flag_control_auto_enabled; } @@ -92,6 +91,9 @@ void DifferentialDrive::Run() vehicle_status_s vehicle_status{}; if (_vehicle_status_sub.copy(&vehicle_status)) { + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s); + _differential_drive_kinematics.setArmed(spooled_up); _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); } } diff --git a/src/modules/differential_drive/DifferentialDrive.hpp b/src/modules/differential_drive/DifferentialDrive.hpp index 98fd0b596a..255bb0c526 100644 --- a/src/modules/differential_drive/DifferentialDrive.hpp +++ b/src/modules/differential_drive/DifferentialDrive.hpp @@ -99,6 +99,7 @@ private: (ParamFloat) _param_rdd_speed_scale, (ParamFloat) _param_rdd_wheel_base, (ParamFloat) _param_rdd_wheel_speed, - (ParamFloat) _param_rdd_wheel_radius + (ParamFloat) _param_rdd_wheel_radius, + (ParamFloat) _param_com_spoolup_time ) }; diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp index 57f3c4db7a..728c5e666a 100644 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp +++ b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp @@ -34,7 +34,6 @@ #include "DifferentialDriveControl.hpp" using namespace matrix; -using namespace time_literals; DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent) { @@ -90,16 +89,6 @@ void DifferentialDriveControl::control(float dt) } } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - - if (_vehicle_status_sub.copy(&vehicle_status)) { - - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - _spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s; - } - } - _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); // PID to reach setpoint using control_output @@ -115,12 +104,6 @@ void DifferentialDriveControl::control(float dt) pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); } - - if (!_spooled_up) { - differential_drive_control_output.speed = 0.0f; - differential_drive_control_output.yaw_rate = 0.0f; - } - differential_drive_control_output.timestamp = hrt_absolute_time(); _differential_drive_control_output_pub.publish(differential_drive_control_output); } diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp index 8df4ef7214..8d3b7e1c21 100644 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp +++ b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp @@ -48,7 +48,6 @@ #include #include #include -#include class DifferentialDriveControl : public ModuleParams { @@ -68,7 +67,6 @@ private: uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)}; @@ -84,14 +82,10 @@ private: PID_t _pid_angular_velocity; ///< The PID controller for yaw rate. PID_t _pid_speed; ///< The PID controller for velocity. - bool _spooled_up{false}; - DEFINE_PARAMETERS( (ParamFloat) _param_rdd_p_gain_speed, (ParamFloat) _param_rdd_i_gain_speed, (ParamFloat) _param_rdd_p_gain_angular_velocity, - (ParamFloat) _param_rdd_i_gain_angular_velocity, - - (ParamFloat) _param_com_spoolup_time + (ParamFloat) _param_rdd_i_gain_angular_velocity ) };