From 3eec11e2846af39f443d90c0208b84a0b3aa18c6 Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Tue, 26 Sep 2017 16:46:46 +0200
Subject: [PATCH] battery safety started. Need to implement filtering and
reaction in GUI
---
.../GUI_Qt/studentGUI/include/MainWindow.h | 2 +
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 8 ++
pps_ws/src/d_fall_pps/param/ClientConfig.yaml | 2 +
.../src/d_fall_pps/param/SafeController.yaml | 4 +-
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 83 +++++++++++++++++++
5 files changed, 97 insertions(+), 2 deletions(-)
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 7dddd254..9e222738 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
@@ -103,6 +103,7 @@ private:
ros::Publisher PPSClientCommandPublisher;
ros::Subscriber CFBatterySubscriber;
ros::Subscriber flyingStateSubscriber;
+ ros::Subscriber batteryStateSubscriber;
ros::Publisher controllerSetpointPublisher;
ros::Subscriber safeSetpointSubscriber;
@@ -129,6 +130,7 @@ private:
void customYamlFileTimerCallback(const ros::TimerEvent&);
void safeYamlFileTimerCallback(const ros::TimerEvent&);
void controllerUsedChangedCallback(const std_msgs::Int32& msg);
+ void batteryStateChangedCallback(const std_msgs::Int32& msg);
float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage);
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 215c1b28..5c879f98 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
@@ -41,6 +41,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
+ batteryStateSubscriber = nodeHandle.subscribe("PPSClient/batteryState", 1, &MainWindow::batteryStateChangedCallback, this);
+
controllerUsedSubscriber = nodeHandle.subscribe("PPSClient/controllerUsed", 1, &MainWindow::controllerUsedChangedCallback, this);
@@ -182,6 +184,12 @@ void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
ui->flying_state_label->setText(qstr);
}
+void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
+{
+ // switch case with unabling buttons motors off, take off, etc... when battery is shit
+}
+
+
void MainWindow::setCrazyRadioStatus(int radio_status)
{
// add more things whenever the status is changed
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index cc9e62d0..454eb62c 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -2,4 +2,6 @@ safeController: "SafeControllerService/RateController"
customController: "FollowN_1Service/FollowController"
strictSafety: true
angleMargin: 0.6
+battery_threshold_while_flying: 2.9
+battery_threshold_while_motors_off: 3.3
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index 4d72fc7a..056562a7 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/pps_ws/src/d_fall_pps/param/SafeController.yaml
@@ -15,10 +15,10 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
-defaultSetpoint: [0.0, 0.0, 0.3, 0.0]
+defaultSetpoint: [0.0, 0.0, 0.4, 0.0]
#take off and landing parameters (in meters and seconds)
-takeOffDistance : 0.7
+takeOffDistance : 0.4
landingDistance : 0.1
durationTakeOff : 3
durationLanding : 3
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 8d56bc0a..007727f6 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -49,6 +49,11 @@
#define STATE_FLYING 3
#define STATE_LAND 4
+// battery states
+
+#define BATTERY_STATE_NORMAL 0
+#define BATTERY_STATE_LOW 1
+
// commands for CrazyRadio
#define CMD_RECONNECT 0
#define CMD_DISCONNECT 1
@@ -82,6 +87,16 @@ ros::ServiceClient customController;
bool strictSafety;
float angleMargin;
+// battery threshold
+float m_battery_threshold_while_flying;
+float m_battery_threshold_while_motors_off;
+
+
+// battery values
+
+int m_battery_state;
+float m_battery_voltage;
+
Setpoint controller_setpoint;
// variables for linear trayectory
@@ -101,6 +116,9 @@ ros::Publisher safeControllerServiceSetpointPublisher;
// publisher for flying state
ros::Publisher flyingStatePublisher;
+// publisher for battery state
+ros::Publisher batteryStatePublisher;
+
// publisher to send commands to itself.
ros::Publisher commandPublisher;
@@ -517,6 +535,14 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
ROS_ERROR("Failed to get angleMargin param");
return;
}
+ if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
+ ROS_ERROR("Failed to get battery_threshold_while_flying param");
+ return;
+ }
+ if(!nodeHandle.getParam("battery_threshold_while_motors_off", m_battery_threshold_while_motors_off)) {
+ ROS_ERROR("Failed to get battery_threshold_while_motors_off param");
+ return;
+ }
}
void loadSafeControllerParameters()
@@ -666,6 +692,55 @@ void safeYAMLloadedCallback(const std_msgs::Int32& msg)
loadSafeControllerParameters();
}
+int getBatteryState()
+{
+ return m_battery_state;
+}
+
+
+void setBatteryStateTo(int new_battery_state)
+{
+ switch(new_battery_state)
+ {
+ case BATTERY_STATE_NORMAL:
+ m_battery_state = BATTERY_STATE_NORMAL;
+ break;
+ case BATTERY_STATE_LOW:
+ m_battery_state = BATTERY_STATE_LOW;
+ changeFlyingStateTo(STATE_LAND);
+ break;
+ default:
+ ROS_INFO("Unknown battery state command, set to normal");
+ m_battery_state = BATTERY_STATE_NORMAL;
+ break;
+ }
+
+ std_msgs::Int32 battery_state_msg;
+ battery_state_msg.data = getBatteryState();
+ batteryStatePublisher.publish(battery_state_msg);
+}
+
+
+void CFBatteryCallback(const std_msgs::Int32& msg)
+{
+ m_battery_voltage = msg.data;
+ // filter and check if inside limits, and if, change status
+ // need to do the filtering first
+ float filtered_battery_voltage = m_battery_voltage; //need to perform filtering here
+ if((flying_state != STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_flying)) ||
+ (flying_state == STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_motors_off)))
+ {
+ setBatteryStateTo(BATTERY_STATE_LOW);
+ }
+ else //maybe add hysteresis somewhere here?
+ {
+ if(getBatteryState() != BATTERY_STATE_NORMAL)
+ {
+ setBatteryStateTo(BATTERY_STATE_NORMAL);
+ }
+ }
+}
+
int main(int argc, char* argv[])
{
ros::init(argc, argv, "PPSClient");
@@ -713,6 +788,9 @@ int main(int argc, char* argv[])
// this topic will publish flying state whenever it changes.
flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
+ // will publish battery state when it changes
+ batteryStatePublisher = nodeHandle.advertise<std_msgs::Int32>("batteryState", 1);
+
controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
// crazy radio status
@@ -740,11 +818,16 @@ int main(int argc, char* argv[])
ros::Subscriber safeYAMloadedSubscriber = namespaceNodeHandle.subscribe("student_GUI/safeYAMLloaded", 1, safeYAMLloadedCallback);
+ // know the battery level of the CF
+ ros::Subscriber CFBatterySubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CFBattery", 1, CFBatteryCallback);
+
//start with safe controller
flying_state = STATE_MOTORS_OFF;
setControllerUsed(SAFE_CONTROLLER);
setInstantController(SAFE_CONTROLLER); //initialize this also, so we notify GUI
+ setBatteryStateTo(BATTERY_STATE_NORMAL); //initialize battery state
+
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO_STREAM(package_path);
--
GitLab