First implementation
This commit is contained in:
Yury Shuvakin
350
CanDriver/core/CanMessage.cpp
Normal file
350
CanDriver/core/CanMessage.cpp
Normal file
@@ -0,0 +1,350 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2015, 2016 Hubert Denkmair <hubert@denkmair.de>
|
||||
|
||||
This file is part of cangaroo.
|
||||
|
||||
cangaroo is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 2 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
cangaroo is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with cangaroo. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "CanMessage.h"
|
||||
#include <core/portable_endian.h>
|
||||
|
||||
enum {
|
||||
id_flag_extended = 0x80000000,
|
||||
id_flag_rtr = 0x40000000,
|
||||
id_flag_error = 0x20000000,
|
||||
id_mask_extended = 0x1FFFFFFF,
|
||||
id_mask_standard = 0x7FF
|
||||
};
|
||||
|
||||
CanMessage::CanMessage()
|
||||
: _raw_id(0), _dlc(8), _isFD(false), _interface(0), _u8()
|
||||
{
|
||||
_timestamp.tv_sec = 0;
|
||||
_timestamp.tv_usec = 0;
|
||||
}
|
||||
|
||||
CanMessage::CanMessage(uint32_t can_id)
|
||||
: _dlc(8), _isFD(false), _interface(0), _u8()
|
||||
{
|
||||
_timestamp.tv_sec = 0;
|
||||
_timestamp.tv_usec = 0;
|
||||
setId(can_id);
|
||||
}
|
||||
|
||||
CanMessage::CanMessage(const CanMessage &msg)
|
||||
{
|
||||
cloneFrom(msg);
|
||||
}
|
||||
|
||||
void CanMessage::cloneFrom(const CanMessage &msg)
|
||||
{
|
||||
_raw_id = msg._raw_id;
|
||||
_dlc = msg._dlc;
|
||||
|
||||
// Copy data
|
||||
for(int i=0; i<64; i++)
|
||||
{
|
||||
_u8[i] = msg._u8[i];
|
||||
}
|
||||
|
||||
_interface = msg._interface;
|
||||
_timestamp = msg._timestamp;
|
||||
}
|
||||
|
||||
|
||||
uint32_t CanMessage::getRawId() const {
|
||||
return _raw_id;
|
||||
}
|
||||
|
||||
void CanMessage::setRawId(const uint32_t raw_id) {
|
||||
_raw_id = raw_id;
|
||||
}
|
||||
|
||||
uint32_t CanMessage::getId() const {
|
||||
if (isExtended()) {
|
||||
return _raw_id & id_mask_extended;
|
||||
} else {
|
||||
return _raw_id & id_mask_standard;
|
||||
}
|
||||
}
|
||||
|
||||
void CanMessage::setId(const uint32_t id) {
|
||||
_raw_id &= ~ id_mask_extended;
|
||||
_raw_id = id;
|
||||
if (id>0x7FF) {
|
||||
setExtended(true);
|
||||
}
|
||||
}
|
||||
|
||||
bool CanMessage::isExtended() const {
|
||||
return (_raw_id & id_flag_extended) != 0;
|
||||
}
|
||||
|
||||
void CanMessage::setExtended(const bool isExtended) {
|
||||
if (isExtended) {
|
||||
_raw_id |= id_flag_extended;
|
||||
} else {
|
||||
_raw_id &= ~id_flag_extended;
|
||||
}
|
||||
}
|
||||
|
||||
bool CanMessage::isRTR() const {
|
||||
return (_raw_id & id_flag_rtr) != 0;
|
||||
}
|
||||
|
||||
void CanMessage::setRTR(const bool isRTR) {
|
||||
if (isRTR) {
|
||||
_raw_id |= id_flag_rtr;
|
||||
} else {
|
||||
_raw_id &= ~id_flag_rtr;
|
||||
}
|
||||
}
|
||||
|
||||
bool CanMessage::isFD() const {
|
||||
return _isFD;
|
||||
}
|
||||
|
||||
void CanMessage::setFD(const bool isFD) {
|
||||
_isFD = isFD;
|
||||
}
|
||||
|
||||
bool CanMessage::isBRS() const {
|
||||
return _isBRS;
|
||||
}
|
||||
|
||||
void CanMessage::setBRS(const bool isBRS) {
|
||||
_isBRS = isBRS;
|
||||
}
|
||||
|
||||
bool CanMessage::isErrorFrame() const {
|
||||
return (_raw_id & id_flag_error) != 0;
|
||||
}
|
||||
|
||||
void CanMessage::setErrorFrame(const bool isErrorFrame) {
|
||||
if (isErrorFrame) {
|
||||
_raw_id |= id_flag_error;
|
||||
} else {
|
||||
_raw_id &= ~id_flag_error;
|
||||
}
|
||||
}
|
||||
|
||||
CanInterfaceId CanMessage::getInterfaceId() const
|
||||
{
|
||||
return _interface;
|
||||
}
|
||||
|
||||
void CanMessage::setInterfaceId(CanInterfaceId interface)
|
||||
{
|
||||
_interface = interface;
|
||||
}
|
||||
|
||||
uint8_t CanMessage::getLength() const {
|
||||
return _dlc;
|
||||
}
|
||||
|
||||
void CanMessage::setLength(const uint8_t dlc) {
|
||||
// Limit to CANFD max length
|
||||
if (dlc<=64) {
|
||||
_dlc = dlc;
|
||||
} else {
|
||||
_dlc = 8;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t CanMessage::getByte(const uint8_t index) const {
|
||||
if (index<sizeof(_u8)) {
|
||||
return _u8[index];
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void CanMessage::setByte(const uint8_t index, const uint8_t value) {
|
||||
if (index<sizeof(_u8)) {
|
||||
_u8[index] = value;
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t CanMessage::extractRawSignal(uint8_t start_bit, const uint8_t length, const bool isBigEndian) const
|
||||
{
|
||||
// if ((start_bit+length) > (getLength()*8)) {
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// FIXME: This only gives access to data bytes 0-8. Need to rework for CANFD.
|
||||
uint64_t data = le64toh(_u64[0]);
|
||||
|
||||
data >>= start_bit;
|
||||
|
||||
uint64_t mask = 0xFFFFFFFFFFFFFFFF;
|
||||
mask <<= length;
|
||||
mask = ~mask;
|
||||
|
||||
data &= mask;
|
||||
|
||||
// If the length is greater than 8, we need to byteswap to preserve endianness
|
||||
if (isBigEndian && (length > 8))
|
||||
{
|
||||
|
||||
// Swap bytes
|
||||
data = __builtin_bswap64(data);
|
||||
|
||||
// Shift out unused bits
|
||||
data >>= 64 - length;
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
void CanMessage::setDataAt(uint8_t position, uint8_t data)
|
||||
{
|
||||
if(position < 64)
|
||||
_u8[position] = data;
|
||||
else
|
||||
return;
|
||||
}
|
||||
|
||||
void CanMessage::setData(const uint8_t d0) {
|
||||
_dlc = 1;
|
||||
_u8[0] = d0;
|
||||
}
|
||||
|
||||
void CanMessage::setData(const uint8_t d0, const uint8_t d1) {
|
||||
_dlc = 2;
|
||||
_u8[0] = d0;
|
||||
_u8[1] = d1;
|
||||
}
|
||||
|
||||
void CanMessage::setData(const uint8_t d0, const uint8_t d1, const uint8_t d2) {
|
||||
_dlc = 3;
|
||||
_u8[0] = d0;
|
||||
_u8[1] = d1;
|
||||
_u8[2] = d2;
|
||||
}
|
||||
|
||||
void CanMessage::setData(const uint8_t d0, const uint8_t d1, const uint8_t d2,
|
||||
const uint8_t d3) {
|
||||
_dlc = 4;
|
||||
_u8[0] = d0;
|
||||
_u8[1] = d1;
|
||||
_u8[2] = d2;
|
||||
_u8[3] = d3;
|
||||
}
|
||||
|
||||
void CanMessage::setData(const uint8_t d0, const uint8_t d1, const uint8_t d2,
|
||||
const uint8_t d3, const uint8_t d4) {
|
||||
_dlc = 5;
|
||||
_u8[0] = d0;
|
||||
_u8[1] = d1;
|
||||
_u8[2] = d2;
|
||||
_u8[3] = d3;
|
||||
_u8[4] = d4;
|
||||
}
|
||||
|
||||
void CanMessage::setData(const uint8_t d0, const uint8_t d1, const uint8_t d2,
|
||||
const uint8_t d3, const uint8_t d4, const uint8_t d5) {
|
||||
_dlc = 6;
|
||||
_u8[0] = d0;
|
||||
_u8[1] = d1;
|
||||
_u8[2] = d2;
|
||||
_u8[3] = d3;
|
||||
_u8[4] = d4;
|
||||
_u8[5] = d5;
|
||||
}
|
||||
|
||||
void CanMessage::setData(const uint8_t d0, const uint8_t d1, const uint8_t d2,
|
||||
const uint8_t d3, const uint8_t d4, const uint8_t d5,
|
||||
const uint8_t d6) {
|
||||
_dlc = 7;
|
||||
_u8[0] = d0;
|
||||
_u8[1] = d1;
|
||||
_u8[2] = d2;
|
||||
_u8[3] = d3;
|
||||
_u8[4] = d4;
|
||||
_u8[5] = d5;
|
||||
_u8[6] = d6;
|
||||
}
|
||||
|
||||
void CanMessage::setData(const uint8_t d0, const uint8_t d1, const uint8_t d2,
|
||||
const uint8_t d3, const uint8_t d4, const uint8_t d5, const uint8_t d6,
|
||||
const uint8_t d7) {
|
||||
_dlc = 8;
|
||||
_u8[0] = d0;
|
||||
_u8[1] = d1;
|
||||
_u8[2] = d2;
|
||||
_u8[3] = d3;
|
||||
_u8[4] = d4;
|
||||
_u8[5] = d5;
|
||||
_u8[6] = d6;
|
||||
_u8[7] = d7;
|
||||
}
|
||||
|
||||
const QByteArray CanMessage::getData() const
|
||||
{
|
||||
return QByteArray(reinterpret_cast<const char *>(_u8), _dlc);
|
||||
}
|
||||
|
||||
timeval CanMessage::getTimestamp() const
|
||||
{
|
||||
return _timestamp;
|
||||
}
|
||||
|
||||
void CanMessage::setTimestamp(const timeval timestamp)
|
||||
{
|
||||
_timestamp = timestamp;
|
||||
}
|
||||
|
||||
void CanMessage::setTimestamp(const uint64_t seconds, const uint32_t micro_seconds)
|
||||
{
|
||||
_timestamp.tv_sec = seconds;
|
||||
_timestamp.tv_usec = micro_seconds;
|
||||
}
|
||||
|
||||
double CanMessage::getFloatTimestamp() const
|
||||
{
|
||||
return (double)_timestamp.tv_sec + ((double)_timestamp.tv_usec/1000000);
|
||||
}
|
||||
|
||||
QDateTime CanMessage::getDateTime() const
|
||||
{
|
||||
return QDateTime::fromMSecsSinceEpoch((qint64)(1000*getFloatTimestamp()));
|
||||
}
|
||||
|
||||
QString CanMessage::getIdString() const
|
||||
{
|
||||
if (isExtended()) {
|
||||
return QString().sprintf("0x%08X", getId());
|
||||
} else {
|
||||
return QString().sprintf("0x%03X", getId());
|
||||
}
|
||||
}
|
||||
|
||||
QString CanMessage::getDataHexString() const
|
||||
{
|
||||
if(getLength() == 0)
|
||||
return "";
|
||||
|
||||
QString outstr = "";
|
||||
for(int i=0; i<getLength(); i++)
|
||||
{
|
||||
outstr += QString().sprintf("%02X ", getByte(i));
|
||||
}
|
||||
|
||||
return outstr;
|
||||
}
|
||||
Reference in New Issue
Block a user