diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h index 6a5c80d7e35ece69903c9ece767202f7aeadffaa..cd867af3461bb1b7d8e4c6c4222491bc6aeb77dc 100644 --- a/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h +++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h @@ -127,6 +127,21 @@ inline Canbus::CanMessage toCanMessage(const BatteryVoltageSensorData& data) return message; } +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; + + return message; +} + inline PitotData pitotDataFromCanMessage(const Canbus::CanMessage& msg) { PitotData data; @@ -190,7 +205,7 @@ inline ServoData servoDataFromCanMessage(const Canbus::CanMessage& msg) return data; } -inline BatteryVoltageSensorData voltageDataFromCanMessage( +inline BatteryVoltageSensorData batteryVoltageDataFromCanMessage( const Canbus::CanMessage& msg) { BatteryVoltageSensorData data; @@ -203,4 +218,16 @@ inline BatteryVoltageSensorData voltageDataFromCanMessage( return data; } +inline VoltageData voltageDataFromCanMessage(const Canbus::CanMessage& msg) +{ + VoltageData data; + + uint32_t voltage = msg.payload[0]; + memcpy(&(data.voltage), &voltage, sizeof(data.voltage)); + + data.voltageTimestamp = (msg.payload[0] >> 30) & ~0x3; + + return data; +} + } // namespace Boardcore