Gamepad_node fix flattrim to square, altitude and attitude limit params

siyakunde · Oct 10, 2019 · 03938f7 · 03938f7
1 parent 5ccd43a
commit 03938f7
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 3 deletions.
diff --git a/cfg/Tello.cfg b/cfg/Tello.cfg
@@ -17,10 +17,14 @@ gen.add("video_req_sps_hz", double_t, 0,
     "Rate for regularly requesting SPS data from drone (0: disabled)",
     0.5, 0.0, 4.0)
 
+# Limit altitude control (Jordy)
+gen.add("altitude_limit", int_t, 0, "Limit altitude of Tello", 10, 1, 100)
+# Limit attitude control (Jordy)
+gen.add("attitude_limit", int_t, 0, "Limit attitude of Tello", 15, 1, 90)
+# Set low battery threshold (Jordy)
+gen.add("low_bat_threshold", int_t, 0, "Set low battery threshold of Tello", 7, 1, 100)
 # Scale velocity control (Jordy)
 gen.add("vel_cmd_scale", double_t, 0, "Scale (down) vel_cmd value", 0.5, 0.01, 1.0)
 
-# Limit attitude control (Jordy)
-gen.add("att_limit", double_t, 0, "Limit attitude of Tello", 15, 5, 90)
 
 exit(gen.generate(PACKAGE, "tello_driver_node.py", "Tello"))
diff --git a/src/gamepad_teleop_node.py b/src/gamepad_teleop_node.py
@@ -189,7 +189,7 @@ def joy_cb(self, msg):
             #rospy.logwarn('Issued PALM_LAND')
 
         # Process flat trim
-        if not self.joy_state_prev.Sq and self.joy_state.Sq:
+        if not self.joy_state_prev.Cr and self.joy_state.Cr:
             self.pub_flattrim.publish()
             #rospy.logwarn('Issued FLATTRIM')
 

diff --git a/src/tello_driver_node.py b/src/tello_driver_node.py
@@ -192,6 +192,9 @@ def __init__(self):
 ###########################################BEGIN###################################
 
 ## Add 'Tello' compositions, leave 'TelloPy' package untouched (Jordy) ##        
+
+    def set_fast_mode(self, enabled):
+        self.fast_mode = enabled
 
     def reset_cmd_vel(self):
         self.left_x = 0.
@@ -346,6 +349,12 @@ def cb_dyncfg(self, config, level):
             self.cfg = config
             update_all = True
 
+        if update_all or self.cfg.altitude_limit != config.altitude_limit:
+            self.set_alt_limit(config.altitude_limit)
+        if update_all or self.cfg.attitude_limit != config.attitude_limit:
+            self.set_att_limit(config.attitude_limit)
+        if update_all or self.cfg.low_bat_threshold != config.low_bat_threshold:
+            self.set_low_bat_threshold(config.low_bat_threshold)
         if update_all or self.cfg.fixed_video_rate != config.fixed_video_rate:
             self.set_video_encoder_rate(config.fixed_video_rate)
             req_sps_pps = True