now disconnection always turns motors off first

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

@@ -116,6 +116,10 @@ class PPSRadioClient:
         self._cf.open_link(self.link_uri)
 
     def disconnect(self):
+        print "Motors OFF"
+        self._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR)
+        # change state to motors OFF
+        self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
         print "Disconnecting from %s" % self.link_uri
         self._cf.close_link()
         self.change_status_to(DISCONNECTED)

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -413,13 +413,17 @@ void loadCrazyflieContext() {
 
     if((context.crazyflieName != "") && (new_context.crazyflieName != context.crazyflieName)) //linked crazyflie name changed and it was not empty before
     {
+
+        // Motors off is done in python script now everytime we disconnect
+
         // send motors OFF and disconnect before setting context = new_context
-        std_msgs::Int32 msg;
-        msg.data = CMD_CRAZYFLY_MOTORS_OFF;
-        commandPublisher.publish(msg);
+        // std_msgs::Int32 msg;
+        // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+        // commandPublisher.publish(msg);
 
-        // maybe some delay here?
+        ROS_INFO("CF is now different for this student. Disconnect and turn it off");
 
+        std_msgs::Int32 msg;
         msg.data = CMD_DISCONNECT;
         crazyRadioCommandPublisher.publish(msg);
     }