From 6c098b8bda38caa8965499799351d7e33edefabe Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Mon, 27 Apr 2020 19:14:47 +0200
Subject: [PATCH] Fixed errors in CrazyRadio python script and tested to work
with actual radio and Crazyflie 2.1 running the new dfall firmware
---
.../flyingAgentGUI/src/controllertabs.cpp | 36 +++----
.../src/dfall_pkg/crazyradio/CrazyRadio.py | 99 +++++++++----------
2 files changed, 66 insertions(+), 69 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
index c1fca375..eaa34686 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
@@ -390,15 +390,15 @@ void ControllerTabs::poseDataMocapReceivedCallback(const dfall_pkg::ViconData& v
// float originZ = (area.zmin + area.zmax) / 2.0;
//pose_in_global_frame.z -= originZ;
- emit measuredPoseValueChanged(
- pose_in_global_frame.x,
- pose_in_global_frame.y,
- pose_in_global_frame.z,
- pose_in_global_frame.roll,
- pose_in_global_frame.pitch,
- pose_in_global_frame.yaw,
- pose_in_global_frame.isValid
- );
+ // emit measuredPoseValueChanged(
+ // pose_in_global_frame.x,
+ // pose_in_global_frame.y,
+ // pose_in_global_frame.z,
+ // pose_in_global_frame.roll,
+ // pose_in_global_frame.pitch,
+ // pose_in_global_frame.yaw,
+ // pose_in_global_frame.isValid
+ // );
}
}
}
@@ -417,15 +417,15 @@ void ControllerTabs::poseDataOnboardReceivedCallback(const dfall_pkg::FlyingVehi
{
//if(pose_in_local_frame.vehicleName == m_object_name_for_emitting_pose_data)
//{
- // emit measuredPoseValueChanged(
- // pose_in_local_frame.x,
- // pose_in_local_frame.y,
- // pose_in_local_frame.z,
- // pose_in_local_frame.roll,
- // pose_in_local_frame.pitch,
- // pose_in_local_frame.yaw,
- // pose_in_local_frame.isValid
- // );
+ emit measuredPoseValueChanged(
+ pose_in_local_frame.x,
+ pose_in_local_frame.y,
+ pose_in_local_frame.z,
+ pose_in_local_frame.roll,
+ pose_in_local_frame.pitch,
+ pose_in_local_frame.yaw,
+ pose_in_local_frame.isValid
+ );
//}
}
#endif
diff --git a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index 4974bcb7..c202a391 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -35,8 +35,8 @@
# Import the ROS library package
import roslib
-# Import the "dfall package" manifest for ROS
-import roslib.load_manifest('dfall_pkg')
+# Load the "dfall package" manifest for ROS
+roslib.load_manifest('dfall_pkg')
# Import the ROS-Python package
import rospy
# Import standard messages
@@ -87,21 +87,21 @@ logging.basicConfig(level=logging.ERROR)
# CF_COMMAND_TYPE_RATE = 9
# CF_COMMAND_TYPE_ANGLE = 10
-CF_ONBOARD_CONTROLLER_MODE_OFF 0
-CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE 1
-CF_ONBOARD_CONTROLLER_MODE_ANGLE 2
-CF_ONBOARD_CONTROLLER_MODE_VELOCITY 3
-CF_ONBOARD_CONTROLLER_MODE_POSITION 4
-
-# CF_PACKET_DECODER_STOP_TYPE 0
-# CF_PACKET_DECODER_VELOCITY_WORLD_TYPE 1
-# CF_PACKET_DECODER_Z_DISTANCE_TYEP 2
-# CF_PACKET_DECODER_CPPM_EMU_TYPE 3
-# CF_PACKET_DECODER_ALT_HOLD_TYPE 4
-# CF_PACKET_DECODER_HOVER_TYPE 5
-# CF_PACKET_DECODER_FULL_STATE_TYPE 6
-# CF_PACKET_DECODER_POSITION_TYPE 7
-CF_PACKET_DECODER_DFALL_TYPE 8
+CF_ONBOARD_CONTROLLER_MODE_OFF = 0
+CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE = 1
+CF_ONBOARD_CONTROLLER_MODE_ANGLE = 2
+CF_ONBOARD_CONTROLLER_MODE_VELOCITY = 3
+CF_ONBOARD_CONTROLLER_MODE_POSITION = 4
+
+# CF_PACKET_DECODER_STOP_TYPE = 0
+# CF_PACKET_DECODER_VELOCITY_WORLD_TYPE = 1
+# CF_PACKET_DECODER_Z_DISTANCE_TYEP = 2
+# CF_PACKET_DECODER_CPPM_EMU_TYPE = 3
+# CF_PACKET_DECODER_ALT_HOLD_TYPE = 4
+# CF_PACKET_DECODER_HOVER_TYPE = 5
+# CF_PACKET_DECODER_FULL_STATE_TYPE = 6
+# CF_PACKET_DECODER_POSITION_TYPE = 7
+CF_PACKET_DECODER_DFALL_TYPE = 8
RAD_TO_DEG = 57.296
@@ -133,11 +133,11 @@ 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
+STATE_MOTORS_OFF = 1
+#STATE_TAKE_OFF = 2
+#STATE_FLYING = 3
+#STATE_LAND = 4
+#STATE_UNAVAILABLE = 5
@@ -273,9 +273,9 @@ class CrazyRadioClient:
cfStateEstimate.pitch = -data["stateEstimateZ.pitch"] / 1000.0
cfStateEstimate.yaw = data["stateEstimateZ.yaw"] / 1000.0
# > (roll,pitch,yaw) anglular rates (direct from gryo)
- cfStateEstimate.rollRate = data["stateEstimateZ.rollRate"] / 1000.0
- cfStateEstimate.pitchRate = -data["stateEstimateZ.pitchRate"] / 1000.0
- cfStateEstimate.yawRate = data["stateEstimateZ.yawRate"] / 1000.0
+ cfStateEstimate.rollRate = data["stateEstimateZ.rateRoll"] / 1000.0
+ cfStateEstimate.pitchRate = -data["stateEstimateZ.ratePitch"] / 1000.0
+ cfStateEstimate.yawRate = data["stateEstimateZ.rateYaw"] / 1000.0
# Print out one value for DEBUGGING
#print "[CRAZY RADIO] received (z,vz) = ( %6.3f , %6.3f )" %( cfStateEstimate.z , cfStateEstimate.vz )
@@ -338,8 +338,6 @@ class CrazyRadioClient:
else:
print "[CRAZY RADIO] The log config for the battery voltage is invalid"
-
-
# A "LOGGING GROUP" FOR THE STATE ESTIMATE:
# Initialise a log config object
@@ -362,9 +360,9 @@ class CrazyRadioClient:
self.logconf_stateEstimate.add_variable("stateEstimateZ.pitch", "int16_t");
self.logconf_stateEstimate.add_variable("stateEstimateZ.yaw", "int16_t");
# > (roll,pitch,yaw) anglular rates (direct from gryo)
- self.logconf_stateEstimate.add_variable("stateEstimateZ.rollRate", "int16_t");
- self.logconf_stateEstimate.add_variable("stateEstimateZ.pitchRate", "int16_t");
- self.logconf_stateEstimate.add_variable("stateEstimateZ.yawRate", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.rateRoll", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.ratePitch", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.rateYaw", "int16_t");
# Add the log config to the crazyflie object
self._cf.log.add_config(self.logconf_stateEstimate)
if self.logconf_stateEstimate.valid:
@@ -593,7 +591,7 @@ class CrazyRadioClient:
):
# Call the CrazyRadio function that sets the
# paramter for reseting the onboard estimate
- self._cf.param.set_value("kalman.resetEstimation", 1)
+ self._cf.param.set_value("kalman.resetEstimation", "1")
# Inform the user
#print "[CRAZY RADIO] reqested the Crazyflie to reset the onboard estimate"
@@ -624,8 +622,9 @@ def controlCommandCallback(data):
or \
(data.xControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
):
- # Convert the setpoint to degrees
+ # Negate the setpoint and convert to degrees
x_controller_setpoint = -data.xControllerSetpoint * RAD_TO_DEG;
+ #
else:
# Take the setpoint as is
x_controller_setpoint = data.xControllerSetpoint;
@@ -638,8 +637,9 @@ def controlCommandCallback(data):
or \
(data.yControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
):
- # Negate the setpoint and convert to degrees
- y_controller_setpoint = -data.yControllerSetpoint * RAD_TO_DEG;
+ # Convert the setpoint to degrees
+ y_controller_setpoint = data.yControllerSetpoint * RAD_TO_DEG;
+ #
else:
# Take the setpoint as is
y_controller_setpoint = data.yControllerSetpoint;
@@ -655,6 +655,7 @@ def controlCommandCallback(data):
rospy.logerr("[CRAZY RADIO] z controller requested disallowed mode, now turning OFF the z-controller.")
z_controller_mode = CF_ONBOARD_CONTROLLER_MODE_OFF;
z_controller_setpoint = 0.0;
+ #
else:
# Take the mode and setpoint as is
z_controller_mode = data.zControllerMode;
@@ -671,7 +672,13 @@ def controlCommandCallback(data):
):
# Negate the setpoint and convert to degrees
yaw_controller_mode = data.yawControllerMode;
- yaw_controller_setpoint = -data.yawControllerSetpoint * RAD_TO_DEG;
+ yaw_controller_setpoint = data.yawControllerSetpoint * RAD_TO_DEG;
+ #
+ elif (data.yawControllerMode==CF_ONBOARD_CONTROLLER_MODE_OFF):
+ # Set the setpoint to zero
+ yaw_controller_mode = data.yawControllerMode;
+ yaw_controller_setpoint = 0.0;
+ #
else:
# Set the yaw-controller to zero angle setpoint and inform the user
rospy.logerr("[CRAZY RADIO] yaw controller requested disallowed mode, now turning to ANGULAR_RATE mode with zero setpoint.")
@@ -686,13 +693,13 @@ def controlCommandCallback(data):
data.motorCmd3,
data.motorCmd4,
data.xControllerMode,
- data.xControllerValue,
+ x_controller_setpoint,
data.yControllerMode,
y_controller_setpoint,
z_controller_mode,
- z_controller_value,
+ z_controller_setpoint,
yaw_controller_mode,
- yaw_controller_value,
+ yaw_controller_setpoint,
)
# END OF: def controlCommandCallback(data):
@@ -733,14 +740,12 @@ if __name__ == '__main__':
# radio_address = "radio://0/72/2M"
# rospy.loginfo("manual address loaded")
-
-
# Fetch the YAML paramter "battery polling period"
global battery_polling_period
- battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadioCopyOfBatteryMonitor/battery_polling_period")
+ battery_polling_period = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioCopyOfBatteryMonitor/battery_polling_period")
global cfStateEstimate_polling_period
- cfStateEstimate_polling_period = rospy.get_param(ros_namespace + "/CrazyRadioConfig/cfStateEstimate_polling_period")
+ cfStateEstimate_polling_period = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/cfStateEstimate_polling_period")
# Fetch the YAML paramter "agentID" and "coordID"
global m_agentID
@@ -749,8 +754,6 @@ if __name__ == '__main__':
# Convert the coordinator ID to a zero-padded string
coordID_as_string = format(coordID, '03')
-
-
# Initialise a publisher for the battery voltage
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
@@ -759,15 +762,11 @@ if __name__ == '__main__':
global cfstateEstimate_pub
cfstateEstimate_pub = rospy.Publisher(node_name + '/CFStateEstimate', FlyingVehicleState, queue_size=10)
-
-
# Initialise a "CrazyRadioClient" type variable that handles
# all communication over the CrazyRadio
global cf_client
cf_client = CrazyRadioClient()
-
-
# Subscribe to the commands for when to (dis-)connect the
# CrazyRadio connection with the Crazyflie
# > For the radio commands from the FlyingAgentClient of this agent
@@ -784,9 +783,7 @@ if __name__ == '__main__':
# 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)
-
-
+ rospy.Subscriber("FlyingAgentClient/FlyingState", Int32, cf_client.flyingStateCallback)
# Sleep briefly to allow everything to start-up
time.sleep(1.0)
--
GitLab