finished mode switching service

diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h
index 7a3d590ffd33f6578bd2456f096037cdd97988cf..99a30e56fdd51e524bf4f712032e77be1da7bc12 100644 (file)
--- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h
+++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h
@@ -1,12 +1,14 @@
 #ifndef FMW_MESSAGE_H
 #define FMW_MESSAGE_H
 
+struct FMW_PidConstants_Fields {
+  float proportional;
+  float integral;
+  float derivative;
+};
+
 typedef union {
-  struct FMW_PidConstants_Fields {
-    float proportional;
-    float integral;
-    float derivative;
-  };
+  struct FMW_PidConstants_Fields fields;
   float values[3];
 } FMW_PidConstants;
 

diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c
index 58d7a2c3361323c9533b736a06d6b1536a28326d..817722062465e9b28841f802437fb1f6c77f85de 100644 (file)
--- a/pioneer_controller/Core/Src/firmware/fwm_core.c
+++ b/pioneer_controller/Core/Src/firmware/fwm_core.c
@@ -248,13 +248,13 @@ void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear,
 // PID
 int32_t fmw_pid_update(FMW_PidController *pid, float velocity) {
   pid->error = pid->setpoint - velocity;
-  float output = pid->error * pid->ks.proportional;
+  float output = pid->error * pid->ks.fields.proportional;
 
   // integral term without windup
   pid->error_sum += pid->error;
-  output += pid->error_sum * pid->ks.integral;
+  output += pid->error_sum * pid->ks.fields.integral;
 
-  output += (pid->error - pid->previous_error) * pid->ks.derivative;
+  output += (pid->error - pid->previous_error) * pid->ks.fields.derivative;
   pid->previous_error = pid->error;
 
   // anti windup

diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c
index 45104e112a4fdcf6fc6561d408944b29e2ddf23b..32a945078315b0f4684d2e28028ba38f8172e1aa 100644 (file)
--- a/pioneer_controller/Core/Src/main.c
+++ b/pioneer_controller/Core/Src/main.c
@@ -88,7 +88,7 @@ FMW_Odometry odometry = {
 \r
 // TODO(lb): fill with sensible default\r
 FMW_PidController pid_left = {\r
-  .ks = {\r
+  .ks.fields = {\r
     .proportional = 0.1f,\r
     .integral     = 0.1f,\r
     .derivative   = 0.1f,\r
@@ -97,7 +97,7 @@ FMW_PidController pid_left = {
 \r
 // TODO(lb): fill with sensible default\r
 FMW_PidController pid_right = {\r
-  .ks = {\r
+  .ks.fields = {\r
     .proportional = 0.1f,\r
     .integral     = 0.1f,\r
     .derivative   = 0.1f,\r
@@ -106,7 +106,7 @@ FMW_PidController pid_right = {
 \r
 // TODO(lb): fill with sensible default\r
 FMW_PidController pid_cross = {\r
-  .ks = {\r
+  .ks.fields = {\r
     .proportional = 0.1f,\r
     .integral     = 0.1f,\r
     .derivative   = 0.1f,\r
@@ -884,6 +884,7 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) {
       pled.voltage_hysteresis = msg->config_led.voltage_hysteresis;\r
       led_update_period = msg->config_led.update_period;\r
     } break;\r
+    case FMW_MessageType_ModeChange_Config:\r
     case FMW_MessageType_Run_GetStatus:\r
     case FMW_MessageType_Run_SetVelocity: {\r
       result = FMW_Result_Error_Command_NotAvailable;\r

diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp
index bfc315743c4086c25c891a35c2b2438ed1c305ee..ca34bdb0f1f1c993011f2702b0b81365c3c1d097 100644 (file)
--- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp
+++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp
@@ -5,7 +5,6 @@ using namespace std::chrono_literals;
 P3DX_Controller_Node::P3DX_Controller_Node() :
   rclcpp::Node("P3DX_Controller_Node"),
   serial_fd(serial_open("/dev/ttyACM0")),
-  stm32_running(false),
   publisher_odometry(this->create_publisher<nav_msgs::msg::Odometry>("odometry", 10)),
   subscriber_velocity(this->create_subscription<geometry_msgs::msg::Twist>("p3dx_command_velocity", 10,
                                                                            std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity,
@@ -15,30 +14,15 @@ P3DX_Controller_Node::P3DX_Controller_Node() :
   timer_publisher_odometry(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this)))
 {
   assert(this->serial_fd != -1);
-
   this->declare_parameter("frame_id_odometry", std::string("p3dx_controller_odometry"));
-
-  // TODO(lb): force the switch to config mode maybe?
-  //           switching to run mode shouldn't happen now, this is just for testing
-  FMW_Message response, msg_run;
-  msg_run.header.type = FMW_MessageType_ModeChange_Run;
-  msg_run.header.crc = (uint32_t)-1;
-  size_t bytes_written = ::write(this->serial_fd, &msg_run, sizeof(FMW_Message));
-  (void)bytes_written;
-  for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) {
-    ssize_t n = ::read(this->serial_fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read);
-    if (n >= 0) { bytes_read += (uint32_t)n; }
-  }
-  stm32_message_print(&response);
-  assert(response.header.type == FMW_MessageType_Response);
-  assert(response.response.result == FMW_Result_Ok || response.response.result == FMW_Result_Error_Command_NotAvailable);
-  this->stm32_running = true;
+  RCLCPP_INFO(this->get_logger(), "ros2 node ready");
 }
 
 void P3DX_Controller_Node::callback_publish_odometry(void)
 {
   if (!this->stm32_running) { return; }
 
+#if 0
   FMW_Message response, msg_get_status;
   msg_get_status.header.type = FMW_MessageType_Run_GetStatus;
   msg_get_status.header.crc = (uint32_t)-1;
@@ -73,6 +57,7 @@ void P3DX_Controller_Node::callback_publish_odometry(void)
   assert(output.twist.twist.angular.z == 0);
 
   this->publisher_odometry->publish(output);
+#endif
 }
 
 void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd)
@@ -102,7 +87,35 @@ void P3DX_Controller_Node::callback_service_command_change_mode_s(const std::sha
       assert(false && "unreachable");
     } break;
     }
+
+    FMW_Message msg = p3dx_controller->stm32_message_send_change_mode(mode);
+    assert(msg.header.type == FMW_MessageType_Response);
+    response->status_response = (uint8_t)msg.response.result;
+  }
+}
+
+FMW_Message P3DX_Controller_Node::stm32_message_send_change_mode(P3DX_Cmd_ChangeMode_Kind kind)
+{
+  assert(kind > P3DX_Cmd_ChangeMode_Kind::None);
+  assert(kind < P3DX_Cmd_ChangeMode_Kind::COUNT);
+  FMW_Message msg;
+  ::memset(&msg, 0, sizeof(msg));
+  msg.header.type = (kind == P3DX_Cmd_ChangeMode_Kind::Config
+                     ? FMW_MessageType_ModeChange_Config
+                     : FMW_MessageType_ModeChange_Run);
+  msg.header.crc = (uint32_t)-1;
+  this->stm32_message_print(&msg);
+  size_t bytes_written = ::write(this->serial_fd, &msg, sizeof(FMW_Message));
+  RCLCPP_INFO(this->get_logger(), "message sent to stm32");
+  (void)bytes_written;
+  ::memset(&msg, 0, sizeof(msg));
+  for (uint32_t bytes_read = 0; bytes_read < sizeof(msg);) {
+    ssize_t n = ::read(this->serial_fd, ((uint8_t*)&msg) + bytes_read, sizeof(msg) - bytes_read);
+    if (n >= 0) { bytes_read += (uint32_t)n; }
   }
+  assert(msg.header.type == FMW_MessageType_Response);
+  this->stm32_message_print(&msg);
+  return msg;
 }
 
 void P3DX_Controller_Node::stm32_message_print(const FMW_Message *msg)
@@ -144,5 +157,5 @@ void P3DX_Controller_Node::stm32_message_print(const FMW_Message *msg)
     assert(false && "unreachable");
   } break;
   }
-  // RCLCPP_INFO(this->get_logger(), "%s\n}", buffer);
+  RCLCPP_INFO(this->get_logger(), "%s\n}", buffer);
 }

diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp
index 94fb4f4fad06fa4b524c84d7cef34484ce4a6b94..72772bbc5249fe7da4d57832ba59637bd1549d12 100644 (file)
--- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp
+++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp
@@ -26,7 +26,7 @@ enum P3DX_Cmd_ChangeMode_Kind: uint8_t {
 
 struct P3DX_Controller_Node: public rclcpp::Node {
   const int32_t serial_fd;
-  bool stm32_running;
+  bool          stm32_running;
 
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr                 publisher_odometry;
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr            subscriber_velocity;
@@ -38,12 +38,13 @@ struct P3DX_Controller_Node: public rclcpp::Node {
 
   P3DX_Controller_Node(void);
 
-  void callback_publish_odometry(void);
-  void callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd);
-  static void callback_service_command_change_mode_s(const std::shared_ptr<pioneer3dx_controller::srv::ChangeMode::Request> request,
-                                                     std::shared_ptr<pioneer3dx_controller::srv::ChangeMode::Response> response);
+  void          callback_publish_odometry(void);
+  void          callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd);
+  static void   callback_service_command_change_mode_s(const std::shared_ptr<pioneer3dx_controller::srv::ChangeMode::Request> request,
+                                                       std::shared_ptr<pioneer3dx_controller::srv::ChangeMode::Response> response);
 
-  void stm32_message_print(const FMW_Message *msg);
+  FMW_Message   stm32_message_send_change_mode(P3DX_Cmd_ChangeMode_Kind kind);
+  void          stm32_message_print(const FMW_Message *msg);
 };
 
 extern std::shared_ptr<P3DX_Controller_Node> p3dx_controller;

diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h b/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h
index 7a3d590ffd33f6578bd2456f096037cdd97988cf..99a30e56fdd51e524bf4f712032e77be1da7bc12 100644 (file)
--- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h
+++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h
@@ -1,12 +1,14 @@
 #ifndef FMW_MESSAGE_H
 #define FMW_MESSAGE_H
 
+struct FMW_PidConstants_Fields {
+  float proportional;
+  float integral;
+  float derivative;
+};
+
 typedef union {
-  struct FMW_PidConstants_Fields {
-    float proportional;
-    float integral;
-    float derivative;
-  };
+  struct FMW_PidConstants_Fields fields;
   float values[3];
 } FMW_PidConstants;