mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
Add halsim_ds_socket to allow a simulated robot to talk to the real DS (#1180)
This implements enough of the UDP and TCP protocol used by the FRC driver station to allow us to talk to either QDriverStation or to the real Driver Station. This was inspired by a similar function in Toast by Jaci, and also uses a lot of the research found in the QDriverStation project.
This commit is contained in:
Jeremy White
committed by
Peter Johnson
Peter Johnson
parent
5bf5821138
commit
74a306d47a
@@ -0,0 +1,16 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
#include <vector>
|
||||
|
||||
typedef struct {
|
||||
std::vector<int16_t> axes;
|
||||
uint8_t button_count;
|
||||
uint32_t buttons;
|
||||
std::vector<int16_t> povs;
|
||||
} DSCommJoystickPacket;
|
||||
@@ -0,0 +1,79 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <DSCommJoystickPacket.h>
|
||||
#include <FRCComm.h>
|
||||
#include <MockData/DriverStationData.h>
|
||||
|
||||
class DSCommPacket {
|
||||
public:
|
||||
DSCommPacket(void) { std::fill_n(m_joystick_types, kMaxJoysticks, -1); }
|
||||
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);
|
||||
|
||||
/* TCP (FMS) types */
|
||||
static const uint8_t kGameDataType = 0x0e;
|
||||
static const uint8_t kJoystickNameType = 0x02;
|
||||
|
||||
/* Control word bits */
|
||||
static const uint8_t kTest = 0x01;
|
||||
static const uint8_t kEnabled = 0x04;
|
||||
static const uint8_t kAutonomous = 0x02;
|
||||
static const uint8_t kFMS_Attached = 0x08;
|
||||
static const uint8_t kEmergencyStop = 0x80;
|
||||
|
||||
/* Control request bitmask */
|
||||
static const uint8_t kRequestNormalMask = 0xF0;
|
||||
|
||||
/* 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];
|
||||
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;
|
||||
double m_match_time;
|
||||
};
|
||||
@@ -0,0 +1,74 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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_
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <windows.h>
|
||||
#elif defined(__vxworks)
|
||||
#include <vxWorks.h>
|
||||
#elif defined(__linux)
|
||||
#include <stdint.h>
|
||||
#endif
|
||||
|
||||
#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