Merge pull request #588 from sugikazu75/PR/high_voltage_failsafe

[navigation] failsafe for high input voltage
jsk-ros-pkg · Dec 26, 2023 · be22c8f · be22c8f
2 parents ae6ea70 + 413df64
commit be22c8f
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h
@@ -336,6 +336,8 @@ namespace aerial_robot_navigation
     /* battery info */
     double low_voltage_thre_;
     bool low_voltage_flag_;
+    double high_voltage_cell_thre_;
+    bool high_voltage_flag_;
     int bat_cell_;
     double bat_resistance_;
     double bat_resistance_voltage_rate_;

diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp
@@ -16,6 +16,7 @@ BaseNavigator::BaseNavigator():
   trajectory_reset_time_(0),
   teleop_reset_time_(0),
   low_voltage_flag_(false),
+  high_voltage_flag_(false),
   prev_xy_control_mode_(ACC_CONTROL_MODE),
   xy_control_flag_(false),
   vel_based_waypoint_(false),
@@ -96,6 +97,7 @@ void BaseNavigator::batteryCheckCallback(const std_msgs::Float32ConstPtr &msg)
     voltage += ( (bat_resistance_voltage_rate_ * voltage +  bat_resistance_) * hovering_current_);
 
   float average_voltage = voltage / bat_cell_;
+  float input_cell = voltage / VOLTAGE_100P;
   float percentage = 0;
   if(average_voltage  > VOLTAGE_90P) percentage = (average_voltage - VOLTAGE_90P) / (VOLTAGE_100P - VOLTAGE_90P) * 10 + 90;
   else if (average_voltage  > VOLTAGE_80P) percentage = (average_voltage - VOLTAGE_80P) / (VOLTAGE_90P - VOLTAGE_80P) * 10 + 80;
@@ -124,6 +126,13 @@ void BaseNavigator::batteryCheckCallback(const std_msgs::Float32ConstPtr &msg)
   else
     low_voltage_flag_  = false;
 
+  if(input_cell - bat_cell_ > high_voltage_cell_thre_)
+    {
+      high_voltage_flag_ = true;
+    }
+  else
+    high_voltage_flag_ = false;
+
   if(power_info_pub_.getNumSubscribers() == 0) return;
 
   geometry_msgs::Vector3Stamped power_info_msgs;
@@ -769,6 +778,12 @@ void BaseNavigator::update()
             setNaviState(ARM_OFF_STATE);
             break;
           }
+        if(high_voltage_flag_)
+          {
+            setNaviState(ARM_OFF_STATE);
+            ROS_ERROR("high voltage!");
+            break;
+          }
 
         estimator_->setSensorFusionFlag(true);
         force_landing_flag_ = false;
@@ -1080,6 +1095,7 @@ void BaseNavigator::rosParamInit()
   ros::NodeHandle bat_nh(nh_, "bat_info");
   getParam<int>(bat_nh, "bat_cell", bat_cell_, 0); // Lipo battery cell
   getParam<double>(bat_nh, "low_voltage_thre", low_voltage_thre_, 0.1); // Lipo battery cell
+  getParam<double>(bat_nh, "high_voltage_cell_thre", high_voltage_cell_thre_, 1.0);
   getParam<double>(bat_nh, "bat_resistance", bat_resistance_, 0.0); //Battery internal resistance
   getParam<double>(bat_nh, "bat_resistance_voltage_rate", bat_resistance_voltage_rate_, 0.0); //Battery internal resistance_voltage_rate
   getParam<double>(bat_nh, "hovering_current", hovering_current_, 0.0); // current at hovering state