From 3c8870849636b05d351d377cb41c9f29a5d3c070 Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Fri, 25 Aug 2017 11:54:27 +0200
Subject: [PATCH] Now if the CF disconnects, the state goes back to motors off
so when we start over we dont have the device flying
---
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 14 +++++++++----
.../src/d_fall_pps/crazyradio/CrazyRadio.py | 20 +++++++++++++++++++
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 5 ++++-
3 files changed, 34 insertions(+), 5 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index c1bd7a8e..66a4291e 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -8,13 +8,15 @@
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow),
- m_radio_status(DISCONNECTED),
m_battery_level(0)
{
m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
ui->setupUi(this);
m_rosNodeThread->init();
+ setCrazyRadioStatus(DISCONNECTED);
+
+
std::string ros_namespace = ros::this_node::getNamespace();
ROS_INFO("namespace: %s", ros_namespace.c_str());
@@ -45,13 +47,10 @@ MainWindow::~MainWindow()
void MainWindow::disableGUI()
{
- ui->battery_bar->setValue(0);
- ui->battery_bar->setEnabled(false);
}
void MainWindow::enableGUI()
{
- ui->battery_bar->setEnabled(true);
}
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
@@ -118,6 +117,13 @@ float MainWindow::fromVoltageToPercent(float voltage)
}
float percentage = 100.0 * m_battery_level/num_cutoffs;
+
+ // should not hapen, but just in case...
+ if(percentage > 100)
+ percentage = 100;
+ if(percentage < 0)
+ percentage = 0;
+
return percentage;
}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 886c13cb..4b4f945b 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -47,6 +47,13 @@ DISCONNECTED = 2
# Commands coming
CMD_RECONNECT = 0
+# Commands for PPSClient
+CMD_USE_SAFE_CONTROLLER = 1
+CMD_USE_CUSTOM_CONTROLLER = 2
+CMD_CRAZYFLY_TAKE_OFF = 3
+CMD_CRAZYFLY_LAND = 4
+CMD_CRAZYFLY_MOTORS_OFF = 5
+
rp = RosPack()
record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
@@ -73,6 +80,7 @@ class PPSRadioClient:
self.link_uri = link_uri
self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
+ self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', Int32, queue_size=1)
# Initialize the CrazyFlie and add callbacks
self._init_cf()
@@ -155,11 +163,16 @@ class PPSRadioClient:
quadrotor is established.
"""
self.change_status_to(CONNECTED)
+ # change state to motors OFF
+ cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+
rospy.loginfo("Connection to %s successful: " % link_uri)
# Config for Logging
self._start_logging()
+
+
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
@@ -177,6 +190,9 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED)
rospy.logwarn("Disconnected from %s" % link_uri)
+ # change state to motors OFF
+ self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+
self.logconf.stop()
rospy.loginfo("logconf stopped")
self.logconf.delete()
@@ -247,9 +263,13 @@ if __name__ == '__main__':
rospy.loginfo("Turning off crazyflie")
cf_client._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR)
+ # change state to motors OFF
+ cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
#wait for client to send its commands
time.sleep(1.0)
+
+
bag.close()
rospy.loginfo("bag closed")
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index c2415fce..6cbad5bd 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -243,6 +243,7 @@ void viconCallback(const ViconData& viconData) {
if(changed_state_flag) // stuff that will be run only once when changing state
{
changed_state_flag = false;
+ ROS_INFO("STATE_MOTORS_OFF");
}
break;
case STATE_TAKE_OFF: //we need to move up from where we are now.
@@ -252,6 +253,7 @@ void viconCallback(const ViconData& viconData) {
takeOffCF(local);
counter = 0;
finished_take_off = false;
+ ROS_INFO("STATE_TAKE_OFF");
}
counter++;
if(counter >= DURATION_TAKE_OFF)
@@ -271,6 +273,7 @@ void viconCallback(const ViconData& viconData) {
changed_state_flag = false;
// need to change setpoint to the one from file
goToOrigin();
+ ROS_INFO("STATE_FLYING");
}
break;
case STATE_LAND:
@@ -280,6 +283,7 @@ void viconCallback(const ViconData& viconData) {
landCF(local);
counter = 0;
finished_land = false;
+ ROS_INFO("STATE_LAND");
}
counter++;
if(counter >= DURATION_LANDING)
@@ -297,7 +301,6 @@ void viconCallback(const ViconData& viconData) {
break;
}
-
// control part that should always be running, calls to controller, computation of control output
if(flying_state != STATE_MOTORS_OFF)
{
--
GitLab