thooge/OBPkp61
1
0
Fork 0
Code Issues Pull Requests Packages Releases Wiki Activity
Files
3b6b97011b8d6c9ac621d0403261b0bc778cdcae
OBPkp61/lib/Nmea2kTwai/Nmea2kTwai.cpp

435 lines
12 KiB
C++
Raw Blame History

#include "../../include/main.h"
#include "Nmea2kTwai.h"
#include "driver/gpio.h"
#include "driver/twai.h"
#define LOGID(id) ((id >> 8) & 0x1ffff)
// Logging
static const char* TAG = "TWAI";
static const int TIMEOUT_OFFLINE = 256; // number of timeouts to consider offline
Nmea2kTwai::Nmea2kTwai(gpio_num_t _TxPin, gpio_num_t _RxPin, unsigned long recP, unsigned long logP):
tNMEA2000(), RxPin(_RxPin), TxPin(_TxPin)
{
if (RxPin < 0 || TxPin < 0) {
disabled = true;
} else {
// timers.addAction(logP,[this](){ logStatus(); });
// timers.addAction(recP,[this](){ checkRecovery(); });
}
}
bool Nmea2kTwai::CANSendFrame(unsigned long id, unsigned char len, const unsigned char *buf, bool wait_sent)
{
if (disabled) {
return true;
}
twai_message_t message;
memset(&message, 0, sizeof(message));
message.identifier = id;
message.extd = 1;
message.data_length_code = len;
memcpy(message.data, buf, len);
esp_err_t rt = twai_transmit(&message, 0);
if (rt != ESP_OK) {
if (rt == ESP_ERR_TIMEOUT) {
if (txTimeouts < TIMEOUT_OFFLINE) {
txTimeouts++;
}
}
LOGW(TAG, "twai transmit for %ld failed: %x", LOGID(id), (int)rt);
return false;
}
txTimeouts = 0;
LOGV(TAG, "twai transmit id %ld, len %d", LOGID(id), (int)len);
return true;
}
bool Nmea2kTwai::CANOpen()
{
if (disabled) {
LOGD(TAG, "CAN disabled");
return true;
}
esp_err_t rt = twai_start();
if (rt != ESP_OK) {
LOGE(TAG, "CANOpen failed: %x", rt);
return false;
} else {
LOGI(TAG, "CANOpen ok");
}
return true;
}
bool Nmea2kTwai::CANGetFrame(unsigned long &id, unsigned char &len, unsigned char *buf)
{
if (disabled) {
return false;
}
twai_message_t message;
esp_err_t rt = twai_receive(&message, 0);
if (rt != ESP_OK){
return false;
}
if (! message.extd) {
return false;
}
id = message.identifier;
len = message.data_length_code;
if (len > 8) {
LOGD(TAG, "twai: received invalid message %lld, len %d", LOGID(id), len);
len = 8;
}
LOGV(TAG, "twai rcv id=%ld,len=%d, ext=%d",
LOGID(message.identifier), message.data_length_code, message.extd);
if (! message.rtr) {
memcpy(buf, message.data, message.data_length_code);
}
return true;
}
void Nmea2kTwai::initDriver()
{
if (disabled) {
return;
}
twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TxPin,RxPin, TWAI_MODE_NORMAL);
g_config.tx_queue_len = 20;
twai_timing_config_t t_config = TWAI_TIMING_CONFIG_250KBITS();
twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
esp_err_t rt = twai_driver_install(&g_config, &t_config, &f_config);
if (rt == ESP_OK) {
LOGI(TAG, "twai driver initialzed, rx=%d,tx=%d", RxPin, TxPin);
} else {
LOGE(TAG,"twai driver init failed: %x", rt);
}
}
/* This will be called on Open() before any other initialization.
* Inherit this, if buffers can be set for the driver and you want to
* change size of library send frame buffer size.
* See e.g. NMEA2000_teensy.cpp.
*/
void Nmea2kTwai::InitCANFrameBuffers()
{
if (disabled) {
LOGI(TAG, "twai init - disabled");
} else{
initDriver();
}
tNMEA2000::InitCANFrameBuffers();
}
Nmea2kTwai::Status Nmea2kTwai::getStatus()
{
twai_status_info_t state;
Status rt;
if (disabled) {
rt.state = ST_DISABLED;
return rt;
}
if (twai_get_status_info(&state) != ESP_OK) {
return rt;
}
switch (state.state) {
case TWAI_STATE_STOPPED:
rt.state = ST_STOPPED;
break;
case TWAI_STATE_RUNNING:
rt.state = ST_RUNNING;
break;
case TWAI_STATE_BUS_OFF:
rt.state = ST_BUS_OFF;
break;
case TWAI_STATE_RECOVERING:
rt.state = ST_RECOVERING;
break;
}
rt.rx_errors = state.rx_error_counter;
rt.tx_errors = state.tx_error_counter;
rt.tx_failed = state.tx_failed_count;
rt.rx_missed = state.rx_missed_count;
rt.rx_overrun = state.rx_overrun_count;
rt.tx_timeouts = txTimeouts;
if (rt.tx_timeouts >= TIMEOUT_OFFLINE && rt.state == ST_RUNNING) {
rt.state = ST_OFFLINE;
}
return rt;
}
bool Nmea2kTwai::checkRecovery()
{
if (disabled) {
return false;
}
Status canState = getStatus();
bool strt = false;
if (canState.state != Nmea2kTwai::ST_RUNNING) {
if (canState.state == Nmea2kTwai::ST_BUS_OFF) {
strt = true;
bool rt = startRecovery();
LOGI(TAG, "twai BUS_OFF: start can recovery - result %d", rt);
}
if (canState.state == Nmea2kTwai::ST_STOPPED) {
bool rt = CANOpen();
LOGI(TAG, "twai STOPPED: restart can driver - result %d", rt);
}
}
return strt;
}
void Nmea2kTwai::loop()
{
if (disabled) {
return;
}
LOGW(TAG, "twai loop() not implemented");
// TODO?
// timers.loop();
}
Nmea2kTwai::Status Nmea2kTwai::logStatus()
{
Status canState = getStatus();
LOGD(TAG, "twai state %s, rxerr %d, txerr %d, txfail %d, txtimeout %d, rxmiss %d, rxoverrun %d",
stateStr(canState.state),
canState.rx_errors,
canState.tx_errors,
canState.tx_failed,
canState.tx_timeouts,
canState.rx_missed,
canState.rx_overrun);
return canState;
}
bool Nmea2kTwai::startRecovery()
{
if (disabled) {
return false;
}
lastRecoveryStart = millis();
esp_err_t rt = twai_driver_uninstall();
if (rt != ESP_OK) {
LOGE(TAG, "twai: deinit for recovery failed with %x", rt);
}
initDriver();
bool frt = CANOpen();
return frt;
}
const char * Nmea2kTwai::stateStr(const Nmea2kTwai::STATE &st)
{
switch (st) {
case ST_BUS_OFF: return "BUS_OFF";
case ST_RECOVERING: return "RECOVERING";
case ST_RUNNING: return "RUNNING";
case ST_STOPPED: return "STOPPED";
case ST_OFFLINE: return "OFFLINE";
case ST_DISABLED: return "DISABLED";
}
return "ERROR";
}
const Nmea2kTwai::ManufacturerEntry Nmea2kTwai::ManufacturerTable[] = {
{0, "Reserved"},
{69, "ARKS Enterprises, Inc."},
{78, "FW Murphy/Enovation Controls"},
{80, "Twin Disc"},
{85, "Kohler Power Systems"},
{88, "Hemisphere GPS Inc"},
{116, "BEP Marine"},
{135, "Airmar"},
{137, "Maretron"},
{140, "Lowrance"},
{144, "Mercury Marine"},
{147, "Nautibus Electronic GmbH"},
{148, "Blue Water Data"},
{154, "Westerbeke"},
{161, "Offshore Systems (UK) Ltd."},
{163, "Evinrude/BRP"},
{165, "CPAC Systems AB"},
{168, "Xantrex Technology Inc."},
{172, "Yanmar Marine"},
{174, "Volvo Penta"},
{175, "Honda Marine"},
{176, "Carling Technologies Inc. (Moritz Aerospace)"},
{185, "Beede Instruments"},
{192, "Floscan Instrument Co. Inc."},
{193, "Nobletec"},
{198, "Mystic Valley Communications"},
{199, "Actia"},
{200, "Honda Marine"},
{201, "Disenos Y Technologia"},
{211, "Digital Switching Systems"},
{215, "Xintex/Atena"},
{224, "EMMI NETWORK S.L."},
{225, "Honda Marine"},
{228, "ZF"},
{229, "Garmin"},
{233, "Yacht Monitoring Solutions"},
{235, "Sailormade Marine Telemetry/Tetra Technology LTD"},
{243, "Eride"},
{250, "Honda Marine"},
{257, "Honda Motor Company LTD"},
{272, "Groco"},
{273, "Actisense"},
{274, "Amphenol LTW Technology"},
{275, "Navico"},
{283, "Hamilton Jet"},
{285, "Sea Recovery"},
{286, "Coelmo SRL Italy"},
{295, "BEP Marine"},
{304, "Empir Bus"},
{305, "NovAtel"},
{306, "Sleipner Motor AS"},
{307, "MBW Technologies"},
{311, "Fischer Panda"},
{315, "ICOM"},
{328, "Qwerty"},
{329, "Dief"},
{341, "Böning Automationstechnologie GmbH & Co. KG"},
{345, "Korean Maritime University"},
{351, "Thrane and Thrane"},
{355, "Mastervolt"},
{356, "Fischer Panda Generators"},
{358, "Victron Energy"},
{370, "Rolls Royce Marine"},
{373, "Electronic Design"},
{374, "Northern Lights"},
{378, "Glendinning"},
{381, "B & G"},
{384, "Rose Point Navigation Systems"},
{385, "Johnson Outdoors Marine Electronics Inc Geonav"},
{394, "Capi 2"},
{396, "Beyond Measure"},
{400, "Livorsi Marine"},
{404, "ComNav"},
{409, "Chetco"},
{419, "Fusion Electronics"},
{421, "Standard Horizon"},
{422, "True Heading AB"},
{426, "Egersund Marine Electronics AS"},
{427, "em-trak Marine Electronics"},
{431, "Tohatsu Co, JP"},
{437, "Digital Yacht"},
{438, "Comar Systems Limited"},
{440, "Cummins"},
{443, "VDO (aka Continental-Corporation)"},
{451, "Parker Hannifin aka Village Marine Tech"},
{459, "Alltek Marine Electronics Corp"},
{460, "SAN GIORGIO S.E.I.N"},
{466, "Veethree Electronics & Marine"},
{467, "Humminbird Marine Electronics"},
{470, "SI-TEX Marine Electronics"},
{471, "Sea Cross Marine AB"},
{475, "GME aka Standard Communications Pty LTD"},
{476, "Humminbird Marine Electronics"},
{478, "Ocean Sat BV"},
{481, "Chetco Digitial Instruments"},
{493, "Watcheye"},
{499, "Lcj Capteurs"},
{502, "Attwood Marine"},
{503, "Naviop S.R.L."},
{504, "Vesper Marine Ltd"},
{510, "Marinesoft Co. LTD"},
{517, "NoLand Engineering"},
{518, "Transas USA"},
{529, "National Instruments Korea"},
{532, "Onwa Marine"},
{571, "Marinecraft (South Korea)"},
{573, "McMurdo Group aka Orolia LTD"},
{578, "Advansea"},
{579, "KVH"},
{580, "San Jose Technology"},
{583, "Yacht Control"},
{586, "Suzuki Motor Corporation"},
{591, "US Coast Guard"},
{595, "Ship Module aka Customware"},
{600, "Aquatic AV"},
{605, "Aventics GmbH"},
{606, "Intellian"},
{612, "SamwonIT"},
{614, "Arlt Tecnologies"},
{637, "Bavaria Yacts"},
{641, "Diverse Yacht Services"},
{644, "Wema U.S.A dba KUS"},
{645, "Garmin"},
{658, "Shenzhen Jiuzhou Himunication"},
{688, "Rockford Corp"},
{704, "JL Audio"},
{715, "Autonnic"},
{717, "Yacht Devices"},
{734, "REAP Systems"},
{735, "Au Electronics Group"},
{739, "LxNav"},
{743, "DaeMyung"},
{744, "Woosung"},
{773, "Clarion US"},
{776, "HMI Systems"},
{777, "Ocean Signal"},
{778, "Seekeeper"},
{781, "Poly Planar"},
{785, "Fischer Panda DE"},
{795, "Broyda Industries"},
{796, "Canadian Automotive"},
{797, "Tides Marine"},
{798, "Lumishore"},
{799, "Still Water Designs and Audio"},
{802, "BJ Technologies (Beneteau)"},
{803, "Gill Sensors"},
{811, "Blue Water Desalination"},
{815, "FLIR"},
{824, "Undheim Systems"},
{838, "TeamSurv"},
{844, "Fell Marine"},
{847, "Oceanvolt"},
{862, "Prospec"},
{868, "Data Panel Corp"},
{890, "L3 Technologies"},
{894, "Rhodan Marine Systems"},
{896, "Nexfour Solutions"},
{905, "ASA Electronics"},
{909, "Marines Co (South Korea)"},
{911, "Nautic-on"},
{930, "Ecotronix"},
{962, "Timbolier Industries"},
{963, "TJC Micro"},
{968, "Cox Powertrain"},
{969, "Blue Seas"},
{1850, "Teleflex Marine (SeaStar Solutions)"},
{1851, "Raymarine"},
{1852, "Navionics"},
{1853, "Japan Radio Co"},
{1854, "Northstar Technologies"},
{1855, "Furuno"},
{1856, "Trimble"},
{1857, "Simrad"},
{1858, "Litton"},
{1859, "Kvasar AB"},
{1860, "MMP"},
{1861, "Vector Cantech"},
{1862, "Yamaha Marine"},
{1863, "Faria Instruments"},
{2046, "Open Boat Projects"}
};
const char* Nmea2kTwai::GetManufacturerName(uint16_t code) {
constexpr size_t ManufacturerCount =
sizeof(Nmea2kTwai::ManufacturerTable) /
sizeof(Nmea2kTwai::ManufacturerTable[0]);
int low = 0;
int high = ManufacturerCount - 1;
while (low <= high) {
int mid = (low + high) / 2;
uint16_t midCode = ManufacturerTable[mid].code;
if (midCode == code) return ManufacturerTable[mid].name;
if (midCode < code) low = mid + 1;
else high = mid - 1;
}
return "Unknown";
}
Reference in New Issue View Git Blame Copy Permalink