From 4e14772df54d241e5cdfb34827502d69b5e43b29 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Thu, 6 Dec 2018 09:54:30 +0100
Subject: [PATCH] Continued developing the battery monitor
---
.../d_fall_pps/include/nodes/BatteryMonitor.h | 26 ++-
.../src/d_fall_pps/param/BatteryMonitor.yaml | 16 +-
.../d_fall_pps/src/nodes/BatteryMonitor.cpp | 193 +++++++++++-------
3 files changed, 144 insertions(+), 91 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h b/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
index 7ee4abd0..7f4a1efc 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
@@ -66,7 +66,7 @@
// DDDD EEEEE F III N N EEEEE SSSS
// ----------------------------------------------------------------------------------
-// Battery states
+// Battery levels
#define BATTERY_LEVEL_000 0
#define BATTERY_LEVEL_010 1
#define BATTERY_LEVEL_020 2
@@ -80,6 +80,10 @@
#define BATTERY_LEVEL_100 10
#define BATTERY_LEVEL_UNAVAILABLE -1
+// Battery states
+#define BATTERY_STATE_NORMAL 0
+#define BATTERY_STATE_LOW 1
+
// The types, i.e., agent or coordinator
#define TYPE_INVALID -1
#define TYPE_COORDINATOR 1
@@ -109,9 +113,6 @@
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
-// The level of the battery
-int m_battery_level;
-
// The status of the crazyradio as received via messages
//int m_crazyradio_status;
@@ -121,20 +122,27 @@ int m_agent_operating_state;
// Publisher for the filtered battery voltage
ros::Publisher filteredBatteryVoltagePublisher;
-// Publisher for the battery state
+// Publisher for the battery level
ros::Publisher batteryLevelPublisher;
+// Publisher for changes in the battery state
+ros::Publisher batteryStateChangedPublisher;
+
// VARIABLES THAT ARE LOADED FROM THE YAML FILE
// Battery thresholds while in the "motors off" state, in [Volts]
-float m_battery_threshold_lower_while_standby = 3.30f;
-float m_battery_threshold_upper_while_standby = 4.20f;
+float yaml_battery_voltage_threshold_lower_while_standby = 3.30f;
+float yaml_battery_voltage_threshold_upper_while_standby = 4.20f;
// Battery thresholds while in the "flying" state, in [Volts]
-float m_battery_threshold_lower_while_flying = 2.60f;
-float m_battery_threshold_upper_while_flying = 3.70f;
+float yaml_battery_voltage_threshold_lower_while_flying = 2.60f;
+float yaml_battery_voltage_threshold_upper_while_flying = 3.70f;
+
+// Delay before changing the state of the battery, in [number of measurements]
+// > Note that the delay in seconds therefore depends on the polling period
+int yaml_battery_delay_threshold_to_change_state = 5;
diff --git a/pps_ws/src/d_fall_pps/param/BatteryMonitor.yaml b/pps_ws/src/d_fall_pps/param/BatteryMonitor.yaml
index 86b384ec..3aeb5976 100755
--- a/pps_ws/src/d_fall_pps/param/BatteryMonitor.yaml
+++ b/pps_ws/src/d_fall_pps/param/BatteryMonitor.yaml
@@ -1,10 +1,14 @@
+# Frequency of requesting the battery voltage, in [milliseconds]
+battery_polling_period: 200
+
# Battery thresholds while in the "motors off" state, in [Volts]
-battery_threshold_lower_while_standby: 3.30
-battery_threshold_upper_while_standby: 4.20
+battery_voltage_threshold_lower_while_standby: 3.30
+battery_voltage_threshold_upper_while_standby: 4.20
# Battery thresholds while in the "flying" state, in [Volts]
-battery_threshold_lower_while_flying: 2.60
-battery_threshold_upper_while_flying: 3.70
+battery_voltage_threshold_lower_while_flying: 2.60
+battery_voltage_threshold_upper_while_flying: 3.70
-# Frequency of requesting the battery voltage, in [milliseconds]
-battery_polling_period: 200
\ No newline at end of file
+# Delay before changing the state of the battery, in [number of measurements]
+# > Note that the delay in seconds therefore depends on the polling period
+battery_delay_threshold_to_change_state: 5
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp b/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
index b3bb4d0c..53384c11 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
@@ -64,22 +64,13 @@ using namespace d_fall_pps;
+void agentOperatingStateCallback(const std_msgs::Int32& msg)
+{
+ // Extract the data
+ m_agent_operating_state = msg.data;
+}
-// ----------------------------------------------------------------------------------
-// L OOO A DDDD
-// L O O A A D D
-// L O O A A D D
-// L O O AAAAA D D
-// LLLLL OOO A A DDDD
-//
-// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
-// P P A A R R A A MM MM E T E R R S
-// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
-// P AAAAA R R AAAAA M M E T E R R S
-// P A A R R A A M M EEEEE T EEEEE R R SSSS
-// ----------------------------------------------------------------------------------
-
@@ -100,6 +91,8 @@ void crazyRadioStatusCallback(const std_msgs::Int32& msg)
+
+
void newBatteryVoltageCallback(const std_msgs::Float32& msg)
{
// Extract the data
@@ -115,48 +108,75 @@ void newBatteryVoltageCallback(const std_msgs::Float32& msg)
// Convert the battery percentage to a level
int filtered_battery_voltage_as_level = convertPercentageToLevel(filtered_battery_voltage_as_percentage);
- // Then publish that the battery level is unavailable
+ // Publish that the battery voltage
+ std_msgs::Float32 filtered_battery_voltage_msg;
+ filtered_battery_voltage_msg.data = filtered_battery_voltage;
+ filteredBatteryVoltagePublisher.publish(filtered_battery_voltage_msg);
+
+ // Publish that the battery level
std_msgs::Int32 battery_level_msg;
- battery_level_msg.data = BATTERY_LEVEL_UNAVAILABLE;
+ battery_level_msg.data = filtered_battery_voltage_as_level;
batteryLevelPublisher.publish(battery_level_msg);
- // ROS_INFO_STREAM("filtered data: " << filtered_battery_voltage);
- 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))
- )
- {
- if(getBatteryState() != BATTERY_STATE_LOW)
- {
- setBatteryStateTo(BATTERY_STATE_LOW);
- ROS_INFO("[PPS CLIENT] low level battery triggered");
- }
+ // Update the battery state using the level
+ // > Note that the function called sends a message
+ // only if the battery state changes
+ updateBatteryStateBasedOnLevel(filtered_battery_voltage_as_level);
+}
+
+
+
+
+
+// > For converting a voltage to a percentage, depending on the current "my_flying_state" value
+float fromVoltageToPercent(float voltage , float operating_state )
+{
+ // INITIALISE THE LOCAL VARIABLE FOR THE VOLTAGE WHEN FULL/EMPTY
+ float voltage_when_full;
+ float voltage_when_empty;
+ // COMPUTE THE PERCENTAGE DIFFERENTLY DEPENDING ON
+ // THE CURRENT FLYING STATE
+ if (operating_state == STATE_MOTORS_OFF)
+ {
+ // Voltage limits for a "standby" type of state
+ voltage_when_empty = yaml_battery_voltage_threshold_lower_while_standby;
+ voltage_when_full = yaml_battery_voltage_threshold_upper_while_standby;
}
else
{
- // TO AVOID BEING ABLE TO IMMEDIATELY TAKE-OFF AFTER A
- // "BATTERY LOW" EVENT IS TRIGGERED, WE DO NOT SET THE
- // BATTERY STATE BACK TO NORMAL
- // if(getBatteryState() != BATTERY_STATE_NORMAL)
- // {
- // setBatteryStateTo(BATTERY_STATE_NORMAL);
- // }
+ // Voltage limits for a "flying" type of state
+ voltage_when_empty = yaml_battery_voltage_threshold_lower_while_flying;
+ voltage_when_full = yaml_battery_voltage_threshold_upper_while_flying;
}
-}
+ // COMPUTE THE PERCENTAGE
+ float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
+ // CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
+ // > This should not happen to often
+ if(percentage > 100.0f)
+ {
+ percentage = 100.0f;
+ }
+ if(percentage < 0.0f)
+ {
+ percentage = 0.0f;
+ }
-void agentOperatingStateCallback(const std_msgs::Int32& msg)
-{
- // Extract the data
- m_agent_operating_state = msg.data;
+ // RETURN THE PERCENTAGE
+ return percentage;
}
+
+
+
int convertPercentageToLevel(float percentage)
{
+ // Initialise the battery level
+ static int battery_level = BATTERY_LEVEL_UNAVAILABLE;
+
// Iterate through the levels
for (i_level=0;i_level<10;i++)
{
@@ -164,7 +184,7 @@ int convertPercentageToLevel(float percentage)
float threshold = float(i_level) * 10.0f;
// Add a buffer to the threshold to prevent
// high frequency changes to the level
- if (m_battery_level==i_level)
+ if (battery_level==i_level)
{
threshold += 2.0f;
}
@@ -178,51 +198,66 @@ int convertPercentageToLevel(float percentage)
// returning, then the percentage is at or above
// the maximum level
return BATTERY_LEVEL_100;
-
}
-// > For converting a voltage to a percentage, depending on the current "my_flying_state" value
-float fromVoltageToPercent(float voltage , float operating_state )
+
+
+
+void updateBatteryStateBasedOnLevel(level)
{
- // INITIALISE THE LOCAL VARIABLE FOR THE VOLTAGE WHEN FULL/EMPTY
- float voltage_when_full;
- float voltage_when_empty;
+ // Initialise the battery state
+ static int battery_state = BATTERY_STATE_NORMAL;
- // COMPUTE THE PERCENTAGE DIFFERENTLY DEPENDING ON
- // THE CURRENT FLYING STATE
- if (operating_state == STATE_MOTORS_OFF)
+ // Initialise a counter for delaying a change of state
+ static int num_since_change = 0;
+
+ if (level == 0)
{
- // Voltage limits for a "standby" type of state
- voltage_when_empty = m_battery_threshold_lower_while_standby;
- voltage_when_full = m_battery_threshold_upper_while_standby;
+ if (battery_state == BATTERY_STATE_NORMAL)
+ {
+ num_since_change++;
+ }
+ else
+ {
+ num_since_change = 0;
+ }
}
- else
+ else if (level >= 1)
{
- // Voltage limits for a "flying" type of state
- voltage_when_empty = m_battery_threshold_lower_while_flying;
- voltage_when_full = m_battery_threshold_upper_while_flying;
+ if (battery_state == BATTERY_STATE_LOW)
+ {
+ num_since_change++;
+ }
+ else
+ {
+ num_since_change = 0;
+ }
}
- // COMPUTE THE PERCENTAGE
- float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
-
- // CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
- // > This should not happen to often
- if(percentage > 100.0f)
- {
- percentage = 100.0f;
- }
- if(percentage < 0.0f)
+ // Check if the "delay-to-change" threshold is reached
+ if (num_since_change >= yaml_battery_delay_threshold_to_change_state)
{
- percentage = 0.0f;
+ if (battery_state == BATTERY_STATE_NORMAL)
+ {
+ battery_state = BATTERY_STATE_LOW;
+ }
+ else
+ {
+ battery_state = BATTERY_STATE_NORMAL;
+ }
+ // Publish the change
+ std_msgs::Int32 battery_state_msg;
+ battery_state_msg.data = battery_state;
+ batteryStateChangedPublisher.publish(battery_state_msg);
}
-
- // RETURN THE PERCENTAGE
- return percentage;
}
+
+
+
+
// ----------------------------------------------------------------------------------
// M M A III N N
// MM MM A A I NN N
@@ -251,9 +286,18 @@ int main(int argc, char* argv[])
// Advertise the service for loading Yaml Files
ros::ServiceServer service = nodeHandle.advertiseService("LoadYamlFiles", loadYamlFiles);
- // Publish the battery level
- batteryLevelPublisher = nodeHandle.advertise<std_msgs::Int32>("BatteryLevel",1);
+ // PUBLISHERS
+ // Publisher for the filtered battery voltage
+ ros::Publisher filteredBatteryVoltagePublisher = nodeHandle.advertise<std_msgs::Float32>("FilteredVoltage",1);
+
+ // Publisher for the battery level
+ batteryLevelPublisher = nodeHandle.advertise<std_msgs::Int32>("Level",1);
+
+ // Publisher for changes in the battery state
+ ros::Publisher batteryStateChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("ChangedStateTo",1);
+
+ // SUBSCRIBERS
// Subscribe to the voltage of the battery
ros::Subscriber newBatteryVoltageSubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, newBatteryVoltageCallback);
@@ -263,9 +307,6 @@ int main(int argc, char* argv[])
// Subscribe to the flying state of the agent
ros::Subscriber agentOperatingStateSubscriber = nodeHandle.subscribe("PPS_Client/flyingState", 1, agentOperatingStateCallback);
- // Initialise the battery level
- m_battery_level = BATTERY_LEVEL_UNAVAILABLE;
-
// Initialise the operating state
m_agent_operating_state = AGENT_OPERATING_STATE_MOTORS_OFF;
--
GitLab