To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 533ac4fd authored by beuchatp's avatar beuchatp
Browse files

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

parent bbb9f67d
......@@ -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);
}
......
......@@ -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);
......
Markdown is supported
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