#define FMW_MESSAGE_H
typedef union {
- struct {
+ struct FMW_PidConstants_Fields {
float proportional;
float integral;
float derivative;
#pragma pack(push, 1)
typedef struct FMW_Message {
- struct {
+ struct FMW_Message_Header {
FMW_MessageType type;
uint32_t crc;
} header;
float angular;
} run_set_velocity;
+ // TODO(lb): add odometry information
struct {
int32_t ticks_left;
int32_t ticks_right;
subscriber_command_velocity(this->create_subscription<geometry_msgs::msg::Twist>("p3dx_command_velocity", 10,
std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity,
this, std::placeholders::_1))),
- publisher_odometry_timer(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this)))
+ publisher_odometry_timer(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this))),
+ stm32_running(false)
{
- this->declare_parameter("frame_id_odometry", std::string("p3dx_controller_odometry"));
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;
+ (void)::write(this->serial_fd, &msg_run, sizeof(FMW_Message));
+ 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;
}
void P3DX_Controller_Node::callback_publish_odometry(void)
{
+ if (!this->stm32_running) { return; }
+
+ FMW_Message response, msg_get_status;
+ msg_get_status.header.type = FMW_MessageType_Run_GetStatus;
+ msg_get_status.header.crc = (uint32_t)-1;
+ (void)::write(this->serial_fd, &msg_get_status, sizeof(FMW_Message));
+ 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; }
+ }
+ assert(response.header.type == FMW_MessageType_Response);
+ stm32_message_print(&response);
+
+
nav_msgs::msg::Odometry output;
output.header.frame_id = this->get_parameter("frame_id_odometry").as_string();
- // output.header.stamp = ; // TODO(lb): generate some kind of current timestamp
+ output.header.stamp = rclcpp::Time(0, 0, this->get_clock()->now().get_clock_type());
// output.pose.pose.position.x = ; // TODO(lb): fill with pioneer data
// output.pose.pose.position.y = ; // TODO(lb): fill with pioneer data
assert(output.twist.twist.angular.z == 0);
this->publisher_odometry->publish(output);
- RCLCPP_INFO(this->get_logger(), "Hello, World!");
}
void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd)
{
(void)cmd;
}
+
+void P3DX_Controller_Node::stm32_message_print(const FMW_Message *msg)
+{
+ static const char *message_types[] = {
+ #define X(Variant, Id) #Variant,
+ FMW_MESSAGE_TYPE_VARIANTS()
+ #undef X
+ };
+
+ static const char *result_types[] = {
+ #define X(Variant, Id) #Variant,
+ FMW_RESULT_VARIANTS()
+ #undef X
+ };
+
+ char buffer[512] = {0};
+ int32_t buffer_len = snprintf(buffer, sizeof(buffer),
+ "FMW_Message {"
+ "\n header.type = %s"
+ "\n header.crc = %u",
+ message_types[msg->header.type], msg->header.crc);
+ assert(buffer_len > 0);
+ assert(buffer_len < sizeof(buffer));
+
+ switch (msg->header.type) {
+ case FMW_MessageType_Response: {
+ buffer_len += snprintf(buffer + buffer_len, sizeof(buffer) - buffer_len,
+ "\n response:"
+ "\n result = %s"
+ "\n delta_millis = %d"
+ "\n ticks_left = %d"
+ "\n ticks_right = %d",
+ result_types[msg->response.result], msg->response.delta_millis,
+ msg->response.ticks_left, msg->response.ticks_right);
+ assert(buffer_len < sizeof(buffer));
+ } break;
+ default: {
+ assert(false && "unreachable");
+ } break;
+ }
+ RCLCPP_INFO(this->get_logger(), "%s\n}", buffer);
+}
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
+extern "C" {
+#include "stm32_messages.h"
+}
+
#include "main.hpp"
struct P3DX_Controller_Node: public rclcpp::Node {
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr publisher_odometry;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscriber_command_velocity;
rclcpp::TimerBase::SharedPtr publisher_odometry_timer;
+ bool stm32_running;
P3DX_Controller_Node(void);
void callback_publish_odometry(void);
void callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd);
+
+ void stm32_message_print(const FMW_Message *msg);
};
+
#endif
--- /dev/null
+#ifndef FMW_MESSAGE_H
+#define FMW_MESSAGE_H
+
+typedef union {
+ struct FMW_PidConstants_Fields {
+ float proportional;
+ float integral;
+ float derivative;
+ };
+ float values[3];
+} FMW_PidConstants;
+
+// NOTE(lb): The expansion of this macro relies on another macro called "X" (hence the name X-macro).
+// X isn't define yet but, for it to work with this macro expansion, it must be a function-like macro
+// with exactly 2 arguemnts: 1) the enumation variant name. 2) the corresponding numerical id.
+#define FMW_MESSAGE_TYPE_VARIANTS() \
+ X(FMW_MessageType_None, 0) \
+ X(FMW_MessageType_Response, 1) \
+ X(FMW_MessageType_ModeChange_Config, 2) \
+ X(FMW_MessageType_ModeChange_Run, 3) \
+ X(FMW_MessageType_Config_Robot, 4) \
+ X(FMW_MessageType_Config_PID, 5) \
+ X(FMW_MessageType_Config_LED, 6) \
+ X(FMW_MessageType_Run_GetStatus, 7) \
+ X(FMW_MessageType_Run_SetVelocity, 8) \
+ X(FMW_MessageType_COUNT, 9)
+
+// NOTE(lb): Here i want to take all of the variants that are generated from the macro
+// expension of FMW_MESSAGE_TYPE_VARIANTS and use them to populate the enum definition.
+// For it to be a valid enum definition you need to take every `X(VariantName, Id)`
+// and turn them into `VariantName = Id,`.
+// It's important to undefine `X` after use to avoid redefinition and, more importantly,
+// to use it in another X-macro expansion by accident.
+// This seems pointless but when paired with `#VariantName` you can automatically
+// turn all of the symbolic variant names into strings and now printing the variant
+// can be done via an array lookup `[VariantName] = #VariantName` instead of a runtime check.
+typedef uint8_t FMW_MessageType;
+enum {
+#define X(Variant, Id) Variant = Id,
+ FMW_MESSAGE_TYPE_VARIANTS()
+#undef X
+};
+
+#define FMW_RESULT_VARIANTS() \
+ X(FMW_Result_Ok, 0) \
+ X(FMW_Result_Error_InvalidArguments, 1) \
+ X(FMW_Result_Error_FaultPinTriggered, 2) \
+ X(FMW_Result_Error_UART_Crc, 3) \
+ X(FMW_Result_Error_UART_NegativeTimeout, 4) \
+ X(FMW_Result_Error_UART_ReceiveTimeoutElapsed, 5) \
+ X(FMW_Result_Error_UART_Parity, 6) \
+ X(FMW_Result_Error_UART_Frame, 7) \
+ X(FMW_Result_Error_UART_Noise, 8) \
+ X(FMW_Result_Error_UART_Overrun, 9) \
+ X(FMW_Result_Error_Encoder_InvalidTimer, 10) \
+ X(FMW_Result_Error_Encoder_NonPositiveTicksPerRevolution, 11) \
+ X(FMW_Result_Error_Encoder_NonPositiveWheelCircumference, 12) \
+ X(FMW_Result_Error_Encoder_GetTick, 13) \
+ X(FMW_Result_Error_Buzzer_Timer, 14) \
+ X(FMW_Result_Error_MessageHandler_InvalidState, 15) \
+ X(FMW_Result_Error_MessageHandler_Config_NonPositiveBaseline, 16) \
+ X(FMW_Result_Error_MessageHandler_Config_NonPositiveWheelCircumference, 17) \
+ X(FMW_Result_Error_MessageHandler_Config_NonPositiveTicksPerRevolution, 18) \
+ X(FMW_Result_Error_MessageHandler_Config_NonPositiveLEDUpdatePeriod, 19) \
+ X(FMW_Result_Error_Command_NotRecognized, 20) \
+ X(FMW_Result_Error_Command_NotAvailable, 21) \
+ X(FMW_Result_COUNT, 22)
+
+typedef uint8_t FMW_Result;
+enum {
+#define X(Variant, Id) Variant = Id,
+ FMW_RESULT_VARIANTS()
+#undef X
+};
+
+#pragma pack(push, 1)
+typedef struct FMW_Message {
+ struct FMW_Message_Header {
+ FMW_MessageType type;
+ uint32_t crc;
+ } header;
+
+ union {
+ struct {
+ float baseline;
+ float wheel_circumference_left;
+ float wheel_circumference_right;
+ uint32_t ticks_per_revolution_left;
+ uint32_t ticks_per_revolution_right;
+ } config_robot;
+ struct {
+ FMW_PidConstants left;
+ FMW_PidConstants right;
+ FMW_PidConstants cross;
+ } config_pid;
+ struct {
+ float voltage_red;
+ float voltage_orange;
+ float voltage_hysteresis;
+ uint32_t update_period;
+ } config_led;
+
+ struct {
+ float linear;
+ float angular;
+ } run_set_velocity;
+
+ // TODO(lb): add odometry information
+ struct {
+ int32_t ticks_left;
+ int32_t ticks_right;
+ uint16_t delta_millis;
+ FMW_Result result;
+ } response;
+ };
+} FMW_Message;
+#pragma pack(pop)
+
+static_assert(sizeof(uint8_t) == 1);
+static_assert(sizeof(uint16_t) == 2);
+static_assert(sizeof(uint32_t) == 4);
+static_assert(sizeof(float) == 4);
+
+#endif /* INC_COMMUNICATION_OTTO_MESSAGES_H_ */
projects / pioneer-stm32 / commitdiff