From d3c098d0531d141f903d26ffd2a168277be0e6cb Mon Sep 17 00:00:00 2001 From: Kyle Stachowicz Date: Mon, 5 Jul 2021 00:08:31 -0400 Subject: [PATCH] New radio protocol using protobuf --- .gitignore | 2 + .gitmodules | 3 + control/CMakeLists.txt | 6 + control/external/CMakeLists.txt | 1 + control/external/nanopb | 1 + control/include/robocup/MicroPackets.hpp | 44 +++- .../motion-control/RobotController.hpp | 1 + .../robocup/modules/MotionControlModule.cpp | 87 ++++---- control/src/robocup/modules/RadioModule.cpp | 28 +-- control/src/robocup/proto/CMakeLists.txt | 6 + .../src/robocup/proto/radio_protocol.proto | 129 ++++++++++++ control/src/robocup/radio/RadioLink.cpp | 191 +++++++++++------- 12 files changed, 364 insertions(+), 135 deletions(-) create mode 160000 control/external/nanopb create mode 100644 control/src/robocup/proto/CMakeLists.txt create mode 100644 control/src/robocup/proto/radio_protocol.proto diff --git a/.gitignore b/.gitignore index ed3a20187..323e754bb 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,8 @@ # build directories /build +cmake-build-* +build-* */build /gmon.out *.swp diff --git a/.gitmodules b/.gitmodules index bc03666a8..76e138c90 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "control/mtrain"] path = control/mtrain url = https://github.com/RoboJackets/mtrain-firmware.git +[submodule "control/external/nanopb"] + path = control/external/nanopb + url = https://github.com/nanopb/nanopb diff --git a/control/CMakeLists.txt b/control/CMakeLists.txt index a500b760b..5af32ceb8 100644 --- a/control/CMakeLists.txt +++ b/control/CMakeLists.txt @@ -6,8 +6,12 @@ project( LANGUAGES ASM C CXX ) +set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/external/nanopb/extra) +find_package(Nanopb REQUIRED) + add_subdirectory(mtrain) add_subdirectory(external) +add_subdirectory(src/robocup/proto) file(GLOB_RECURSE SRC "src/*.cpp") @@ -15,6 +19,8 @@ add_library(control-lib ${SRC} ) +target_link_libraries(control-lib robocup_protos) + ## Shouldnt need explicit inclusion since mtrain dev relies on these target_include_directories(control-lib PUBLIC include diff --git a/control/external/CMakeLists.txt b/control/external/CMakeLists.txt index f69957d95..39cc2a9fb 100644 --- a/control/external/CMakeLists.txt +++ b/control/external/CMakeLists.txt @@ -1 +1,2 @@ add_subdirectory(robocup-fshare) +add_subdirectory(nanopb) diff --git a/control/external/nanopb b/control/external/nanopb new file mode 160000 index 000000000..ba0628f09 --- /dev/null +++ b/control/external/nanopb @@ -0,0 +1 @@ +Subproject commit ba0628f09a6f7faf9127478145a12d34cd5bad86 diff --git a/control/include/robocup/MicroPackets.hpp b/control/include/robocup/MicroPackets.hpp index b6b962cb2..ad007dd95 100644 --- a/control/include/robocup/MicroPackets.hpp +++ b/control/include/robocup/MicroPackets.hpp @@ -2,6 +2,7 @@ #include "cstdint" #include +#include // Micropackets are shared memory locations for values // where only the latest matters. There is no guarantee that @@ -137,8 +138,12 @@ struct KickerCommand { bool isValid = false; /**< Stores whether given data is valid */ uint32_t lastUpdate; /**< Time at which MotionCommand was last updated (milliseconds) */ - enum ShootMode { KICK = 0, CHIP = 1 }; - enum TriggerMode { OFF = 0, IMMEDIATE = 1, ON_BREAK_BEAM = 2, INVALID = 3 }; + enum ShootMode { + KICK = 0, CHIP = 1 + }; + enum TriggerMode { + OFF = 0, IMMEDIATE = 1, ON_BREAK_BEAM = 2, INVALID = 3 + }; ShootMode shootMode; /**< 1-bit encoding of whether the kicker should perform a ground kicker or a chip */ TriggerMode triggerMode; /**< 2-bit encoding of when kicker should kick (none, immediately, on breakbeam, cancel) */ @@ -174,20 +179,37 @@ struct KickerInfo { * Passed from any module to @ref RadioModule */ struct DebugFrame { - uint64_t ticks; + uint32_t frame_time_ms; - float gyro_z; - float accel_x; - float accel_y; + std::array wheel_speeds_radps; + std::array motor_outputs_pwm; - float filtered_velocity[3]; + std::array filtered_pose; + std::array filtered_velocity; - float motor_outputs[4]; + bool has_last_motion_command; + uint32_t last_motion_command_sequence_number; - float encDeltas[4]; // encoder changes since last packet + float gyro_z_radps; + std::array measured_acceleration; +}; + +struct LogMessage { + enum class LogLevel { + LEVEL_FATAL = 0, + LEVEL_ERROR = 1, + LEVEL_WARNING = 2, + LEVEL_INFO = 3, + LEVEL_VERBOSE = 4, + }; + + uint32_t timestamp_ms; + LogLevel level; + std::array subsystem; + std::array message; }; struct DebugInfo { - int num_debug_frames = 0; - std::array debug_frames; + std::vector debug_frames; + std::vector log_messages; }; diff --git a/control/include/robocup/motion-control/RobotController.hpp b/control/include/robocup/motion-control/RobotController.hpp index 0060f0372..0f98d7f64 100644 --- a/control/include/robocup/motion-control/RobotController.hpp +++ b/control/include/robocup/motion-control/RobotController.hpp @@ -58,6 +58,7 @@ class RobotController { * Body velocity control gains (1/s, 1/s, 1/s) */ Eigen::Matrix BodyKp; + float WheelKp = 0.0; /** * Interval of control calculations (seconds) diff --git a/control/src/robocup/modules/MotionControlModule.cpp b/control/src/robocup/modules/MotionControlModule.cpp index 343c6d201..0b87947f2 100644 --- a/control/src/robocup/modules/MotionControlModule.cpp +++ b/control/src/robocup/modules/MotionControlModule.cpp @@ -33,22 +33,24 @@ MotionControlModule::MotionControlModule(LockedStruct& batteryVo } void MotionControlModule::entry() { - auto motionCommandLock = motionCommand.lock(); - auto motorCommandLock = motorCommand.lock(); - auto motorFeedbackLock = motorFeedback.lock(); - auto imuDataLock = imuData.lock(); - auto batteryVoltageLock = batteryVoltage.lock(); - - DebugFrame frame; - frame.ticks = xTaskGetTickCount(); - frame.gyro_z = imuDataLock->omegas[2]; - frame.accel_x = imuDataLock->accelerations[0]; - frame.accel_y = imuDataLock->accelerations[1]; + auto motion_command = motionCommand.lock().value(); + auto motor_feedback = motorFeedback.lock().value(); + auto imu_data = imuData.lock().value(); + auto battery_voltage_data = batteryVoltage.lock().value(); + + MotorCommand motor_command; + + DebugFrame frame{}; + frame.frame_time_ms = xTaskGetTickCount(); + frame.gyro_z_radps = imu_data.omegas[2]; + frame.measured_acceleration[0] = imu_data.accelerations[0]; + frame.measured_acceleration[1] = imu_data.accelerations[1]; + frame.measured_acceleration[2] = imu_data.accelerations[2]; // No radio comm in a little while. Return and die. - if (!motionCommandLock->isValid || !isRecentUpdate(motionCommandLock->lastUpdate)) { - motorCommandLock->isValid = false; - motorCommandLock->lastUpdate = HAL_GetTick(); + if (!motion_command.isValid || !isRecentUpdate(motion_command.lastUpdate)) { + motor_command.isValid = false; + motor_command.lastUpdate = HAL_GetTick(); } // Fill data from shared mem @@ -57,11 +59,11 @@ void MotionControlModule::entry() { measurements << 0, 0, 0, 0, 0; currentWheels << 0, 0, 0, 0; - if (motorFeedbackLock->isValid && isRecentUpdate(motorFeedbackLock->lastUpdate)) { + if (motor_feedback.isValid && isRecentUpdate(motor_feedback.lastUpdate)) { for (int i = 0; i < 4; i++) { if (!isnan(measurements(i,0))) { - measurements(i, 0) = motorFeedbackLock->encoders[i]; - currentWheels(i, 0) = motorFeedbackLock->encoders[i]; + measurements(i, 0) = motor_feedback.encoders[i]; + currentWheels(i, 0) = motor_feedback.encoders[i]; } else { measurements(i, 0) = 0; currentWheels(i, 0) = 0; @@ -69,31 +71,30 @@ void MotionControlModule::entry() { } } - if (imuDataLock->isValid && isRecentUpdate(imuDataLock->lastUpdate)) { - measurements(4, 0) = imuDataLock->omegas[2]; // Z gyro + if (imu_data.isValid && isRecentUpdate(imu_data.lastUpdate)) { + measurements(4, 0) = imu_data.omegas[2]; // Z gyro } // Update targets Eigen::Matrix targetState; targetState << 0, 0, 0; - if (motionCommandLock->isValid && isRecentUpdate(motionCommandLock->lastUpdate)) { - targetState << motionCommandLock->bodyXVel, - motionCommandLock->bodyYVel, - motionCommandLock->bodyWVel; + if (motion_command.isValid && isRecentUpdate(motion_command.lastUpdate)) { + targetState << motion_command.bodyXVel, + motion_command.bodyYVel, + motion_command.bodyWVel; } // Run estimators robotEstimator.predict(prevCommand); - // Only use the feedback if we have good inputs // NAN's most likely came from the divide by dt in the fpga // which was 0, resulting in bad behavior // Leaving this hear until someone can test, then remove // this // - Joe Aug 2019 - if (motorFeedbackLock->isValid && // imuData->isValid && + if (motor_feedback.isValid && // imuData->isValid && !isnan(measurements(0,0)) && !isnan(measurements(1,0)) && !isnan(measurements(2,0)) && @@ -114,7 +115,7 @@ void MotionControlModule::entry() { // Run controllers uint8_t dribblerCommand = 0; - dribblerController.calculate(motionCommandLock->dribbler, dribblerCommand); + dribblerController.calculate(motion_command.dribbler, dribblerCommand); Eigen::Matrix targetWheels; Eigen::Matrix motorCommands; @@ -124,40 +125,44 @@ void MotionControlModule::entry() { prevCommand = motorCommands; - motorCommandLock->isValid = true; - motorCommandLock->lastUpdate = HAL_GetTick(); + motor_command.isValid = true; + motor_command.lastUpdate = HAL_GetTick(); // Good to run motors // todo Check stall and motor errors? - if (batteryVoltageLock->isValid && !batteryVoltageLock->isCritical) { + if (battery_voltage_data.isValid && !battery_voltage_data.isCritical) { // set motors to real targets for (int i = 0; i < 4; i++) { - motorCommandLock->wheels[i] = motorCommands(i, 0); + motor_command.wheels[i] = motorCommands(i, 0); } - motorCommandLock->dribbler = dribblerCommand; + motor_command.dribbler = dribblerCommand; } else { // rip battery // stop for (int i = 0; i < 4; i++) { - motorCommandLock->wheels[i] = 0.0f; + motor_command.wheels[i] = 0.0f; } - motorCommandLock->dribbler = 0; + motor_command.dribbler = 0; } for (int i = 0; i < 4; i++) { - frame.motor_outputs[i] = static_cast(motorCommandLock->wheels[i] * 511); - frame.encDeltas[i] = static_cast(currentWheels(i)); + frame.motor_outputs_pwm[i] = motor_command.wheels[i]; + frame.wheel_speeds_radps[i] = currentWheels(i); + } + + { + auto debugInfoLock = debugInfo.lock(); + if (debugInfoLock->debug_frames.size() < 16) { + debugInfoLock->debug_frames.push_back(frame); + } } -#if 0 - auto debugInfoLock = debugInfo.lock(); - if (debugInfoLock->num_debug_frames < debugInfoLock->debug_frames.size()) { - debugInfoLock->debug_frames[debugInfoLock->num_debug_frames] = frame; - debugInfoLock->num_debug_frames++; + { + auto motorCommandLock = motorCommand.lock(); + motorCommandLock.value() = motor_command; } -#endif } bool MotionControlModule::isRecentUpdate(uint32_t lastUpdateTime) { diff --git a/control/src/robocup/modules/RadioModule.cpp b/control/src/robocup/modules/RadioModule.cpp index a35fdf70b..a67e010c3 100644 --- a/control/src/robocup/modules/RadioModule.cpp +++ b/control/src/robocup/modules/RadioModule.cpp @@ -70,22 +70,22 @@ void RadioModule::entry() { link.send(battery, fpga, kicker, id, debug); } - { - auto motionCommandLock = motionCommand.lock(); - auto radioErrorLock = radioError.lock(); - auto kickerCommandLock = kickerCommand.lock(); + MotionCommand received_motion_command; + KickerCommand received_kicker_command; - // Try read - // Clear buffer of old packets such that we can get the lastest packet - // If you don't do this there is a significant lag of 300ms or more - while (link.receive(kickerCommandLock.value(), motionCommandLock.value())) { - kickerCommandLock->isValid = true; - kickerCommandLock->lastUpdate = HAL_GetTick(); - - motionCommandLock->isValid = true; - motionCommandLock->lastUpdate = HAL_GetTick(); - } + // Try read + // Clear buffer of old packets such that we can get the lastest packet + // If you don't do this there is a significant lag of 300ms or more + while (link.receive(received_kicker_command, received_motion_command)) {} + if (received_motion_command.isValid){ + motionCommand.lock().value() = received_motion_command; + } + if (received_kicker_command.isValid){ + kickerCommand.lock().value() = received_kicker_command; + } + { + auto radioErrorLock = radioError.lock(); radioErrorLock->isValid = true; radioErrorLock->lastUpdate = HAL_GetTick(); radioErrorLock->hasConnectionError = link.isRadioConnected(); diff --git a/control/src/robocup/proto/CMakeLists.txt b/control/src/robocup/proto/CMakeLists.txt new file mode 100644 index 000000000..0f6867493 --- /dev/null +++ b/control/src/robocup/proto/CMakeLists.txt @@ -0,0 +1,6 @@ +nanopb_generate_cpp(PROTO_SRCS PROTO_HDRS radio_protocol.proto) +add_library(robocup_protos ${PROTO_SRCS} ${PROTO_HDRS}) + +target_include_directories(robocup_protos PUBLIC ${CMAKE_CURRENT_BINARY_DIR} ${NANOPB_INCLUDE_DIRS}) +set_source_files_properties(${PROTO_SRCS} ${PROTO_HDRS} + PROPERTIES GENERATED TRUE) diff --git a/control/src/robocup/proto/radio_protocol.proto b/control/src/robocup/proto/radio_protocol.proto new file mode 100644 index 000000000..c431527d8 --- /dev/null +++ b/control/src/robocup/proto/radio_protocol.proto @@ -0,0 +1,129 @@ +syntax = "proto2"; + +import "nanopb.proto"; + +package rj_radio; + +// Throughout this file we use 32-bit millisecond timestamps, which corresponds to a rollover time of about 47 days. All +// timestamps are relative to the robot's bootup time. + +message Twist { + required float x_velocity_mps = 1; + required float y_velocity_mps = 2; + required float w_velocity_radps = 3; +} + +message AccelTwist { + required float x_acceleration_mpss = 1; + required float y_acceleration_mpss = 2; + required float w_acceleration_radpss = 3; +} + +message Pose { + required float x_position_m = 1; + required float y_position_m = 2; + required float heading_rad = 3; +} + +message RobotConfig { + optional bool send_motion_debug_info = 1; + optional bool send_log_messages = 2; +} + +message MotionCommand { + message LocalVelocityCommand { + required Twist robot_relative_velocity = 1; + optional AccelTwist robot_relative_acceleration = 2; + } + + message GlobalVelocityCommand { + required Twist field_relative = 1; + optional AccelTwist field_relative_acceleration = 2; + optional float heading_rad = 3; + } + + oneof motion_command { + LocalVelocityCommand local_velocity = 1; + GlobalVelocityCommand global_velocity = 2; + } +} + +message RobotCommand { + enum KickerTriggerMode { + TRIGGER_NONE = 0; + TRIGGER_IMMEDIATE = 1; + TRIGGER_ON_BREAK_BEAM = 2; + } + + message KickCommand { + required KickerTriggerMode trigger_mode = 2; + required float speed_mps = 3; + } + + message ChipCommand { + required KickerTriggerMode trigger_mode = 2; + required float distance_m = 3; + } + + // A sequence number. Packets sent to the robot must be monotonically increasing in sequence number. Out-of-order + // packets will be discarded and packets with unexpected sequence numbers will be rejected and logged. + required uint32 command_sequence = 1; + required MotionCommand motion_command = 2; + + oneof kicker_command { + KickCommand kick = 3; + ChipCommand chip = 4; + } + + required float dribbler_speed_radps = 5; + + optional RobotConfig config_info = 6; +} + +message MotionDebugInfo { + required uint32 frame_time_ms = 1; + repeated float wheel_speeds_radps = 2 [(nanopb).max_count = 4]; + repeated float motor_outputs_pwm = 3 [(nanopb).max_count = 4]; + + // Relative to the field (if we have a vision lock) + optional Pose filtered_pose = 4; + // Relative to the robot + optional Twist filtered_velocity = 5; + + message LatchedCommand { + required uint32 receive_time_ms = 1; + required MotionCommand command = 2; + } + optional LatchedCommand latest_command = 6; + + optional float gyro_rate_radps = 7; + optional float measured_acceleration_x_mpss = 8; + optional float measured_acceleration_y_mpss = 9; + optional float measured_acceleration_z_mpss = 10; +} + +message LogMessage { + enum LogLevel { + LEVEL_FATAL = 0; + LEVEL_ERROR = 1; + LEVEL_WARNING = 2; + LEVEL_INFO = 3; + LEVEL_VERBOSE = 4; + } + + required LogLevel level = 1; + required uint32 timestamp_ms = 2; + optional string subsystem = 3 [(nanopb).max_size = 16]; + required string message = 4 [(nanopb).max_size = 256]; +} + +message RobotStatus { + required uint32 time_ms = 1; + + optional uint32 last_received_command = 2; + optional uint32 last_kicker_fire = 3; + required bool ball_sense_triggered = 4; + + repeated MotionDebugInfo motion_debug_info = 5 [(nanopb).max_count = 8]; + repeated LogMessage log_messages = 6 [(nanopb).max_count = 16]; +} diff --git a/control/src/robocup/radio/RadioLink.cpp b/control/src/robocup/radio/RadioLink.cpp index 5a9e36a5e..8eed89b93 100644 --- a/control/src/robocup/radio/RadioLink.cpp +++ b/control/src/robocup/radio/RadioLink.cpp @@ -2,6 +2,11 @@ #include "drivers/ISM43340.hpp" #include "MicroPackets.hpp" +#include "radio_protocol.pb.h" +#include +#include +#include +#include RadioLink::RadioLink() {} @@ -14,93 +19,141 @@ void RadioLink::init() { radioInitialized = radio->isInitialized(); } -void RadioLink::send(const BatteryVoltage& batteryVoltage, - const FPGAStatus& fpgaStatus, - const KickerInfo& kickerInfo, - const RobotID& robotID, - DebugInfo& debugInfo) { - std::array packet; - rtp::Header* header = reinterpret_cast(&packet[0]); - rtp::RobotStatusMessage* status = reinterpret_cast(&packet[rtp::HeaderSize]); - - header->address = rtp::BASE_STATION_ADDRESS; - header->port = rtp::PortType::CONTROL; - header->type = rtp::MessageType::CONTROL; - - uint8_t motorErrors = 0; - for (int i = 0; i < 4; i++) - motorErrors |= static_cast(fpgaStatus.motorHasErrors[i]) << i; - - status->uid = robotID.robotID; - status->battVoltage = batteryVoltage.rawVoltage; - status->motorErrors = motorErrors; - status->ballSenseStatus = static_cast(kickerInfo.ballSenseTriggered); - status->kickStatus = static_cast(kickerInfo.kickerCharged); - status->kickHealthy = static_cast(kickerInfo.kickerHasError); - status->fpgaStatus = static_cast(fpgaStatus.FPGAHasError); - -#if 0 - status->num_frames = 0; - - for (int i = 0; i < debugInfo.num_debug_frames && status->num_frames < rtp::MAX_DEBUG_FRAMES; i++) { - auto& to = status->debug_frames[status->num_frames]; - const auto& from = debugInfo.debug_frames[i]; - - to.time_ticks = from.ticks; - to.gyro_z = from.gyro_z; - to.accel_x = from.accel_x; - to.accel_y = from.accel_y; - std::copy(std::begin(from.filtered_velocity), - std::end(from.filtered_velocity), - std::begin(to.filtered_velocity)); - std::copy(std::begin(from.motor_outputs), - std::end(from.motor_outputs), - std::begin(to.motor_outputs)); - std::copy(std::begin(from.encDeltas), - std::end(from.encDeltas), - std::begin(to.encDeltas)); - - status->num_frames++; +// Keep these variables in static memory. We'd rather get link errors if they're too big. +// We only ever use one of these at a time +static union { + rj_radio_RobotStatus robot_status_proto; + rj_radio_RobotCommand robot_command_proto; +}; +// Block off a huge chunk of memory. It's really only this big because of (optional) log messages. +static std::array buffer{}; + +void RadioLink::send(const BatteryVoltage &batteryVoltage, + const FPGAStatus &fpgaStatus, + const KickerInfo &kickerInfo, + const RobotID &robotID, + DebugInfo &debugInfo) { + // Build the proto struct + robot_status_proto = {}; + + robot_status_proto.has_last_received_command = false; + robot_status_proto.has_last_kicker_fire = false; + robot_status_proto.ball_sense_triggered = kickerInfo.ballSenseTriggered; + + // Set up motion debug frames + { + constexpr size_t kMaxNumMotionDebugInfo = + sizeof(robot_status_proto.motion_debug_info) / sizeof(rj_radio_MotionDebugInfo); + robot_status_proto.motion_debug_info_count = std::min(debugInfo.debug_frames.size(), + kMaxNumMotionDebugInfo); + size_t start = std::max(kMaxNumMotionDebugInfo, debugInfo.debug_frames.size()) - kMaxNumMotionDebugInfo; + size_t end = std::min(start + kMaxNumMotionDebugInfo, debugInfo.debug_frames.size()); + for (size_t i = start; i < end; i++) { + const auto &debug_frame = debugInfo.debug_frames.at(i); + auto *debug_frame_proto = &(robot_status_proto.motion_debug_info[i]); + debug_frame_proto->frame_time_ms = debug_frame.frame_time_ms; + debug_frame_proto->wheel_speeds_radps_count = debug_frame.wheel_speeds_radps.size(); + debug_frame_proto->motor_outputs_pwm_count = debug_frame.motor_outputs_pwm.size(); + std::copy(debug_frame.wheel_speeds_radps.begin(), debug_frame.wheel_speeds_radps.end(), + debug_frame_proto->wheel_speeds_radps); + std::copy(debug_frame.motor_outputs_pwm.begin(), debug_frame.motor_outputs_pwm.end(), + debug_frame_proto->motor_outputs_pwm); + + debug_frame_proto->has_filtered_pose = true; + debug_frame_proto->filtered_pose.x_position_m = debug_frame.filtered_pose[0]; + debug_frame_proto->filtered_pose.y_position_m = debug_frame.filtered_pose[1]; + debug_frame_proto->filtered_pose.heading_rad = debug_frame.filtered_pose[2]; + + debug_frame_proto->has_filtered_velocity = true; + debug_frame_proto->filtered_velocity.x_velocity_mps = debug_frame.filtered_velocity[0]; + debug_frame_proto->filtered_velocity.y_velocity_mps = debug_frame.filtered_velocity[1]; + debug_frame_proto->filtered_velocity.w_velocity_radps = debug_frame.filtered_velocity[2]; + + debug_frame_proto->has_latest_command = false; + + debug_frame_proto->gyro_rate_radps = debug_frame.gyro_z_radps; + debug_frame_proto->measured_acceleration_x_mpss = debug_frame.measured_acceleration[0]; + debug_frame_proto->measured_acceleration_y_mpss = debug_frame.measured_acceleration[1]; + debug_frame_proto->measured_acceleration_z_mpss = debug_frame.measured_acceleration[2]; + } } -#endif - radio->send(packet.data(), rtp::ReverseSize); + // Only send the _last_ 16 messages + { + constexpr size_t kMaxNumLogMessages = sizeof(robot_status_proto.log_messages) / sizeof(rj_radio_LogMessage); + robot_status_proto.log_messages_count = std::min(debugInfo.log_messages.size(), kMaxNumLogMessages); + size_t start = std::max(kMaxNumLogMessages, debugInfo.log_messages.size()) - kMaxNumLogMessages; + size_t end = std::min(start + kMaxNumLogMessages, debugInfo.log_messages.size()); + for (size_t i = start; i < end; i++) { + const auto &log_message = debugInfo.log_messages.at(i); + robot_status_proto.log_messages[i].level = static_cast(static_cast(log_message.level)); + robot_status_proto.log_messages[i].timestamp_ms = log_message.timestamp_ms; + strncpy(robot_status_proto.log_messages[i].subsystem, log_message.subsystem.data(), + sizeof(robot_status_proto.log_messages[i].subsystem) / sizeof(char)); + strncpy(robot_status_proto.log_messages[i].message, log_message.message.data(), + sizeof(robot_status_proto.log_messages[i].message) / sizeof(char)); + } + } + + pb_ostream_t buffer_stream = pb_ostream_from_buffer(buffer.data(), buffer.size()); + + if (!pb_encode(&buffer_stream, rj_radio_RobotCommand_fields, &robot_status_proto)) { + // Failed to encode + // TODO: warn + return; + } + + radio->send(buffer.data(), buffer_stream.bytes_written); } -bool RadioLink::receive(KickerCommand& kickerCommand, - MotionCommand& motionCommand) { +bool RadioLink::receive(KickerCommand &kickerCommand, + MotionCommand &motionCommand) { // Make sure there is actually data to read if (!radio->isAvailable()) { cyclesWithoutPackets++; return false; } - std::array packet; + size_t num_bytes = radio->receive(buffer.data(), buffer.size()); + pb_istream_t buffer_stream = pb_istream_from_buffer(buffer.data(), num_bytes); + + robot_command_proto = {}; - if (radio->receive(packet.data(), rtp::ForwardSize) != rtp::ForwardSize) { - // didn't get enough bytes + if (!pb_decode(&buffer_stream, rj_radio_RobotCommand_fields, &robot_command_proto)) { + // TODO: Log an error cyclesWithoutPackets++; return false; } - // In the udp radio, only 1 robot's data is sent at a time - // Dangerous: Should probably check size to make sure we got a valid packet - rtp::RobotTxMessage* tx = reinterpret_cast(&packet[rtp::HeaderSize]); - rtp::ControlMessage* control = reinterpret_cast(&tx->message); - kickerCommand.isValid = true; kickerCommand.lastUpdate = HAL_GetTick(); // All possible values must be defined in the enum otherwise this is undefined behavior - kickerCommand.shootMode = static_cast(control->shootMode); - kickerCommand.triggerMode = static_cast(control->triggerMode); - kickerCommand.kickStrength = control->kickStrength; - - motionCommand.isValid = true; - motionCommand.lastUpdate = HAL_GetTick(); - motionCommand.bodyXVel = static_cast(control->bodyX) / rtp::ControlMessage::VELOCITY_SCALE_FACTOR; - motionCommand.bodyYVel = static_cast(control->bodyY) / rtp::ControlMessage::VELOCITY_SCALE_FACTOR; - motionCommand.bodyWVel = static_cast(control->bodyW) / rtp::ControlMessage::VELOCITY_SCALE_FACTOR; - motionCommand.dribbler = control->dribbler; + if (robot_command_proto.which_kicker_command == rj_radio_RobotCommand_kick_tag) { + kickerCommand.kickStrength = 128; + kickerCommand.shootMode = KickerCommand::ShootMode::KICK; + kickerCommand.triggerMode = kickerCommand.triggerMode; + } else if (robot_command_proto.which_kicker_command == rj_radio_RobotCommand_chip_tag) { + kickerCommand.kickStrength = 128; + kickerCommand.shootMode = KickerCommand::ShootMode::KICK; + kickerCommand.triggerMode = kickerCommand.triggerMode; + } + + if (robot_command_proto.motion_command.which_motion_command == rj_radio_MotionCommand_local_velocity_tag) { + const auto &velocity = robot_command_proto.motion_command.motion_command.local_velocity.robot_relative_velocity; + motionCommand.bodyWVel = velocity.w_velocity_radps; + motionCommand.bodyXVel = velocity.x_velocity_mps; + motionCommand.bodyYVel = velocity.y_velocity_mps; + + motionCommand.isValid = true; + motionCommand.lastUpdate = HAL_GetTick(); + } else if (robot_command_proto.motion_command.which_motion_command == rj_radio_MotionCommand_global_velocity_tag) { + // Error! + return false; + } + + if (robot_command_proto.dribbler_speed_radps > 0) { + motionCommand.dribbler = 128; + } cyclesWithoutPackets = 0; return true;