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 7dddd254e5094339a5b8ef93dee7c928989cafc8..9e222738453c21dd4990157b9e7abd7fc6d00a38 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 215c1b28ddb8bb9081eb96efdfa33ae719151fa1..5c879f982036bc1b66f1ada2b7394ece84caa85d 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 cc9e62d0fbc75c8980fdc6e2e1795fde9d3228f5..454eb62c3db1fb8fc6d6f9fdd3683d7c3a15f03b 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 4d72fc7aaa50018722cab598d68b70be19eea96b..056562a7e62191c0e23a36a27a3a2f5de370bd0a 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 8d56bc0a9b0f7172ef3f88843060190cccc5f0ef..007727f656b705390b37d2f83bb158b788ee7089 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);