Adjusted PPS Client so that the CF cannot be re-launched directly after a battery low event

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp

@@ -526,20 +526,6 @@ void MainWindow::setCrazyRadioStatus(int 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;
-
 	// INITIALISE THE LOCAL VARIABLE FOR THE VOLTAGE WHEN FULL/EMPTY
 	float voltage_when_full;
 	float voltage_when_empty;
@@ -591,7 +577,7 @@ void MainWindow::updateBatteryVoltage(float battery_voltage)
 	// COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
 	float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);
 
-	ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
+	//ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
 
 	// UPDATE THE IMAGE DISPLAYED IN THE BATTERY VOLTAGE LABEL IMAGE
 	switch(m_battery_state)
@@ -734,7 +720,7 @@ void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
 
 void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
 {
-    ROS_INFO("Callback CrazyRadioStatus called");
+    ROS_INFO("[Student GUI] Callback CrazyRadioStatus called");
     this->setCrazyRadioStatus(msg.data);
 }
 

pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp

@@ -506,6 +506,11 @@ void commandAllAgentsCallback(const std_msgs::Int32& msg)
 void crazyRadioStatusCallback(const std_msgs::Int32& msg)
 {
     crazyradio_status = msg.data;
+    // RESET THE BATTERY STATE IF DISCONNECTED
+    if (crazyradio_status == DISCONNECTED)
+    {
+        setBatteryStateTo(BATTERY_STATE_NORMAL);
+    }
 }
 
 void controllerSetPointCallback(const Setpoint& newSetpoint)
@@ -708,12 +713,13 @@ void setBatteryStateTo(int new_battery_state)
 float movingAverageBatteryFilter(float new_input)
 {
     const int N = 7;
-    static float previous_output = 0;
-    static float inputs[N];
+    static float previous_output = 4.2f;
+    static float inputs[N] = {4.2f,4.2f,4.2f,4.2f,4.2f,4.2f,4.2f};
 
 
-    // imagine an array of an even number of samples, we will output the one in the middle averaged with information from all of them.
-    // for that, we only need to store some past of the signal
+    // imagine an array of an even number of samples, we will output the one
+    // in the middle averaged with information from all of them.
+    // For that, we only need to store some past of the signal
     float output = previous_output + new_input/N - inputs[N-1]/N;
 
     // update array of inputs
@@ -738,17 +744,28 @@ void CFBatteryCallback(const std_msgs::Float32& msg)
     float filtered_battery_voltage = movingAverageBatteryFilter(m_battery_voltage); //need to perform filtering here
 
     // 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(
+        (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");
+            ROS_INFO("[PPS CLIENT] low level battery triggered");
+        }
+        
     }
-    else                        //maybe add hysteresis somewhere here?
+    else
     {
-        if(getBatteryState() != BATTERY_STATE_NORMAL)
-            setBatteryStateTo(BATTERY_STATE_NORMAL);
+        // 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);
+        // }
     }
 }
 
@@ -1063,6 +1080,11 @@ int main(int argc, char* argv[])
     // know the battery level of the CF
     ros::Subscriber CFBatterySubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CFBattery", 1, CFBatteryCallback);
 
+    // INITIALISE THE BATTERY VOLTAGE TO SOMETHING CLOSE TO FULL
+    // > This is used to prevent the "Low Battery" being trigged when the 
+    //   first battery voltage data is received
+    m_battery_voltage = 4.2f;
+
 	//start with safe controller
     flying_state = STATE_MOTORS_OFF;
     setControllerUsed(SAFE_CONTROLLER);