diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h
index 5ba21104f759841ec4d1d5a4eb84fb166e33795b..946a32532fad1dbdc7fcdd32e3619ce96ba1702e 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h
@@ -34,6 +34,20 @@
namespace Boardcore
{
+inline uint32_t floatToInt32(float val)
+{
+ uint32_t val2 = 0;
+ std::memcpy(&val2, &val, sizeof(float));
+ return val2;
+}
+
+inline float int32ToFloat(uint32_t val)
+{
+ float val2 = 0;
+ std::memcpy(&val2, &val, sizeof(uint32_t));
+ return val2;
+}
+
struct CanPitotData : PitotData
{
uint8_t secondaryType = 0;
@@ -162,17 +176,121 @@ struct CanVoltageData : VoltageData
}
};
+struct DeviceStatus
+{
+ uint64_t timestamp = 0;
+ int16_t logNumber = 0;
+ uint8_t state = 0;
+
+ bool armed = false;
+ bool hil = false;
+ bool logGood = false;
+
+ static std::string header()
+ {
+ return "timestamp,state,logNumber,armed,hil,logGood\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << static_cast<int>(state) << "," << logNumber
+ << "," << (armed ? 1 : 0) << "," << (hil ? 1 : 0) << ","
+ << (logGood ? 1 : 0) << "\n";
+ }
+};
+
+struct CanDeviceStatus : DeviceStatus
+{
+ uint8_t secondaryType = 0;
+ uint8_t source = 0;
+
+ static std::string header()
+ {
+ return "timestamp,state,logNumber,armed,hil,logGood,secondaryType,"
+ "source\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << static_cast<int>(state) << "," << logNumber
+ << "," << (armed ? 1 : 0) << "," << (hil ? 1 : 0) << ","
+ << (logGood ? 1 : 0) << static_cast<int>(secondaryType) << ","
+ << static_cast<int>(source) << "\n";
+ }
+};
+
+struct ServoCommand
+{
+ uint64_t timestamp = 0;
+ float aperture = 0;
+ uint16_t openingTime = 0;
+
+ static std::string header() { return "timestamp,aperture,openingTime\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << aperture << "," << openingTime << "\n";
+ }
+};
+
+struct CanServoCommand : ServoCommand
+{
+ uint8_t secondaryType = 0;
+ uint8_t source = 0;
+
+ static std::string header()
+ {
+ return "timestamp,aperture,openingTime,secondaryType,source\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << aperture << "," << openingTime
+ << static_cast<int>(secondaryType) << "," << static_cast<int>(source)
+ << "\n";
+ }
+};
+
+struct ServoFeedback
+{
+ uint64_t timestamp = 0;
+ float aperture = 0;
+ bool open = false;
+
+ static std::string header() { return "timestamp,aperture,open\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << aperture << "," << (open ? 1 : 0) << "\n";
+ }
+};
+
+struct CanServoFeedback : ServoFeedback
+{
+ uint8_t secondaryType = 0;
+ uint8_t source = 0;
+
+ static std::string header()
+ {
+ return "timestamp,aperture,open,secondaryType,source\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << aperture << "," << (open ? 1 : 0)
+ << static_cast<int>(secondaryType) << "," << static_cast<int>(source)
+ << "\n";
+ }
+};
+
inline Canbus::CanMessage toCanMessage(const PitotData& data)
{
Canbus::CanMessage message;
- uint32_t airspeed;
- memcpy(&airspeed, &(data.airspeed), sizeof(airspeed));
-
message.id = -1;
message.length = 1;
message.payload[0] = (data.timestamp & ~0x3) << 30;
- message.payload[0] |= airspeed;
+ message.payload[0] |= floatToInt32(data.airspeed);
return message;
}
@@ -181,13 +299,10 @@ inline Canbus::CanMessage toCanMessage(const PressureData& data)
{
Canbus::CanMessage message;
- uint32_t pressure;
- memcpy(&pressure, &(data.pressure), sizeof(pressure));
-
message.id = -1;
message.length = 1;
message.payload[0] = (data.pressureTimestamp & ~0x3) << 30;
- message.payload[0] |= pressure;
+ message.payload[0] |= floatToInt32(data.pressure);
return message;
}
@@ -195,13 +310,10 @@ inline Canbus::CanMessage toCanMessage(const TemperatureData& data)
{
Canbus::CanMessage message;
- uint32_t temperature;
- memcpy(&temperature, &(data.temperature), sizeof(temperature));
-
message.id = -1;
message.length = 1;
message.payload[0] = (data.temperatureTimestamp & ~0x3) << 30;
- message.payload[0] |= temperature;
+ message.payload[0] |= floatToInt32(data.temperature);
return message;
}
@@ -210,13 +322,10 @@ inline Canbus::CanMessage toCanMessage(const CurrentData& data)
{
Canbus::CanMessage message;
- uint32_t current;
- memcpy(¤t, &(data.current), sizeof(current));
-
message.id = -1;
message.length = 1;
message.payload[0] = (data.currentTimestamp & ~0x3) << 30;
- message.payload[0] |= current;
+ message.payload[0] |= floatToInt32(data.current);
return message;
}
@@ -244,13 +353,10 @@ inline Canbus::CanMessage toCanMessage(const BatteryVoltageSensorData& data)
{
Canbus::CanMessage message;
- uint32_t voltage;
- memcpy(&voltage, &(data.batVoltage), sizeof(voltage));
-
message.id = -1;
message.length = 1;
message.payload[0] = (data.voltageTimestamp & ~0x3) << 30;
- message.payload[0] |= voltage;
+ message.payload[0] |= floatToInt32(data.batVoltage);
return message;
}
@@ -259,13 +365,52 @@ inline Canbus::CanMessage toCanMessage(const VoltageData& data)
{
Canbus::CanMessage message;
- uint32_t voltage;
- memcpy(&voltage, &(data.voltage), sizeof(voltage));
-
message.id = -1;
message.length = 1;
message.payload[0] = (data.voltageTimestamp & ~0x3) << 30;
- message.payload[0] |= voltage;
+ message.payload[0] |= floatToInt32(data.voltage);
+
+ return message;
+}
+
+inline Canbus::CanMessage toCanMessage(const DeviceStatus& data)
+{
+ Canbus::CanMessage message;
+
+ message.id = -1;
+ message.length = 1;
+ message.payload[0] = (data.timestamp & ~0x3) << 30;
+ message.payload[0] |= data.state;
+ message.payload[0] |= static_cast<uint16_t>(data.logNumber) << 8;
+ message.payload[0] |= (data.armed ? 1 : 0) << 24;
+ message.payload[0] |= (data.hil ? 1 : 0) << 25;
+ message.payload[0] |= (data.logGood ? 1 : 0) << 26;
+
+ return message;
+}
+
+inline Canbus::CanMessage toCanMessage(const ServoCommand& data)
+{
+ Canbus::CanMessage message;
+
+ message.id = -1;
+ message.length = 1;
+ message.payload[0] = (data.timestamp & ~0x3) << 30;
+ message.payload[0] |= static_cast<uint16_t>(data.aperture * 65535);
+ message.payload[0] |= data.openingTime << 16;
+
+ return message;
+}
+
+inline Canbus::CanMessage toCanMessage(const ServoFeedback& data)
+{
+ Canbus::CanMessage message;
+
+ message.id = -1;
+ message.length = 1;
+ message.payload[0] = (data.timestamp & ~0x3) << 30;
+ message.payload[0] |= static_cast<uint16_t>(data.aperture * 65535);
+ message.payload[0] |= (data.open ? 1 : 0) << 16;
return message;
}
@@ -274,10 +419,8 @@ inline CanPitotData pitotDataFromCanMessage(const Canbus::CanMessage& msg)
{
CanPitotData data;
- uint32_t airspeed = msg.payload[0];
- memcpy(&(data.airspeed), &airspeed, sizeof(data.airspeed));
-
data.deltaP = 0.0; // put to 0 to avoid undefined behaviour
+ data.airspeed = floatToInt32(msg.payload[0]);
data.timestamp = (msg.payload[0] >> 30) & ~0x3;
data.secondaryType = msg.getSecondaryType();
data.source = msg.getSource();
@@ -289,10 +432,8 @@ inline CanPressureData pressureDataFromCanMessage(const Canbus::CanMessage& msg)
{
CanPressureData data;
- uint32_t pressure = msg.payload[0];
- memcpy(&(data.pressure), &pressure, sizeof(data.pressure));
-
data.pressureTimestamp = (msg.payload[0] >> 30) & ~0x3;
+ data.pressure = int32ToFloat(msg.payload[0]);
data.secondaryType = msg.getSecondaryType();
data.source = msg.getSource();
@@ -304,10 +445,8 @@ inline CanTemperatureData temperatureDataFromCanMessage(
{
CanTemperatureData data;
- uint32_t temperature = msg.payload[0];
- memcpy(&(data.temperature), &temperature, sizeof(data.temperature));
-
data.temperatureTimestamp = (msg.payload[0] >> 30) & ~0x3;
+ data.temperature = int32ToFloat(msg.payload[0]);
data.secondaryType = msg.getSecondaryType();
data.source = msg.getSource();
@@ -318,10 +457,8 @@ inline CanCurrentData currentDataFromCanMessage(const Canbus::CanMessage& msg)
{
CanCurrentData data;
- uint32_t current = msg.payload[0];
- memcpy(&(data.current), ¤t, sizeof(data.current));
-
data.currentTimestamp = (msg.payload[0] >> 30) & ~0x3;
+ data.current = int32ToFloat(msg.payload[0]);
data.secondaryType = msg.getSecondaryType();
data.source = msg.getSource();
@@ -347,10 +484,8 @@ inline CanBatteryVoltageSensorData batteryVoltageDataFromCanMessage(
{
CanBatteryVoltageSensorData data;
- uint32_t voltage = msg.payload[0];
- memcpy(&(data.batVoltage), &voltage, sizeof(data.batVoltage));
-
data.voltageTimestamp = (msg.payload[0] >> 30) & ~0x3;
+ data.batVoltage = int32ToFloat(msg.payload[0]);
data.secondaryType = msg.getSecondaryType();
data.source = msg.getSource();
@@ -361,14 +496,55 @@ inline CanVoltageData voltageDataFromCanMessage(const Canbus::CanMessage& msg)
{
CanVoltageData data;
- uint32_t voltage = msg.payload[0];
- memcpy(&(data.voltage), &voltage, sizeof(data.voltage));
-
data.voltageTimestamp = (msg.payload[0] >> 30) & ~0x3;
+ data.voltage = int32ToFloat(msg.payload[0]);
data.secondaryType = msg.getSecondaryType();
data.source = msg.getSource();
return data;
}
+inline CanDeviceStatus deviceStatusFromCanMessage(const Canbus::CanMessage& msg)
+{
+ CanDeviceStatus data;
+
+ data.timestamp = (msg.payload[0] >> 30) & ~0x3;
+ data.state = static_cast<uint8_t>(msg.payload[0]);
+ data.logNumber = static_cast<int16_t>(msg.payload[0] >> 8);
+ data.armed = ((msg.payload[0] >> 24) & 1) != 0;
+ data.hil = ((msg.payload[0] >> 25) & 1) != 0;
+ data.logGood = ((msg.payload[0] >> 26) & 1) != 0;
+ data.secondaryType = msg.getSecondaryType();
+ data.source = msg.getSource();
+
+ return data;
+}
+
+inline CanServoCommand servoCommandFromCanMessage(const Canbus::CanMessage& msg)
+{
+ CanServoCommand data;
+
+ data.timestamp = (msg.payload[0] >> 30) & ~0x3;
+ data.aperture = static_cast<uint16_t>(msg.payload[0]) / 65535.f;
+ data.openingTime = static_cast<uint16_t>(msg.payload[0] >> 16);
+ data.secondaryType = msg.getSecondaryType();
+ data.source = msg.getSource();
+
+ return data;
+}
+
+inline CanServoFeedback servoFeedbackFromCanMessage(
+ const Canbus::CanMessage& msg)
+{
+ CanServoFeedback data;
+
+ data.timestamp = (msg.payload[0] >> 30) & ~0x3;
+ data.aperture = static_cast<uint16_t>(msg.payload[0]) / 65535.f;
+ data.open = ((msg.payload[0] >> 16) & 1) != 0;
+ data.secondaryType = msg.getSecondaryType();
+ data.source = msg.getSource();
+
+ return data;
+}
+
} // namespace Boardcore
diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h
index d7b770c6ec423c5735aacde7330e266525c48ba7..804c29ff5c3e978ed786dc50adc7c410a4e83a6c 100644
--- a/src/shared/logger/LogTypes.h
+++ b/src/shared/logger/LogTypes.h
@@ -141,6 +141,9 @@ void registerTypes(Deserializer& ds)
ds.registerType<HSCMRNN015PAData>();
ds.registerType<SSCMRNN030PAData>();
ds.registerType<PitotData>();
+ ds.registerType<DeviceStatus>();
+ ds.registerType<ServoCommand>();
+ ds.registerType<ServoFeedback>();
ds.registerType<CanPitotData>();
ds.registerType<CanPressureData>();
ds.registerType<CanTemperatureData>();
@@ -148,6 +151,9 @@ void registerTypes(Deserializer& ds)
ds.registerType<CanServoData>();
ds.registerType<CanBatteryVoltageSensorData>();
ds.registerType<CanVoltageData>();
+ ds.registerType<CanDeviceStatus>();
+ ds.registerType<CanServoCommand>();
+ ds.registerType<CanServoFeedback>();
}
} // namespace LogTypes