Battery part already developed, need to test it downstairs

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h

@@ -4,6 +4,7 @@
 #include <QMainWindow>
 
 #include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
 
 #include "rosNodeThread.h"
 
@@ -36,9 +37,7 @@ public:
 
 private slots:
     void updateNewViconData(const ptrToMessage& p_msg);
-
     void on_RF_Connect_button_clicked();
-
     void on_enable_disable_CF_button_clicked();
 
 private:
@@ -46,17 +45,27 @@ private:
 
     rosNodeThread* m_rosNodeThread;
     int m_radio_status;
+    float m_battery_voltage;
+
+    int m_battery_level;
 
     ros::Publisher crazyRadioCommandPublisher;
     ros::Subscriber crazyRadioStatusSubscriber;
-
     ros::Publisher PPSClientCommadPublisher;
+    ros::Subscriber CFBatterySubscriber;
 
+    // callbacks
     void crazyRadioStatusCallback(const std_msgs::Int32& msg);
+    void CFBatteryCallback(const std_msgs::Float32& msg);
+
+    float fromVoltageToPercent(float voltage);
+    void updateBatteryVoltage(float battery_voltage);
     void setCrazyRadioStatus(int radio_status);
 
     void disableGUI();
     void enableGUI();
+
+    const std::vector<float> m_cutoff_voltages {3.1966,        3.2711,        3.3061,        3.3229,        3.3423,        3.3592,        3.3694,        3.385,        3.4006,        3.4044,        3.4228,        3.4228,        3.4301,        3.4445,        3.4531,        3.4677,        3.4705,        3.4712,        3.4756,        3.483,        3.4944,        3.5008,        3.5008,        3.5084,        3.511,        3.5122,        3.5243,        3.5329,        3.5412,        3.5529,        3.5609,        3.5625,        3.5638,        3.5848,        3.6016,        3.6089,        3.6223,        3.628,        3.6299,        3.6436,        3.6649,        3.6878,        3.6983,        3.7171,        3.7231,        3.7464,        3.7664,        3.7938,        3.8008,        3.816,        3.8313,        3.8482,        3.866,        3.8857,        3.8984,        3.9159,        3.9302,        3.9691,        3.997,        4.14    };
 };
 
 #endif // MAINWINDOW_H

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp

@@ -8,7 +8,8 @@
 MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
     QMainWindow(parent),
     ui(new Ui::MainWindow),
-    m_radio_status(DISCONNECTED)
+    m_radio_status(DISCONNECTED),
+    m_battery_level(0)
 {
     m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
     ui->setupUi(this);
@@ -23,6 +24,9 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
     ros::NodeHandle nodeHandle(ros_namespace);
     crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
 
+    CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);
+
+
     // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
     ros::NodeHandle nh_PPSClient(ros_namespace + "/PPSClient");
     crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
@@ -40,11 +44,14 @@ MainWindow::~MainWindow()
 void MainWindow::disableGUI()
 {
     ui->enable_disable_CF_button->setEnabled(false);
+    ui->battery_bar->setValue(0);
+    ui->battery_bar->setEnabled(false);
 }
 
 void MainWindow::enableGUI()
 {
     ui->enable_disable_CF_button->setEnabled(true);
+    ui->battery_bar->setEnabled(true);
 }
 
 void MainWindow::setCrazyRadioStatus(int radio_status)
@@ -73,6 +80,36 @@ void MainWindow::setCrazyRadioStatus(int radio_status)
     this->m_radio_status = radio_status;
 }
 
+float MainWindow::fromVoltageToPercent(float voltage)
+{
+    int num_cutoffs = m_cutoff_voltages.size();
+    float hysteresis = 0.05;
+
+    while(m_battery_level < num_cutoffs && voltage >= m_cutoff_voltages[m_battery_level])
+    {
+        ++m_battery_level;
+    }
+    while(m_battery_level > 0 && voltage < m_cutoff_voltages[m_battery_level - 1] - hysteresis)
+    {
+        --m_battery_level;
+    }
+
+    float percentage = 100.0 * m_battery_level/num_cutoffs;
+    return percentage;
+}
+
+void MainWindow::updateBatteryVoltage(float battery_voltage)
+{
+    m_battery_voltage = battery_voltage;
+    // Need to take voltage, display it and transform it to percentage
+    ui->battery_bar->setValue(fromVoltageToPercent(m_battery_voltage));
+}
+
+void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
+{
+    updateBatteryVoltage(msg.data);
+}
+
 void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
 {
     ROS_INFO("Callback CrazyRadioStatus called");
@@ -96,14 +133,14 @@ void MainWindow::on_enable_disable_CF_button_clicked()
     if(ui->enable_disable_CF_button->text().toStdString() == "EnableCF") //enabled, disable if success
     {
         std_msgs::Int32 msg;
-        msg.data = CMD_USE_CRAZYFLY_DISABLE;
+        msg.data = CMD_USE_CRAZYFLY_ENABLE;
         this->PPSClientCommadPublisher.publish(msg);
         ui->enable_disable_CF_button->setText("DisableCF");
     }
     else                        //disabled, enable if success
     {
         std_msgs::Int32 msg;
-        msg.data = CMD_USE_CRAZYFLY_ENABLE;
+        msg.data = CMD_USE_CRAZYFLY_DISABLE;
         this->PPSClientCommadPublisher.publish(msg);
         ui->enable_disable_CF_button->setText("EnableCF");
     }

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui

@@ -73,7 +73,7 @@
         </widget>
        </item>
        <item row="1" column="2">
-        <widget class="QProgressBar" name="progressBar">
+        <widget class="QProgressBar" name="battery_bar">
          <property name="value">
           <number>24</number>
          </property>

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user

 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.2, 2017-08-22T14:42:27. -->
+<!-- Written by QtCreator 4.0.2, 2017-08-22T18:03:35. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

@@ -220,7 +220,7 @@ if __name__ == '__main__':
     # rospy.loginfo("manual address loaded")
 
     global cfbattery_pub
-    cfbattery_pub = rospy.Publisher(node_name + '/cfbattery', Float32, queue_size=10)
+    cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
 
     global cf_client