diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h index 0a171082e0bca3071a35ec90ffb5d8e2bc2b6354..6a5c80d7e35ece69903c9ece767202f7aeadffaa 100644 --- a/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h +++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h @@ -24,6 +24,7 @@ #include <actuators/Servo/ServoData.h> #include <sensors/SensorData.h> +#include <sensors/analog/BatteryVoltageSensorData.h> #include <sensors/analog/Pitot/PitotData.h> #include <cstring> @@ -111,6 +112,21 @@ inline Canbus::CanMessage toCanMessage(const ServoData& data) return message; } +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; + + return message; +} + inline PitotData pitotDataFromCanMessage(const Canbus::CanMessage& msg) { PitotData data; @@ -174,4 +190,17 @@ inline ServoData servoDataFromCanMessage(const Canbus::CanMessage& msg) return data; } +inline BatteryVoltageSensorData voltageDataFromCanMessage( + const Canbus::CanMessage& msg) +{ + BatteryVoltageSensorData data; + + uint32_t voltage = msg.payload[0]; + memcpy(&(data.batVoltage), &voltage, sizeof(data.batVoltage)); + + data.voltageTimestamp = (msg.payload[0] >> 30) & ~0x3; + + return data; +} + } // namespace Boardcore