Skip to content

Commit

Permalink
DifferentialDrive: move spoolup consideration to the main module
Browse files Browse the repository at this point in the history
  • Loading branch information
MaEtUgR committed Feb 12, 2024
1 parent bef694f commit 1998f54
Show file tree
Hide file tree
Showing 4 changed files with 6 additions and 26 deletions.
4 changes: 3 additions & 1 deletion src/modules/differential_drive/DifferentialDrive.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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);
}
}
Expand Down
3 changes: 2 additions & 1 deletion src/modules/differential_drive/DifferentialDrive.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModulePar
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
(ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_wheel_speed,
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
)
};
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@
#include "DifferentialDriveControl.hpp"

using namespace matrix;
using namespace time_literals;

DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent)
{
Expand Down Expand Up @@ -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
Expand All @@ -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);
}
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>

class DifferentialDriveControl : public ModuleParams
{
Expand All @@ -68,7 +67,6 @@ class DifferentialDriveControl : public ModuleParams
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_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};

Expand All @@ -84,14 +82,10 @@ class DifferentialDriveControl : public ModuleParams
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<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity,

(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity
)
};

0 comments on commit 1998f54

Please sign in to comment.