implemented the cRIO side of relaying DS data (nicely)
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
index da584fb..51dc68a 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
@@ -19,17 +19,27 @@
const double NetworkRobot::kDisableTime = 0.15;
-NetworkRobot::NetworkRobot(UINT16 port, const char *sender_address)
- : port_(port), sender_address_(sender_address), socket_(-1),
+NetworkRobot::NetworkRobot(UINT16 receive_port, const char *sender_address,
+ UINT16 send_port, const char *receiver_address)
+ : receive_port_(receive_port), sender_address_(sender_address),
+ send_port_(send_port), receiver_address_(receiver_address),
+ receive_socket_(-1), send_socket_(-1),
+ joystick_values_(),
+ send_task_("DS_Send", reinterpret_cast<FUNCPTR>(StaticSendLoop)),
last_received_timestamp_(0.0),
digital_modules_(), solenoid_bases_(),
allocated_digital_outputs_() {
}
+
NetworkRobot::~NetworkRobot() {
- if (socket_ != -1) {
- // Nothing we can do about any errors.
- close(socket_);
+ // Nothing we can really do about any errors for either of these.
+ if (receive_socket_ != -1) {
+ close(receive_socket_);
}
+ if (send_socket_ != -1) {
+ close(send_socket_);
+ }
+
for (size_t i = 0;
i < sizeof(solenoid_bases_) / sizeof(solenoid_bases_[0]);
++i) {
@@ -46,22 +56,46 @@
}
}
+bool NetworkRobot::FillinInAddr(const char *const_ip, in_addr *inet_address) {
+ // A copy of the passed in address string because vxworks has the function
+ // signature without the const and I don't really trust it not to do something
+ // weird and change it.
+ // The size is the maximum length of an IP address (including the terminating
+ // NUL) (ie "123.456.789.123").
+ char non_const_ip[3 + 1 + 3 + 1 + 3 + 1 + 3 + 1];
+ size_t ip_length = strlen(const_ip);
+ if (ip_length >= sizeof(non_const_ip)) {
+ char buf[128];
+ snprintf(buf, sizeof(buf), "IP address '%s' is %zd bytes long"
+ " but should only be %zd", const_ip,
+ ip_length, sizeof(non_const_ip) - 1);
+ wpi_setErrorWithContext(-1, buf);
+ return false;
+ }
+ memcpy(non_const_ip, const_ip, ip_length + 1);
+ errno = 0;
+ if (inet_aton(non_const_ip, inet_address) != 0) {
+ char buf[64];
+ snprintf(buf, sizeof(buf), "inet_aton(%s)", const_ip);
+ wpi_setErrnoErrorWithContext(buf);
+ return false;
+ }
+ return true;
+}
+
void NetworkRobot::StartCompetition() {
- // Give ourself plenty of time to get around to feeding it before it
- // completely cuts out.
+ // Multiplied by 2 to give ourselves plenty of time to get around to feeding
+ // it before it completely cuts out everything.
m_watchdog.SetExpiration(kDisableTime * 2);
- CreateSocket();
+ CreateReceiveSocket();
+ if (StatusIsFatal()) return;
+ CreateSendSocket();
if (StatusIsFatal()) return;
- errno = 0;
- if (inet_aton(const_cast<char *>(sender_address_),
- &expected_sender_address_) == ERROR) {
- char buf[64];
- snprintf(buf, sizeof(buf), "Converting IP Address %s", sender_address_);
- wpi_setErrnoErrorWithContext(buf);
- return;
- }
+ send_task_.Start(reinterpret_cast<uintptr_t>(this));
+
+ if (!FillinInAddr(sender_address_, &expected_sender_address_)) return;
while (!StatusIsFatal()) {
if ((Timer::GetPPCTimestamp() - last_received_timestamp_) > kDisableTime) {
@@ -112,9 +146,9 @@
fd_set fds;
FD_ZERO(&fds);
- FD_SET(socket_, &fds);
+ FD_SET(receive_socket_, &fds);
- int ret = select(socket_ + 1,
+ int ret = select(receive_socket_ + 1,
&fds, // read fds
NULL, // write fds
NULL, // exception fds (not supported)
@@ -137,7 +171,7 @@
sockaddr_in in;
} sender_address;
int sender_address_length = sizeof(sender_address);
- int received = recvfrom(socket_,
+ int received = recvfrom(receive_socket_,
reinterpret_cast<char *>(&values_),
sizeof(values_),
0,
@@ -276,13 +310,56 @@
last_received_timestamp_ = Timer::GetPPCTimestamp();
}
-void NetworkRobot::CreateSocket() {
- wpi_assertEqual(socket_, -1);
+void NetworkRobot::SendLoop() {
+ while (!StatusIsFatal()) {
+ m_ds->WaitForData();
- socket_ = socket(AF_INET, SOCK_DGRAM, 0);
- if (socket_ < 0) {
+ {
+ RWLock::Locker data_locker(m_ds->GetDataReadLock());
+ // Get a pointer to the data and then cast away the volatile because it's
+ // annoying to propagate it all over and it's unnecessary here because
+ // we have a lock on the data so it's not going to change.
+ const FRCCommonControlData *data =
+ const_cast<const FRCCommonControlData *>(m_ds->GetControlData());
+ CopyStickValues(0, data->stick0Axes, data->stick0Buttons);
+ CopyStickValues(1, data->stick1Axes, data->stick1Buttons);
+ CopyStickValues(2, data->stick2Axes, data->stick2Buttons);
+ CopyStickValues(3, data->stick3Axes, data->stick3Buttons);
+
+ joystick_values_.control_packet_index = data->packetIndex;
+
+ joystick_values_.team_number = data->teamID;
+ joystick_values_.alliance = data->dsID_Alliance;
+ joystick_values_.position = data->dsID_Position;
+
+ joystick_values_.control.set_test_mode(data->test);
+ joystick_values_.control.set_fms_attached(data->fmsAttached);
+ joystick_values_.control.set_autonomous(data->autonomous);
+ joystick_values_.control.set_enabled(data->enabled);
+ }
+
+ ++joystick_values_.index;
+
+ joystick_values_.FillInHashValue();
+ }
+}
+
+void NetworkRobot::CopyStickValues(int number,
+ const INT8 (&axes)[6],
+ UINT16 buttons) {
+ for (int i = 0; i < 6; ++i) {
+ joystick_values_.joysticks[number].axes[i] = axes[i];
+ }
+ joystick_values_.joysticks[number].buttons = buttons;
+}
+
+void NetworkRobot::CreateReceiveSocket() {
+ wpi_assertEqual(receive_socket_, -1);
+
+ receive_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
+ if (receive_socket_ < 0) {
wpi_setErrnoErrorWithContext("Creating UDP Socket");
- socket_ = -1;
+ receive_socket_ = -1;
return;
}
@@ -292,17 +369,42 @@
} address;
memset(&address, 0, sizeof(address));
address.in.sin_family = AF_INET;
- address.in.sin_port = port_;
+ address.in.sin_port = receive_port_;
address.in.sin_addr.s_addr = INADDR_ANY;
- if (bind(socket_, &address.addr, sizeof(address.addr)) == ERROR) {
+ if (bind(receive_socket_, &address.addr, sizeof(address.addr)) == ERROR) {
wpi_setErrnoErrorWithContext("Binding Socket");
return;
}
int on = 1;
errno = 0;
- if (ioctl(socket_, FIONBIO, reinterpret_cast<int>(&on)) < 0) {
+ if (ioctl(receive_socket_, FIONBIO, reinterpret_cast<int>(&on)) < 0) {
wpi_setErrnoErrorWithContext("Setting Socket to Non-Blocking Mode");
return;
}
}
+
+void NetworkRobot::CreateSendSocket() {
+ wpi_assertEqual(send_socket_, -1);
+
+ send_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
+ if (send_socket_ < 0) {
+ wpi_setErrnoErrorWithContext("Creating UDP Socket");
+ send_socket_ = -1;
+ return;
+ }
+
+ union {
+ sockaddr_in in;
+ sockaddr addr;
+ } address;
+ memset(&address, 0, sizeof(address));
+ address.in.sin_family = AF_INET;
+ address.in.sin_port = send_port_;
+ if (!FillinInAddr(receiver_address_, &address.in.sin_addr)) return;
+
+ if (bind(send_socket_, &address.addr, sizeof(address.addr)) == ERROR) {
+ wpi_setErrnoErrorWithContext("Binding Socket");
+ return;
+ }
+}
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
index 198156c..b17bef2 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
@@ -15,21 +15,29 @@
#include "WPILib/DigitalModule.h"
#include "WPILib/ErrorBase.h"
#include "WPILib/SolenoidBase.h"
+#include "WPILib/Task.h"
-// A simple implementation of receiving motor values over UDP.
+// A simple implementation of receiving motor values over UDP and sending
+// joystick data back out.
// Deals with turning a compressor on and off at the same time.
// You should not try to change any of the outputs on any of the modules that
// you have this class control outside of this class.
// This class takes care of disabling outputs when no packets are received in
// kDisableTime and feeding the Watchdog correctly.
//
-// The interface consists of receiving NetworkRobotValues structs on a given UDP
-// port from a given sender address. Each time a set of motor values is
-// received, this class will update all output values.
+// The receiving interface consists of receiving NetworkRobotValues structs on
+// a given UDP port from a given sender address. Each time a set of motor values
+// is received, this class will update all output values.
+//
+// The sending interface consists of sending NetworkRobotJoysticks structs on a
+// given UDP port to a given sender address. Each time a new Driver's Station
+// packet is received from the FMS code this class will send out another packet
+// with the new values.
class NetworkRobot : public RobotBase, public ErrorBase {
protected:
- // Does not take ownership of *sender_address.
- NetworkRobot(UINT16 port, const char *sender_address);
+ // Does not take ownership of *sender_address or *receiver_address.
+ NetworkRobot(UINT16 receive_port, const char *sender_address,
+ UINT16 send_port, const char *receiver_address);
virtual ~NetworkRobot();
virtual void StartCompetition();
@@ -48,26 +56,55 @@
private:
// How long between packets we wait until we disable all of the outputs (in
// seconds).
- // Must stay less than 1 second.
+ // Must stay less than 1 second with the current implementation.
static const double kDisableTime;
- // Waits for socket_ to become readable for up to kDisableTime.
+ // Deals with calling inet_aton and passing any errors on to the logging
+ // system. A helper is necessary here because inet_aton is normally just kind
+ // of annoying to deal with, but under vxworks, you have to make a copy of the
+ // string before doing anything with etc etc so it's really complicated.
+ // Returns whether or not it was successful. If it returns false, then an
+ // error will have already been recorded.
+ bool FillinInAddr(const char *const_ip, in_addr *inet_address);
+
+ // Waits for receive_socket_ to become readable for up to kDisableTime.
// Returns whether or not there is readable data available.
bool WaitForData();
// Receives a packet and calls ProcessPacket() if it's a good one.
void ReceivePacket();
- // Sets socket_ to an opened socket listening on port to UDP packets from
- // sender_address.
- void CreateSocket();
+ // Gets run in a separate task to take DS data and send it out.
+ void SendLoop();
+ static void StaticSendLoop(void *self) {
+ static_cast<NetworkRobot *>(self)->SendLoop();
+ }
- const UINT16 port_;
+ // Sets receive_socket_ to an opened socket listening on receive_port_ to UDP
+ // packets from sender_address_.
+ void CreateReceiveSocket();
+ // Sets send_socket_ to an opened socket sending UDP packets on send_port_ to
+ // receiver_address_.
+ void CreateSendSocket();
+
+ const UINT16 receive_port_;
const char *const sender_address_;
struct in_addr expected_sender_address_;
+ const UINT16 send_port_;
+ const char *const receiver_address_;
+
+ int receive_socket_;
NetworkRobotValues values_;
- int socket_;
+
+ int send_socket_;
+ NetworkRobotJoysticks joystick_values_;
+ Task send_task_;
+
+ // Helper function to copy all of the data for a single joystick into
+ // joystick_values_.
+ // axes and buttons get copied into joystick_values_.joysticks[number].
+ void CopyStickValues(int number, const INT8 (&axes)[6], UINT16 buttons);
// Using Timer::GetPPCTimestamp() values.
double last_received_timestamp_;
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp
index 8e25983..1d2e805 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp
@@ -31,3 +31,15 @@
sizeof(hash_value),
sizeof(*this) - sizeof(hash_value));
}
+
+void NetworkRobotJoysticks::FillInHashValue() {
+ hash_value = CalculateHashValue(reinterpret_cast<const char *>(this) +
+ sizeof(hash_value),
+ sizeof(*this) - sizeof(hash_value));
+}
+
+bool NetworkRobotJoysticks::CheckHashValue() const {
+ return hash_value == CalculateHashValue(reinterpret_cast<const char *>(this) +
+ sizeof(hash_value),
+ sizeof(*this) - sizeof(hash_value));
+}
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h
index 409be4b..b66ad7c 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h
@@ -11,10 +11,11 @@
// This file needs to not have any dependencies on any other parts of WPILib so
// that it can be #included by other code (like that which sends these values).
-
-// The structure that actually gets sent over the network.
// All multi-byte values are sent over the network in big endian (network)
// byte order.
+
+// The structure that actually gets sent over the network to the cRIO with motor
+// values.
// All channel and module numbers are 1-based like usual.
struct NetworkRobotValues {
// A hash value to make sure that corrupted packets don't get used.
@@ -53,4 +54,75 @@
bool CheckHashValue() const;
} __attribute__((packed));
+// The structure that the cRIO sends out with joystick positions etc.
+struct NetworkRobotJoysticks {
+ // A hash value to make sure that corrupted packets don't get used.
+ // IMPORTANT: Must stay at the beginning.
+ uint32_t hash_value;
+
+ // A structure that stores the information about a joystick and instances for
+ // each of the 4 joysticks.
+ struct Joystick {
+ // Bitmask of the button values.
+ // The LSB is button 1 and only a maximum of 12 are supported.
+ uint16_t buttons;
+ // Raw values for each of the 6 joystick axes.
+ // The range of values depends on the joystick.
+ int8_t axes[6];
+ } __attribute__((packed)) joysticks[4];
+
+ // The index number from the DS.
+ uint16_t control_packet_index;
+ // An index for this structure. Gets incremented by 1 for each one of these
+ // structures that is sent.
+ uint16_t index;
+
+ // The team number that the DS is configured for.
+ uint16_t team_number;
+ // Which alliance the robot is on. Should be 'R' or 'B'.
+ char alliance;
+ // Which position the DS is in on the field. Should be '1', '2', or '3'.
+ char position;
+
+ // A structure that efficiently stores the control data bits from the DS and
+ // has methods for safely and easily getting and setting them and an instance
+ // of it for actually sending the information.
+ // Not just a C bitfield because those are a mess for portability.
+ struct ControlInformation {
+ bool test_mode() { return GetBit(kTestMode); }
+ void set_test_mode(bool value) { SetBit(kTestMode, value); }
+ bool fms_attached() { return GetBit(kFmsAttached); }
+ void set_fms_attached(bool value) { SetBit(kFmsAttached, value); }
+ bool autonomous() { return GetBit(kAutonomous); }
+ void set_autonomous(bool value) { SetBit(kAutonomous, value); }
+ bool enabled() { return GetBit(kEnabled); }
+ void set_enabled(bool value) { SetBit(kEnabled, value); }
+
+ private:
+ // Constants for which bit is which.
+ static const int kTestMode = 0;
+ static const int kFmsAttached = 1;
+ static const int kAutonomous = 2;
+ static const int kEnabled = 3;
+
+ bool GetBit(int bit) {
+ return bits & (1 << bit);
+ }
+ void SetBit(int bit, bool value) {
+ uint8_t mask = 1 << bit;
+ bits &= ~mask;
+ bits |= (mask & (value ? 0xFF : 0x00));
+ }
+
+ uint8_t bits;
+ } __attribute__((packed)) control;
+
+ // Sets hash_value to the correct value for the rest of the data.
+ void FillInHashValue();
+ // Returns whether or not hash_value matches the rest of the data. Any byte
+ // order conversion must be performed ONLY on the hash_value field before
+ // calling this.
+ bool CheckHashValue() const;
+} __attribute__((packed));
+
#endif // WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_VALUES_H_
diff --git a/frc971/crio/dumb_main.cc b/frc971/crio/dumb_main.cc
index ade0af1..4accd9c 100644
--- a/frc971/crio/dumb_main.cc
+++ b/frc971/crio/dumb_main.cc
@@ -8,6 +8,9 @@
public:
MyRobot() : NetworkRobot(static_cast<uint16_t>(::aos::NetworkPort::kMotors),
::aos::configuration::GetIPAddress(
+ ::aos::configuration::NetworkDevice::kAtom),
+ static_cast<uint16_t>(::aos::NetworkPort::kDS),
+ ::aos::configuration::GetIPAddress(
::aos::configuration::NetworkDevice::kAtom)) {}
};