目次
基本的な考え方
CANカード
まず、CAN の基本的な概念が必要です。
CANとは、Controller Area Network(以下CAN)の略で、ISOの国際標準化されたシリアル通信プロトコルです。自動車産業では、安全性、快適性、利便性、低消費電力、低コストの要件から、さまざまな電子制御システムが開発されています。
CAN カードの CAN バス データ送受信は、CAN コントローラーと CAN トランシーバーによって完成されます. このインターフェイス カードは、自動車業界で広く使用されており、産業用制御、ロボット工学、医療機器、医療機器などの分野で急速に発展しています。センサー。
現在 apollo で使用されているドイツの esd can カード ESD CAN-PCIe/402-1 と apollo1.0-1.5 で使用されているこの can カード。
缶カードには、通常、機器メーカーが提供する独自のドライバーが搭載されているため、ここでの CAN 監視は、缶ドライバーが正しい信号を出力できるかどうかを確認するための監視の 1 つです。
CAN - 生のソケット
当社の業務ソフトが車のデバイスの状態を知りたい場合はcanからステータスを読み取る必要がありますが、ここでは主にsocket canを使用し、socket CANは独自のソケットを使用します。このインターフェイスを使用すると、IP、ICMP などの下位レベルのプロトコルを操作できます。raw ソケットは、多くの場合、新しいプロトコルの実装をテストしたり、既存のサービスで構成された新しいデバイスにアクセスしたりするために使用されます。
ソケットのワークフローは次のとおりです。
最初にサーバーを起動し、socket() 関数を呼び出してソケットを作成し、次に bind() 関数を呼び出してソケットをローカル ネットワーク アドレスに関連付け、次に listen() 関数を呼び出してソケットをリッスンできる状態にします。要求キューの長さを指定してから、accept() 関数を呼び出して接続を受信します。クライアントはソケットを確立した後、connect() を呼び出してサーバーとの接続を確立できます。接続が確立されると、クライアントとサーバーは recv()/recvfrom() 関数と send()/sendto 関数を呼び出してデータを送受信できます。最後に、データ送信が終了したら、両者が close() 関数を呼び出してソケットを閉じます。
参考リンク:Linux SocketCANプログラミング(C++、マルチスレッド受信可能)
したがって、Apollo の缶監視のもう 1 つの監視は、link-can のソケット通信リンク用です。
ESD CAN監視解析
class EsdCanMonitor : public RecurrentRunner {
public:
EsdCanMonitor();
void RunOnce(const double current_time) override;
};
void EsdCanMonitor::RunOnce(const double current_time) {
Component* component = apollo::common::util::FindOrNull(
*MonitorManager::Instance()->GetStatus()->mutable_components(),
FLAGS_esdcan_component_name);
if (component == nullptr) {
// Canbus is not monitored in current mode, skip.
return;
}
auto* status = component->mutable_other_status();
status->clear_status();
EsdCanTest(FLAGS_esdcan_id, status);
}
まず、設定ファイルがこの内容で設定されているかどうかを判断する必要があることがわかります.この監視が設定されていない場合は、直接戻ります.
次に、関数 EsdCanTest を使用して Can カードのステータスをチェックします。これにより、コアは EsdCanTest になります。
NTCAN_RESULT EsdCanTest(const int can_id, NTCAN_HANDLE* handle) {
NTCAN_RESULT ret = canOpen(can_id, 0, 1, 1, 0, 0, handle);
if (ret == NTCAN_SUCCESS) {
AINFO << "Successfully opened ESD-CAN device " << can_id;
} else {
AERROR << "Failed to open ESD-CAN device " << can_id << ", error: " << ret
<< " (" << StatusString(ret) << ")";
return ret;
}
CAN_IF_STATUS if_status;
ret = canStatus(*handle, &if_status);
if (ret != NTCAN_SUCCESS) {
AERROR << "Cannot get status of ESD-CAN, ret=" << ret << " ("
<< StatusString(ret) << ")";
return ret;
}
NTCAN_BUS_STATISTIC stats;
ret = canIoctl(*handle, NTCAN_IOCTL_GET_BUS_STATISTIC, &stats);
if (ret != NTCAN_SUCCESS) {
AERROR << "NTCAN_IOCTL_GET_BUS_STATISTIC failed for device with error: "
<< ret << " (" << StatusString(ret) << ")";
return ret;
}
NTCAN_CTRL_STATE ctrl_state;
ret = canIoctl(*handle, NTCAN_IOCTL_GET_CTRL_STATUS, &ctrl_state);
if (ret != NTCAN_SUCCESS) {
AERROR << "NTCAN_IOCTL_GET_CTRL_STATUS failed for device with error: "
<< ret << " (" << StatusString(ret) << ")";
return ret;
}
NTCAN_BITRATE bitrate;
ret = canIoctl(*handle, NTCAN_IOCTL_GET_BITRATE_DETAILS, &bitrate);
if (ret != NTCAN_SUCCESS) {
AERROR << "NTCAN_IOCTL_GET_BITRATE_ for device with error: " << ret << " ("
<< StatusString(ret) << ")";
return ret;
}
return ret;
}
void EsdCanTest(const int can_id, ComponentStatus* status) {
NTCAN_HANDLE handle;
const NTCAN_RESULT ret = EsdCanTest(can_id, &handle);
canClose(handle);
SummaryMonitor::EscalateStatus(
ret == NTCAN_SUCCESS ? ComponentStatus::OK : ComponentStatus::ERROR,
StatusString(ret), status);
}
#else
// USE_ESD_CAN is not set, do dummy check.
void EsdCanTest(const int can_id, ComponentStatus* status) {
SummaryMonitor::EscalateStatus(ComponentStatus::ERROR,
"USE_ESD_CAN is not defined during compiling",
status);
}
#endif
ここでわかるように、CAN カードがインストールされていないことが判明した場合は、直接エラーが報告されます。インストールされている場合は、canopen を呼び出して can デバイスを開き、canstatus と canIoctl を使用して現在の状態を判断します。 can デバイスのステータスを取得し、最後にステータスを ret に戻します。
ret が NTCAN_SUCCESS でない場合は ERROR を報告し、それ以外の場合は OK を報告します。
ソケット缶モニタリング分析
class SocketCanMonitor : public RecurrentRunner {
public:
SocketCanMonitor();
void RunOnce(const double current_time) override;
};
void SocketCanMonitor::RunOnce(const double current_time) {
auto manager = MonitorManager::Instance();
Component* component = apollo::common::util::FindOrNull(
*manager->GetStatus()->mutable_components(),
FLAGS_socket_can_component_name);
if (component == nullptr) {
// Canbus is not monitored in current mode, skip.
return;
}
auto* status = component->mutable_other_status();
status->clear_status();
std::string message;
const bool ret = SocketCanTest(&message);
SummaryMonitor::EscalateStatus(
ret ? ComponentStatus::OK : ComponentStatus::ERROR, message, status);
}
同じルーチンの最初のステップは、ソケット can が監視オブジェクトとして構成されているかどうかを確認することです。構成されている場合は、SocketCanTest を呼び出し、ソケット can を確認し、ret を返します。
ret が false の場合、エラーの失敗が報告され、それ以外の場合は OK ステータスが報告されます。
bool SocketCanTest(std::string* message) {
const int dev_handler = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (dev_handler < 0) {
*message = "Open can device failed";
return false;
}
const bool ret = SocketCanHandlerTest(dev_handler, message);
close(dev_handler);
return ret;
}
- socket(PF_CAN, SOCK_RAW, CAN_RAW); ネイティブソケットを作成することです
- 検査のために SocketCanHandlerTest に渡す
// Test Socket CAN on an open handler.
bool SocketCanHandlerTest(const int dev_handler, std::string* message) {
// init config and state
// 1. set receive message_id filter, ie white list
struct can_filter filter[1];
filter[0].can_id = 0x000;
filter[0].can_mask = CAN_SFF_MASK;
int ret = setsockopt(dev_handler, SOL_CAN_RAW, CAN_RAW_FILTER, &filter,
sizeof(filter));
if (ret < 0) {
*message = "set message filter failed";
return false;
}
// 2. enable reception of can frames.
const int enable = 1;
ret = setsockopt(dev_handler, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable,
sizeof(enable));
if (ret < 0) {
*message = "Enable reception of can frames failed";
return false;
}
struct ifreq ifr;
std::strncpy(ifr.ifr_name, "can0", IFNAMSIZ);
if (ioctl(dev_handler, SIOCGIFINDEX, &ifr) < 0) {
*message = "ioctl failed";
return false;
}
// bind socket to network interface
struct sockaddr_can addr;
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
ret = bind(dev_handler, reinterpret_cast<struct sockaddr*>(&addr),
sizeof(addr));
if (ret < 0) {
*message = "bind socket can failed";
return false;
}
return true;
}
- フィルタを設定します。ここでは識別子が 0x00 のメッセージのみを受信し、設定が失敗した場合は false を返し、例外ログを送信します
- can フレームの受信を開始し、起動に失敗した場合は false を返し、例外ログを送信します
- 実際の can インターフェイスをマップし、失敗した場合は false をフィードバックし、例外ログを送信します
- ソケットをネットワーク インターフェイスにバインドします。バインドに失敗した場合は、false を返し、例外ログを送信します
- 上記のすべてが成功した場合、true を返します