Commit 3c887084 authored by roangel's avatar roangel
Browse files

Now if the CF disconnects, the state goes back to motors off so when we start...

Now if the CF disconnects, the state goes back to motors off so when we start over we dont have the device flying
parent fc0d46b0
......@@ -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;
}
......
......@@ -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")
......
......@@ -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)
{
......
Supports Markdown
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