mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 22:23:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

boardd: add CAN health to pandaStates (#25800)

Browse Source 
* init

* try this

* mistake

* fix

* bump cereal

* make obvious

* fixes

* remove comment

* one helath header

* ..

* preallocate vectors
This commit is contained in:
Igor Biletskyy
2022-09-20 12:39:12 -07:00
committed by GitHub
parent 2cba29eabe
commit 53959082e7
4 changed files with 53 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

Submodule cereal updated: e4130c9055...3baa20e1da

46
selfdrive/boardd/boardd.cc
View File

@@ -295,13 +295,19 @@ void send_empty_panda_state(PubMaster *pm) {
std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started) {
bool ignition_local = false;
const uint32_t pandas_cnt = pandas.size();
// build msg
MessageBuilder msg;
auto evt = msg.initEvent();
auto pss = evt.initPandaStates(pandas.size());
auto pss = evt.initPandaStates(pandas_cnt);
std::vector<health_t> pandaStates;
pandaStates.reserve(pandas_cnt);
std::vector<std::array<can_health_t, PANDA_CAN_CNT>> pandaCanStates;
pandaCanStates.reserve(pandas_cnt);
for (const auto& panda : pandas){
auto health_opt = panda->get_state();
if (!health_opt) {
@@ -310,6 +316,16 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
health_t health = *health_opt;
std::array<can_health_t, PANDA_CAN_CNT> can_health{};
for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) {
auto can_health_opt = panda->get_can_state(i);
if (!can_health_opt) {
return std::nullopt;
}
can_health[i] = *can_health_opt;
}
pandaCanStates.push_back(can_health);
if (spoofing_started) {
health.ignition_line_pkt = 1;
}
@@ -319,7 +335,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
pandaStates.push_back(health);
}
for (uint32_t i = 0; i < pandas.size(); i++) {
for (uint32_t i = 0; i < pandas_cnt; i++) {
auto panda = pandas[i];
const auto &health = pandaStates[i];
@@ -366,6 +382,32 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
ps.setInterruptLoad(health.interrupt_load);
ps.setFanPower(health.fan_power);
std::array<cereal::PandaState::PandaCanState::Builder, PANDA_CAN_CNT> cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) {
const auto &can_health = pandaCanStates[i][j];
cs[j].setBusOff((bool)can_health.bus_off);
cs[j].setBusOffCnt(can_health.bus_off_cnt);
cs[j].setErrorWarning((bool)can_health.error_warning);
cs[j].setErrorPassive((bool)can_health.error_passive);
cs[j].setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error));
cs[j].setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error));
cs[j].setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error));
cs[j].setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error));
cs[j].setReceiveErrorCnt(can_health.receive_error_cnt);
cs[j].setTransmitErrorCnt(can_health.transmit_error_cnt);
cs[j].setTotalErrorCnt(can_health.total_error_cnt);
cs[j].setTotalTxLostCnt(can_health.total_tx_lost_cnt);
cs[j].setTotalRxLostCnt(can_health.total_rx_lost_cnt);
cs[j].setTotalTxCnt(can_health.total_tx_cnt);
cs[j].setTotalRxCnt(can_health.total_rx_cnt);
cs[j].setTotalFwdCnt(can_health.total_fwd_cnt);
cs[j].setCanSpeed(can_health.can_speed);
cs[j].setCanDataSpeed(can_health.can_data_speed);
cs[j].setCanfdEnabled(can_health.canfd_enabled);
cs[j].setBrsEnabled(can_health.canfd_enabled);
}
// Convert faults bitset to capnp list
std::bitset<sizeof(health.faults_pkt) * 8> fault_bits(health.faults_pkt);
auto faults = ps.initFaults(fault_bits.count());

6
selfdrive/boardd/panda.cc
View File

@@ -317,6 +317,12 @@ std::optional<health_t> Panda::get_state() {
return err >= 0 ? std::make_optional(health) : std::nullopt;
}
std::optional<can_health_t> Panda::get_can_state(uint16_t can_number) {
can_health_t can_health {0};
int err = usb_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health));
return err >= 0 ? std::make_optional(can_health) : std::nullopt;
}
void Panda::set_loopback(bool loopback) {
usb_write(0xe5, loopback, 0);
}

2
selfdrive/boardd/panda.h
View File

@@ -16,6 +16,7 @@
#include "panda/board/health.h"
#define TIMEOUT 0
#define PANDA_CAN_CNT 3
#define PANDA_BUS_CNT 4
#define RECV_SIZE (0x4000U)
#define USB_TX_SOFT_LIMIT (0x100U)
@@ -81,6 +82,7 @@ class Panda {
uint16_t get_fan_speed();
void set_ir_pwr(uint16_t ir_pwr);
std::optional<health_t> get_state();
std::optional<can_health_t> get_can_state(uint16_t can_number);
void set_loopback(bool loopback);
std::optional<std::vector<uint8_t>> get_firmware_version();
std::optional<std::string> get_serial();