Merge pull request #24 from AutoModality/BB-820/Parameterize_rc

Todd Keitel · web-flow · commit 94b37c24a400 · 2020-04-27T16:31:25.000-07:00
Bb 820/parameterize rc
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -95,7 +95,8 @@ add_library(am_utils
   src/vb_util_lib/dji_challenge_util.cpp 
   src/vb_util_lib/am_histogram.cpp
   src/vb_util_lib/row_marker.cpp 
-  src/vb_util_lib/rc_controls.cpp 
+  src/vb_util_lib/rc_controls.cpp
+  src/vb_util_lib/rc_mapper.cpp 
   src/vb_util_lib/i2c.cpp 
   src/vb_util_lib/circ_list.cpp 
   src/vb_util_lib/time_synchronizer.cpp
diff --git a/include/vb_util_lib/rc_controls.h b/include/vb_util_lib/rc_controls.h
@@ -9,19 +9,37 @@
 #define VISBOX_PACKAGES_COMMON_UTILS_UTIL_LIB_INCLUDE_VB_UTIL_LIB_RC_CONTROLS_H_
 
 #include <stdio.h>
-
-//3.3#include <dji_sdk/RCChannels.h>
+#include "rc_mapper.h"
+//#include <dji_sdk/RCChannels.h>
 #include <ros/ros.h>
 #include <sensor_msgs/Joy.h>
 #include <mavros_msgs/RCIn.h>
 
 #define MAX_DJI_CONTROL 10000.0
 
-enum class RC_MODE {ATT=0, ALT, POS, UNKNOWN};
-enum class RC_GEAR {DOWN=0, UP, UNKNOWN};
-enum class RC_TYPE {PX4=0, DJI, ACSL, UNKNOWN};
+enum class RC_MODE 
+{
+    ATT = 0, 
+    ALT, 
+    POS, 
+    UNKNOWN
+};
+enum class RC_GEAR 
+{
+    DOWN = 0, 
+    UP, 
+    UNKNOWN
+};
+enum class RC_TYPE 
+{
+    PX4 = rc_mapper::eRCT_PX4, 
+    DJI = rc_mapper::eRCT_DJI, 
+    ACSL = rc_mapper::eRCT_ACSL,
+    UNKNOWN = rc_mapper::eRCT_Unknown
+};
 
-class RC_Controls {
+class RC_Controls 
+{
 public:
     RC_Controls();
     RC_Controls(ros::NodeHandle& n, RC_TYPE type = RC_TYPE::UNKNOWN);
@@ -51,6 +69,7 @@ class RC_Controls {
     double pitch {0.0};
 
     ros::Time stamp;
+    rc_mapper* rc_mapper_;
 
 //    const double MAX_DJI_CONTROL {10000.0};
 //
@@ -79,10 +98,10 @@ class RC_Controls {
     int MAX_VALUE_ {2000};
 	int MIN_VALUE_ {1000};
 
-	int throttle_sign {1};
-	int yaw_sign {1};
-	int roll_sign {1};
-	int pitch_sign {1};
+	int throttle_sign_ {1};
+	int yaw_sign_ {1};
+	int roll_sign_ {1};
+	int pitch_sign_ {1};
 
 	unsigned int RANGE_;
 	unsigned int ZERO_VALUE_;
diff --git a/include/vb_util_lib/rc_mapper.h b/include/vb_util_lib/rc_mapper.h
@@ -0,0 +1,149 @@
+/* rm_mapper.h
+ *
+ * Created on: 6 March 2020
+ *     Author: tkeitel
+ */
+
+#ifndef VISBOX_PACKAGES_COMMON_UTILS_UTIL_LIB_INCLUDE_VB_UTIL_LIB_RC_MAPPER_H_
+#define VISBOX_PACKAGES_COMMON_UTILS_UTIL_LIB_INCLUDE_VB_UTIL_LIB_RC_MAPPER_H_
+
+#include <stdio.h>
+#include <string>
+#include <ros/ros.h>
+//#include <sensor_msgs/Joy.h>
+//#include <mavros_msgs/RCIn.h>
+
+
+class rc_mapper
+ {
+    public:
+    // DJI
+    #define  DJI_RC_THROTTLE 3 
+    #define  DJI_RC_YAW 2 
+    #define  DJI_RC_ROLL 0 
+    #define  DJI_RC_PITCH 1 
+    #define  DJI_RC_GEAR 5 
+    #define  DJI_RC_MODE 4 
+    #define  DJI_RC_ARM 6 
+
+    #define  DJI_RC_THROTTLE_SIGN 1 
+    #define  DJI_RC_YAW_SIGN 1 
+    #define  DJI_RC_ROLL_SIGN 1 
+    #define  DJI_RC_PITCH_SIGN 1 
+
+
+    #define  DJI_RC_MAX_VALUE 1987 
+    #define  DJI_RC_MIN_VALUE 998 
+
+    // PX4
+    #define  PX4_RC_THROTTLE 2 
+    #define  PX4_RC_YAW 3 
+    #define  PX4_RC_ROLL 0 
+    #define  PX4_RC_PITCH 1 
+    #define  PX4_RC_GEAR 7 
+    #define  PX4_RC_MODE 5 
+    #define  PX4_RC_ARM 6 
+
+    #define  PX4_RC_THROTTLE_SIGN 1 
+    #define  PX4_RC_YAW_SIGN 1 
+    #define  PX4_RC_ROLL_SIGN 1 
+    #define  PX4_RC_PITCH_SIGN 1 
+
+    #define  PX4_RC_MAX_VALUE 1987 
+    #define  PX4_RC_MIN_VALUE 998 
+
+    // ACSL
+    #define  ACSL_RC_THROTTLE 2 
+    #define  ACSL_RC_YAW 3 
+    #define  ACSL_RC_ROLL 0 
+    #define  ACSL_RC_PITCH 1 
+    #define  ACSL_RC_GEAR 8 
+    #define  ACSL_RC_MODE 6 
+    #define  ACSL_RC_ARM 9 
+
+    #define  ACSL_RC_THROTTLE_SIGN -1 
+    #define  ACSL_RC_YAW_SIGN 1 
+    #define  ACSL_RC_ROLL_SIGN 1 
+    #define  ACSL_RC_PITCH_SIGN -1      
+
+    #define  ACSL_RC_MAX_VALUE 2020 
+    #define  ACSL_RC_MIN_VALUE 1020 
+
+    // Default
+    #define DEFAULT_MIN_VALUE 1020 
+    #define DEFAULT_MAX_VALUE 2020 
+
+    enum RC_Definitions
+    {
+            eRCThrottle = 0,
+            eRCYaw,
+            eRCRoll,
+            eRCPitch,
+            eRCGear,
+            eRCMode,
+            eRCArm,
+            eEndOfRCDefinitions
+    };
+
+    enum RC_Signs
+    {
+            eThrottleSign = 0,
+            eYawSign,
+            eRollSign,
+            ePitchSign,
+            eEndOfRCSigns
+     };
+
+    enum RC_TYPES
+    {
+        eRCT_PX4 = 0,
+        eRCT_DJI,
+        eRCT_ACSL,
+        eRCT_Unknown,
+        eEndOfRCTypes
+    };
+
+    rc_mapper(ros::NodeHandle n);
+    
+    virtual ~rc_mapper();
+
+    int GetChannel(int _chan);
+    
+    bool SetChannel(int _chan, int _signal);
+    
+
+    int GetMax(void);
+    int GetMin(void);
+
+    void SetMax(int _max);
+    void SetMin(int _min);
+
+    int GetSign(int _chan);
+
+    bool SetSign(int _chan, int _sign);
+
+    std::string GetRCType(void);
+    std::string GetRCTypeName(void);
+    RC_TYPES GetSetRC(void);
+
+    private:
+        ros::NodeHandle m_Handle;
+        RC_TYPES m_SetRC; 
+        std::string m_RCType;
+        std::string m_RCTypeNames[eEndOfRCTypes];
+        int m_Channels[eEndOfRCDefinitions];
+        int m_Signs[eEndOfRCSigns];
+        int m_MinValue;
+        int m_MaxValue;
+
+        static bool Valid(int _chan);
+
+        void SetFromDefaults(void);
+
+        RC_TYPES DetermineRCSetting(std::string* _rctype);
+
+        void SetFromParameters(void);
+    };
+
+#endif
+
diff --git a/src/vb_util_lib/rc_controls.cpp b/src/vb_util_lib/rc_controls.cpp
@@ -6,6 +6,7 @@
  */
 
 #include <vb_util_lib/rc_controls.h>
+#include <vb_util_lib/rc_mapper.h>
 #include <vb_util_lib/topics.h>
 
 using namespace am;
@@ -52,13 +53,15 @@ RC_Controls::~RC_Controls() {
     // TODO Auto-generated destructor stub
 }
 
-RC_Controls::RC_Controls(ros::NodeHandle& n, RC_TYPE type) : rc_type_(type) {
+RC_Controls::RC_Controls(ros::NodeHandle& n, RC_TYPE type) : rc_type_(type) 
+{
+	rc_mapper_ = new rc_mapper(n);
 	setSubscriber(n);
 }
 
 void RC_Controls::setSubscriber(ros::NodeHandle& n) {
 	// Initialize the min and max constants
-	n.param("/RC/max_value", MAX_VALUE_, MAX_VALUE_);
+	/*n.param("/RC/max_value", MAX_VALUE_, MAX_VALUE_);
 	n.param("/RC/min_value", MIN_VALUE_, MIN_VALUE_);
 
 	std::string rc_type;
@@ -101,10 +104,10 @@ void RC_Controls::setSubscriber(ros::NodeHandle& n) {
 		mode_idx_ = ACSL_MODE;
 		arm_idx_ =  ACSL_ARM;
 
-		throttle_sign = ACSL_THROTTLE_SIGN;
-		yaw_sign = ACSL_YAW_SIGN;
-		roll_sign = ACSL_ROLL_SIGN;
-		pitch_sign = ACSL_PITCH_SIGN;
+		throttle_sign_ = ACSL_THROTTLE_SIGN;
+		yaw_sign_ = ACSL_YAW_SIGN;
+		roll_sign_ = ACSL_ROLL_SIGN;
+		pitch_sign_ = ACSL_PITCH_SIGN;
 
 	    break;
 
@@ -120,13 +123,33 @@ void RC_Controls::setSubscriber(ros::NodeHandle& n) {
 		mode_idx_ = PX4_MODE;
 		arm_idx_ =  PX4_ARM;
 
-		throttle_sign = PX4_THROTTLE_SIGN;
-		yaw_sign = PX4_YAW_SIGN;
-		roll_sign = PX4_ROLL_SIGN;
-		pitch_sign = PX4_PITCH_SIGN;
+		throttle_sign_ = PX4_THROTTLE_SIGN;
+		yaw_sign_ = PX4_YAW_SIGN;
+		roll_sign_ = PX4_ROLL_SIGN;
+		pitch_sign_ = PX4_PITCH_SIGN;
 
 		break;
-	}
+	}*/
+
+	rc_type_ = static_cast<RC_TYPE>(rc_mapper_->GetSetRC());
+
+	MIN_VALUE_ = rc_mapper_->GetMin();
+	MAX_VALUE_ = rc_mapper_->GetMax();
+
+	throttle_idx_ 	= rc_mapper_->GetChannel(rc_mapper::eRCThrottle);
+	yaw_idx_ 		= rc_mapper_->GetChannel(rc_mapper::eRCYaw);
+	roll_idx_ 		= rc_mapper_->GetChannel(rc_mapper::eRCRoll);
+	pitch_idx_		= rc_mapper_->GetChannel(rc_mapper::eRCPitch);
+	gear_idx_		= rc_mapper_->GetChannel(rc_mapper::eRCGear);
+	mode_idx_		= rc_mapper_->GetChannel(rc_mapper::eRCMode);
+	arm_idx_		= rc_mapper_->GetChannel(rc_mapper::eRCArm);
+
+	throttle_sign_ 	= rc_mapper_->GetSign(rc_mapper::eThrottleSign);
+	yaw_sign_ 		= rc_mapper_->GetSign(rc_mapper::eRCYaw);
+	roll_sign_ 		= rc_mapper_->GetSign(rc_mapper::eRollSign);
+	pitch_sign_ 		= rc_mapper_->GetSign(rc_mapper::eRCPitch);
+
+	ROS_INFO("RC: RC Param Settings: throttle[%d], yaw[%d], roll[%d], pitch[%d], gear[%d], mode[%d], arm[%d], throttle_sign_[%d], yaw_sign_[%d], roll_sign_[%d], pitch_sign_[%d], max[%d], min[%d]", throttle_idx_, yaw_idx_, roll_idx_, pitch_idx_, gear_idx_, mode_idx_, arm_idx_, throttle_sign_, yaw_sign_, roll_sign_, pitch_sign_, MAX_VALUE_, MIN_VALUE_);
 
 	RANGE_ = ((MAX_VALUE_ - MIN_VALUE_)/2);
 	ZERO_VALUE_ =  (MIN_VALUE_ + RANGE_);
@@ -151,12 +174,19 @@ void RC_Controls::setDJIControls(const sensor_msgs::Joy::ConstPtr& msg) {
 	stamp = msg->header.stamp;
 
 	// Set the raw values;
-	throttle_ = msg->axes[3];
+	/*throttle_ = msg->axes[3];
 	yaw_ = msg->axes[2];
 	pitch_ = msg->axes[1];
 	roll_ = msg->axes[0];
 	gear_ = msg->axes[5];
-	mode_ = msg->axes[4];
+	mode_ = msg->axes[4];*/
+
+	throttle_ = msg->axes[throttle_idx_];
+	yaw_ = msg->axes[yaw_idx_];
+	pitch_ = msg->axes[pitch_idx_];
+	roll_ = msg->axes[roll_idx_];
+	gear_ = msg->axes[gear_idx_];
+	mode_ = msg->axes[mode_idx_];
 
 	setDJIStickValue(yaw_, yaw);
 	setDJIStickValue(pitch_, pitch);
@@ -259,10 +289,10 @@ void RC_Controls::setPX4Controls(const mavros_msgs::RCIn::ConstPtr&  msg) {
 	mode_ = msg->channels[mode_idx_];
 	armed_ = msg->channels[arm_idx_];
 
-	setPX4StickValue(yaw_, yaw, yaw_sign);
-	setPX4StickValue(pitch_, pitch, pitch_sign);
-	setPX4StickValue(roll_, roll, roll_sign);
-	setPX4StickValue(throttle_, throttle, throttle_sign);
+	setPX4StickValue(yaw_, yaw, yaw_sign_);
+	setPX4StickValue(pitch_, pitch, pitch_sign_);
+	setPX4StickValue(roll_, roll, roll_sign_);
+	setPX4StickValue(throttle_, throttle, throttle_sign_);
 
 	setPX4ModeSwitch(mode_, mode);
 	setPX4GearSwitch(gear_, gear);
diff --git a/src/vb_util_lib/rc_mapper.cpp b/src/vb_util_lib/rc_mapper.cpp
