Merge branch 'pps_project' of https://gitlab.ethz.ch/D-FaLL/D-FaLL-System into pps_project

merging stuff from Marco with mine

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

@@ -28,6 +28,10 @@ from cflib.crazyflie.log import LogConfig
 # Logging settings
 logging.basicConfig(level=logging.ERROR)
 
+CONTROLLER_MOTOR = 2
+CONTROLLER_ANGLE = 1
+CONTROLLER_RATE = 0
+
 class PPSRadioClient:
     """
        CrazyRadio client that recieves the commands from the controller and 
@@ -86,20 +90,19 @@ class PPSRadioClient:
         pk.data = struct.pack('<fffHHHHHH', roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
         self._cf.send_packet(pk)
 
-def motorControlCallback(data):
+def motorCommandCallback(data):
         """Callback for motor controller actions"""
-    cf_client._send_to_commander(0, 0, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, ???type???)
+    cf_client._send_to_commander(0, 0, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, CONTROLLER_MOTOR)
     rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
 
-def angleControlCallback(data):
+def angleCommandCallback(data):
         """Callback for angle controller actions"""
-    cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, ???type???)
+    cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_ANGLE)
     rospy.loginfo("angle controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
 
-def rateControlCallback(data):
+def rateCommandCallback(data):
         """Callback for rate controller actions"""
-    #cf_client._send_commands(data.cmd1,data.cmd2,data.cmd3,data.cmd4)
-    cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, ???type???)
+    cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_RATE)
     rospy.loginfo("rate controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
 
 if __name__ == '__main__':
@@ -123,9 +126,9 @@ if __name__ == '__main__':
             cf_client = PPSRadioClient(available[0][0])
             time.sleep(3.0)
             #TODO: change publisher name if not correct
-            rospy.Subscriber("PPSClient/MotorCommand", MotorCommand, motorControlCallback)
-            rospy.Subscriber("PPSClient/AngleCommand", AngleCommand, angleControlCallback)
-            rospy.Subscriber("PPSClient/RateCommand", RateCommand, rateControlCallback)
+            rospy.Subscriber("PPSClient/MotorCommands", MotorCommand, motorCommandCallback)
+            rospy.Subscriber("PPSClient/AngleCommands", AngleCommand, angleCommandCallback)
+            rospy.Subscriber("PPSClient/RateCommands", RateCommand, rateCommandCallback)
 
             rospy.spin()
         else:

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -85,6 +85,68 @@ void viconCallback(const d_fall_pps::ViconData& data){
 
 }
 
+
+
+
+
+//Send to Crayzradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+void CControlMgr::callbackRunRateController(const ros::TimerEvent&)
+{
+    if(m_pRateController!=NULL)
+        if(m_pRateController->fIsInit())
+        {
+            DistributePowerAndSendToCrazyflie(m_pRateController->computeOutput());
+        }
+}
+
+void CControlMgr::DistributePowerAndSendToCrazyflie(ControllerOutput rateControllerOutput)
+{
+    assert(rateControllerOutput.onboardControllerType=eOnboardMotorCmdController);
+    assert(m_isRateOffboard==true);
+
+
+    rateControllerOutput.motorCmd1=m_CrazyControllerOutput.motorCmd1-rateControllerOutput.roll/2.0-rateControllerOutput.pitch/2.0-rateControllerOutput.yaw;
+    rateControllerOutput.motorCmd2=m_CrazyControllerOutput.motorCmd2-rateControllerOutput.roll/2.0+rateControllerOutput.pitch/2.0+rateControllerOutput.yaw;
+    rateControllerOutput.motorCmd3=m_CrazyControllerOutput.motorCmd3+rateControllerOutput.roll/2.0+rateControllerOutput.pitch/2.0-rateControllerOutput.yaw;
+    rateControllerOutput.motorCmd4=m_CrazyControllerOutput.motorCmd4+rateControllerOutput.roll/2.0-rateControllerOutput.pitch/2.0+rateControllerOutput.yaw;
+
+    SendToCrazyflie(rateControllerOutput);
+}
+
+void CControlMgr::SendToCrazyflie(ControllerOutput package)
+{
+    if(m_isStopped)
+    {
+        m_packageToSend.motorCmd1=0;
+        m_packageToSend.motorCmd2=0;
+        m_packageToSend.motorCmd3=0;
+        m_packageToSend.motorCmd4=0;
+        m_packageToSend.onboardControllerType=eOnboardMotorCmdController;
+        m_pPublisherControllerOutput->publish(m_packageToSend);
+        return;
+    }
+    else
+    {
+        m_packageToSend.roll=package.roll*RAD2DEG;
+        m_packageToSend.pitch=package.pitch*RAD2DEG;
+        m_packageToSend.yaw=package.yaw*RAD2DEG;
+
+        m_packageToSend.thrust=SaturateToUINT16(package.thrust);
+        m_packageToSend.motorCmd1=SaturateToUINT16(package.motorCmd1);
+        m_packageToSend.motorCmd2=SaturateToUINT16(package.motorCmd2);
+        m_packageToSend.motorCmd3=SaturateToUINT16(package.motorCmd3);
+        m_packageToSend.motorCmd4=SaturateToUINT16(package.motorCmd4);
+
+        m_packageToSend.onboardControllerType=package.onboardControllerType;
+
+        m_pPublisherControllerOutput->publish(m_packageToSend);
+    }
+}
+
+
+
+//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+
 int main(int argc, char* argv[]){
 	ROS_INFO_STREAM("PPSClient started");
 
@@ -103,6 +165,20 @@ int main(int argc, char* argv[]){
 	ROS_INFO_STREAM("about to subscribe");
 	ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
 	ROS_INFO_STREAM("subscribed");
+	
+	
+	//publish package_for_crazyradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+	    //m_pNodeHandle=nodeHandle;
+    	//m_pCallbackQueueControlMgr=new ros::CallbackQueue();
+    	//m_pNodeHandle->setCallbackQueue(m_pCallbackQueueControlMgr);
+
+
+    ROS_INFO_STREAM("creating publishers for package_for_crazyradio");
+	ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::AngleCommandsPackage>("AngleCommands", 10));
+	ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::RateCommandsPackage>("RateCommands", 10));
+	ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::MotorCommandsPackage>("MotorCommands", 10));
+
+	//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<