diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index e191cb235f8f23fa1e3f458d98ad81c146bb2b9b..fbd56ad93500202d2978e353dffa3746a4a7c8c8 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/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
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 4453745924aa539f87cd0f878b3a833dadbe5cd4..e96199a262974ad6e93fdb7f07f69c7249950f6a 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,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");
}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index 3ce33712809ec7990a0ffe2baa3f548cd81c0d5a..9f8799ba2a2fd1f022978e07cbe5372d11a33383 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/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>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
index 0999881d56c61ab3547dbcefd32761dd4e1b19c8..7bde36e126e35a4673283e6cc0126e5f98707b26 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
@@ -1,6 +1,6 @@
<?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>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 78517bd1c50a994915fcf916a54097b18f6c77c5..edc8997218b15e8ce16a14c9c923c681e506bbb5 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/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