Added Kalman filter reset to the CrazyRadio python node anytime the flying...

Added Kalman filter reset to the CrazyRadio python node anytime the flying state changes from motors off to something else

dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py

@@ -132,6 +132,13 @@ CMD_CRAZYFLY_MOTORS_OFF = 13
 FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE    = 2
 #FLYING_VEHICLE_STATE_TYPE_12_STATE_ESTIMATE           = 3
 
+# Flying states
+STATE_MOTORS_OFF     1
+#STATE_TAKE_OFF       2
+#STATE_FLYING         3
+#STATE_LAND           4
+#STATE_UNAVAILABLE    5
+
 
 
 # Open the ROS bag for logging
@@ -156,6 +163,9 @@ class CrazyRadioClient:
         self.link_uri = ""
         # > For the name of the connected crazyflie
         self.crazyflie_name = ""
+        # > For keeping track of the "flying state"
+        #   of the "FlyingAgentClient"
+        self._flyingState_of_flyingAgentClient = STATE_MOTORS_OFF
 
         # > INIIALISE PUBLISHERS
         # > For informing the network of the status of
@@ -552,6 +562,46 @@ class CrazyRadioClient:
         # Directly return the current status
         return self._status
 
+
+
+    def flyingStateCallback(self, msg):
+        """Callback for keeping track of the flying state"""
+
+        # > This is used to know when to reset the Crazyflie's
+        #   onboard Kalman filter.
+        # > The filter is reset when the state changes from
+        #   "motors off" to something else
+        # > This is required because the onboard state estimate
+        #   tends to drift when the Crazyflie stays on the ground
+        #   for an even a short amount of time
+
+        # The data in this message is always relevant because
+        # of the namespace of the message, hence there is no
+        # need to check the header.
+
+        # Get the new state from the message
+        new_flying_state = msg.data
+
+        # Get the current flying state into a local variable
+        current_flying_state = self._flyingState_of_flyingAgentClient
+
+        # Only consider the command if it is relevant
+        if ( \
+            (current_flying_state==STATE_MOTORS_OFF)
+            and \
+            not(new_flying_state==STATE_MOTORS_OFF) \
+        ):
+            # Call the CrazyRadio function that sets the
+            # paramter for reseting the onboard estimate
+            self._cf.param.set_value("kalman.resetEstimation", 1)
+            # Inform the user
+            #print "[CRAZY RADIO] reqested the Crazyflie to reset the onboard estimate"
+
+        # Update the class variable for keeping track of the
+        # current state
+        self._flyingState_of_flyingAgentClient = new_flying_state
+
+
 # END OF: class CrazyRadioClient:
 # ============================= #
 
@@ -728,6 +778,12 @@ if __name__ == '__main__':
     getCurrentCrazyRadioStatusService = rospy.Service(node_name + "/getCurrentCrazyRadioStatus", IntIntService , cf_client.getCurrentCrazyRadioStatusServiceCallback)
 
 
+    # Subscribe to the "flying state" of the FlyingAgentClient
+    # > This is used to determine when to reset the Crazyflie's
+    #   onboard Kalman filter
+    rospy.Subscriber("FlyingAgentClient/FlyingState", IntWithHeader, cf_client.flyingStateCallback)
+
+
 
     # Sleep briefly to allow everything to start-up
     time.sleep(1.0)