You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
80 lines
2.8 KiB
80 lines
2.8 KiB
1 year ago
|
#include "selfdrive/pandad/pandad.h"
|
||
|
|
#include "cereal/messaging/messaging.h"
|
||
|
|
#include "common/swaglog.h"
|
||
|
|
|
||
|
|
void PandaSafety::configureSafetyMode() {
|
||
|
|
bool is_onroad = params_.getBool("IsOnroad");
|
||
|
|
|
||
|
|
if (is_onroad && !safety_configured_) {
|
||
|
|
updateMultiplexingMode();
|
||
|
|
|
||
|
|
auto car_params = fetchCarParams();
|
||
|
|
if (!car_params.empty()) {
|
||
|
|
LOGW("got %lu bytes CarParams", car_params.size());
|
||
|
|
setSafetyMode(car_params);
|
||
|
|
safety_configured_ = true;
|
||
|
|
}
|
||
|
|
} else if (!is_onroad) {
|
||
|
|
initialized_ = false;
|
||
|
|
safety_configured_ = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void PandaSafety::updateMultiplexingMode() {
|
||
|
|
// Initialize to ELM327 without OBD multiplexing for initial fingerprinting
|
||
|
|
if (!initialized_) {
|
||
|
|
prev_obd_multiplexing_ = false;
|
||
|
|
for (int i = 0; i < pandas_.size(); ++i) {
|
||
|
|
pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
|
||
|
|
}
|
||
|
|
initialized_ = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Switch between multiplexing modes based on the OBD multiplexing request
|
||
|
|
bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled");
|
||
|
|
if (obd_multiplexing_requested != prev_obd_multiplexing_) {
|
||
|
|
for (int i = 0; i < pandas_.size(); ++i) {
|
||
|
|
const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U;
|
||
|
|
pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
|
||
|
|
}
|
||
|
|
prev_obd_multiplexing_ = obd_multiplexing_requested;
|
||
|
|
params_.putBool("ObdMultiplexingChanged", true);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
std::string PandaSafety::fetchCarParams() {
|
||
|
|
if (!params_.getBool("FirmwareQueryDone")) {
|
||
|
|
return {};
|
||
|
|
}
|
||
|
|
LOGW("Finished FW query");
|
||
|
|
|
||
|
|
LOGW("Waiting for params to set safety model");
|
||
|
|
if (!params_.getBool("ControlsReady")) {
|
||
|
|
return {};
|
||
|
|
}
|
||
|
|
return params_.get("CarParams");
|
||
|
|
}
|
||
|
|
|
||
|
|
void PandaSafety::setSafetyMode(const std::string ¶ms_string) {
|
||
|
|
AlignedBuffer aligned_buf;
|
||
|
|
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size()));
|
||
|
|
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
|
||
|
|
|
||
|
|
auto safety_configs = car_params.getSafetyConfigs();
|
||
|
|
uint16_t alternative_experience = car_params.getAlternativeExperience();
|
||
|
|
|
||
|
|
for (int i = 0; i < pandas_.size(); ++i) {
|
||
|
|
// Default to SILENT safety model if not specified
|
||
|
|
cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT;
|
||
|
|
uint16_t safety_param = 0U;
|
||
|
|
if (i < safety_configs.size()) {
|
||
|
|
safety_model = safety_configs[i].getSafetyModel();
|
||
|
|
safety_param = safety_configs[i].getSafetyParam();
|
||
|
|
}
|
||
|
|
|
||
|
|
LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience);
|
||
|
|
pandas_[i]->set_alternative_experience(alternative_experience);
|
||
|
|
pandas_[i]->set_safety_model(safety_model, safety_param);
|
||
|
|
}
|
||
|
|
}
|