From: LeonardoBizzoni Date: Sun, 15 Mar 2026 09:54:06 +0000 (+0100) Subject: finished mode switching service X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=b2362675680d3c4528e4f7e0054e32629e92a0e0;p=pioneer-stm32 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 7a3d590..99a30e5 100644 --- 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 58d7a2c..8177220 100644 --- 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 45104e1..32a9450 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -88,7 +88,7 @@ FMW_Odometry odometry = { // TODO(lb): fill with sensible default FMW_PidController pid_left = { - .ks = { + .ks.fields = { .proportional = 0.1f, .integral = 0.1f, .derivative = 0.1f, @@ -97,7 +97,7 @@ FMW_PidController pid_left = { // TODO(lb): fill with sensible default FMW_PidController pid_right = { - .ks = { + .ks.fields = { .proportional = 0.1f, .integral = 0.1f, .derivative = 0.1f, @@ -106,7 +106,7 @@ FMW_PidController pid_right = { // TODO(lb): fill with sensible default FMW_PidController pid_cross = { - .ks = { + .ks.fields = { .proportional = 0.1f, .integral = 0.1f, .derivative = 0.1f, @@ -884,6 +884,7 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { pled.voltage_hysteresis = msg->config_led.voltage_hysteresis; led_update_period = msg->config_led.update_period; } break; + case FMW_MessageType_ModeChange_Config: case FMW_MessageType_Run_GetStatus: case FMW_MessageType_Run_SetVelocity: { result = FMW_Result_Error_Command_NotAvailable; 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 bfc3157..ca34bdb 100644 --- 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("odometry", 10)), subscriber_velocity(this->create_subscription("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 94fb4f4..72772bb 100644 --- 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::SharedPtr publisher_odometry; rclcpp::Subscription::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 request, - std::shared_ptr 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 request, + std::shared_ptr 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; 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 7a3d590..99a30e5 100644 --- 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;