mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
halsim_ds_socket: Update tag parsing, and add rumble support (#1214)
Outputs are now sent. Ensure only the proper number of outputs are actually sent though. Also adds match time, and proper enable tags.
This commit is contained in:
Thad House
committed by
Peter Johnson
Peter Johnson
parent
d54c2665dc
commit
63c1f80d60
@@ -6,15 +6,26 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
#include <vector>
|
||||
|
||||
#include <cstring>
|
||||
|
||||
#include <hal/DriverStation.h>
|
||||
|
||||
namespace halsim {
|
||||
|
||||
typedef struct {
|
||||
std::vector<int16_t> axes;
|
||||
uint8_t button_count;
|
||||
uint32_t buttons;
|
||||
std::vector<int16_t> povs;
|
||||
HAL_JoystickAxes axes;
|
||||
HAL_JoystickButtons buttons;
|
||||
HAL_JoystickPOVs povs;
|
||||
HAL_JoystickDescriptor descriptor;
|
||||
|
||||
void ResetUdp() {
|
||||
std::memset(&axes, 0, sizeof(axes));
|
||||
std::memset(&buttons, 0, sizeof(buttons));
|
||||
std::memset(&povs, 0, sizeof(povs));
|
||||
}
|
||||
|
||||
void ResetTcp() { std::memset(&descriptor, 0, sizeof(descriptor)); }
|
||||
} DSCommJoystickPacket;
|
||||
|
||||
} // namespace halsim
|
||||
|
||||
@@ -7,46 +7,35 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include <array>
|
||||
|
||||
#include <DSCommJoystickPacket.h>
|
||||
#include <FRCComm.h>
|
||||
#include <mockdata/DriverStationData.h>
|
||||
#include <wpi/uv/Buffer.h>
|
||||
#include <wpi/ArrayRef.h>
|
||||
#include <wpi/raw_uv_ostream.h>
|
||||
|
||||
class DSCommPacketTest;
|
||||
|
||||
namespace halsim {
|
||||
|
||||
class DSCommPacket {
|
||||
public:
|
||||
DSCommPacket(void) {
|
||||
std::fill_n(m_joystick_types, kMaxJoysticks, -1);
|
||||
sendDataBuffer = wpi::uv::Buffer::Allocate(8);
|
||||
}
|
||||
void Lock() { m_mutex.lock(); }
|
||||
void Unlock() { m_mutex.unlock(); }
|
||||
void SetIndex(uint8_t hi, uint8_t lo);
|
||||
void GetIndex(uint8_t& hi, uint8_t& lo);
|
||||
void SetControl(uint8_t control, uint8_t request);
|
||||
void GetControl(uint8_t& control);
|
||||
void GetStatus(uint8_t& status);
|
||||
void SetAlliance(uint8_t station_code);
|
||||
int AddDSCommJoystickPacket(uint8_t* data, int len);
|
||||
void GetControlWord(struct ControlWord_t& control_word);
|
||||
void GetAllianceStation(enum AllianceStationID_t& allianceStation);
|
||||
int DecodeTCP(uint8_t* packet, int len);
|
||||
void DecodeUDP(uint8_t* packet, int len);
|
||||
void SendTCPToHALSim(void);
|
||||
void SendUDPToHALSim(void);
|
||||
void SendJoysticks(void);
|
||||
wpi::uv::Buffer& GetSendBuffer(void) { return sendDataBuffer; }
|
||||
friend class ::DSCommPacketTest;
|
||||
|
||||
/* TCP (FMS) types */
|
||||
static const uint8_t kGameDataType = 0x0e;
|
||||
static const uint8_t kJoystickNameType = 0x02;
|
||||
public:
|
||||
DSCommPacket(void);
|
||||
void DecodeTCP(wpi::ArrayRef<uint8_t> packet);
|
||||
void DecodeUDP(wpi::ArrayRef<uint8_t> packet);
|
||||
void SendUDPToHALSim(void);
|
||||
void SetupSendBuffer(wpi::raw_uv_ostream& buf);
|
||||
|
||||
/* TCP Tags */
|
||||
static const uint8_t kGameDataTag = 0x0e;
|
||||
static const uint8_t kJoystickNameTag = 0x02;
|
||||
static const uint8_t kMatchInfoTag = 0x07;
|
||||
|
||||
/* UDP Tags*/
|
||||
static const uint8_t kJoystickDataTag = 0x0c;
|
||||
static const uint8_t kMatchTimeTag = 0x07;
|
||||
|
||||
/* Control word bits */
|
||||
static const uint8_t kTest = 0x01;
|
||||
@@ -61,29 +50,26 @@ class DSCommPacket {
|
||||
/* Status bits */
|
||||
static const uint8_t kRobotHasCode = 0x20;
|
||||
|
||||
/* Joystick tag bits */
|
||||
static const uint8_t kTagDsCommJoystick = 0x0c;
|
||||
|
||||
/* Joystick max count */
|
||||
/* TODO(jwhite@codeweavers.com) This is a magic number in the HAL; fix it
|
||||
* there */
|
||||
static const uint8_t kMaxJoysticks = 6;
|
||||
|
||||
private:
|
||||
uint8_t m_game_data[64];
|
||||
void SendJoysticks(void);
|
||||
void SetControl(uint8_t control, uint8_t request);
|
||||
void SetAlliance(uint8_t station_code);
|
||||
void SetupSendHeader(wpi::raw_uv_ostream& buf);
|
||||
void SetupJoystickTag(wpi::raw_uv_ostream& buf);
|
||||
void ReadMatchtimeTag(wpi::ArrayRef<uint8_t> tagData);
|
||||
void ReadJoystickTag(wpi::ArrayRef<uint8_t> data, int index);
|
||||
void ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data);
|
||||
void ReadGameSpecificMessageTag(wpi::ArrayRef<uint8_t> data);
|
||||
void ReadJoystickDescriptionTag(wpi::ArrayRef<uint8_t> data);
|
||||
|
||||
uint8_t m_hi;
|
||||
uint8_t m_lo;
|
||||
uint8_t m_control_sent;
|
||||
struct ControlWord_t m_control_word;
|
||||
enum AllianceStationID_t m_alliance_station;
|
||||
std::vector<DSCommJoystickPacket> m_joystick_packets;
|
||||
std::string m_joystick_names[kMaxJoysticks];
|
||||
int m_joystick_types[kMaxJoysticks];
|
||||
std::mutex m_mutex;
|
||||
int m_udp_packets;
|
||||
std::chrono::high_resolution_clock::time_point m_packet_time;
|
||||
HAL_ControlWord m_control_word;
|
||||
HAL_AllianceStationID m_alliance_station;
|
||||
HAL_MatchInfo matchInfo;
|
||||
std::array<DSCommJoystickPacket, HAL_kMaxJoysticks> m_joystick_packets;
|
||||
double m_match_time;
|
||||
wpi::uv::Buffer sendDataBuffer;
|
||||
};
|
||||
|
||||
} // namespace halsim
|
||||
|
||||
@@ -1,68 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
/*----------------------------------------------------------------------------
|
||||
* The defines and enums in this file were copied from:
|
||||
* ni-libraries/include/FRC_NetworkCommunication/FRCComm.h
|
||||
* to avoid the complexities of trying to get gradle to
|
||||
* reliably build against it.
|
||||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#ifndef WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_
|
||||
#define WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define ERR_FRCSystem_NetCommNotResponding -44049
|
||||
#define ERR_FRCSystem_NoDSConnection -44018
|
||||
|
||||
enum AllianceStationID_t {
|
||||
kAllianceStationID_red1,
|
||||
kAllianceStationID_red2,
|
||||
kAllianceStationID_red3,
|
||||
kAllianceStationID_blue1,
|
||||
kAllianceStationID_blue2,
|
||||
kAllianceStationID_blue3,
|
||||
};
|
||||
|
||||
enum MatchType_t {
|
||||
kMatchType_none,
|
||||
kMatchType_practice,
|
||||
kMatchType_qualification,
|
||||
kMatchType_elimination,
|
||||
};
|
||||
|
||||
struct ControlWord_t {
|
||||
#ifndef __vxworks
|
||||
uint32_t enabled : 1;
|
||||
uint32_t autonomous : 1;
|
||||
uint32_t test : 1;
|
||||
uint32_t eStop : 1;
|
||||
uint32_t fmsAttached : 1;
|
||||
uint32_t dsAttached : 1;
|
||||
uint32_t control_reserved : 26;
|
||||
#else
|
||||
uint32_t control_reserved : 26;
|
||||
uint32_t dsAttached : 1;
|
||||
uint32_t fmsAttached : 1;
|
||||
uint32_t eStop : 1;
|
||||
uint32_t test : 1;
|
||||
uint32_t autonomous : 1;
|
||||
uint32_t enabled : 1;
|
||||
#endif
|
||||
};
|
||||
|
||||
struct JoystickAxes_t {
|
||||
uint16_t count;
|
||||
int16_t axes[1];
|
||||
};
|
||||
|
||||
struct JoystickPOV_t {
|
||||
uint16_t count;
|
||||
int16_t povs[1];
|
||||
};
|
||||
#endif // WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_
|
||||
Reference in New Issue
Block a user