Skip to content

feat: added new RC controller for logitech #69

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Jan 22, 2021
Merged

feat: added new RC controller for logitech #69

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2f5c0a4
feat: added new RC controller for logitech
edkoch Jan 15, 2021
a360863
fix: logitech updates
danhennage Jan 15, 2021
64159d8
Merge branch 'master' into BB-886/collision_avoidance
amgitbot Jan 18, 2021
fc58ef0
feat: Took out debug
edkoch Jan 19, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions include/vb_util_lib/rc_controls.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,14 @@ class RC_Controls
bool setDJIGearSwitch(double gear_switch, RC_GEAR& state);
void setDJIStickValue(double input, double& state) { state = input; }

// Logitech callback functions
void logitechRemoteCB(const sensor_msgs::Joy::ConstPtr& msg);
ros::Subscriber logitech_rc_channels_sub_;
void setLogitechControls(const sensor_msgs::Joy::ConstPtr& msg);
bool setLogitechModeSwitch(double mode_switch, RC_MODE& state);
bool setLogitechGearSwitch(double gear_switch, RC_GEAR& state);
void setLogitechStickValue(double input, double& state) { state = input; }

// PX4 callback functions
void px4RemoteCB(const mavros_msgs::RCIn::ConstPtr& msg);
ros::Subscriber px4_rc_channels_sub_;
Expand Down
18 changes: 18 additions & 0 deletions include/vb_util_lib/rc_mapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,23 @@ class rc_mapper
#define ACSL_RC_MAX_VALUE 2020
#define ACSL_RC_MIN_VALUE 1020

// LOGITECH
#define LOGITECH_RC_THROTTLE 1
#define LOGITECH_RC_YAW 0
#define LOGITECH_RC_ROLL 2
#define LOGITECH_RC_PITCH 3
#define LOGITECH_RC_GEAR 0
#define LOGITECH_RC_MODE 0
#define LOGITECH_RC_ARM 0

#define LOGITECH_RC_THROTTLE_SIGN 1
#define LOGITECH_RC_YAW_SIGN 1
#define LOGITECH_RC_ROLL_SIGN 1
#define LOGITECH_RC_PITCH_SIGN 1

#define LOGITECH_RC_MAX_VALUE 1
#define LOGITECH_RC_MIN_VALUE -1

// Default
#define DEFAULT_MIN_VALUE 1020
#define DEFAULT_MAX_VALUE 2020
Expand Down Expand Up @@ -99,6 +116,7 @@ class rc_mapper
eRCT_PX4 = 0,
eRCT_DJI,
eRCT_ACSL,
eRCT_LOGITECH,
eRCT_Unknown,
eEndOfRCTypes
};
Expand Down
6 changes: 3 additions & 3 deletions src/vb_util_lib/kinematics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -218,9 +218,9 @@ void Kinematics::setPosition(const ros::Time& update_time, double x, double y, d
{
Eigen::Vector3d vel_debug;
getLinearVelocity(drone_ENU, vel_debug);
ROS_INFO("Kinematics Velocity standard[%s : %0.3f %0.3f, %0.3f] body[%s : %0.3f %0.3f, %0.3f]",
frame_id_.c_str(), velLinear_[0], velLinear_[1], velLinear_[2],
drone_ENU, vel_debug[0], vel_debug[1], vel_debug[2]);
// ROS_INFO("Kinematics Velocity standard[%s : %0.3f %0.3f, %0.3f] body[%s : %0.3f %0.3f, %0.3f]",
// frame_id_.c_str(), velLinear_[0], velLinear_[1], velLinear_[2],
// drone_ENU, vel_debug[0], vel_debug[1], vel_debug[2]);
}
}

Expand Down
50 changes: 50 additions & 0 deletions src/vb_util_lib/rc_controls.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,9 @@ void RC_Controls::setSubscriber(ros::NodeHandle& n) {
UPPER_VALUE_ = (MAX_VALUE_ - (RANGE_/2));
LOWER_VALUE_ = (MIN_VALUE_ + (RANGE_/2));

logitech_rc_channels_sub_ = n.subscribe<sensor_msgs::Joy>("/joy", 10,
&RC_Controls::logitechRemoteCB, this);

dji_rc_channels_sub_ = n.subscribe<sensor_msgs::Joy>(am_topics::DJI_SDK_RC, 10,
&RC_Controls::djiRemoteCB, this);

Expand Down Expand Up @@ -267,6 +270,53 @@ void RC_Controls::djiRemoteCB(const sensor_msgs::Joy::ConstPtr& rc_msg) {
}
}

