Log reason for docking. Allow for short voltage drops during mowing d…

…ue to current spikes. (#82)

* Log dock reason

* Allow for voltage droop in battery low logic

src/mower_logic/cfg/MowerLogic.cfg

@@ -19,7 +19,8 @@ gen.add("mow_angle_increment", double_t, 0, "Mowing angle automatic increment. W
 gen.add("tool_width", double_t, 0, "Width of the mower", 0.14, 0.1, 2)
 gen.add("enable_mower", bool_t, 0, "True to enable mow motor", False)
 gen.add("manual_pause_mowing", bool_t, 0, "True to disable mowing automatically", False)
-gen.add("battery_empty_voltage", double_t, 0, "Voltage to return to docking station", 25.0, 20.0, 32.0)
+gen.add("battery_empty_voltage", double_t, 0, "Voltage to return to docking station (over 20s interval)", 24.0, 20.0, 32.0)
+gen.add("battery_critical_voltage", double_t, 0, "Voltage to return to docking station (immediate)", 23.0, 20.0, 32.0)
 gen.add("battery_full_voltage", double_t, 0, "Voltage to start mowing again", 29.0, 20.0, 32.0)
 gen.add("motor_hot_temperature", double_t, 0, "Motor temperature to pause mowing", 70.0, 20.0, 150.0)
 gen.add("motor_cold_temperature", double_t, 0, "Motor temperature to allow mowing", 40.0, 20.0, 150.0)

src/mower_logic/src/mower_logic/mower_logic.cpp

@@ -51,6 +51,8 @@
 #include "xbot_msgs/RegisterActionsSrv.h"
 #include <mutex>
 #include <atomic>
+#include <sstream>
+#include <ios>
 
 ros::ServiceClient pathClient, mapClient, dockingPointClient, gpsClient, mowClient, emergencyClient, pathProgressClient, setNavPointClient, clearNavPointClient, clearMapClient, positioningClient, actionRegistrationClient;
 
@@ -81,6 +83,8 @@ std::atomic<bool> mowerEnabled;
 
 Behavior *currentBehavior = &IdleBehavior::INSTANCE;
 
+ros::Time last_v_battery_check;
+double max_v_battery_seen = 0.0;
 
 /**
  * Some thread safe methods to get a copy of the logic state
@@ -468,8 +472,32 @@ void checkSafety(const ros::TimerEvent &timer_event) {
 
     // we are in non emergency, check if we should pause. This could be empty battery, rain or hot mower motor etc.
     bool dockingNeeded = false;
-    if (last_status.v_battery < last_config.battery_empty_voltage || last_status.mow_esc_status.temperature_motor >= last_config.motor_hot_temperature ||
-        last_config.manual_pause_mowing) {
+    std::stringstream dockingReason("Docking: ", std::ios_base::ate | std::ios_base::in | std::ios_base::out);
+
+    if (last_config.manual_pause_mowing) {
+        dockingReason << "Manual pause";
+        dockingNeeded = true;
+    }
+
+    // Dock if below critical voltage to avoid BMS undervoltage protection
+    if(!dockingNeeded && (last_status.v_battery < last_config.battery_critical_voltage)) {
+        dockingReason << "Battery voltage min critical: " << last_status.v_battery;
+        dockingNeeded = true;
+    }
+
+    // Otherwise take the max battery voltage over 20s to ignore droop during short current spikes
+    max_v_battery_seen = std::max<double>(max_v_battery_seen, last_status.v_battery);
+    if (ros::Time::now() - last_v_battery_check > ros::Duration(20.0)) {
+        if(!dockingNeeded && (max_v_battery_seen < last_config.battery_empty_voltage)) {
+            dockingReason << "Battery average voltage low: " << max_v_battery_seen;
+            dockingNeeded = true;
+        }
+        max_v_battery_seen = 0.0;
+        last_v_battery_check = ros::Time::now();
+    }
+
+    if (!dockingNeeded && last_status.mow_esc_status.temperature_motor >= last_config.motor_hot_temperature) {
+        dockingReason << "Mow motor over temp: " << last_status.mow_esc_status.temperature_motor;
         dockingNeeded = true;
     }
 
@@ -630,6 +658,7 @@ int main(int argc, char **argv) {
         }
         r.sleep();
     }
+
     ROS_INFO("Waiting for a pose message");
     while (pose_time == ros::Time(0.0)) {
         if (!ros::ok()) {
@@ -762,6 +791,7 @@ int main(int argc, char **argv) {
 
 
 
+    last_v_battery_check = ros::Time::now();
     ros::Timer safety_timer = n->createTimer(ros::Duration(0.5), checkSafety);
     ros::Timer ui_timer = n->createTimer(ros::Duration(1.0), updateUI);
 

src/open_mower/config/mower_config.sh.example

@@ -143,8 +143,11 @@ export OM_MOWING_ANGLE_INCREMENT=0
 export OM_TOOL_WIDTH=0.13
 
 # Voltages for battery to be considered full or empty
-export OM_BATTERY_EMPTY_VOLTAGE=25.0
 export OM_BATTERY_FULL_VOLTAGE=28.5
+# Dock if voltage remains below empty for 20s
+export OM_BATTERY_EMPTY_VOLTAGE=24.0
+# Immediate dock if voltage is critical
+export OM_BATTERY_CRITICAL_VOLTAGE=23.0
 
 # Mower motor temperatures to stop and start mowing
 export OM_MOWING_MOTOR_TEMP_HIGH=80.0

src/open_mower/launch/open_mower.launch

@@ -7,6 +7,9 @@
         <arg name="prefix" value="mow_area"/>
     </include>
 
+    <arg name="battery_empty_voltage" value="$(env OM_BATTERY_EMPTY_VOLTAGE)" />
+    <arg name="battery_critical_voltage" value="$(optenv OM_BATTERY_CRITICAL_VOLTAGE)" />
+
     <node pkg="mower_map" type="mower_map_service" name="map_service" output="screen"/>
     <node pkg="mower_logic" type="mower_logic" name="mower_logic" output="screen">
         <param name="automatic_mode" value="$(optenv OM_AUTOMATIC_MODE 0)"/>
@@ -15,14 +18,18 @@
         <param name="perimeter_signal" value="$(optenv OM_PERIMETER_SIGNAL)"/>
         <param name="tool_width" value="$(env OM_TOOL_WIDTH)"/>
         <param name="enable_mower" value="$(env OM_ENABLE_MOWER)"/>
-        <param name="battery_empty_voltage" value="$(env OM_BATTERY_EMPTY_VOLTAGE)"/>
+        <param name="battery_empty_voltage" value="$(arg battery_empty_voltage)"/>
+        <param
+            name="battery_critical_voltage"
+            value="$(eval battery_empty_voltage if battery_critical_voltage=='' else battery_critical_voltage)"
+        />
+        <param name="battery_full_voltage" value="$(env OM_BATTERY_FULL_VOLTAGE)"/>
         <param name="outline_count" value="$(env OM_OUTLINE_COUNT)"/>
         <param name="outline_overlap_count" value="$(optenv OM_OUTLINE_OVERLAP_COUNT 0)"/>
         <param name="outline_offset" value="$(env OM_OUTLINE_OFFSET)"/>
         <param name="mow_angle_offset" value="$(optenv OM_MOWING_ANGLE_OFFSET 0)"/>
         <param name="mow_angle_offset_is_absolute" value="$(optenv OM_MOWING_ANGLE_OFFSET_IS_ABSOLUTE False)"/>
         <param name="mow_angle_increment" value="$(optenv OM_MOWING_ANGLE_INCREMENT 0)"/>
-        <param name="battery_full_voltage" value="$(env OM_BATTERY_FULL_VOLTAGE)"/>
         <param name="motor_hot_temperature" value="$(env OM_MOWING_MOTOR_TEMP_HIGH)"/>
         <param name="motor_cold_temperature" value="$(env OM_MOWING_MOTOR_TEMP_LOW)"/>
         <param name="gps_wait_time" value="$(optenv OM_GPS_WAIT_TIME_SEC 10.0)"/>

src/open_mower/launch/sim_mower_logic.launch

@@ -7,6 +7,9 @@
     <include file="$(find open_mower)/launch/include/_move_base.launch" />
     <include file="$(find open_mower)/launch/include/_teleop.launch"/>
 
+    <arg name="battery_empty_voltage" value="$(env OM_BATTERY_EMPTY_VOLTAGE)" />
+    <arg name="battery_critical_voltage" value="$(optenv OM_BATTERY_CRITICAL_VOLTAGE)" />
+
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find open_mower)/rviz/sim_mower_logic.rviz" required="true" />
     <node pkg="mower_map" type="mower_map_service" name="mower_map" required="true" />
     <node pkg="slic3r_coverage_planner" type="slic3r_coverage_planner" name="slic3r_coverage_planner" output="screen" required="true" />
@@ -18,14 +21,18 @@
         <param name="undock_distance" value="$(env OM_UNDOCK_DISTANCE)"/>
         <param name="tool_width" value="$(env OM_TOOL_WIDTH)"/>
         <param name="enable_mower" value="$(env OM_ENABLE_MOWER)"/>
-        <param name="battery_empty_voltage" value="$(env OM_BATTERY_EMPTY_VOLTAGE)"/>
+        <param name="battery_empty_voltage" value="$(arg battery_empty_voltage)"/>
+        <param
+            name="battery_critical_voltage"
+            value="$(eval battery_empty_voltage if battery_critical_voltage=='' else battery_critical_voltage)"
+        />
+        <param name="battery_full_voltage" value="$(env OM_BATTERY_FULL_VOLTAGE)"/>
         <param name="outline_count" value="$(env OM_OUTLINE_COUNT)"/>
         <param name="outline_overlap_count" value="$(optenv OM_OUTLINE_OVERLAP_COUNT 0)"/>
         <param name="outline_offset" value="$(env OM_OUTLINE_OFFSET)"/>
         <param name="mow_angle_offset" value="$(optenv OM_MOWING_ANGLE_OFFSET 0)"/>
         <param name="mow_angle_offset_is_absolute" value="$(optenv OM_MOWING_ANGLE_OFFSET_IS_ABSOLUTE False)"/>
         <param name="mow_angle_increment" value="$(optenv OM_MOWING_ANGLE_INCREMENT 0)"/>
-        <param name="battery_full_voltage" value="$(env OM_BATTERY_FULL_VOLTAGE)"/>
         <param name="motor_hot_temperature" value="$(env OM_MOWING_MOTOR_TEMP_HIGH)"/>
         <param name="motor_cold_temperature" value="$(env OM_MOWING_MOTOR_TEMP_LOW)"/>
     </node>