mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-21 17:22:24 +08:00
Vehicle Researcher
683b6151ce
4f82d01e gitignore 5cb83454 Honda FCM: diagnostic signals d309cdce Added linter to opendbc (#203) d452706f add requirements.txt ec3b4595 deterministic dependency order a265d351 Azure pipelines ci (#202) bce9a2e1 packer depends on libdbc 5d5fdd6a no more python version of libdbc, everything through cython 541705bf move CANDefine to parser code da25c52a add test for can define 0ba7926b unify can packer and parser 25d88009 consistent naming a5c640a5 fix linter be210fef remove obsolete make file ffd9dca7 opendbc needs cereal b559f63d remove more make d0929496 seems to work now 41e80836 don't make 3254d1fc think scons works eb78f6aa scons sort of working 0ef1e35d fix gitignore e155e017 Can migration (#199) 3eded83a Honda: correct steering torque sensor sign to be consistent with standard convention (left+) 32f70e2f Fix outback endianness consistency (#196) a7da471f Update subaru_outback_2015_eyesight.dbc (#195) git-subtree-dir: opendbc git-subtree-split: 4f82d01ebc78109888954d9807d320e3c27896fd
109 lines
3.5 KiB
C++
109 lines
3.5 KiB
C++
#include <cassert>
|
|
#include <utility>
|
|
#include <algorithm>
|
|
#include <map>
|
|
#include <cmath>
|
|
|
|
#include "common.h"
|
|
|
|
#define WARN printf
|
|
|
|
// this is the same as read_u64_le, but uses uint64_t as in/out
|
|
uint64_t ReverseBytes(uint64_t x) {
|
|
return ((x & 0xff00000000000000ull) >> 56) |
|
|
((x & 0x00ff000000000000ull) >> 40) |
|
|
((x & 0x0000ff0000000000ull) >> 24) |
|
|
((x & 0x000000ff00000000ull) >> 8) |
|
|
((x & 0x00000000ff000000ull) << 8) |
|
|
((x & 0x0000000000ff0000ull) << 24) |
|
|
((x & 0x000000000000ff00ull) << 40) |
|
|
((x & 0x00000000000000ffull) << 56);
|
|
}
|
|
|
|
uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){
|
|
int shift = sig.is_little_endian? sig.b1 : sig.bo;
|
|
uint64_t mask = ((1ULL << sig.b2)-1) << shift;
|
|
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift;
|
|
if (sig.is_little_endian) {
|
|
dat = ReverseBytes(dat);
|
|
mask = ReverseBytes(mask);
|
|
}
|
|
ret &= ~mask;
|
|
ret |= dat;
|
|
return ret;
|
|
}
|
|
|
|
CANPacker::CANPacker(const std::string& dbc_name) {
|
|
dbc = dbc_lookup(dbc_name);
|
|
assert(dbc);
|
|
|
|
for (int i=0; i<dbc->num_msgs; i++) {
|
|
const Msg* msg = &dbc->msgs[i];
|
|
message_lookup[msg->address] = *msg;
|
|
for (int j=0; j<msg->num_sigs; j++) {
|
|
const Signal* sig = &msg->sigs[j];
|
|
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
|
|
}
|
|
}
|
|
init_crc_lookup_tables();
|
|
}
|
|
|
|
uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
|
|
uint64_t ret = 0;
|
|
for (const auto& sigval : signals) {
|
|
std::string name = std::string(sigval.name);
|
|
double value = sigval.value;
|
|
|
|
auto sig_it = signal_lookup.find(std::make_pair(address, name));
|
|
if (sig_it == signal_lookup.end()) {
|
|
WARN("undefined signal %s - %d\n", name.c_str(), address);
|
|
continue;
|
|
}
|
|
auto sig = sig_it->second;
|
|
|
|
int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor));
|
|
if (ival < 0) {
|
|
ival = (1ULL << sig.b2) + ival;
|
|
}
|
|
|
|
ret = set_value(ret, sig, ival);
|
|
}
|
|
|
|
if (counter >= 0){
|
|
auto sig_it = signal_lookup.find(std::make_pair(address, "COUNTER"));
|
|
if (sig_it == signal_lookup.end()) {
|
|
WARN("COUNTER not defined\n");
|
|
return ret;
|
|
}
|
|
auto sig = sig_it->second;
|
|
|
|
if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) {
|
|
WARN("COUNTER signal type not valid\n");
|
|
}
|
|
|
|
ret = set_value(ret, sig, counter);
|
|
}
|
|
|
|
auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
|
|
if (sig_it_checksum != signal_lookup.end()) {
|
|
auto sig = sig_it_checksum->second;
|
|
if (sig.type == SignalType::HONDA_CHECKSUM) {
|
|
unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size);
|
|
ret = set_value(ret, sig, chksm);
|
|
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
|
|
unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size);
|
|
ret = set_value(ret, sig, chksm);
|
|
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
|
|
// FIXME: Hackish fix for an endianness issue. The message is in reverse byte order
|
|
// until later in the pack process. Checksums can be run backwards, CRCs not so much.
|
|
// The correct fix is unclear but this works for the moment.
|
|
unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size);
|
|
ret = set_value(ret, sig, chksm);
|
|
} else {
|
|
//WARN("CHECKSUM signal type not valid\n");
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|