Add mow_enabled to status message

Update status msg and replicate mower_comms logic from
ClemensElflein/open_mower_ros#125

stm32/ros_usbnode/src/ros/ros_custom/cpp_main.cpp

@@ -89,6 +89,7 @@ static uint8_t left_dir = 0;
 static uint8_t right_dir = 0;
 
 // blade motor control
+static uint8_t target_blade_on_off = 0;
 static uint8_t blade_on_off = 0;
 static uint8_t blade_direction = 0;
 
@@ -203,7 +204,7 @@ extern "C" void CommandHighLevelStatusMessageCb(const mower_msgs::HighLevelStatu
 	{
 		PANEL_Set_LED(PANEL_LED_LOCK, PANEL_LED_ON);
 	}
-	if (blade_on_off)
+	if (target_blade_on_off)
 	{
 		if (BLADEMOTOR_bActivated)
 		{
@@ -278,7 +279,7 @@ extern "C" void CommandHighLevelStatusMessageCb(const mower_msgs::HighLevelStatu
 		PANEL_Set_LED(PANEL_LED_8H, PANEL_LED_OFF);
 		main_eOpenmowerStatus = OPENMOWER_STATUS_IDLE;
 		left_dir = right_dir = 1;
-		left_speed = right_speed = blade_on_off = 0;
+		left_speed = right_speed = blade_on_off = target_blade_on_off = 0;
 		break;
 	}
 }
@@ -374,6 +375,7 @@ extern "C" void motors_handler()
 {
 	if (NBT_handler(&motors_nbt))
 	{
+		blade_on_off = target_blade_on_off;
 		if (Emergency_State())
 		{
 			DRIVEMOTOR_SetSpeed(0, 0, 0, 0);
@@ -597,7 +599,7 @@ extern "C" void broadcast_handler()
 		om_mower_status_msg.mow_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK;
 		om_mower_status_msg.left_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK;
 		om_mower_status_msg.right_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK;
-
+		om_mower_status_msg.mow_enabled = target_blade_on_off;
 		pubOMStatus.publish(&om_mower_status_msg);
 
 	}
@@ -611,12 +613,12 @@ void cbEnableMowerMotor(const mower_msgs::MowerControlSrvRequest &req, mower_msg
 {
 	if (req.mow_enabled && !Emergency_State())
 	{
-		blade_on_off = 1;
+		target_blade_on_off = 1;
 		blade_direction = req.mow_direction;
 	}
 	else
 	{
-		blade_on_off = 0;
+		target_blade_on_off = 0;
 	}
 }
 

stm32/ros_usbnode/src/ros/ros_lib/mower_msgs/Status.h

@@ -41,6 +41,8 @@ namespace mower_msgs
       _v_battery_type v_battery;
       typedef float _charge_current_type;
       _charge_current_type charge_current;
+      typedef bool _mow_enabled_type;
+      _mow_enabled_type mow_enabled;
       typedef mower_msgs::ESCStatus _left_esc_status_type;
       _left_esc_status_type left_esc_status;
       typedef mower_msgs::ESCStatus _right_esc_status_type;
@@ -65,6 +67,7 @@ namespace mower_msgs
       v_charge(0),
       v_battery(0),
       charge_current(0),
+      mow_enabled(0),
       left_esc_status(),
       right_esc_status(),
       mow_esc_status()
@@ -184,6 +187,13 @@ namespace mower_msgs
       *(outbuffer + offset + 2) = (u_charge_current.base >> (8 * 2)) & 0xFF;
       *(outbuffer + offset + 3) = (u_charge_current.base >> (8 * 3)) & 0xFF;
       offset += sizeof(this->charge_current);
+      union {
+        bool real;
+        uint8_t base;
+      } u_mow_enabled;
+      u_mow_enabled.real = this->mow_enabled;
+      *(outbuffer + offset + 0) = (u_mow_enabled.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->mow_enabled);
       offset += this->left_esc_status.serialize(outbuffer + offset);
       offset += this->right_esc_status.serialize(outbuffer + offset);
       offset += this->mow_esc_status.serialize(outbuffer + offset);
@@ -315,14 +325,22 @@ namespace mower_msgs
       u_charge_current.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
       this->charge_current = u_charge_current.real;
       offset += sizeof(this->charge_current);
+      union {
+        bool real;
+        uint8_t base;
+      } u_mow_enabled;
+      u_mow_enabled.base = 0;
+      u_mow_enabled.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->mow_enabled = u_mow_enabled.real;
+      offset += sizeof(this->mow_enabled);
       offset += this->left_esc_status.deserialize(inbuffer + offset);
       offset += this->right_esc_status.deserialize(inbuffer + offset);
       offset += this->mow_esc_status.deserialize(inbuffer + offset);
      return offset;
     }
 
     virtual const char * getType() override { return "mower_msgs/Status"; };
-    virtual const char * getMD5() override { return "33cd1b298baa54308c3c9343754a34fc"; };
+    virtual const char * getMD5() override { return "1e002de26b9c7d2ad33d3b887ae6ad16"; };
 
   };