//===================================================
// Logitech specific RC stuff
//===================================================

void RC_Controls::setLogitechControls(const sensor_msgs::Joy::ConstPtr& msg) {

stamp = msg->header.stamp;

throttle_ = msg->axes[throttle_idx_];
yaw_ = msg->axes[yaw_idx_];
pitch_ = msg->axes[pitch_idx_];
roll_ = msg->axes[roll_idx_];
gear_ = 0;
mode_ = 0;

setLogitechStickValue(yaw_, yaw);
setLogitechStickValue(pitch_, pitch);
setLogitechStickValue(roll_, roll);
setLogitechStickValue(throttle_, throttle);

ROS_INFO_THROTTLE(1.0, "XXX RC: roll[%0.2f], pitch[%0.2f], yaw[%0.2f], throttle[%0.2f], gear[%0.2f], mode[%0.2f]\n",
roll_, pitch_, yaw_, throttle_, gear_, mode_);
}

bool RC_Controls::setLogitechModeSwitch(double mode_switch, RC_MODE& state) {
bool change_state = false;
state = RC_MODE::POS;

return change_state;
}

bool RC_Controls::setLogitechGearSwitch(double gear_switch, RC_GEAR& state) {
bool change = false;
state = RC_GEAR::DOWN;

return change;
}

void RC_Controls::logitechRemoteCB(const sensor_msgs::Joy::ConstPtr& rc_msg) {
setLogitechControls(rc_msg);

if (callback_ != NULL)
{
callback_(this);
}
}

//===================================================
// PX4 specific RC stuff
//===================================================
Expand Down
20 changes: 19 additions & 1 deletion src/vb_util_lib/rc_mapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,14 @@ rc_mapper::rc_mapper(ros::NodeHandle n)
m_RCTypeNames[eRCT_PX4].assign("PX4");
m_RCTypeNames[eRCT_DJI].assign("DJI");
m_RCTypeNames[eRCT_ACSL].assign("ACSL");
m_RCTypeNames[eRCT_LOGITECH].assign("LOGITECH");
m_RCTypeNames[eRCT_Unknown].assign("Unknown");

//default settings
m_RCType.assign(m_RCTypeNames[m_SetRC].c_str());
SetFromDefaults();

//set parameters form the input parameters (if present)
//set parameters from the input parameters (if present)
SetFromParameters();
}

Expand Down Expand Up @@ -190,6 +191,23 @@ void rc_mapper::SetFromParameters(void)
m_Handle.param("/RC/maxvalue", m_MaxValue, ACSL_RC_MAX_VALUE);
break;
}
case eRCT_LOGITECH:
{
m_Handle.param("/RC/throttle", m_Channels[eRCThrottle], LOGITECH_RC_THROTTLE);
m_Handle.param("/RC/yaw", m_Channels[eRCYaw], LOGITECH_RC_YAW);
m_Handle.param("/RC/roll", m_Channels[eRCRoll], LOGITECH_RC_ROLL);
m_Handle.param("/RC/pitch", m_Channels[eRCPitch], LOGITECH_RC_PITCH);
m_Handle.param("/RC/gear", m_Channels[eRCGear], LOGITECH_RC_GEAR);
m_Handle.param("/RC/mode", m_Channels[eRCMode], LOGITECH_RC_MODE);
m_Handle.param("/RC/arm", m_Channels[eRCArm], LOGITECH_RC_ARM);
m_Handle.param("/RC/throttle_sign", m_Signs[eThrottleSign], LOGITECH_RC_THROTTLE_SIGN);
m_Handle.param("/RC/yaw_sign", m_Signs[eYawSign], LOGITECH_RC_YAW_SIGN);
m_Handle.param("/RC/roll_sign", m_Signs[eRollSign], LOGITECH_RC_ROLL_SIGN);
m_Handle.param("/RC/pitch_sign", m_Signs[ePitchSign], LOGITECH_RC_PITCH_SIGN);
m_Handle.param("/RC/minvalue", m_MinValue, LOGITECH_RC_MIN_VALUE);
m_Handle.param("/RC/maxvalue", m_MaxValue, LOGITECH_RC_MAX_VALUE);
break;
}
}
ROS_INFO("RC: Parameter driven values: throttle[%d], yaw[%d], roll[%d], pitch[%d], gear[%d], mode[%d], arm[%d], min[%d], max[%d]",
m_Channels[eRCThrottle], m_Channels[eRCYaw], m_Channels[eRCRoll], m_Channels[eRCPitch],
Expand Down