feat: added ability to engage a user defined mode of operation with the controller

Author: edkoch

include/vb_util_lib/rc_controls.h

@@ -17,6 +17,8 @@
 
 #define MAX_DJI_CONTROL 10000.0
 
+#define ENGAGED_THRESHOLD 0.95
+
 enum class RC_MODE 
 {
     ATT = 0, 
@@ -62,6 +64,10 @@ class RC_Controls
 
     bool armed_switch {false};
 
+    // This is a special mode we can engage and disengage with the congtroler sticks
+    //both up and in engage and both up and out disengage
+    bool user_function_engaged {false};
+
     // All stick values are between -1.0 and 1.0
     double throttle {0.0};
     double yaw {0.0};
@@ -139,6 +145,9 @@ class RC_Controls
     bool setPX4GearSwitch(double gear_switch, RC_GEAR& state);
     bool setPX4ArmSwitch(double arm_switch, bool& state);
     void setPX4StickValue(double input, double& state, double polarity);
+
+    // Routine to set the engaged state of the controller
+    bool setEngagedState();
 };
 
 #endif /* VISBOX_PACKAGES_COMMON_UTILS_UTIL_LIB_INCLUDE_VB_UTIL_LIB_RC_CONTROLS_H_ */

src/vb_util_lib/rc_controls.cpp

@@ -168,6 +168,24 @@ void RC_Controls::setSubscriber(ros::NodeHandle& n) {
 	stamp = ros::Time::now();
 }
 
+bool RC_Controls::setEngagedState() {
+	if (throttle > ENGAGED_THRESHOLD && pitch > ENGAGED_THRESHOLD)
+	{
+		if (yaw > ENGAGED_THRESHOLD && roll < -ENGAGED_THRESHOLD)
+		{
+			ROS_INFO(">>>>>>>> RC_CONTROL SPECIAL FUNCTION ENGAGED <<<<<<<<<<");
+			user_function_engaged = true;
+		}
+		else if (yaw < -ENGAGED_THRESHOLD && roll > ENGAGED_THRESHOLD)
+		{
+			ROS_INFO("<<<<<<<<<< RC_CONTROL SPECIAL FUNCTION DISENGAGED >>>>>>>>>>");
+			user_function_engaged = false;
+		}
+	}
+
+	return user_function_engaged;
+}
+
 //===================================================
 // DJI specific RC stuff
 //===================================================
@@ -198,6 +216,9 @@ void RC_Controls::setDJIControls(const sensor_msgs::Joy::ConstPtr& msg) {
 
 	setDJIModeSwitch(mode_, mode);
 	setDJIGearSwitch(gear_, gear);
+
+	setEngagedState();
+
 //    printf("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_);
 }
@@ -290,8 +311,10 @@ void RC_Controls::setLogitechControls(const sensor_msgs::Joy::ConstPtr& msg) {
 	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_);
+	setEngagedState();
+
+    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], engaged[%d]\n",
+          roll_, pitch_, yaw_, throttle_, gear_, mode_, user_function_engaged);
 }
 
 bool RC_Controls::setLogitechModeSwitch(double mode_switch, RC_MODE& state) {
@@ -347,6 +370,9 @@ void RC_Controls::setPX4Controls(const mavros_msgs::RCIn::ConstPtr&  msg) {
 	setPX4ModeSwitch(mode_, mode);
 	setPX4GearSwitch(gear_, gear);
 	setPX4ArmSwitch(armed_, armed_switch);
+
+	setEngagedState();
+
      //ROS_INFO_THROTTLE(2, "XXX RC: roll[%0.2f], pitch[%0.2f], yaw[%0.2f], throttle[%0.2f], gear[%d], mode[%d] armed[%d]\n",
      //      roll, pitch, yaw, throttle, static_cast<int>(gear), static_cast<int>(mode), armed_switch);
 }