From a04fc4feca99b27a24c38458e62064758f2b302e Mon Sep 17 00:00:00 2001
From: Apehaenger <joerg@ebeling.ws>
Date: Wed, 4 Sep 2024 23:53:13 +0200
Subject: [PATCH] Add threshold/limits from open_mower config and OpenMower FW

---
 src/mower_logic/src/monitoring/monitoring.cpp | 64 +++++++++++--------
 src/open_mower/launch/open_mower.launch       |  8 +++
 2 files changed, 46 insertions(+), 26 deletions(-)

diff --git a/src/mower_logic/src/monitoring/monitoring.cpp b/src/mower_logic/src/monitoring/monitoring.cpp
index 59f15ad5..47d4b62a 100644
--- a/src/mower_logic/src/monitoring/monitoring.cpp
+++ b/src/mower_logic/src/monitoring/monitoring.cpp
@@ -114,33 +114,45 @@ void pose_received(const xbot_msgs::AbsolutePose::ConstPtr &msg) {
 }
 
 void set_sensor_limits(Sensor &sensor) {
-    // At the moment we process only those with a params parameter path
-    if(!strlen(sensor.param_path.c_str())) {
-        return;
-    }
-    switch (sensor.value_desc) {
-      case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT:
+  switch (sensor.value_desc) {
+    case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_VOLTAGE:
+      if (sensor.si.sensor_id == "om_v_battery") {
+        sensor.si.max_value = paramNh->param("battery_full_voltage", 0.0f);
+        sensor.si.min_value = paramNh->param("battery_empty_voltage", 0.0f);
+        sensor.si.lower_critical_value = paramNh->param("battery_critical_voltage", 0.0f);
+        sensor.si.upper_critical_value = 29.0f;  // Taken from OpenMower FW
+      } else if (sensor.si.sensor_id == "om_v_charge") {
+        sensor.si.upper_critical_value = 30.0f;  // Taken from OpenMower FW
+      }
+      break;
+    case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT:
+      if (sensor.si.sensor_id.find("_motor_") != std::string::npos) {
         sensor.si.max_value = paramNh->param(sensor.param_path + "/motor_current_limit", 0.0f);
-        break;
-      case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE:
-        if (sensor.si.sensor_id.find("_motor_") != std::string::npos)
-          sensor.si.max_value = paramNh->param(sensor.param_path + "/max_motor_temp", 0);
-        else // i.e. _esc_
-          sensor.si.max_value = paramNh->param(sensor.param_path + "/max_pcb_temp", 0);
-        break;
-      case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_RPM:
-        sensor.si.min_value = paramNh->param(sensor.param_path + "/min_motor_rpm", 0);
-        sensor.si.max_value = paramNh->param(sensor.param_path + "/max_motor_rpm", 0);
-        sensor.si.lower_critical_value = paramNh->param(sensor.param_path + "/min_motor_rpm_critical", 0);
-        break;
-    }
-    // Set has* sensor infos
-    if (sensor.si.min_value || sensor.si.max_value)
-        sensor.si.has_min_max = true;
-    if (sensor.si.lower_critical_value)
-        sensor.si.has_critical_low = true;
-    if (sensor.si.upper_critical_value)
-        sensor.si.has_critical_high = true;
+      } else if (sensor.si.sensor_id == "om_charge_current") {
+        sensor.si.upper_critical_value = 1.5f;  // Taken from OpenMower FW
+      }
+
+      break;
+    case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE:
+      if (sensor.si.sensor_id.find("_motor_") != std::string::npos) {
+        // mower_config settings have precedence before xesc param file
+        sensor.si.max_value =
+            paramNh->param("motor_hot_temperature", paramNh->param(sensor.param_path + "/max_motor_temp", 0.0f));
+        sensor.si.min_value =
+            paramNh->param("motor_cold_temperature", paramNh->param(sensor.param_path + "/min_motor_temp", 0.0f));
+      } else  // i.e. _esc_
+        sensor.si.max_value = paramNh->param(sensor.param_path + "/max_pcb_temp", 0);
+      break;
+    case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_RPM:
+      sensor.si.min_value = paramNh->param(sensor.param_path + "/min_motor_rpm", 0);
+      sensor.si.max_value = paramNh->param(sensor.param_path + "/max_motor_rpm", 0);
+      sensor.si.lower_critical_value = paramNh->param(sensor.param_path + "/min_motor_rpm_critical", 0);
+      break;
+  }
+  // Set has* sensor infos
+  if (sensor.si.min_value || sensor.si.max_value) sensor.si.has_min_max = true;
+  if (sensor.si.lower_critical_value) sensor.si.has_critical_low = true;
+  if (sensor.si.upper_critical_value) sensor.si.has_critical_high = true;
 }
 
 void registerSensors() {
diff --git a/src/open_mower/launch/open_mower.launch b/src/open_mower/launch/open_mower.launch
index b0cdf6c8..29e09e81 100644
--- a/src/open_mower/launch/open_mower.launch
+++ b/src/open_mower/launch/open_mower.launch
@@ -67,5 +67,13 @@
 
     <node pkg="mower_logic" type="monitoring" name="monitoring" output="screen" respawn="true" respawn_delay="10">
         <rosparam unless="$(eval env('OM_MOWER')=='CUSTOM')" file="$(find open_mower)/params/hardware_specific/$(env OM_MOWER)/comms_$(env OM_MOWER_ESC_TYPE)_params.yaml"/>
+        <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="motor_hot_temperature" value="$(env OM_MOWING_MOTOR_TEMP_HIGH)"/>
+        <param name="motor_cold_temperature" value="$(env OM_MOWING_MOTOR_TEMP_LOW)"/>
     </node>
 </launch>