void Environment::handleRadioTx(bool blue, const Packet::RadioTx& tx) {
for (int i = 0; i < tx.robots_size(); ++i) {
const Packet::RadioTx::Robot& cmd = tx.robots(i);
Robot* r = robot(blue, cmd.robot_id());
if (r) {
// run controls update
r->radioTx(&cmd);
// trigger signals to update visualization
Geometry2d::Point pos2 = r->getPosition();
QVector3D pos3(pos2.x, pos2.y, 0.0);
QVector3D axis(0.0, 0.0, 1.0);
} else {
printf("Commanding nonexistent robot %s:%d\n",
blue ? "Blue" : "Yellow", cmd.robot_id());
}
Packet::RadioRx rx = r->radioRx();
rx.set_robot_id(r->shell);
// Send the RX packet
std::string out;
rx.SerializeToString(&out);
if (blue)
_radioSocketBlue.writeDatagram(&out[0], out.size(), LocalAddress,
RadioRxPort + 1);
else
_radioSocketYellow.writeDatagram(&out[0], out.size(), LocalAddress,
RadioRxPort);
}
// FIXME: the interface changed for this part
// Robot *rev = robot(blue, tx.robot_id());
// if (rev)
// {
// Packet::RadioRx rx = rev->radioRx();
// rx.set_robot_id(tx.robot_id());
//
// // Send the RX packet
// std::string out;
// rx.SerializeToString(&out);
// _radioSocket[ch].writeDatagram(&out[0], out.size(), LocalAddress,
// RadioRxPort + ch);
// }
}
void USBRadio::send(Packet::RadioTx& packet) {
QMutexLocker lock(&_mutex);
if (!_device) {
if (!open()) {
return;
}
}
uint8_t forward_packet[rtp::Forward_Size];
// ensure Forward_Size is correct
static_assert(sizeof(rtp::header_data) + 6 * sizeof(rtp::ControlMessage) ==
rtp::Forward_Size,
"Forward packet contents exceeds buffer size");
// Unit conversions
static const float Seconds_Per_Cycle = 0.005f;
static const float Meters_Per_Tick = 0.026f * 2 * M_PI / 6480.0f;
static const float Radians_Per_Tick = 0.026f * M_PI / (0.0812f * 3240.0f);
rtp::header_data* header = (rtp::header_data*)forward_packet;
header->port = rtp::Port::CONTROL;
header->address = rtp::BROADCAST_ADDRESS;
header->type = rtp::header_data::Type::Control;
// Build a forward packet
for (int slot = 0; slot < 6; ++slot) {
// Calculate the offset into the @forward_packet for this robot's
// control message and cast it to a ControlMessage pointer for easy
// access
size_t offset =
sizeof(rtp::header_data) + slot * sizeof(rtp::ControlMessage);
rtp::ControlMessage* msg =
(rtp::ControlMessage*)(forward_packet + offset);
if (slot < packet.robots_size()) {
const Packet::Control& robot = packet.robots(slot).control();
msg->uid = packet.robots(slot).uid();
msg->bodyX =
robot.xvelocity() * rtp::ControlMessage::VELOCITY_SCALE_FACTOR;
msg->bodyY =
robot.yvelocity() * rtp::ControlMessage::VELOCITY_SCALE_FACTOR;
msg->bodyW =
robot.avelocity() * rtp::ControlMessage::VELOCITY_SCALE_FACTOR;
msg->dribbler =
max(0, min(255, static_cast<uint16_t>(robot.dvelocity()) * 2));
msg->kickStrength = robot.kcstrength();
msg->shootMode = robot.shootmode();
msg->triggerMode = robot.triggermode();
msg->song = robot.song();
} else {
// empty slot
msg->uid = rtp::INVALID_ROBOT_UID;
}
}
// Send the forward packet
int sent = 0;
int transferRetCode =
libusb_bulk_transfer(_device, LIBUSB_ENDPOINT_OUT | 2, forward_packet,
sizeof(forward_packet), &sent, Control_Timeout);
if (transferRetCode != LIBUSB_SUCCESS || sent != sizeof(forward_packet)) {
fprintf(stderr, "USBRadio: Bulk write failed\n");
if (transferRetCode != LIBUSB_SUCCESS)
fprintf(stderr, " Error: '%s'\n",
libusb_error_name(transferRetCode));
libusb_close(_device);
_device = nullptr;
}
}