Commit 7fb7d5d4 authored by roangel's avatar roangel
Browse files

Battery part already developed, need to test it downstairs

parent 9bf104cc
......@@ -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
......@@ -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");
}
......
......@@ -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>
......
<?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>
......
......@@ -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
......
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