Commit 3eec11e2 authored by roangel's avatar roangel
Browse files

battery safety started. Need to implement filtering and reaction in GUI

parent c211fd79
......@@ -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);
......
......@@ -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
......
......@@ -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
......@@ -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
......
......@@ -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);
......
Supports Markdown
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