Skip to content
Snippets Groups Projects
Commit 6c098b8b authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Fixed errors in CrazyRadio python script and tested to work with actual radio... 

Fixed errors in CrazyRadio python script and tested to work with actual radio and Crazyflie 2.1 running the new dfall firmware
parent fe2b937e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+ 18
− 18
View file @ 6c098b8b
......@@ -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
......
dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+ 48
− 51
View file @ 6c098b8b
......@@ -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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment