Files
sunnypilot/common/params.cc
Kacper Rączy 236dffe400 locationd no GPS (#33029)
* Pose kf draft

old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b

* Fix it

old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b

* Add translation noise

old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d

* Add gravity to acc

old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613

* Use pyx

old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa

* Indent

old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67

* Reset function

old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc

* Add device_from_ned and ned_from_device transformations

old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae

* Fix rotations

old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6

* kms

old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2

* Centripetal acceleration

old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71

* Rewrite draft

old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c

* Remove old locationd stuff

old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba

* Python process now

old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3

* Process replay fix

old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9

* Add checks for timing and validity

old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36

* Fixes

old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267

* Process replay config fixes

old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e

* static analysis fixes

old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866

* lp in latcontrol

old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca

* Fix SensorEvent name for acceleration

old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd

* Ignore sensor readings from segments with multiple imus

old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0

* Update shebang

old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797

* Replace llk with lp

old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0

* Refactor locationd scenario test

old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84

* Add more debugging tools

old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1

* Param name update

old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337

* Fix expected observations

old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75

* Handle invalid measurements

old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf

* Fix spelling

old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b

* Include observations in debug info too

old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868

* Store error instead of expected observation

old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c

* Renames

old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d

* Zero the yaw

old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179

* New state_dot for orientation

old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7

* Fix the state transformations

old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db

* Update process in test_onroad

old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617

* Test polling on cameraOdometry

old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8

* Keep the copy of x and P returned from predict

old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd

* Remove polling again

old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68

* Remove locationd.cc

old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663

* Optim in _finite_check

old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5

* Access .t once

old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399

* Move the timing check to cam odo code path

old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e

* Call all_checks only once

old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78

* Do not sort

old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025

* Check sm.updated

old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4

* Remove test_params_gps

old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858

* Increase tolerance

old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3

* Fix static

old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490

* Try separate sockets for sensors

old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1

* sensor_all_checks

old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3

* Fix static

old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e

* Set the cpu limit to 25

old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d

* Make it prettier

old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7

* Prettier

old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5

* Increase the cpu budget to 260

old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4

* Change trans std mult. 2 works better

* Update ref commit

* Update ref commit
2024-09-04 11:54:57 +02:00

348 lines
12 KiB
C++

#include "common/params.h"
#include <dirent.h>
#include <sys/file.h>
#include <algorithm>
#include <cassert>
#include <csignal>
#include <unordered_map>
#include "common/queue.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "system/hardware/hw.h"
namespace {
volatile sig_atomic_t params_do_exit = 0;
void params_sig_handler(int signal) {
params_do_exit = 1;
}
int fsync_dir(const std::string &path) {
int result = -1;
int fd = HANDLE_EINTR(open(path.c_str(), O_RDONLY, 0755));
if (fd >= 0) {
result = HANDLE_EINTR(fsync(fd));
HANDLE_EINTR(close(fd));
}
return result;
}
bool create_params_path(const std::string &param_path, const std::string &key_path) {
// Make sure params path exists
if (!util::file_exists(param_path) && !util::create_directories(param_path, 0775)) {
return false;
}
// See if the symlink exists, otherwise create it
if (!util::file_exists(key_path)) {
// 1) Create temp folder
// 2) Symlink it to temp link
// 3) Move symlink to <params>/d
std::string tmp_path = param_path + "/.tmp_XXXXXX";
// this should be OK since mkdtemp just replaces characters in place
char *tmp_dir = mkdtemp((char *)tmp_path.c_str());
if (tmp_dir == NULL) {
return false;
}
std::string link_path = std::string(tmp_dir) + ".link";
if (symlink(tmp_dir, link_path.c_str()) != 0) {
return false;
}
// don't return false if it has been created by other
if (rename(link_path.c_str(), key_path.c_str()) != 0 && errno != EEXIST) {
return false;
}
}
return true;
}
std::string ensure_params_path(const std::string &prefix, const std::string &path = {}) {
std::string params_path = path.empty() ? Path::params() : path;
if (!create_params_path(params_path, params_path + prefix)) {
throw std::runtime_error(util::string_format(
"Failed to ensure params path, errno=%d, path=%s, param_prefix=%s",
errno, params_path.c_str(), prefix.c_str()));
}
return params_path;
}
class FileLock {
public:
FileLock(const std::string &fn) {
fd_ = HANDLE_EINTR(open(fn.c_str(), O_CREAT, 0775));
if (fd_ < 0 || HANDLE_EINTR(flock(fd_, LOCK_EX)) < 0) {
LOGE("Failed to lock file %s, errno=%d", fn.c_str(), errno);
}
}
~FileLock() { close(fd_); }
private:
int fd_ = -1;
};
std::unordered_map<std::string, uint32_t> keys = {
{"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
{"AlwaysOnDM", PERSISTENT},
{"ApiCache_Device", PERSISTENT},
{"AssistNowToken", PERSISTENT},
{"AthenadPid", PERSISTENT},
{"AthenadUploadQueue", PERSISTENT},
{"AthenadRecentlyViewedRoutes", PERSISTENT},
{"BootCount", PERSISTENT},
{"CalibrationParams", PERSISTENT},
{"CameraDebugExpGain", CLEAR_ON_MANAGER_START},
{"CameraDebugExpTime", CLEAR_ON_MANAGER_START},
{"CarBatteryCapacity", PERSISTENT},
{"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"CarParamsCache", CLEAR_ON_MANAGER_START},
{"CarParamsPersistent", PERSISTENT},
{"CarParamsPrevRoute", PERSISTENT},
{"CompletedTrainingVersion", PERSISTENT},
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"CurrentBootlog", PERSISTENT},
{"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"DisablePowerDown", PERSISTENT},
{"DisableUpdates", PERSISTENT},
{"DisengageOnAccelerator", PERSISTENT},
{"DmModelInitialized", CLEAR_ON_ONROAD_TRANSITION},
{"DongleId", PERSISTENT},
{"DoReboot", CLEAR_ON_MANAGER_START},
{"DoShutdown", CLEAR_ON_MANAGER_START},
{"DoUninstall", CLEAR_ON_MANAGER_START},
{"ExperimentalLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY},
{"ExperimentalMode", PERSISTENT},
{"ExperimentalModeConfirmed", PERSISTENT},
{"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"ForcePowerDown", PERSISTENT},
{"GitBranch", PERSISTENT},
{"GitCommit", PERSISTENT},
{"GitCommitDate", PERSISTENT},
{"GitDiff", PERSISTENT},
{"GithubSshKeys", PERSISTENT},
{"GithubUsername", PERSISTENT},
{"GitRemote", PERSISTENT},
{"GsmApn", PERSISTENT},
{"GsmMetered", PERSISTENT},
{"GsmRoaming", PERSISTENT},
{"HardwareSerial", PERSISTENT},
{"HasAcceptedTerms", PERSISTENT},
{"IMEI", PERSISTENT},
{"InstallDate", PERSISTENT},
{"IsDriverViewEnabled", CLEAR_ON_MANAGER_START},
{"IsEngaged", PERSISTENT},
{"IsLdwEnabled", PERSISTENT},
{"IsMetric", PERSISTENT},
{"IsOffroad", CLEAR_ON_MANAGER_START},
{"IsOnroad", PERSISTENT},
{"IsRhdDetected", PERSISTENT},
{"IsReleaseBranch", CLEAR_ON_MANAGER_START},
{"IsTakingSnapshot", CLEAR_ON_MANAGER_START},
{"IsTestedBranch", CLEAR_ON_MANAGER_START},
{"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
{"LanguageSetting", PERSISTENT},
{"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
{"LastGPSPosition", PERSISTENT},
{"LastManagerExitReason", CLEAR_ON_MANAGER_START},
{"LastOffroadStatusPacket", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
{"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
{"LastUpdateException", CLEAR_ON_MANAGER_START},
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
{"LiveTorqueParameters", PERSISTENT | DONT_LOG},
{"LocationFilterInitialState", PERSISTENT},
{"LongitudinalPersonality", PERSISTENT},
{"NetworkMetered", PERSISTENT},
{"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"Offroad_BadNvme", CLEAR_ON_MANAGER_START},
{"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START},
{"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START},
{"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START},
{"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START},
{"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"Offroad_Recalibration", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"Offroad_StorageMissing", CLEAR_ON_MANAGER_START},
{"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START},
{"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},
{"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
{"OpenpilotEnabledToggle", PERSISTENT},
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
{"PandaSomResetTriggered", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
{"PandaSignatures", CLEAR_ON_MANAGER_START},
{"PrimeType", PERSISTENT},
{"RecordFront", PERSISTENT},
{"RecordFrontLock", PERSISTENT}, // for the internal fleet
{"RouteCount", PERSISTENT},
{"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
{"SshEnabled", PERSISTENT},
{"TermsVersion", PERSISTENT},
{"TrainingVersion", PERSISTENT},
{"UbloxAvailable", PERSISTENT},
{"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"UpdateFailedCount", CLEAR_ON_MANAGER_START},
{"UpdaterAvailableBranches", PERSISTENT},
{"UpdaterCurrentDescription", CLEAR_ON_MANAGER_START},
{"UpdaterCurrentReleaseNotes", CLEAR_ON_MANAGER_START},
{"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START},
{"UpdaterNewDescription", CLEAR_ON_MANAGER_START},
{"UpdaterNewReleaseNotes", CLEAR_ON_MANAGER_START},
{"UpdaterState", CLEAR_ON_MANAGER_START},
{"UpdaterTargetBranch", CLEAR_ON_MANAGER_START},
{"UpdaterLastFetchTime", PERSISTENT},
{"Version", PERSISTENT},
};
} // namespace
Params::Params(const std::string &path) {
params_prefix = "/" + util::getenv("OPENPILOT_PREFIX", "d");
params_path = ensure_params_path(params_prefix, path);
}
Params::~Params() {
if (future.valid()) {
future.wait();
}
assert(queue.empty());
}
std::vector<std::string> Params::allKeys() const {
std::vector<std::string> ret;
for (auto &p : keys) {
ret.push_back(p.first);
}
return ret;
}
bool Params::checkKey(const std::string &key) {
return keys.find(key) != keys.end();
}
ParamKeyType Params::getKeyType(const std::string &key) {
return static_cast<ParamKeyType>(keys[key]);
}
int Params::put(const char* key, const char* value, size_t value_size) {
// Information about safely and atomically writing a file: https://lwn.net/Articles/457667/
// 1) Create temp file
// 2) Write data to temp file
// 3) fsync() the temp file
// 4) rename the temp file to the real name
// 5) fsync() the containing directory
std::string tmp_path = params_path + "/.tmp_value_XXXXXX";
int tmp_fd = mkstemp((char*)tmp_path.c_str());
if (tmp_fd < 0) return -1;
int result = -1;
do {
// Write value to temp.
ssize_t bytes_written = HANDLE_EINTR(write(tmp_fd, value, value_size));
if (bytes_written < 0 || (size_t)bytes_written != value_size) {
result = -20;
break;
}
// fsync to force persist the changes.
if ((result = fsync(tmp_fd)) < 0) break;
FileLock file_lock(params_path + "/.lock");
// Move temp into place.
if ((result = rename(tmp_path.c_str(), getParamPath(key).c_str())) < 0) break;
// fsync parent directory
result = fsync_dir(getParamPath());
} while (false);
close(tmp_fd);
if (result != 0) {
::unlink(tmp_path.c_str());
}
return result;
}
int Params::remove(const std::string &key) {
FileLock file_lock(params_path + "/.lock");
int result = unlink(getParamPath(key).c_str());
if (result != 0) {
return result;
}
return fsync_dir(getParamPath());
}
std::string Params::get(const std::string &key, bool block) {
if (!block) {
return util::read_file(getParamPath(key));
} else {
// blocking read until successful
params_do_exit = 0;
void (*prev_handler_sigint)(int) = std::signal(SIGINT, params_sig_handler);
void (*prev_handler_sigterm)(int) = std::signal(SIGTERM, params_sig_handler);
std::string value;
while (!params_do_exit) {
if (value = util::read_file(getParamPath(key)); !value.empty()) {
break;
}
util::sleep_for(100); // 0.1 s
}
std::signal(SIGINT, prev_handler_sigint);
std::signal(SIGTERM, prev_handler_sigterm);
return value;
}
}
std::map<std::string, std::string> Params::readAll() {
FileLock file_lock(params_path + "/.lock");
return util::read_files_in_dir(getParamPath());
}
void Params::clearAll(ParamKeyType key_type) {
FileLock file_lock(params_path + "/.lock");
// 1) delete params of key_type
// 2) delete files that are not defined in the keys.
if (DIR *d = opendir(getParamPath().c_str())) {
struct dirent *de = NULL;
while ((de = readdir(d))) {
if (de->d_type != DT_DIR) {
auto it = keys.find(de->d_name);
if (it == keys.end() || (it->second & key_type)) {
unlink(getParamPath(de->d_name).c_str());
}
}
}
closedir(d);
}
fsync_dir(getParamPath());
}
void Params::putNonBlocking(const std::string &key, const std::string &val) {
queue.push(std::make_pair(key, val));
// start thread on demand
if (!future.valid() || future.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
future = std::async(std::launch::async, &Params::asyncWriteThread, this);
}
}
void Params::asyncWriteThread() {
// TODO: write the latest one if a key has multiple values in the queue.
std::pair<std::string, std::string> p;
while (queue.try_pop(p, 0)) {
// Params::put is Thread-Safe
put(p.first, p.second);
}
}