From ecee67dd64b04eb06b32f6b9abae37afe63f8b99 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Sep 2025 23:39:55 -0400 Subject: [PATCH] Locationd: reimplement `liveLocationKalman` and use position_geodetic in `liveMapDataSP` (#1275) * init * desc * llk welcome back * more * new param to write * update mapd * no migration * no refactor for now * exec * rename * bearing * fix test * lint --- cereal/log.capnp | 2 +- cereal/services.py | 1 + common/params_keys.h | 1 + selfdrive/test/process_replay/migration.py | 6 +- .../test/process_replay/process_replay.py | 2 +- sunnypilot/SConscript | 3 +- .../mapd/live_map_data/base_map_data.py | 13 +- sunnypilot/mapd/live_map_data/osm_map_data.py | 19 +- sunnypilot/selfdrive/locationd/.gitignore | 3 + sunnypilot/selfdrive/locationd/SConscript | 37 + sunnypilot/selfdrive/locationd/__init__.py | 0 sunnypilot/selfdrive/locationd/locationd.cc | 750 ++++++++++++++++++ sunnypilot/selfdrive/locationd/locationd.h | 100 +++ .../selfdrive/locationd/models/.gitignore | 1 + .../selfdrive/locationd/models/__init__.py | 0 .../selfdrive/locationd/models/car_kf.py | 180 +++++ .../selfdrive/locationd/models/constants.py | 92 +++ .../selfdrive/locationd/models/live_kf.cc | 122 +++ .../selfdrive/locationd/models/live_kf.h | 66 ++ .../selfdrive/locationd/models/live_kf.py | 242 ++++++ .../selfdrive/locationd/tests/.gitignore | 1 + .../selfdrive/locationd/tests/__init__.py | 0 .../locationd/tests/test_locationd.py | 89 +++ sunnypilot/system/sensord/.gitignore | 1 + sunnypilot/system/sensord/SConscript | 13 + .../system/sensord/sensors/bmx055_accel.cc | 85 ++ .../system/sensord/sensors/bmx055_accel.h | 41 + .../system/sensord/sensors/bmx055_gyro.cc | 92 +++ .../system/sensord/sensors/bmx055_gyro.h | 41 + .../system/sensord/sensors/bmx055_magn.cc | 258 ++++++ .../system/sensord/sensors/bmx055_magn.h | 64 ++ .../system/sensord/sensors/bmx055_temp.cc | 31 + .../system/sensord/sensors/bmx055_temp.h | 13 + sunnypilot/system/sensord/sensors/constants.h | 18 + .../system/sensord/sensors/i2c_sensor.cc | 50 ++ .../system/sensord/sensors/i2c_sensor.h | 51 ++ .../system/sensord/sensors/lsm6ds3_accel.cc | 250 ++++++ .../system/sensord/sensors/lsm6ds3_accel.h | 49 ++ .../system/sensord/sensors/lsm6ds3_gyro.cc | 233 ++++++ .../system/sensord/sensors/lsm6ds3_gyro.h | 45 ++ .../system/sensord/sensors/lsm6ds3_temp.cc | 37 + .../system/sensord/sensors/lsm6ds3_temp.h | 26 + .../system/sensord/sensors/mmc5603nj_magn.cc | 108 +++ .../system/sensord/sensors/mmc5603nj_magn.h | 37 + sunnypilot/system/sensord/sensors/sensor.h | 24 + sunnypilot/system/sensord/sensors_qcom2.cc | 179 +++++ .../system/sensord/tests/test_sensord.py | 251 ++++++ sunnypilot/system/sensord/tests/ttff_test.py | 48 ++ system/manager/process_config.py | 3 + 49 files changed, 3755 insertions(+), 23 deletions(-) create mode 100644 sunnypilot/selfdrive/locationd/.gitignore create mode 100644 sunnypilot/selfdrive/locationd/SConscript create mode 100644 sunnypilot/selfdrive/locationd/__init__.py create mode 100644 sunnypilot/selfdrive/locationd/locationd.cc create mode 100644 sunnypilot/selfdrive/locationd/locationd.h create mode 100644 sunnypilot/selfdrive/locationd/models/.gitignore create mode 100644 sunnypilot/selfdrive/locationd/models/__init__.py create mode 100755 sunnypilot/selfdrive/locationd/models/car_kf.py create mode 100644 sunnypilot/selfdrive/locationd/models/constants.py create mode 100644 sunnypilot/selfdrive/locationd/models/live_kf.cc create mode 100644 sunnypilot/selfdrive/locationd/models/live_kf.h create mode 100755 sunnypilot/selfdrive/locationd/models/live_kf.py create mode 100644 sunnypilot/selfdrive/locationd/tests/.gitignore create mode 100644 sunnypilot/selfdrive/locationd/tests/__init__.py create mode 100644 sunnypilot/selfdrive/locationd/tests/test_locationd.py create mode 100644 sunnypilot/system/sensord/.gitignore create mode 100644 sunnypilot/system/sensord/SConscript create mode 100644 sunnypilot/system/sensord/sensors/bmx055_accel.cc create mode 100644 sunnypilot/system/sensord/sensors/bmx055_accel.h create mode 100644 sunnypilot/system/sensord/sensors/bmx055_gyro.cc create mode 100644 sunnypilot/system/sensord/sensors/bmx055_gyro.h create mode 100644 sunnypilot/system/sensord/sensors/bmx055_magn.cc create mode 100644 sunnypilot/system/sensord/sensors/bmx055_magn.h create mode 100644 sunnypilot/system/sensord/sensors/bmx055_temp.cc create mode 100644 sunnypilot/system/sensord/sensors/bmx055_temp.h create mode 100644 sunnypilot/system/sensord/sensors/constants.h create mode 100644 sunnypilot/system/sensord/sensors/i2c_sensor.cc create mode 100644 sunnypilot/system/sensord/sensors/i2c_sensor.h create mode 100644 sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc create mode 100644 sunnypilot/system/sensord/sensors/lsm6ds3_accel.h create mode 100644 sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc create mode 100644 sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h create mode 100644 sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc create mode 100644 sunnypilot/system/sensord/sensors/lsm6ds3_temp.h create mode 100644 sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc create mode 100644 sunnypilot/system/sensord/sensors/mmc5603nj_magn.h create mode 100644 sunnypilot/system/sensord/sensors/sensor.h create mode 100644 sunnypilot/system/sensord/sensors_qcom2.cc create mode 100644 sunnypilot/system/sensord/tests/test_sensord.py create mode 100755 sunnypilot/system/sensord/tests/ttff_test.py diff --git a/cereal/log.capnp b/cereal/log.capnp index 7babd4d8b5..8925bca8ae 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2684,7 +2684,7 @@ struct Event { lateralPlanDEPRECATED @64 :LateralPlan; navModelDEPRECATED @104 :NavModelData; uiPlanDEPRECATED @106 :UiPlan; - liveLocationKalmanDEPRECATED @72 :LiveLocationKalman; + liveLocationKalman @72 :LiveLocationKalman; liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED); onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED); } diff --git a/cereal/services.py b/cereal/services.py index 17ac2e926d..67548bc79e 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -89,6 +89,7 @@ _services: dict[str, tuple] = { "carStateSP": (True, 100., 10), "liveMapDataSP": (True, 1., 1), "modelDataV2SP": (True, 20.), + "liveLocationKalman": (True, 20.), # debug "uiDebug": (True, 0., 1), diff --git a/common/params_keys.h b/common/params_keys.h index 3faae9ce14..5ba6b53108 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -152,6 +152,7 @@ inline static std::unordered_map keys = { {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}}, {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}}, {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}}, + {"LastGPSPositionLLK", {PERSISTENT, STRING}}, {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}}, {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}}, {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}}, diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 33b363cfd9..74507197b6 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -28,7 +28,8 @@ MigrationFunc = Callable[[list[MessageWithIndex]], MigrationOps] # 3. product is the message type created by the migration function, and the function will be skipped if product type already exists in lr # 4. it must return a list of operations to be applied to the logreader (replace, add, delete) # 5. all migration functions must be independent of each other -def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False): +def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False, + live_location_kalman: bool = True): migrations = [ migrate_sensorEvents, migrate_carParams, @@ -37,7 +38,6 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo migrate_carOutput, migrate_controlsState, migrate_carState, - migrate_liveLocationKalman, migrate_liveTracks, migrate_driverAssistance, migrate_drivingModelData, @@ -51,6 +51,8 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo migrations.extend([migrate_pandaStates, migrate_peripheralState]) if camera_states: migrations.append(migrate_cameraStates) + if live_location_kalman: + migrations.append(migrate_liveLocationKalman) return migrate(lr, migrations) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 895204538f..9b6371dbb9 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -496,7 +496,7 @@ CONFIGS = [ pubs=[ "cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState" ], - subs=["livePose"], + subs=["liveLocationKalman", "livePose"], ignore=["logMonoTime"], should_recv_callback=MessageBasedRcvCallback("cameraOdometry"), tolerance=NUMPY_TOLERANCE, diff --git a/sunnypilot/SConscript b/sunnypilot/SConscript index 3aa302b92b..6b3856cb34 100644 --- a/sunnypilot/SConscript +++ b/sunnypilot/SConscript @@ -1,2 +1,3 @@ SConscript(['modeld/SConscript']) -SConscript(['modeld_v2/SConscript']) \ No newline at end of file +SConscript(['modeld_v2/SConscript']) +SConscript(['selfdrive/locationd/SConscript']) \ No newline at end of file diff --git a/sunnypilot/mapd/live_map_data/base_map_data.py b/sunnypilot/mapd/live_map_data/base_map_data.py index f386eab96b..25925937f5 100644 --- a/sunnypilot/mapd/live_map_data/base_map_data.py +++ b/sunnypilot/mapd/live_map_data/base_map_data.py @@ -6,8 +6,7 @@ See the LICENSE.md file in the root directory for more details. """ from abc import abstractmethod, ABC -from cereal import messaging -from openpilot.common.gps import get_gps_location_service +import cereal.messaging as messaging from openpilot.common.params import Params from openpilot.sunnypilot.navd.helpers import coordinate_from_param @@ -16,14 +15,12 @@ class BaseMapData(ABC): def __init__(self): self.params = Params() - self.gps_location_service = get_gps_location_service(self.params) - gps_packets = [self.gps_location_service] - self.sm = messaging.SubMaster(['livePose'] + gps_packets, ignore_alive=gps_packets, ignore_avg_freq=gps_packets, - ignore_valid=gps_packets, poll='livePose') + self.sm = messaging.SubMaster(['liveLocationKalman']) self.pm = messaging.PubMaster(['liveMapDataSP']) + self.localizer_valid = False self.last_bearing = None - self.last_position = coordinate_from_param("LastGPSPosition", self.params) + self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params) @abstractmethod def update_location(self) -> None: @@ -46,7 +43,7 @@ class BaseMapData(ABC): next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance() mapd_sp_send = messaging.new_message('liveMapDataSP') - mapd_sp_send.valid = self.sm.all_checks(['livePose']) + mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK live_map_data = mapd_sp_send.liveMapDataSP live_map_data.speedLimitValid = bool(speed_limit > 0) diff --git a/sunnypilot/mapd/live_map_data/osm_map_data.py b/sunnypilot/mapd/live_map_data/osm_map_data.py index c5187a8e04..0662e2d589 100644 --- a/sunnypilot/mapd/live_map_data/osm_map_data.py +++ b/sunnypilot/mapd/live_map_data/osm_map_data.py @@ -8,8 +8,8 @@ import json import math import platform +from cereal import log from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData from openpilot.sunnypilot.navd.helpers import Coordinate @@ -20,15 +20,12 @@ class OsmMapData(BaseMapData): self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params def update_location(self) -> None: - gps = self.sm[self.gps_location_service] - gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_MDL < 2.0 + location = self.sm['liveLocationKalman'] + self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid - if gps_ok: - self.last_bearing = gps.bearingDeg * 180/math.pi - self.last_position = Coordinate(gps.latitude, gps.longitude) - - if not gps_ok and self.sm['livePose'].inputsOK: - return + if self.localizer_valid: + self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) + self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) if self.last_position is None: return @@ -36,9 +33,11 @@ class OsmMapData(BaseMapData): params = { "latitude": self.last_position.latitude, "longitude": self.last_position.longitude, - "bearing": self.last_bearing, } + if self.last_bearing is not None: + params['bearing'] = self.last_bearing + self.mem_params.put("LastGPSPosition", json.dumps(params)) def get_current_speed_limit(self) -> float: diff --git a/sunnypilot/selfdrive/locationd/.gitignore b/sunnypilot/selfdrive/locationd/.gitignore new file mode 100644 index 0000000000..11b9f127b2 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/.gitignore @@ -0,0 +1,3 @@ +params_learner +paramsd +locationd diff --git a/sunnypilot/selfdrive/locationd/SConscript b/sunnypilot/selfdrive/locationd/SConscript new file mode 100644 index 0000000000..27cd4d5b40 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/SConscript @@ -0,0 +1,37 @@ +Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations') + +loc_libs = [messaging, common, 'pthread', 'dl'] + +# build ekf models +rednose_gen_dir = 'models/generated' +rednose_gen_deps = [ + "models/constants.py", +] +live_ekf = env.RednoseCompileFilter( + target='live', + filter_gen_script='models/live_kf.py', + output_dir=rednose_gen_dir, + extra_gen_artifacts=['live_kf_constants.h'], + gen_script_deps=rednose_gen_deps, +) +car_ekf = env.RednoseCompileFilter( + target='car', + filter_gen_script='models/car_kf.py', + output_dir=rednose_gen_dir, + extra_gen_artifacts=[], + gen_script_deps=rednose_gen_deps, +) + +# locationd build +locationd_sources = ["locationd.cc", "models/live_kf.cc"] + +lenv = env.Clone() +# ekf filter libraries need to be linked, even if no symbols are used +if arch != "Darwin": + lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"] + +lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath) +lenv["RPATH"].append(Dir(rednose_gen_dir).abspath) +locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations) +lenv.Depends(locationd, rednose) +lenv.Depends(locationd, live_ekf) diff --git a/sunnypilot/selfdrive/locationd/__init__.py b/sunnypilot/selfdrive/locationd/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/locationd/locationd.cc b/sunnypilot/selfdrive/locationd/locationd.cc new file mode 100644 index 0000000000..c42ab13f25 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/locationd.cc @@ -0,0 +1,750 @@ +#include "sunnypilot/selfdrive/locationd/locationd.h" + +#include +#include + +#include +#include +#include + +using namespace EKFS; +using namespace Eigen; + +ExitHandler do_exit; +const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 +const double ROTATION_SANITY_CHECK = 10.0; // rad/s +const double TRANS_SANITY_CHECK = 200.0; // m/s +const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) +const double ALTITUDE_SANITY_CHECK = 10000; // m +const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad +const double VALID_TIME_SINCE_RESET = 1.0; // s +const double VALID_POS_STD = 50.0; // m +const double MAX_RESET_TRACKER = 5.0; +const double SANE_GPS_UNCERTAINTY = 1500.0; // m +const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker +const double RESET_TRACKER_DECAY = 0.99995; +const double DECAY = 0.9993; // ~10 secs to resume after a bad input +const double MAX_FILTER_REWIND_TIME = 0.8; // s +const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30; + +// TODO: GPS sensor time offsets are empirically calculated +// They should be replaced with synced time from a real clock +const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s +const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s +const float GPS_POS_STD_THRESHOLD = 50.0; +const float GPS_VEL_STD_THRESHOLD = 5.0; +const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; +const float GPS_POS_STD_RESET_THRESHOLD = 2.0; +const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; +const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; +const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; + +const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0"; + +static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { + VectorXd res(floatlist.size()); + for (int i = 0; i < floatlist.size(); i++) { + res[i] = floatlist[i]; + } + return res; +} + +static Vector4d quat2vector(const Quaterniond& quat) { + return Vector4d(quat.w(), quat.x(), quat.y(), quat.z()); +} + +static Quaterniond vector2quat(const VectorXd& vec) { + return Quaterniond(vec(0), vec(1), vec(2), vec(3)); +} + +static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { + meas.setValue(kj::arrayPtr(val.data(), val.size())); + meas.setStd(kj::arrayPtr(std.data(), std.size())); + meas.setValid(valid); +} + + +static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) { + // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix + return ((rot_matrix * cov_in) * rot_matrix.transpose()); +} + +static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) { + // Stds cannot be rotated like values, only covariances can be rotated + return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); +} + +Localizer::Localizer(LocalizerGnssSource gnss_source) { + this->kf = std::make_unique(); + this->reset_kalman(); + + this->calib = Vector3d(0.0, 0.0, 0.0); + this->device_from_calib = MatrixXdr::Identity(3, 3); + this->calib_from_device = MatrixXdr::Identity(3, 3); + + for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) { + this->posenet_stds.push_back(10.0); + } + + VectorXd ecef_pos = this->kf->get_x().segment(STATE_ECEF_POS_START); + this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + this->configure_gnss_source(gnss_source); +} + +void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { + VectorXd predicted_state = this->kf->get_x(); + MatrixXdr predicted_cov = this->kf->get_P(); + VectorXd predicted_std = predicted_cov.diagonal().array().sqrt(); + + VectorXd fix_ecef = predicted_state.segment(STATE_ECEF_POS_START); + ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; + VectorXd fix_ecef_std = predicted_std.segment(STATE_ECEF_POS_ERR_START); + VectorXd vel_ecef = predicted_state.segment(STATE_ECEF_VELOCITY_START); + VectorXd vel_ecef_std = predicted_std.segment(STATE_ECEF_VELOCITY_ERR_START); + VectorXd fix_pos_geo_vec = this->get_position_geodetic(); + VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); + MatrixXdr orientation_ecef_cov = predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose(); + VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose()); + + VectorXd acc_calib = this->calib_from_device * predicted_state.segment(STATE_ACCELERATION_START); + MatrixXdr acc_calib_cov = predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START); + VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt(); + VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment(STATE_ANGULAR_VELOCITY_START); + + MatrixXdr vel_angular_cov = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); + VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt(); + + VectorXd vel_device = device_from_ecef * vel_ecef; + VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); + MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); + condensed_cov.topLeftCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + condensed_cov.topRightCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomRightCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomLeftCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size()); + H_input << device_from_ecef_eul, vel_ecef; + MatrixXdr HH = this->kf->H(H_input); + MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose(); + VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt(); + + VectorXd vel_calib = this->calib_from_device * vel_device; + VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); + + VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); + VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt(); + VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef); + VectorXd nextfix_ecef = fix_ecef + vel_ecef; + VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector(); + + VectorXd accDevice = predicted_state.segment(STATE_ACCELERATION_START); + VectorXd accDeviceErr = predicted_std.segment(STATE_ACCELERATION_ERR_START); + + VectorXd angVelocityDevice = predicted_state.segment(STATE_ANGULAR_VELOCITY_START); + VectorXd angVelocityDeviceErr = predicted_std.segment(STATE_ANGULAR_VELOCITY_ERR_START); + + Vector3d nans = Vector3d(NAN, NAN, NAN); + + // TODO fill in NED and Calibrated stds + // write measurements to msg + init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode); + init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode); + init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode); + init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode); + init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); + init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); + init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode); + init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode); + init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); + init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode); + init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); + init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); + init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated); + init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated); + if (DEBUG) { + init_measurement(fix.initFilterState(), predicted_state, predicted_std, true); + } + + double old_mean = 0.0, new_mean = 0.0; + int i = 0; + for (double x : this->posenet_stds) { + if (i < POSENET_STD_HIST_HALF) { + old_mean += x; + } else { + new_mean += x; + } + i++; + } + old_mean /= POSENET_STD_HIST_HALF; + new_mean /= POSENET_STD_HIST_HALF; + // experimentally found these values, no false positives in 20k minutes of driving + bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0); + + fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); + fix.setDeviceStable(!this->device_fell); + fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER); + fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff); + this->device_fell = false; + + //fix.setGpsWeek(this->time.week); + //fix.setGpsTimeOfWeek(this->time.tow); + fix.setUnixTimestampMillis(this->unix_timestamp_millis); + + double time_since_reset = this->kf->get_filter_time() - this->last_reset_time; + fix.setTimeSinceReset(time_since_reset); + if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) { + fix.setStatus(cereal::LiveLocationKalman::Status::VALID); + } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) { + fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED); + } else { + fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED); + } +} + +VectorXd Localizer::get_position_geodetic() { + VectorXd fix_ecef = this->kf->get_x().segment(STATE_ECEF_POS_START); + ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; + Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef); + return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt); +} + +VectorXd Localizer::get_state() { + return this->kf->get_x(); +} + +VectorXd Localizer::get_stdev() { + return this->kf->get_P().diagonal().array().sqrt(); +} + +bool Localizer::are_inputs_ok() { + return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid; +} + +void Localizer::observation_timings_invalid_reset(){ + this->observation_timings_invalid = false; +} + +void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) { + // TODO does not yet account for double sensor readings in the log + + // Ignore empty readings (e.g. in case the magnetometer had no data ready) + if (log.getTimestamp() == 0) { + return; + } + + double sensor_time = 1e-9 * log.getTimestamp(); + + // sensor time and log time should be close + if (std::abs(current_time - sensor_time) > 0.1) { + LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); + this->observation_timings_invalid = true; + return; + } else if (!this->is_timestamp_valid(sensor_time)) { + this->observation_timings_invalid = true; + return; + } + + // TODO: handle messages from two IMUs at the same time + if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) { + return; + } + + // Gyro Uncalibrated + if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { + auto v = log.getGyroUncalibrated().getV(); + auto meas = Vector3d(-v[2], -v[1], -v[0]); + + VectorXd gyro_bias = this->kf->get_x().segment(STATE_GYRO_BIAS_START); + float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]); + float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1]; + bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold; + + if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); + this->observation_values_invalid["gyroscope"] *= DECAY; + } else { + this->observation_values_invalid["gyroscope"] += 1.0; + } + } + + // Accelerometer + if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) { + auto v = log.getAcceleration().getV(); + + // TODO: reduce false positives and re-enable this check + // check if device fell, estimate 10 for g + // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving + // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; + + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ACCEL_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); + this->observation_values_invalid["accelerometer"] *= DECAY; + } else { + this->observation_values_invalid["accelerometer"] += 1.0; + } + } +} + +void Localizer::input_fake_gps_observations(double current_time) { + // This is done to make sure that the error estimate of the position does not blow up + // when the filter is in no-gps mode + // Steps : first predict -> observe current obs with reasonable STD + this->kf->predict(current_time); + + VectorXd current_x = this->kf->get_x(); + VectorXd ecef_pos = current_x.segment(STATE_ECEF_POS_START); + VectorXd ecef_vel = current_x.segment(STATE_ECEF_VELOCITY_START); + const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov(); + const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov(); + + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { + bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); + bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); + bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); + bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); + + if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { + //this->gps_valid = false; + this->determine_gps_mode(current_time); + return; + } + + double sensor_time = current_time - sensor_time_offset; + + // Process message + //this->gps_valid = true; + this->gps_mode = true; + Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; + this->converter = std::make_unique(geodetic); + + VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); + VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; + float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2)); + MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal(); + MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal(); + + this->unix_timestamp_millis = log.getUnixTimestampMillis(); + double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); + + VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg())); + VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; + for (int i = 0; i < orientation_error.size(); i++) { + orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); + if (orientation_error(i) < 0.0) { + orientation_error(i) += 2.0 * M_PI; + } + orientation_error(i) -= M_PI; + } + VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); + + if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { + LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + } else if (gps_est_error > 100.0) { + LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + } + + this->last_gps_msg = sensor_time; + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { + + if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) { + this->determine_gps_mode(current_time); + return; + } + + double sensor_time = log.getMeasTime() * 1e-9; + sensor_time -= this->gps_time_offset; + + auto ecef_pos_v = log.getPositionECEF().getValue(); + VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]); + + // indexed at 0 cause all std values are the same MAE + auto ecef_pos_std = log.getPositionECEF().getStd()[0]; + MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal(); + + auto ecef_vel_v = log.getVelocityECEF().getValue(); + VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); + + // indexed at 0 cause all std values are the same MAE + auto ecef_vel_std = log.getVelocityECEF().getStd()[0]; + MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal(); + + double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); + + VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef); + + LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]}; + VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector(); + double bearing_rad = atan2(ned_vel[1], ned_vel[0]); + + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad); + VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; + for (int i = 0; i < orientation_error.size(); i++) { + orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); + if (orientation_error(i) < 0.0) { + orientation_error(i) += 2.0 * M_PI; + } + orientation_error(i) -= M_PI; + } + VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); + + if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { + this->determine_gps_mode(current_time); + return; + } + + // prevent jumping gnss measurements (covered lots, standstill...) + bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; + orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; + orientation_reset &= !this->standstill; + if (orientation_reset) { + this->orientation_reset_count++; + } else { + this->orientation_reset_count = 0; + } + + if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { + // always reset on first gps message and if the location is off but the accuracy is high + LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) { + LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + this->orientation_reset_count = 0; + } + + this->gps_mode = true; + this->last_gps_msg = sensor_time; + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { + this->car_speed = std::abs(log.getVEgo()); + this->standstill = log.getStandstill(); + if (this->standstill) { + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) }); + } +} + +void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { + VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); + VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } + + if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + VectorXd rot_calib_std = floatlist2vector(log.getRotStd()); + VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); + + if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + this->posenet_stds.pop_front(); + this->posenet_stds.push_back(trans_calib_std[0]); + + // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise + trans_calib_std *= 10.0; + rot_calib_std *= 10.0; + MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal(); + MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal(); + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, + { rot_device }, { rot_device_cov }); + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, + { trans_device }, { trans_device_cov }); + this->observation_values_invalid["cameraOdometry"] *= DECAY; + this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]); +} + +void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } + + if (log.getRpyCalib().size() > 0) { + auto live_calib = floatlist2vector(log.getRpyCalib()); + if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) { + this->observation_values_invalid["liveCalibration"] += 1.0; + return; + } + + this->calib = live_calib; + this->device_from_calib = euler2rot(this->calib); + this->calib_from_device = this->device_from_calib.transpose(); + this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; + this->observation_values_invalid["liveCalibration"] *= DECAY; + } +} + +void Localizer::reset_kalman(double current_time) { + const VectorXd &init_x = this->kf->get_initial_x(); + const MatrixXdr &init_P = this->kf->get_initial_P(); + this->reset_kalman(current_time, init_x, init_P); +} + +void Localizer::finite_check(double current_time) { + bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all(); + if (!all_finite) { + LOGE("Non-finite values detected, kalman reset"); + this->reset_kalman(current_time); + } +} + +void Localizer::time_check(double current_time) { + if (std::isnan(this->last_reset_time)) { + this->last_reset_time = current_time; + } + if (std::isnan(this->first_valid_log_time)) { + this->first_valid_log_time = current_time; + } + double filter_time = this->kf->get_filter_time(); + bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10); + if (big_time_gap) { + LOGE("Time gap of over 10s detected, kalman reset"); + this->reset_kalman(current_time); + } +} + +void Localizer::update_reset_tracker() { + // reset tracker is tuned to trigger when over 1reset/10s over 2min period + if (this->is_gps_ok()) { + this->reset_tracker *= RESET_TRACKER_DECAY; + } else { + this->reset_tracker = 0.0; + } +} + +void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) { + // too nonlinear to init on completely wrong + VectorXd current_x = this->kf->get_x(); + MatrixXdr current_P = this->kf->get_P(); + MatrixXdr init_P = this->kf->get_initial_P(); + const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P(); + int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); + + current_x.segment(STATE_ECEF_ORIENTATION_START) = init_orient; + current_x.segment(STATE_ECEF_VELOCITY_START) = init_vel; + current_x.segment(STATE_ECEF_POS_START) = init_pos; + + init_P.block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal(); + init_P.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal(); + init_P.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal(); + init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, + STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal(); + + this->reset_kalman(current_time, current_x, init_P); +} + +void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) { + this->kf->init_state(init_x, init_P, current_time); + this->last_reset_time = current_time; + this->reset_tracker += 1.0; +} + +void Localizer::handle_msg_bytes(const char *data, const size_t size) { + AlignedBuffer aligned_buf; + + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size)); + cereal::Event::Reader event = cmsg.getRoot(); + + this->handle_msg(event); +} + +void Localizer::handle_msg(const cereal::Event::Reader& log) { + double t = log.getLogMonoTime() * 1e-9; + this->time_check(t); + if (log.isAccelerometer()) { + this->handle_sensor(t, log.getAccelerometer()); + } else if (log.isGyroscope()) { + this->handle_sensor(t, log.getGyroscope()); + } else if (log.isGpsLocation()) { + this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); + //} else if (log.isGnssMeasurements()) { + // this->handle_gnss(t, log.getGnssMeasurements()); + } else if (log.isCarState()) { + this->handle_car_state(t, log.getCarState()); + } else if (log.isCameraOdometry()) { + this->handle_cam_odo(t, log.getCameraOdometry()); + } else if (log.isLiveCalibration()) { + this->handle_live_calib(t, log.getLiveCalibration()); + } + this->finite_check(); + this->update_reset_tracker(); +} + +kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK, + bool sensorsOK, bool gpsOK, bool msgValid) { + cereal::Event::Builder evt = msg_builder.initEvent(); + evt.setValid(msgValid); + cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); + this->build_live_location(liveLoc); + liveLoc.setSensorsOK(sensorsOK); + liveLoc.setGpsOK(gpsOK); + liveLoc.setInputsOK(inputsOK); + return msg_builder.toBytes(); +} + +bool Localizer::is_gps_ok() { + return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0; +} + +bool Localizer::critical_services_valid(const std::map &critical_services) { + for (auto &kv : critical_services){ + if (kv.second >= INPUT_INVALID_THRESHOLD){ + return false; + } + } + return true; +} + +bool Localizer::is_timestamp_valid(double current_time) { + double filter_time = this->kf->get_filter_time(); + if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { + LOGE("Observation timestamp is older than the max rewind threshold of the filter"); + return false; + } + return true; +} + +void Localizer::determine_gps_mode(double current_time) { + // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode + // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs + // 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is + VectorXd current_pos_std = this->kf->get_P().block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt(); + if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){ + if (this->gps_mode){ + this->gps_mode = false; + this->reset_kalman(current_time); + } else { + this->input_fake_gps_observations(current_time); + } + } +} + +void Localizer::configure_gnss_source(const LocalizerGnssSource &source) { + this->gnss_source = source; + if (source == LocalizerGnssSource::UBLOX) { + this->gps_std_factor = 10.0; + this->gps_variance_factor = 1.0; + this->gps_vertical_variance_factor = 1.0; + this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET; + } else { + this->gps_std_factor = 2.0; + this->gps_variance_factor = 0.0; + this->gps_vertical_variance_factor = 3.0; + this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET; + } +} + +int Localizer::locationd_thread() { + Params params; + LocalizerGnssSource source; + const char* gps_location_socket; + if (params.getBool("UbloxAvailable")) { + source = LocalizerGnssSource::UBLOX; + gps_location_socket = "gpsLocationExternal"; + } else { + source = LocalizerGnssSource::QCOM; + gps_location_socket = "gpsLocation"; + } + + this->configure_gnss_source(source); + const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + "carState", "accelerometer", "gyroscope"}; + + SubMaster sm(service_list, {}, nullptr, {gps_location_socket}); + PubMaster pm({"liveLocationKalman"}); + + uint64_t cnt = 0; + bool filterInitialized = false; + const std::vector critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"}; + for (std::string service : critical_input_services) { + this->observation_values_invalid.insert({service, 0.0}); + } + + while (!do_exit) { + sm.update(); + if (filterInitialized){ + this->observation_timings_invalid_reset(); + for (const char* service : service_list) { + if (sm.updated(service) && sm.valid(service)){ + const cereal::Event::Reader log = sm[service]; + this->handle_msg(log); + } + } + } else { + filterInitialized = sm.allAliveAndValid(); + } + + const char* trigger_msg = "cameraOdometry"; + if (sm.updated(trigger_msg)) { + bool inputsOK = sm.allValid() && this->are_inputs_ok(); + bool gpsOK = this->is_gps_ok(); + bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); + + // Log time to first fix + if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) { + this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); + } + + MessageBuilder msg_builder; + kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); + pm.send("liveLocationKalman", bytes.begin(), bytes.size()); + + if (cnt % 1200 == 0 && gpsOK) { // once a minute + VectorXd posGeo = this->get_position_geodetic(); + std::string lastGPSPosJSON = util::string_format( + "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); + params.putNonBlocking("LastGPSPositionLLK", lastGPSPosJSON); + } + cnt++; + } + } + return 0; +} + +int main() { + util::set_realtime_priority(5); + + Localizer localizer; + return localizer.locationd_thread(); +} diff --git a/sunnypilot/selfdrive/locationd/locationd.h b/sunnypilot/selfdrive/locationd/locationd.h new file mode 100644 index 0000000000..4d04492102 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/locationd.h @@ -0,0 +1,100 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/transformations/coordinates.hpp" +#include "common/transformations/orientation.hpp" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +#include "sunnypilot/system/sensord/sensors/constants.h" +#define VISION_DECIMATION 2 +#define SENSOR_DECIMATION 10 +#include "sunnypilot/selfdrive/locationd/models/live_kf.h" + +#define POSENET_STD_HIST_HALF 20 + +enum LocalizerGnssSource { + UBLOX, QCOM +}; + +class Localizer { +public: + Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); + + int locationd_thread(); + + void reset_kalman(double current_time = NAN); + void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R); + void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P); + void finite_check(double current_time = NAN); + void time_check(double current_time = NAN); + void update_reset_tracker(); + bool is_gps_ok(); + bool critical_services_valid(const std::map &critical_services); + bool is_timestamp_valid(double current_time); + void determine_gps_mode(double current_time); + bool are_inputs_ok(); + void observation_timings_invalid_reset(); + + kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); + void build_live_location(cereal::LiveLocationKalman::Builder& fix); + + Eigen::VectorXd get_position_geodetic(); + Eigen::VectorXd get_state(); + Eigen::VectorXd get_stdev(); + + void handle_msg_bytes(const char *data, const size_t size); + void handle_msg(const cereal::Event::Reader& log); + void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); + void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); + void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log); + void handle_car_state(double current_time, const cereal::CarState::Reader& log); + void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); + void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); + + void input_fake_gps_observations(double current_time); + +private: + std::unique_ptr kf; + + Eigen::VectorXd calib; + MatrixXdr device_from_calib; + MatrixXdr calib_from_device; + bool calibrated = false; + + double car_speed = 0.0; + double last_reset_time = NAN; + std::deque posenet_stds; + + std::unique_ptr converter; + + int64_t unix_timestamp_millis = 0; + double reset_tracker = 0.0; + bool device_fell = false; + bool gps_mode = false; + double first_valid_log_time = NAN; + double ttff = NAN; + double last_gps_msg = 0; + LocalizerGnssSource gnss_source; + bool observation_timings_invalid = false; + std::map observation_values_invalid; + bool standstill = true; + int32_t orientation_reset_count = 0; + float gps_std_factor; + float gps_variance_factor; + float gps_vertical_variance_factor; + double gps_time_offset; + Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std + + void configure_gnss_source(const LocalizerGnssSource &source); +}; diff --git a/sunnypilot/selfdrive/locationd/models/.gitignore b/sunnypilot/selfdrive/locationd/models/.gitignore new file mode 100644 index 0000000000..9ab870da89 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/.gitignore @@ -0,0 +1 @@ +generated/ diff --git a/sunnypilot/selfdrive/locationd/models/__init__.py b/sunnypilot/selfdrive/locationd/models/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/locationd/models/car_kf.py b/sunnypilot/selfdrive/locationd/models/car_kf.py new file mode 100755 index 0000000000..9964cab973 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/car_kf.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 +import math +import sys +from typing import Any + +import numpy as np + +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind +from openpilot.common.swaglog import cloudlog + +from rednose.helpers.kalmanfilter import KalmanFilter + +if __name__ == '__main__': # Generating sympy + import sympy as sp + from rednose.helpers.ekf_sym import gen_code +else: + from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx + + +i = 0 + +def _slice(n): + global i + s = slice(i, i + n) + i += n + + return s + + +class States: + # Vehicle model params + STIFFNESS = _slice(1) # [-] + STEER_RATIO = _slice(1) # [-] + ANGLE_OFFSET = _slice(1) # [rad] + ANGLE_OFFSET_FAST = _slice(1) # [rad] + + VELOCITY = _slice(2) # (x, y) [m/s] + YAW_RATE = _slice(1) # [rad/s] + STEER_ANGLE = _slice(1) # [rad] + ROAD_ROLL = _slice(1) # [rad] + + +class CarKalman(KalmanFilter): + name = 'car' + + initial_x = np.array([ + 1.0, + 15.0, + 0.0, + 0.0, + + 10.0, 0.0, + 0.0, + 0.0, + 0.0 + ]) + + # process noise + Q = np.diag([ + (.05 / 100)**2, + .01**2, + math.radians(0.02)**2, + math.radians(0.25)**2, + + .1**2, .01**2, + math.radians(0.1)**2, + math.radians(0.1)**2, + math.radians(1)**2, + ]) + P_initial = Q.copy() + + obs_noise: dict[int, Any] = { + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2), + ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2), + ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2), + ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2), + ObservationKind.STIFFNESS: np.atleast_2d(0.5**2), + ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2), + } + + global_vars = [ + 'mass', + 'rotational_inertia', + 'center_to_front', + 'center_to_rear', + 'stiffness_front', + 'stiffness_rear', + ] + + @staticmethod + def generate_code(generated_dir): + dim_state = CarKalman.initial_x.shape[0] + name = CarKalman.name + + # Linearized single-track lateral dynamics, equations 7.211-7.213 + # Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars + # Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6 + + # globals + global_vars = [sp.Symbol(name) for name in CarKalman.global_vars] + m, j, aF, aR, cF_orig, cR_orig = global_vars + + # make functions and jacobians with sympy + # state variables + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + + # Vehicle model constants + sf = state[States.STIFFNESS, :][0, 0] + + cF, cR = sf * cF_orig, sf * cR_orig + angle_offset = state[States.ANGLE_OFFSET, :][0, 0] + angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0] + theta = state[States.ROAD_ROLL, :][0, 0] + sa = state[States.STEER_ANGLE, :][0, 0] + + sR = state[States.STEER_RATIO, :][0, 0] + u, v = state[States.VELOCITY, :] + r = state[States.YAW_RATE, :][0, 0] + + A = sp.Matrix(np.zeros((2, 2))) + A[0, 0] = -(cF + cR) / (m * u) + A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u + A[1, 0] = -(cF * aF - cR * aR) / (j * u) + A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u) + + B = sp.Matrix(np.zeros((2, 1))) + B[0, 0] = cF / m / sR + B[1, 0] = (cF * aF) / j / sR + + C = sp.Matrix(np.zeros((2, 1))) + C[0, 0] = ACCELERATION_DUE_TO_GRAVITY + C[1, 0] = 0 + + x = sp.Matrix([v, r]) # lateral velocity, yaw rate + x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta + + dt = sp.Symbol('dt') + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.VELOCITY.start + 1, 0] = x_dot[0] + state_dot[States.YAW_RATE.start, 0] = x_dot[1] + + # Basic descretization, 1st order integrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + # + # Observation functions + # + obs_eqs = [ + [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None], + [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None], + [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None], + [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None], + [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None], + [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None], + [sp.Matrix([sf]), ObservationKind.STIFFNESS, None], + [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None], + ] + + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars) + + def __init__(self, generated_dir): + dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0] + self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial, + dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog) + + def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear): + self.filter.set_global("mass", mass) + self.filter.set_global("rotational_inertia", rotational_inertia) + self.filter.set_global("center_to_front", center_to_front) + self.filter.set_global("center_to_rear", center_to_rear) + self.filter.set_global("stiffness_front", stiffness_front) + self.filter.set_global("stiffness_rear", stiffness_rear) + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + CarKalman.generate_code(generated_dir) diff --git a/sunnypilot/selfdrive/locationd/models/constants.py b/sunnypilot/selfdrive/locationd/models/constants.py new file mode 100644 index 0000000000..6d328ce6f5 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/constants.py @@ -0,0 +1,92 @@ +import os + +GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated')) + +class ObservationKind: + UNKNOWN = 0 + NO_OBSERVATION = 1 + GPS_NED = 2 + ODOMETRIC_SPEED = 3 + PHONE_GYRO = 4 + GPS_VEL = 5 + PSEUDORANGE_GPS = 6 + PSEUDORANGE_RATE_GPS = 7 + SPEED = 8 + NO_ROT = 9 + PHONE_ACCEL = 10 + ORB_POINT = 11 + ECEF_POS = 12 + CAMERA_ODO_TRANSLATION = 13 + CAMERA_ODO_ROTATION = 14 + ORB_FEATURES = 15 + MSCKF_TEST = 16 + FEATURE_TRACK_TEST = 17 + LANE_PT = 18 + IMU_FRAME = 19 + PSEUDORANGE_GLONASS = 20 + PSEUDORANGE_RATE_GLONASS = 21 + PSEUDORANGE = 22 + PSEUDORANGE_RATE = 23 + ECEF_VEL = 35 + ECEF_ORIENTATION_FROM_GPS = 32 + NO_ACCEL = 33 + ORB_FEATURES_WIDE = 34 + + ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s] + ROAD_FRAME_YAW_RATE = 25 # [rad/s] + STEER_ANGLE = 26 # [rad] + ANGLE_OFFSET_FAST = 27 # [rad] + STIFFNESS = 28 # [-] + STEER_RATIO = 29 # [-] + ROAD_FRAME_X_SPEED = 30 # (x) [m/s] + ROAD_ROLL = 31 # [rad] + + names = [ + 'Unknown', + 'No observation', + 'GPS NED', + 'Odometric speed', + 'Phone gyro', + 'GPS velocity', + 'GPS pseudorange', + 'GPS pseudorange rate', + 'Speed', + 'No rotation', + 'Phone acceleration', + 'ORB point', + 'ECEF pos', + 'camera odometric translation', + 'camera odometric rotation', + 'ORB features', + 'MSCKF test', + 'Feature track test', + 'Lane ecef point', + 'imu frame eulers', + 'GLONASS pseudorange', + 'GLONASS pseudorange rate', + 'pseudorange', + 'pseudorange rate', + + 'Road Frame x,y speed', + 'Road Frame yaw rate', + 'Steer Angle', + 'Fast Angle Offset', + 'Stiffness', + 'Steer Ratio', + 'Road Frame x speed', + 'Road Roll', + 'ECEF orientation from GPS', + 'NO accel', + 'ORB features wide camera', + 'ECEF_VEL', + ] + + @classmethod + def to_string(cls, kind): + return cls.names[kind] + + +SAT_OBS = [ObservationKind.PSEUDORANGE_GPS, + ObservationKind.PSEUDORANGE_RATE_GPS, + ObservationKind.PSEUDORANGE_GLONASS, + ObservationKind.PSEUDORANGE_RATE_GLONASS] diff --git a/sunnypilot/selfdrive/locationd/models/live_kf.cc b/sunnypilot/selfdrive/locationd/models/live_kf.cc new file mode 100644 index 0000000000..7ef6be638e --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/live_kf.cc @@ -0,0 +1,122 @@ +#include "sunnypilot/selfdrive/locationd/models/live_kf.h" + +using namespace EKFS; +using namespace Eigen; + +Eigen::Map get_mapvec(const Eigen::VectorXd &vec) { + return Eigen::Map((double*)vec.data(), vec.rows(), vec.cols()); +} + +Eigen::Map get_mapmat(const MatrixXdr &mat) { + return Eigen::Map((double*)mat.data(), mat.rows(), mat.cols()); +} + +std::vector> get_vec_mapvec(const std::vector &vec_vec) { + std::vector> res; + for (const Eigen::VectorXd &vec : vec_vec) { + res.push_back(get_mapvec(vec)); + } + return res; +} + +std::vector> get_vec_mapmat(const std::vector &mat_vec) { + std::vector> res; + for (const MatrixXdr &mat : mat_vec) { + res.push_back(get_mapmat(mat)); + } + return res; +} + +LiveKalman::LiveKalman() { + this->dim_state = live_initial_x.rows(); + this->dim_state_err = live_initial_P_diag.rows(); + + this->initial_x = live_initial_x; + this->initial_P = live_initial_P_diag.asDiagonal(); + this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal(); + this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal(); + this->reset_orientation_P = live_reset_orientation_diag.asDiagonal(); + this->Q = live_Q_diag.asDiagonal(); + for (auto& pair : live_obs_noise_diag) { + this->obs_noise[pair.first] = pair.second.asDiagonal(); + } + + // init filter + this->filter = std::make_shared(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), + get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector(), + std::vector{3}, std::vector(), 0.8); +} + +void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) { + MatrixXdr covs = covs_diag.asDiagonal(); + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) { + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(const VectorXd &state, double filter_time) { + MatrixXdr covs = this->filter->covs(); + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +VectorXd LiveKalman::get_x() { + return this->filter->state(); +} + +MatrixXdr LiveKalman::get_P() { + return this->filter->covs(); +} + +double LiveKalman::get_filter_time() { + return this->filter->get_filter_time(); +} + +std::vector LiveKalman::get_R(int kind, int n) { + std::vector R; + for (int i = 0; i < n; i++) { + R.push_back(this->obs_noise[kind]); + } + return R; +} + +std::optional LiveKalman::predict_and_observe(double t, int kind, const std::vector &meas, std::vector R) { + std::optional r; + if (R.size() == 0) { + R = this->get_R(kind, meas.size()); + } + r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R)); + return r; +} + +void LiveKalman::predict(double t) { + this->filter->predict(t); +} + +const Eigen::VectorXd &LiveKalman::get_initial_x() { + return this->initial_x; +} + +const MatrixXdr &LiveKalman::get_initial_P() { + return this->initial_P; +} + +const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() { + return this->fake_gps_pos_cov; +} + +const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() { + return this->fake_gps_vel_cov; +} + +const MatrixXdr &LiveKalman::get_reset_orientation_P() { + return this->reset_orientation_P; +} + +MatrixXdr LiveKalman::H(const VectorXd &in) { + assert(in.size() == 6); + Matrix res; + this->filter->get_extra_routine("H")((double*)in.data(), res.data()); + return res; +} diff --git a/sunnypilot/selfdrive/locationd/models/live_kf.h b/sunnypilot/selfdrive/locationd/models/live_kf.h new file mode 100644 index 0000000000..e4b3e326b3 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/live_kf.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include "generated/live_kf_constants.h" +#include "rednose/helpers/ekf_sym.h" + +#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth) + +using namespace EKFS; + +Eigen::Map get_mapvec(const Eigen::VectorXd &vec); +Eigen::Map get_mapmat(const MatrixXdr &mat); +std::vector> get_vec_mapvec(const std::vector &vec_vec); +std::vector> get_vec_mapmat(const std::vector &mat_vec); + +class LiveKalman { +public: + LiveKalman(); + + void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time); + void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time); + void init_state(const Eigen::VectorXd &state, double filter_time); + + Eigen::VectorXd get_x(); + MatrixXdr get_P(); + double get_filter_time(); + std::vector get_R(int kind, int n); + + std::optional predict_and_observe(double t, int kind, const std::vector &meas, std::vector R = {}); + std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind); + std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind); + std::optional predict_and_update_odo_rot(std::vector rot, double t, int kind); + void predict(double t); + + const Eigen::VectorXd &get_initial_x(); + const MatrixXdr &get_initial_P(); + const MatrixXdr &get_fake_gps_pos_cov(); + const MatrixXdr &get_fake_gps_vel_cov(); + const MatrixXdr &get_reset_orientation_P(); + + MatrixXdr H(const Eigen::VectorXd &in); + +private: + std::string name = "live"; + + std::shared_ptr filter; + + int dim_state; + int dim_state_err; + + Eigen::VectorXd initial_x; + MatrixXdr initial_P; + MatrixXdr fake_gps_pos_cov; + MatrixXdr fake_gps_vel_cov; + MatrixXdr reset_orientation_P; + MatrixXdr Q; // process noise + std::unordered_map obs_noise; +}; diff --git a/sunnypilot/selfdrive/locationd/models/live_kf.py b/sunnypilot/selfdrive/locationd/models/live_kf.py new file mode 100755 index 0000000000..e5626b8f28 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/live_kf.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 + +import sys +import os +import numpy as np + +from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind + +import sympy as sp +import inspect +from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate +from rednose.helpers.ekf_sym import gen_code + +EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) + + +def numpy2eigenstring(arr): + assert(len(arr.shape) == 1) + arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '') + return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()" + + +class States: + ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters + ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef + ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s + ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s + GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases + ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2 + ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2 + + # Error-state has different slices because it is an ESKF + ECEF_POS_ERR = slice(0, 3) + ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error + ECEF_VELOCITY_ERR = slice(6, 9) + ANGULAR_VELOCITY_ERR = slice(9, 12) + GYRO_BIAS_ERR = slice(12, 15) + ACCELERATION_ERR = slice(15, 18) + ACC_BIAS_ERR = slice(18, 21) + + +class LiveKalman: + name = 'live' + + initial_x = np.array([3.88e6, -3.37e6, 3.76e6, + 0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0]) + + # state covariance + initial_P_diag = np.array([10**2, 10**2, 10**2, + 0.01**2, 0.01**2, 0.01**2, + 10**2, 10**2, 10**2, + 1**2, 1**2, 1**2, + 1**2, 1**2, 1**2, + 100**2, 100**2, 100**2, + 0.01**2, 0.01**2, 0.01**2]) + + # state covariance when resetting midway in a segment + reset_orientation_diag = np.array([1**2, 1**2, 1**2]) + + # fake observation covariance, to ensure the uncertainty estimate of the filter is under control + fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2]) + fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2]) + + # process noise + Q_diag = np.array([0.03**2, 0.03**2, 0.03**2, + 0.001**2, 0.001**2, 0.001**2, + 0.01**2, 0.01**2, 0.01**2, + 0.1**2, 0.1**2, 0.1**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + 3**2, 3**2, 3**2, + 0.005**2, 0.005**2, 0.005**2]) + + obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), + ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), + ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]), + ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]), + ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]), + ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])} + + @staticmethod + def generate_code(generated_dir): + name = LiveKalman.name + dim_state = LiveKalman.initial_x.shape[0] + dim_state_err = LiveKalman.initial_P_diag.shape[0] + + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + x, y, z = state[States.ECEF_POS, :] + q = state[States.ECEF_ORIENTATION, :] + v = state[States.ECEF_VELOCITY, :] + vx, vy, vz = v + omega = state[States.ANGULAR_VELOCITY, :] + vroll, vpitch, vyaw = omega + roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] + acceleration = state[States.ACCELERATION, :] + acc_bias = state[States.ACC_BIAS, :] + + dt = sp.Symbol('dt') + + # calibration and attitude rotation matrices + quat_rot = quat_rotate(*q) + + # Got the quat predict equations from here + # A New Quaternion-Based Kalman Filter for + # Real-Time Attitude Estimation Using the Two-Step + # Geometrically-Intuitive Correction Algorithm + A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw], + [vroll, 0, vyaw, -vpitch], + [vpitch, -vyaw, 0, vroll], + [vyaw, vpitch, -vroll, 0]]) + q_dot = A * q + + # Time derivative of the state as a function of state + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.ECEF_POS, :] = v + state_dot[States.ECEF_ORIENTATION, :] = q_dot + state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration + + # Basic descretization, 1st order intergrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1) + state_err = sp.Matrix(state_err_sym) + quat_err = state_err[States.ECEF_ORIENTATION_ERR, :] + v_err = state_err[States.ECEF_VELOCITY_ERR, :] + omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] + acceleration_err = state_err[States.ACCELERATION_ERR, :] + + # Time derivative of the state error as a function of state error and state + quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2]) + q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err) + state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1))) + state_err_dot[States.ECEF_POS_ERR, :] = v_err + state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot + state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err) + f_err_sym = state_err + dt * state_err_dot + + # Observation matrix modifier + H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) + H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start) + H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:] + H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop) + + # these error functions are defined so that say there + # is a nominal x and true x: + # true x = err_function(nominal x, delta x) + # delta x = inv_err_function(nominal x, true x) + nom_x = sp.MatrixSymbol('nom_x', dim_state, 1) + true_x = sp.MatrixSymbol('true_x', dim_state, 1) + delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) + + err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) + delta_quat = sp.Matrix(np.ones(4)) + delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :]) + err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :]) + err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat + err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :]) + + inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) + inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0]) + delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0] + inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:]) + inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0]) + + eskf_params = [[err_function_sym, nom_x, delta_x], + [inv_err_function_sym, nom_x, true_x], + H_mod_sym, f_err_sym, state_err_sym] + # + # Observation functions + # + h_gyro_sym = sp.Matrix([ + vroll + roll_bias, + vpitch + pitch_bias, + vyaw + yaw_bias]) + + pos = sp.Matrix([x, y, z]) + gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) + h_acc_sym = (gravity + acceleration + acc_bias) + h_acc_stationary_sym = acceleration + h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) + h_pos_sym = sp.Matrix([x, y, z]) + h_vel_sym = sp.Matrix([vx, vy, vz]) + h_orientation_sym = q + h_relative_motion = sp.Matrix(quat_rot.T * v) + + obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None], + [h_phone_rot_sym, ObservationKind.NO_ROT, None], + [h_acc_sym, ObservationKind.PHONE_ACCEL, None], + [h_pos_sym, ObservationKind.ECEF_POS, None], + [h_vel_sym, ObservationKind.ECEF_VEL, None], + [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None], + [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], + [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], + [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] + + # this returns a sympy routine for the jacobian of the observation function of the local vel + in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz + h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]])) + extra_routines = [('H', h.jacobian(in_vec), [in_vec])] + + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines) + + # write constants to extra header file for use in cpp + live_kf_header = "#pragma once\n\n" + live_kf_header += "#include \n" + live_kf_header += "#include \n\n" + for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)): + assert(slc.step is None) # unsupported + live_kf_header += f'#define STATE_{state}_START {slc.start}\n' + live_kf_header += f'#define STATE_{state}_END {slc.stop}\n' + live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n' + live_kf_header += "\n" + + for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)): + live_kf_header += f'#define OBSERVATION_{kind} {val}\n' + live_kf_header += "\n" + + live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n" + live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n" + live_kf_header += "static const std::unordered_map> live_obs_noise_diag = {\n" + for kind, noise in LiveKalman.obs_noise_diag.items(): + live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n" + live_kf_header += "};\n\n" + + open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header) + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + LiveKalman.generate_code(generated_dir) diff --git a/sunnypilot/selfdrive/locationd/tests/.gitignore b/sunnypilot/selfdrive/locationd/tests/.gitignore new file mode 100644 index 0000000000..89f9ac04aa --- /dev/null +++ b/sunnypilot/selfdrive/locationd/tests/.gitignore @@ -0,0 +1 @@ +out/ diff --git a/sunnypilot/selfdrive/locationd/tests/__init__.py b/sunnypilot/selfdrive/locationd/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/locationd/tests/test_locationd.py b/sunnypilot/selfdrive/locationd/tests/test_locationd.py new file mode 100644 index 0000000000..c409f5b5a5 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/tests/test_locationd.py @@ -0,0 +1,89 @@ +import pytest +import json +import random +import time +import capnp + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.params import Params +from openpilot.common.transformations.coordinates import ecef2geodetic + +from openpilot.system.manager.process_config import managed_processes + + +class TestLocationdProc: + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', + 'accelerometer', 'gyroscope', 'magnetometer'] + + def setup_method(self): + self.pm = messaging.PubMaster(self.LLD_MSGS) + + self.params = Params() + self.params.put_bool("UbloxAvailable", True) + managed_processes['locationd_llk'].prepare() + managed_processes['locationd_llk'].start() + + def teardown_method(self): + managed_processes['locationd_llk'].stop() + + def get_msg(self, name, t): + try: + msg = messaging.new_message(name) + except capnp.lib.capnp.KjException: + msg = messaging.new_message(name, 0) + + if name == "gpsLocationExternal": + msg.gpsLocationExternal.flags = 1 + msg.gpsLocationExternal.hasFix = True + msg.gpsLocationExternal.verticalAccuracy = 1.0 + msg.gpsLocationExternal.speedAccuracy = 1.0 + msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 + msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] + msg.gpsLocationExternal.latitude = float(self.lat) + msg.gpsLocationExternal.longitude = float(self.lon) + msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 + msg.gpsLocationExternal.altitude = float(self.alt) + #if name == "gnssMeasurements": + # msg.gnssMeasurements.measTime = t + # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + # msg.gnssMeasurements.positionECEF.std = [0,0,0] + # msg.gnssMeasurements.positionECEF.valid = True + # msg.gnssMeasurements.velocityECEF.value = [] + # msg.gnssMeasurements.velocityECEF.std = [0,0,0] + # msg.gnssMeasurements.velocityECEF.valid = True + elif name == 'cameraOdometry': + msg.cameraOdometry.rot = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] + msg.cameraOdometry.trans = [0.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [0.0, 0.0, 0.0] + msg.logMonoTime = t + msg.valid = True + return msg + + def test_params_gps(self): + random.seed(123489234) + self.params.remove('LastGPSPositionLLK') + + self.x = -2710700 + (random.random() * 1e5) + self.y = -4280600 + (random.random() * 1e5) + self.z = 3850300 + (random.random() * 1e5) + self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z]) + + # get fake messages at the correct frequency, listed in services.py + msgs = [] + for sec in range(65): + for name in self.LLD_MSGS: + for j in range(int(SERVICE_LIST[name].frequency)): + msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9))) + + for msg in sorted(msgs, key=lambda x: x.logMonoTime): + self.pm.send(msg.which(), msg) + if msg.which() == "cameraOdometry": + self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005) + time.sleep(1) # wait for async params write + + lastGPS = json.loads(self.params.get('LastGPSPositionLLK')) + assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001) + assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001) + assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001) diff --git a/sunnypilot/system/sensord/.gitignore b/sunnypilot/system/sensord/.gitignore new file mode 100644 index 0000000000..e17675e254 --- /dev/null +++ b/sunnypilot/system/sensord/.gitignore @@ -0,0 +1 @@ +sensord diff --git a/sunnypilot/system/sensord/SConscript b/sunnypilot/system/sensord/SConscript new file mode 100644 index 0000000000..1222f0bfcd --- /dev/null +++ b/sunnypilot/system/sensord/SConscript @@ -0,0 +1,13 @@ +Import('env', 'arch', 'common', 'messaging') + +sensors = [ + 'sensors/i2c_sensor.cc', + 'sensors/lsm6ds3_accel.cc', + 'sensors/lsm6ds3_gyro.cc', + 'sensors/lsm6ds3_temp.cc', + 'sensors/mmc5603nj_magn.cc', +] +libs = [common, messaging, 'pthread'] +if arch == "larch64": + libs.append('i2c') +env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/sunnypilot/system/sensord/sensors/bmx055_accel.cc b/sunnypilot/system/sensord/sensors/bmx055_accel.cc new file mode 100644 index 0000000000..5db10c3b5b --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_accel.cc @@ -0,0 +1,85 @@ +#include "sunnypilot/system/sensord/sensors/bmx055_accel.h" + +#include + +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Accel::init() { + int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}); + if (ret == -1) { + goto fail; + } + + ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE); + if (ret < 0) { + goto fail; + } + + // bmx055 accel has a 1.3ms wakeup time from deep suspend mode + util::sleep_for(10); + + // High bandwidth + // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE); + // if (ret < 0) { + // goto fail; + // } + + // Low bandwidth + ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE); + if (ret < 0) { + goto fail; + } + + ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ); + if (ret < 0) { + goto fail; + } + + enabled = true; + +fail: + return ret; +} + +int BMX055_Accel::shutdown() { + if (!enabled) return 0; + + // enter deep suspend mode (lowest power mode) + int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND); + if (ret < 0) { + LOGE("Could not move BMX055 ACCEL in deep suspend mode!"); + } + + return ret; +} + +bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[6]; + int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer)); + assert(len == 6); + + // 12 bit = +-2g + float scale = 9.81 * 2.0f / (1 << 11); + float x = -read_12_bit(buffer[0], buffer[1]) * scale; + float y = -read_12_bit(buffer[2], buffer[3]) * scale; + float z = read_12_bit(buffer[4], buffer[5]) * scale; + + auto event = msg.initEvent().initAccelerometer2(); + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setSensor(SENSOR_ACCELEROMETER); + event.setType(SENSOR_TYPE_ACCELEROMETER); + event.setTimestamp(start_time); + + float xyz[] = {x, y, z}; + auto svec = event.initAcceleration(); + svec.setV(xyz); + svec.setStatus(true); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/bmx055_accel.h b/sunnypilot/system/sensord/sensors/bmx055_accel.h new file mode 100644 index 0000000000..4aea163c8b --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_accel.h @@ -0,0 +1,41 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define BMX055_ACCEL_I2C_ADDR 0x18 + +// Registers of the chip +#define BMX055_ACCEL_I2C_REG_ID 0x00 +#define BMX055_ACCEL_I2C_REG_X_LSB 0x02 +#define BMX055_ACCEL_I2C_REG_TEMP 0x08 +#define BMX055_ACCEL_I2C_REG_BW 0x10 +#define BMX055_ACCEL_I2C_REG_PMU 0x11 +#define BMX055_ACCEL_I2C_REG_HBW 0x13 +#define BMX055_ACCEL_I2C_REG_FIFO 0x3F + +// Constants +#define BMX055_ACCEL_CHIP_ID 0xFA + +#define BMX055_ACCEL_HBW_ENABLE 0b10000000 +#define BMX055_ACCEL_HBW_DISABLE 0b00000000 +#define BMX055_ACCEL_DEEP_SUSPEND 0b00100000 +#define BMX055_ACCEL_NORMAL_MODE 0b00000000 + +#define BMX055_ACCEL_BW_7_81HZ 0b01000 +#define BMX055_ACCEL_BW_15_63HZ 0b01001 +#define BMX055_ACCEL_BW_31_25HZ 0b01010 +#define BMX055_ACCEL_BW_62_5HZ 0b01011 +#define BMX055_ACCEL_BW_125HZ 0b01100 +#define BMX055_ACCEL_BW_250HZ 0b01101 +#define BMX055_ACCEL_BW_500HZ 0b01110 +#define BMX055_ACCEL_BW_1000HZ 0b01111 + +class BMX055_Accel : public I2CSensor { + uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} +public: + BMX055_Accel(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/bmx055_gyro.cc b/sunnypilot/system/sensord/sensors/bmx055_gyro.cc new file mode 100644 index 0000000000..4701c70d71 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_gyro.cc @@ -0,0 +1,92 @@ +#include "sunnypilot/system/sensord/sensors/bmx055_gyro.h" + +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + + +BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Gyro::init() { + int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID}); + if (ret == -1) return -1; + + ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE); + if (ret < 0) { + goto fail; + } + // bmx055 gyro has a 30ms wakeup time from deep suspend mode + util::sleep_for(50); + + // High bandwidth + // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE); + // if (ret < 0) { + // goto fail; + // } + + // Low bandwidth + ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE); + if (ret < 0) { + goto fail; + } + + // 116 Hz filter + ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ); + if (ret < 0) { + goto fail; + } + + // +- 125 deg/s range + ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125); + if (ret < 0) { + goto fail; + } + + enabled = true; + +fail: + return ret; +} + +int BMX055_Gyro::shutdown() { + if (!enabled) return 0; + + // enter deep suspend mode (lowest power mode) + int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND); + if (ret < 0) { + LOGE("Could not move BMX055 GYRO in deep suspend mode!"); + } + + return ret; +} + +bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[6]; + int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer)); + assert(len == 6); + + // 16 bit = +- 125 deg/s + float scale = 125.0f / (1 << 15); + float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); + float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); + float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + + auto event = msg.initEvent().initGyroscope2(); + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setSensor(SENSOR_GYRO_UNCALIBRATED); + event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); + event.setTimestamp(start_time); + + float xyz[] = {x, y, z}; + auto svec = event.initGyroUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/bmx055_gyro.h b/sunnypilot/system/sensord/sensors/bmx055_gyro.h new file mode 100644 index 0000000000..489184bb18 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_gyro.h @@ -0,0 +1,41 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define BMX055_GYRO_I2C_ADDR 0x68 + +// Registers of the chip +#define BMX055_GYRO_I2C_REG_ID 0x00 +#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02 +#define BMX055_GYRO_I2C_REG_RANGE 0x0F +#define BMX055_GYRO_I2C_REG_BW 0x10 +#define BMX055_GYRO_I2C_REG_LPM1 0x11 +#define BMX055_GYRO_I2C_REG_HBW 0x13 +#define BMX055_GYRO_I2C_REG_FIFO 0x3F + +// Constants +#define BMX055_GYRO_CHIP_ID 0x0F + +#define BMX055_GYRO_HBW_ENABLE 0b10000000 +#define BMX055_GYRO_HBW_DISABLE 0b00000000 +#define BMX055_GYRO_DEEP_SUSPEND 0b00100000 +#define BMX055_GYRO_NORMAL_MODE 0b00000000 + +#define BMX055_GYRO_RANGE_2000 0b000 +#define BMX055_GYRO_RANGE_1000 0b001 +#define BMX055_GYRO_RANGE_500 0b010 +#define BMX055_GYRO_RANGE_250 0b011 +#define BMX055_GYRO_RANGE_125 0b100 + +#define BMX055_GYRO_BW_116HZ 0b0010 + + +class BMX055_Gyro : public I2CSensor { + uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;} +public: + BMX055_Gyro(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/bmx055_magn.cc b/sunnypilot/system/sensord/sensors/bmx055_magn.cc new file mode 100644 index 0000000000..00b35aa136 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_magn.cc @@ -0,0 +1,258 @@ +#include "sunnypilot/system/sensord/sensors/bmx055_magn.h" + +#include + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) { + uint16_t process_comp_x0 = data_rhall; + int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384; + uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000); + int16_t retval = ((int16_t)process_comp_x2); + int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval)); + int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128)); + int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128); + int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5; + int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000)); + int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0))); + int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096); + int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9; + retval = ((int16_t)(process_comp_x10 / 8192)); + retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16; + + return retval; +} + +static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) { + uint16_t process_comp_y0 = trim_data.dig_xyz1; + int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0; + uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000); + int16_t retval = ((int16_t)process_comp_y2); + int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval); + int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128); + int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128)); + int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512); + int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0))); + int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096); + int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8); + retval = (int16_t)(process_comp_y9 / 8192); + retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16; + + return retval; +} + +static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) { + int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1); + int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4; + int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768); + int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2); + int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536); + int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4)); + + /* saturate result to +/- 2 micro-tesla */ + retval = std::clamp(retval, -32767, 32767); + + /* Conversion of LSB to micro-tesla*/ + retval = retval / 16; + + return (int16_t)retval; +} + +BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Magn::init() { + uint8_t trim_x1y1[2] = {0}; + uint8_t trim_x2y2[2] = {0}; + uint8_t trim_xy1xy2[2] = {0}; + uint8_t trim_z1[2] = {0}; + uint8_t trim_z2[2] = {0}; + uint8_t trim_z3[2] = {0}; + uint8_t trim_z4[2] = {0}; + uint8_t trim_xyz1[2] = {0}; + + // suspend -> sleep + int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); + if (ret < 0) { + LOGD("Enabling power failed: %d", ret); + goto fail; + } + util::sleep_for(5); // wait until the chip is powered on + + ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID}); + if (ret == -1) { + goto fail; + } + + // Load magnetometer trim + ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2); + if (ret < 0) goto fail; + + // Read trim data + trim_data.dig_x1 = trim_x1y1[0]; + trim_data.dig_y1 = trim_x1y1[1]; + + trim_data.dig_x2 = trim_x2y2[0]; + trim_data.dig_y2 = trim_x2y2[1]; + + trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped + trim_data.dig_xy2 = trim_xy1xy2[0]; + + trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]); + trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]); + trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]); + trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]); + + trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f); + assert(trim_data.dig_xyz1 != 0); + + perform_self_test(); + + // f_max = 1 / (145us * nXY + 500us * NZ + 980us) + // Chose NXY = 7, NZ = 12, which gives 125 Hz, + // and has the same ratio as the high accuracy preset + ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2); + if (ret < 0) { + goto fail; + } + + ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1); + if (ret < 0) { + goto fail; + } + + enabled = true; + return 0; + + fail: + return ret; +} + +int BMX055_Magn::shutdown() { + if (!enabled) return 0; + + // move to suspend mode + int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0); + if (ret < 0) { + LOGE("Could not move BMX055 MAGN in suspend mode!"); + } + + return ret; +} + +bool BMX055_Magn::perform_self_test() { + uint8_t buffer[8]; + int16_t x, y; + int16_t neg_z, pos_z; + + // Increase z reps for less false positives (~30 Hz ODR) + set_register(BMX055_MAGN_I2C_REG_REPXY, 1); + set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1); + + // Clean existing measurement + read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + + uint8_t forced = BMX055_MAGN_FORCED; + + // Negative current + set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6)); + util::sleep_for(100); + + read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + parse_xyz(buffer, &x, &y, &neg_z); + + // Positive current + set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6)); + util::sleep_for(100); + + read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + parse_xyz(buffer, &x, &y, &pos_z); + + // Put back in normal mode + set_register(BMX055_MAGN_I2C_REG_MAG, 0); + + int16_t diff = pos_z - neg_z; + bool passed = (diff > 180) && (diff < 240); + + if (!passed) { + LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff); + } + + return passed; +} + +bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) { + bool ready = buffer[6] & 0x1; + if (ready) { + int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3; + int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3; + int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1; + uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2; + assert(data_r != 0); + + *x = compensate_x(trim_data, mdata_x, data_r); + *y = compensate_y(trim_data, mdata_y, data_r); + *z = compensate_z(trim_data, mdata_z, data_r); + } + return ready; +} + + +bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[8]; + int16_t _x, _y, x, y, z; + + int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + bool parsed = parse_xyz(buffer, &_x, &_y, &z); + if (parsed) { + + auto event = msg.initEvent().initMagnetometer(); + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(2); + event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); + event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); + event.setTimestamp(start_time); + + // Move magnetometer into same reference frame as accel/gryo + x = -_y; + y = _x; + + // Axis convention + x = -x; + y = -y; + + float xyz[] = {(float)x, (float)y, (float)z}; + auto svec = event.initMagneticUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + } + + // The BMX055 Magnetometer has no FIFO mode. Self running mode only goes + // up to 30 Hz. Therefore we put in forced mode, and request measurements + // at a 100 Hz. When reading the registers we have to check the ready bit + // To verify the measurement was completed this cycle. + set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED); + + return parsed; +} diff --git a/sunnypilot/system/sensord/sensors/bmx055_magn.h b/sunnypilot/system/sensord/sensors/bmx055_magn.h new file mode 100644 index 0000000000..7b1f7853d0 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_magn.h @@ -0,0 +1,64 @@ +#pragma once +#include + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define BMX055_MAGN_I2C_ADDR 0x10 + +// Registers of the chip +#define BMX055_MAGN_I2C_REG_ID 0x40 +#define BMX055_MAGN_I2C_REG_PWR_0 0x4B +#define BMX055_MAGN_I2C_REG_MAG 0x4C +#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42 +#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48 +#define BMX055_MAGN_I2C_REG_REPXY 0x51 +#define BMX055_MAGN_I2C_REG_REPZ 0x52 + +#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D +#define BMX055_MAGN_I2C_REG_DIG_Y1 0x5E +#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB 0x62 +#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB 0x63 +#define BMX055_MAGN_I2C_REG_DIG_X2 0x64 +#define BMX055_MAGN_I2C_REG_DIG_Y2 0x65 +#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB 0x68 +#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB 0x69 +#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB 0x6A +#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB 0x6B +#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C +#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D +#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB 0x6E +#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB 0x6F +#define BMX055_MAGN_I2C_REG_DIG_XY2 0x70 +#define BMX055_MAGN_I2C_REG_DIG_XY1 0x71 + +// Constants +#define BMX055_MAGN_CHIP_ID 0x32 +#define BMX055_MAGN_FORCED (0b01 << 1) + +struct trim_data_t { + int8_t dig_x1; + int8_t dig_y1; + int8_t dig_x2; + int8_t dig_y2; + uint16_t dig_z1; + int16_t dig_z2; + int16_t dig_z3; + int16_t dig_z4; + uint8_t dig_xy1; + int8_t dig_xy2; + uint16_t dig_xyz1; +}; + + +class BMX055_Magn : public I2CSensor{ + uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;} + trim_data_t trim_data = {0}; + bool perform_self_test(); + bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z); +public: + BMX055_Magn(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/bmx055_temp.cc b/sunnypilot/system/sensord/sensors/bmx055_temp.cc new file mode 100644 index 0000000000..3932c66fa6 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_temp.cc @@ -0,0 +1,31 @@ +#include "sunnypilot/system/sensord/sensors/bmx055_temp.h" + +#include + +#include "sunnypilot/system/sensord/sensors/bmx055_accel.h" +#include "common/swaglog.h" +#include "common/timing.h" + +BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Temp::init() { + return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0; +} + +bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[1]; + int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float temp = 23.0f + int8_t(buffer[0]) / 2.0f; + + auto event = msg.initEvent().initTemperatureSensor(); + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); + event.setTimestamp(start_time); + event.setTemperature(temp); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/bmx055_temp.h b/sunnypilot/system/sensord/sensors/bmx055_temp.h new file mode 100644 index 0000000000..02c48dc92c --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_temp.h @@ -0,0 +1,13 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/bmx055_accel.h" +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +class BMX055_Temp : public I2CSensor { + uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} +public: + BMX055_Temp(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown() { return 0; } +}; diff --git a/sunnypilot/system/sensord/sensors/constants.h b/sunnypilot/system/sensord/sensors/constants.h new file mode 100644 index 0000000000..c216f838a5 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/constants.h @@ -0,0 +1,18 @@ +#pragma once + + +#define SENSOR_ACCELEROMETER 1 +#define SENSOR_MAGNETOMETER 2 +#define SENSOR_MAGNETOMETER_UNCALIBRATED 3 +#define SENSOR_GYRO 4 +#define SENSOR_GYRO_UNCALIBRATED 5 +#define SENSOR_LIGHT 7 + +#define SENSOR_TYPE_ACCELEROMETER 1 +#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2 +#define SENSOR_TYPE_GYROSCOPE 4 +#define SENSOR_TYPE_LIGHT 5 +#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13 +#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14 +#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD +#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16 diff --git a/sunnypilot/system/sensord/sensors/i2c_sensor.cc b/sunnypilot/system/sensord/sensors/i2c_sensor.cc new file mode 100644 index 0000000000..e29f98c721 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/i2c_sensor.cc @@ -0,0 +1,50 @@ +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +int16_t read_12_bit(uint8_t lsb, uint8_t msb) { + uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0); + return int16_t(combined) / (1 << 4); +} + +int16_t read_16_bit(uint8_t lsb, uint8_t msb) { + uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb); + return int16_t(combined); +} + +int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) { + uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2); + return int32_t(combined) / (1 << 4); +} + +I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) : + bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {} + +I2CSensor::~I2CSensor() { + if (gpio_fd != -1) { + close(gpio_fd); + } +} + +int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) { + return bus->read_register(get_device_address(), register_address, buffer, len); +} + +int I2CSensor::set_register(uint register_address, uint8_t data) { + return bus->set_register(get_device_address(), register_address, data); +} + +int I2CSensor::init_gpio() { + if (shared_gpio || gpio_nr == 0) { + return 0; + } + + gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr); + if (gpio_fd < 0) { + return -1; + } + + return 0; +} + +bool I2CSensor::has_interrupt_enabled() { + return gpio_nr != 0; +} diff --git a/sunnypilot/system/sensord/sensors/i2c_sensor.h b/sunnypilot/system/sensord/sensors/i2c_sensor.h new file mode 100644 index 0000000000..6513db7934 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/i2c_sensor.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/i2c.h" +#include "common/gpio.h" + +#include "common/swaglog.h" +#include "sunnypilot/system/sensord/sensors/constants.h" +#include "sunnypilot/system/sensord/sensors/sensor.h" + +int16_t read_12_bit(uint8_t lsb, uint8_t msb); +int16_t read_16_bit(uint8_t lsb, uint8_t msb); +int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0); + + +class I2CSensor : public Sensor { +private: + I2CBus *bus; + int gpio_nr; + bool shared_gpio; + virtual uint8_t get_device_address() = 0; + +public: + I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); + ~I2CSensor(); + int read_register(uint register_address, uint8_t *buffer, uint8_t len); + int set_register(uint register_address, uint8_t data); + int init_gpio(); + bool has_interrupt_enabled(); + virtual int init() = 0; + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; + virtual int shutdown() = 0; + + int verify_chip_id(uint8_t address, const std::vector &expected_ids) { + uint8_t chip_id = 0; + int ret = read_register(address, &chip_id, 1); + if (ret < 0) { + LOGD("Reading chip ID failed: %d", ret); + return -1; + } + for (int i = 0; i < expected_ids.size(); ++i) { + if (chip_id == expected_ids[i]) return chip_id; + } + LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]); + return -1; + } +}; diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc new file mode 100644 index 0000000000..2857106944 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc @@ -0,0 +1,250 @@ +#include "sunnypilot/system/sensord/sensors/lsm6ds3_accel.h" + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : + I2CSensor(bus, gpio_nr, shared_gpio) {} + +void LSM6DS3_Accel::wait_for_data_ready() { + uint8_t drdy = 0; + uint8_t buffer[6]; + + do { + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_ACCEL_DRDY_XLDA; + } while (drdy == 0); + + read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); +} + +void LSM6DS3_Accel::read_and_avg_data(float* out_buf) { + uint8_t drdy = 0; + uint8_t buffer[6]; + + float scaling = 0.061f; + if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { + scaling = 0.122f; + } + + for (int i = 0; i < 5; i++) { + do { + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_ACCEL_DRDY_XLDA; + } while (drdy == 0); + + int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + for (int j = 0; j < 3; j++) { + out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling; + } + } + + for (int i = 0; i < 3; i++) { + out_buf[i] /= 5.0f; + } +} + +int LSM6DS3_Accel::self_test(int test_type) { + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; + float test_val[3] = {0}; + uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz + + // prepare sensor for self-test + + // enable block data update and automatic increment + int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU); + if (ret < 0) { + return ret; + } + + if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { + ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ; + } + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(100); + wait_for_data_ready(); + read_and_avg_data(val_st_off); + + // enable Self Test positive (or negative) + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(100); + wait_for_data_ready(); + read_and_avg_data(val_st_on); + + // disable sensor + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0); + if (ret < 0) { + return ret; + } + + // disable self test + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0); + if (ret < 0) { + return ret; + } + + // calculate the mg values for self test + for (int i = 0; i < 3; i++) { + test_val[i] = fabs(val_st_on[i] - val_st_off[i]); + } + + // verify test result + for (int i = 0; i < 3; i++) { + if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) || + (test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) { + return -1; + } + } + + return ret; +} + +int LSM6DS3_Accel::init() { + uint8_t value = 0; + bool do_self_test = false; + + const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST"); + if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { + do_self_test = true; + } + + int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID}); + if (ret == -1) return -1; + + if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + + ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 accel positive self-test failed!"); + if (do_self_test) goto fail; + } + + ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 accel negative self-test failed!"); + if (do_self_test) goto fail; + } + + ret = init_gpio(); + if (ret < 0) { + goto fail; + } + + // enable continuous update, and automatic increase + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC); + if (ret < 0) { + goto fail; + } + + // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ); + if (ret < 0) { + goto fail; + } + + ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE); + if (ret < 0) { + goto fail; + } + + // enable data ready interrupt for accel on INT1 + // (without resetting existing interrupts) + ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value |= LSM6DS3_ACCEL_INT1_DRDY_XL; + ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); + +fail: + return ret; +} + +int LSM6DS3_Accel::shutdown() { + int ret = 0; + + // disable data ready interrupt for accel on INT1 + uint8_t value = 0; + ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL); + ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); + if (ret < 0) { + LOGE("Could not disable lsm6ds3 acceleration interrupt!"); + goto fail; + } + + // enable power-down mode + value = 0; + ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= 0x0F; + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value); + if (ret < 0) { + LOGE("Could not power-down lsm6ds3 accelerometer!"); + goto fail; + } + +fail: + return ret; +} + +bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) { + + // INT1 shared with gyro, check STATUS_REG who triggered + uint8_t status_reg = 0; + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); + if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) { + return false; + } + + uint8_t buffer[6]; + int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float scale = 9.81 * 2.0f / (1 << 15); + float x = read_16_bit(buffer[0], buffer[1]) * scale; + float y = read_16_bit(buffer[2], buffer[3]) * scale; + float z = read_16_bit(buffer[4], buffer[5]) * scale; + + auto event = msg.initEvent().initAccelerometer(); + event.setSource(source); + event.setVersion(1); + event.setSensor(SENSOR_ACCELEROMETER); + event.setType(SENSOR_TYPE_ACCELEROMETER); + event.setTimestamp(ts); + + float xyz[] = {y, -x, z}; + auto svec = event.initAcceleration(); + svec.setV(xyz); + svec.setStatus(true); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h new file mode 100644 index 0000000000..2ac61c5a0f --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h @@ -0,0 +1,49 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define LSM6DS3_ACCEL_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B +#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F +#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D +#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10 +#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C 0x12 +#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C 0x14 +#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL 0x18 +#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E +#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28 + +// Constants +#define LSM6DS3_ACCEL_CHIP_ID 0x69 +#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A +#define LSM6DS3_ACCEL_FS_4G (0b10 << 2) +#define LSM6DS3_ACCEL_ODR_52HZ (0b0011 << 4) +#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1 +#define LSM6DS3_ACCEL_DRDY_XLDA 0b1 +#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7) +#define LSM6DS3_ACCEL_IF_INC 0b00000100 +#define LSM6DS3_ACCEL_IF_INC_BDU 0b01000100 +#define LSM6DS3_ACCEL_XYZ_DEN 0b11100000 +#define LSM6DS3_ACCEL_POSITIVE_TEST 0b01 +#define LSM6DS3_ACCEL_NEGATIVE_TEST 0b10 +#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f +#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f + +class LSM6DS3_Accel : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + + // self test functions + int self_test(int test_type); + void wait_for_data_ready(); + void read_and_avg_data(float* val_st_off); +public: + LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc new file mode 100644 index 0000000000..e881f3942a --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc @@ -0,0 +1,233 @@ +#include "sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h" + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + +LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) : + I2CSensor(bus, gpio_nr, shared_gpio) {} + +void LSM6DS3_Gyro::wait_for_data_ready() { + uint8_t drdy = 0; + uint8_t buffer[6]; + + do { + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_GYRO_DRDY_GDA; + } while (drdy == 0); + + read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); +} + +void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) { + uint8_t drdy = 0; + uint8_t buffer[6]; + + for (int i = 0; i < 5; i++) { + do { + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_GYRO_DRDY_GDA; + } while (drdy == 0); + + int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + for (int j = 0; j < 3; j++) { + out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f; + } + } + + // calculate the mg average values + for (int i = 0; i < 3; i++) { + out_buf[i] /= 5.0f; + } +} + +int LSM6DS3_Gyro::self_test(int test_type) { + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; + float test_val[3] = {0}; + + // prepare sensor for self-test + + // full scale: 2000dps, ODR: 208Hz + int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(150); + wait_for_data_ready(); + read_and_avg_data(val_st_off); + + // enable Self Test positive (or negative) + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(50); + wait_for_data_ready(); + read_and_avg_data(val_st_on); + + // disable sensor + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0); + if (ret < 0) { + return ret; + } + + // disable self test + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0); + if (ret < 0) { + return ret; + } + + // calculate the mg values for self test + for (int i = 0; i < 3; i++) { + test_val[i] = fabs(val_st_on[i] - val_st_off[i]); + } + + // verify test result + for (int i = 0; i < 3; i++) { + if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) || + (test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) { + return -1; + } + } + + return ret; +} + +int LSM6DS3_Gyro::init() { + uint8_t value = 0; + bool do_self_test = false; + + const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST"); + if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { + do_self_test = true; + } + + int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID}); + if (ret == -1) return -1; + + if (ret == LSM6DS3TRC_GYRO_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + + ret = init_gpio(); + if (ret < 0) { + goto fail; + } + + ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 gyro positive self-test failed!"); + if (do_self_test) goto fail; + } + + ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 gyro negative self-test failed!"); + if (do_self_test) goto fail; + } + + // TODO: set scale. Default is +- 250 deg/s + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ); + if (ret < 0) { + goto fail; + } + + ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE); + if (ret < 0) { + goto fail; + } + + // enable data ready interrupt for gyro on INT1 + // (without resetting existing interrupts) + ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value |= LSM6DS3_GYRO_INT1_DRDY_G; + ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); + +fail: + return ret; +} + +int LSM6DS3_Gyro::shutdown() { + int ret = 0; + + // disable data ready interrupt for gyro on INT1 + uint8_t value = 0; + ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(LSM6DS3_GYRO_INT1_DRDY_G); + ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); + if (ret < 0) { + LOGE("Could not disable lsm6ds3 gyroscope interrupt!"); + goto fail; + } + + // enable power-down mode + value = 0; + ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= 0x0F; + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value); + if (ret < 0) { + LOGE("Could not power-down lsm6ds3 gyroscope!"); + goto fail; + } + +fail: + return ret; +} + +bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { + + // INT1 shared with accel, check STATUS_REG who triggered + uint8_t status_reg = 0; + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); + if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) { + return false; + } + + uint8_t buffer[6]; + int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float scale = 8.75 / 1000.0; + float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); + float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); + float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + + auto event = msg.initEvent().initGyroscope(); + event.setSource(source); + event.setVersion(2); + event.setSensor(SENSOR_GYRO_UNCALIBRATED); + event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); + event.setTimestamp(ts); + + float xyz[] = {y, -x, z}; + auto svec = event.initGyroUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h new file mode 100644 index 0000000000..686a6a7169 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h @@ -0,0 +1,45 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define LSM6DS3_GYRO_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B +#define LSM6DS3_GYRO_I2C_REG_ID 0x0F +#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D +#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11 +#define LSM6DS3_GYRO_I2C_REG_CTRL5_C 0x14 +#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E +#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22 +#define LSM6DS3_GYRO_POSITIVE_TEST (0b01 << 2) +#define LSM6DS3_GYRO_NEGATIVE_TEST (0b11 << 2) + +// Constants +#define LSM6DS3_GYRO_CHIP_ID 0x69 +#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A +#define LSM6DS3_GYRO_FS_2000dps (0b11 << 2) +#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_GYRO_ODR_208HZ (0b0101 << 4) +#define LSM6DS3_GYRO_INT1_DRDY_G 0b10 +#define LSM6DS3_GYRO_DRDY_GDA 0b10 +#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7) +#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f +#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f + + +class LSM6DS3_Gyro : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + + // self test functions + int self_test(int test_type); + void wait_for_data_ready(); + void read_and_avg_data(float* val_st_off); +public: + LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc new file mode 100644 index 0000000000..390f192a2d --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc @@ -0,0 +1,37 @@ +#include "sunnypilot/system/sensord/sensors/lsm6ds3_temp.h" + +#include + +#include "common/swaglog.h" +#include "common/timing.h" + +LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {} + +int LSM6DS3_Temp::init() { + int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID}); + if (ret == -1) return -1; + + if (ret == LSM6DS3TRC_TEMP_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + return 0; +} + +bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[2]; + int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f; + float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale; + + auto event = msg.initEvent().initTemperatureSensor(); + event.setSource(source); + event.setVersion(1); + event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); + event.setTimestamp(start_time); + event.setTemperature(temp); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h new file mode 100644 index 0000000000..c314d456e8 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h @@ -0,0 +1,26 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define LSM6DS3_TEMP_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_TEMP_I2C_REG_ID 0x0F +#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20 + +// Constants +#define LSM6DS3_TEMP_CHIP_ID 0x69 +#define LSM6DS3TRC_TEMP_CHIP_ID 0x6A + + +class LSM6DS3_Temp : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + +public: + LSM6DS3_Temp(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown() { return 0; } +}; diff --git a/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc new file mode 100644 index 0000000000..12d438ebd1 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc @@ -0,0 +1,108 @@ +#include "sunnypilot/system/sensord/sensors/mmc5603nj_magn.h" + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {} + +int MMC5603NJ_Magn::init() { + int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID}); + if (ret == -1) return -1; + + // Set ODR to 0 + ret = set_register(MMC5603NJ_I2C_REG_ODR, 0); + if (ret < 0) { + goto fail; + } + + // Set BW to 0b01 for 1-150 Hz operation + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01); + if (ret < 0) { + goto fail; + } + +fail: + return ret; +} + +int MMC5603NJ_Magn::shutdown() { + int ret = 0; + + // disable auto reset of measurements + uint8_t value = 0; + ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN); + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value); + if (ret < 0) { + goto fail; + } + + // set ODR to 0 to leave continuous mode + ret = set_register(MMC5603NJ_I2C_REG_ODR, 0); + if (ret < 0) { + goto fail; + } + return ret; + +fail: + LOGE("Could not disable mmc5603nj auto set reset"); + return ret; +} + +void MMC5603NJ_Magn::start_measurement() { + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01); + util::sleep_for(5); +} + +std::vector MMC5603NJ_Magn::read_measurement() { + int len; + uint8_t buffer[9]; + len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + float scale = 1.0 / 16384.0; + float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0; + float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0; + float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0; + std::vector xyz = {x, y, z}; + return xyz; +} + +bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + // SET - RESET cycle + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET); + util::sleep_for(5); + MMC5603NJ_Magn::start_measurement(); + std::vector xyz = MMC5603NJ_Magn::read_measurement(); + + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET); + util::sleep_for(5); + MMC5603NJ_Magn::start_measurement(); + std::vector reset_xyz = MMC5603NJ_Magn::read_measurement(); + + auto event = msg.initEvent().initMagnetometer(); + event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ); + event.setVersion(1); + event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); + event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); + event.setTimestamp(start_time); + + float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]}; + bool valid = true; + if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) { + valid = false; + } + auto svec = event.initMagneticUncalibrated(); + svec.setV(vals); + svec.setStatus(valid); + return true; +} diff --git a/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h new file mode 100644 index 0000000000..9311631c06 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h @@ -0,0 +1,37 @@ +#pragma once + +#include + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define MMC5603NJ_I2C_ADDR 0x30 + +// Registers of the chip +#define MMC5603NJ_I2C_REG_XOUT0 0x00 +#define MMC5603NJ_I2C_REG_ODR 0x1A +#define MMC5603NJ_I2C_REG_INTERNAL_0 0x1B +#define MMC5603NJ_I2C_REG_INTERNAL_1 0x1C +#define MMC5603NJ_I2C_REG_INTERNAL_2 0x1D +#define MMC5603NJ_I2C_REG_ID 0x39 + +// Constants +#define MMC5603NJ_CHIP_ID 0x10 +#define MMC5603NJ_CMM_FREQ_EN (1 << 7) +#define MMC5603NJ_AUTO_SR_EN (1 << 5) +#define MMC5603NJ_CMM_EN (1 << 4) +#define MMC5603NJ_EN_PRD_SET (1 << 3) +#define MMC5603NJ_SET (1 << 3) +#define MMC5603NJ_RESET (1 << 4) + +class MMC5603NJ_Magn : public I2CSensor { +private: + uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;} + void start_measurement(); + std::vector read_measurement(); +public: + MMC5603NJ_Magn(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/sensor.h b/sunnypilot/system/sensord/sensors/sensor.h new file mode 100644 index 0000000000..ccf998d161 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/sensor.h @@ -0,0 +1,24 @@ +#pragma once + +#include "cereal/messaging/messaging.h" + +class Sensor { +public: + int gpio_fd = -1; + bool enabled = false; + uint64_t start_ts = 0; + uint64_t init_delay = 500e6; // default dealy 500ms + + virtual ~Sensor() {} + virtual int init() = 0; + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; + virtual bool has_interrupt_enabled() = 0; + virtual int shutdown() = 0; + + virtual bool is_data_valid(uint64_t current_ts) { + if (start_ts == 0) { + start_ts = current_ts; + } + return (current_ts - start_ts) > init_delay; + } +}; diff --git a/sunnypilot/system/sensord/sensors_qcom2.cc b/sunnypilot/system/sensord/sensors_qcom2.cc new file mode 100644 index 0000000000..f4ecc7fc21 --- /dev/null +++ b/sunnypilot/system/sensord/sensors_qcom2.cc @@ -0,0 +1,179 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "cereal/services.h" +#include "cereal/messaging/messaging.h" +#include "common/i2c.h" +#include "common/ratekeeper.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" +#include "sunnypilot/system/sensord/sensors/bmx055_accel.h" +#include "sunnypilot/system/sensord/sensors/bmx055_gyro.h" +#include "sunnypilot/system/sensord/sensors/bmx055_magn.h" +#include "sunnypilot/system/sensord/sensors/bmx055_temp.h" +#include "sunnypilot/system/sensord/sensors/constants.h" +#include "sunnypilot/system/sensord/sensors/lsm6ds3_accel.h" +#include "sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h" +#include "sunnypilot/system/sensord/sensors/lsm6ds3_temp.h" +#include "sunnypilot/system/sensord/sensors/mmc5603nj_magn.h" + +#define I2C_BUS_IMU 1 + +ExitHandler do_exit; + +void interrupt_loop(std::vector> sensors) { + PubMaster pm({"gyroscope", "accelerometer"}); + + int fd = -1; + for (auto &[sensor, msg_name] : sensors) { + if (sensor->has_interrupt_enabled()) { + fd = sensor->gpio_fd; + break; + } + } + + uint64_t offset = nanos_since_epoch() - nanos_since_boot(); + struct pollfd fd_list[1] = {0}; + fd_list[0].fd = fd; + fd_list[0].events = POLLIN | POLLPRI; + + while (!do_exit) { + int err = poll(fd_list, 1, 100); + if (err == -1) { + if (errno == EINTR) { + continue; + } + return; + } else if (err == 0) { + LOGE("poll timed out"); + continue; + } + + if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) { + LOGE("no poll events set"); + continue; + } + + // Read all events + struct gpioevent_data evdata[16]; + err = HANDLE_EINTR(read(fd, evdata, sizeof(evdata))); + if (err < 0 || err % sizeof(*evdata) != 0) { + LOGE("error reading event data %d", err); + continue; + } + + uint64_t cur_offset = nanos_since_epoch() - nanos_since_boot(); + uint64_t diff = cur_offset > offset ? cur_offset - offset : offset - cur_offset; + if (diff > 10*1e6) { // 10ms + LOGW("time jumped: %lu %lu", cur_offset, offset); + offset = cur_offset; + + // we don't have a valid timestamp since the + // time jumped, so throw out this measurement. + continue; + } + + int num_events = err / sizeof(*evdata); + uint64_t ts = evdata[num_events - 1].timestamp - cur_offset; + + for (auto &[sensor, msg_name] : sensors) { + if (!sensor->has_interrupt_enabled()) { + continue; + } + + MessageBuilder msg; + if (!sensor->get_event(msg, ts)) { + continue; + } + + if (!sensor->is_data_valid(ts)) { + continue; + } + + pm.send(msg_name.c_str(), msg); + } + } +} + +void polling_loop(Sensor *sensor, std::string msg_name) { + PubMaster pm({msg_name.c_str()}); + RateKeeper rk(msg_name, services.at(msg_name).frequency); + while (!do_exit) { + MessageBuilder msg; + if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) { + pm.send(msg_name.c_str(), msg); + } + rk.keepTime(); + } +} + +int sensor_loop(I2CBus *i2c_bus_imu) { + // Sensor init + std::vector> sensors_init = { + {new BMX055_Accel(i2c_bus_imu), "accelerometer2"}, + {new BMX055_Gyro(i2c_bus_imu), "gyroscope2"}, + {new BMX055_Magn(i2c_bus_imu), "magnetometer"}, + {new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"}, + + {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"}, + {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"}, + {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"}, + + {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer"}, + }; + + // Initialize sensors + std::vector threads; + for (auto &[sensor, msg_name] : sensors_init) { + int err = sensor->init(); + if (err < 0) { + continue; + } + + if (!sensor->has_interrupt_enabled()) { + threads.emplace_back(polling_loop, sensor, msg_name); + } + } + + // increase interrupt quality by pinning interrupt and process to core 1 + setpriority(PRIO_PROCESS, 0, -18); + util::set_core_affinity({1}); + + // TODO: get the IRQ number from gpiochip + std::string irq_path = "/proc/irq/336/smp_affinity_list"; + if (!util::file_exists(irq_path)) { + irq_path = "/proc/irq/335/smp_affinity_list"; + } + std::system(util::string_format("sudo su -c 'echo 1 > %s'", irq_path.c_str()).c_str()); + + // thread for reading events via interrupts + threads.emplace_back(&interrupt_loop, std::ref(sensors_init)); + + // wait for all threads to finish + for (auto &t : threads) { + t.join(); + } + + for (auto &[sensor, msg_name] : sensors_init) { + sensor->shutdown(); + delete sensor; + } + return 0; +} + +int main(int argc, char *argv[]) { + try { + auto i2c_bus_imu = std::make_unique(I2C_BUS_IMU); + return sensor_loop(i2c_bus_imu.get()); + } catch (std::exception &e) { + LOGE("I2CBus init failed"); + return -1; + } +} diff --git a/sunnypilot/system/sensord/tests/test_sensord.py b/sunnypilot/system/sensord/tests/test_sensord.py new file mode 100644 index 0000000000..dc5f93d97b --- /dev/null +++ b/sunnypilot/system/sensord/tests/test_sensord.py @@ -0,0 +1,251 @@ +import os +import pytest +import time +import numpy as np +from collections import namedtuple, defaultdict + +import cereal.messaging as messaging +from cereal import log +from cereal.services import SERVICE_LIST +from openpilot.common.gpio import get_irqs_for_action +from openpilot.common.timeout import Timeout +from openpilot.system.hardware import HARDWARE +from openpilot.system.manager.process_config import managed_processes + +BMX = { + ('bmx055', 'acceleration'), + ('bmx055', 'gyroUncalibrated'), + ('bmx055', 'magneticUncalibrated'), + ('bmx055', 'temperature'), +} + +LSM = { + ('lsm6ds3', 'acceleration'), + ('lsm6ds3', 'gyroUncalibrated'), + ('lsm6ds3', 'temperature'), +} +LSM_C = {(x[0]+'trc', x[1]) for x in LSM} + +MMC = { + ('mmc5603nj', 'magneticUncalibrated'), +} + +SENSOR_CONFIGURATIONS: list[set] = [ + BMX | LSM, + MMC | LSM, + BMX | LSM_C, + MMC| LSM_C, +] +if HARDWARE.get_device_type() == "mici": + SENSOR_CONFIGURATIONS = [ + LSM, + LSM_C, + ] + +Sensor = log.SensorEventData.SensorSource +SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max']) +ALL_SENSORS = { + Sensor.lsm6ds3: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("temperature", 0, 60), + }, + + Sensor.lsm6ds3trc: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("temperature", 0, 60), + }, + + Sensor.bmx055: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("magneticUncalibrated", 0, 300), + SensorConfig("temperature", 0, 60), + }, + + Sensor.mmc5603nj: { + SensorConfig("magneticUncalibrated", 0, 300), + } +} + + +def get_irq_count(irq: int): + with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f: + per_cpu = map(int, f.read().split(",")) + return sum(per_cpu) + +def read_sensor_events(duration_sec): + sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2', + 'gyroscope2', 'temperatureSensor', 'temperatureSensor2'] + socks = {} + poller = messaging.Poller() + events = defaultdict(list) + for stype in sensor_types: + socks[stype] = messaging.sub_sock(stype, poller=poller, timeout=100) + + # wait for sensors to come up + with Timeout(int(os.environ.get("SENSOR_WAIT", "5")), "sensors didn't come up"): + while len(poller.poll(250)) == 0: + pass + time.sleep(1) + for s in socks.values(): + messaging.drain_sock_raw(s) + + st = time.monotonic() + while time.monotonic() - st < duration_sec: + for s in socks: + events[s] += messaging.drain_sock(socks[s]) + time.sleep(0.1) + + assert sum(map(len, events.values())) != 0, "No sensor events collected!" + + return {k: v for k, v in events.items() if len(v) > 0} + +@pytest.mark.tici +class TestSensord: + @classmethod + def setup_class(cls): + # enable LSM self test + os.environ["LSM_SELF_TEST"] = "1" + + # read initial sensor values every test case can use + os.system("pkill -f \\\\./sensord") + try: + managed_processes["sensord"].start() + cls.sample_secs = int(os.getenv("SAMPLE_SECS", "10")) + cls.events = read_sensor_events(cls.sample_secs) + + # determine sensord's irq + cls.sensord_irq = get_irqs_for_action("sensord")[0] + finally: + # teardown won't run if this doesn't succeed + managed_processes["sensord"].stop() + + @classmethod + def teardown_class(cls): + managed_processes["sensord"].stop() + + def teardown_method(self): + managed_processes["sensord"].stop() + + def test_sensors_present(self): + # verify correct sensors configuration + seen = set() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + seen.add((str(m.source), m.which())) + + assert seen in SENSOR_CONFIGURATIONS + + def test_lsm6ds3_timing(self, subtests): + # verify measurements are sampled and published at 104Hz + + sensor_t = { + 1: [], # accel + 5: [], # gyro + } + + for measurement in self.events['accelerometer']: + m = getattr(measurement, measurement.which()) + sensor_t[m.sensor].append(m.timestamp) + + for measurement in self.events['gyroscope']: + m = getattr(measurement, measurement.which()) + sensor_t[m.sensor].append(m.timestamp) + + for s, vals in sensor_t.items(): + with subtests.test(sensor=s): + assert len(vals) > 0 + tdiffs = np.diff(vals) / 1e6 # millis + + high_delay_diffs = list(filter(lambda d: d >= 20., tdiffs)) + assert len(high_delay_diffs) < 15, f"Too many large diffs: {high_delay_diffs}" + + avg_diff = sum(tdiffs)/len(tdiffs) + avg_freq = 1. / (avg_diff * 1e-3) + assert 92. < avg_freq < 114., f"avg freq {avg_freq}Hz wrong, expected 104Hz" + + stddev = np.std(tdiffs) + assert stddev < 2.0, f"Standard-dev to big {stddev}" + + def test_sensor_frequency(self, subtests): + for s, msgs in self.events.items(): + with subtests.test(sensor=s): + freq = len(msgs) / self.sample_secs + ef = SERVICE_LIST[s].frequency + assert ef*0.85 <= freq <= ef*1.15 + + def test_logmonottime_timestamp_diff(self): + # ensure diff between the message logMonotime and sample timestamp is small + + tdiffs = list() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + + # check if gyro and accel timestamps are before logMonoTime + if str(m.source).startswith("lsm6ds3") and m.which() != 'temperature': + err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}" + assert m.timestamp < measurement.logMonoTime, err_msg + + # negative values might occur, as non interrupt packages created + # before the sensor is read + tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6) + + # some sensors have a read procedure that will introduce an expected diff on the order of 20ms + high_delay_diffs = set(filter(lambda d: d >= 25., tdiffs)) + assert len(high_delay_diffs) < 20, f"Too many measurements published: {high_delay_diffs}" + + avg_diff = round(sum(tdiffs)/len(tdiffs), 4) + assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms" + + def test_sensor_values(self): + sensor_values = dict() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + key = (m.source.raw, m.which()) + values = getattr(m, m.which()) + + if hasattr(values, 'v'): + values = values.v + values = np.atleast_1d(values) + + if key in sensor_values: + sensor_values[key].append(values) + else: + sensor_values[key] = [values] + + # Sanity check sensor values + for sensor, stype in sensor_values: + for s in ALL_SENSORS[sensor]: + if s.type != stype: + continue + + key = (sensor, s.type) + mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1)) + err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}" + assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg + + def test_sensor_verify_no_interrupts_after_stop(self): + managed_processes["sensord"].start() + time.sleep(3) + + # read /proc/interrupts to verify interrupts are received + state_one = get_irq_count(self.sensord_irq) + time.sleep(1) + state_two = get_irq_count(self.sensord_irq) + + error_msg = f"no interrupts received after sensord start!\n{state_one} {state_two}" + assert state_one != state_two, error_msg + + managed_processes["sensord"].stop() + time.sleep(1) + + # read /proc/interrupts to verify no more interrupts are received + state_one = get_irq_count(self.sensord_irq) + time.sleep(1) + state_two = get_irq_count(self.sensord_irq) + assert state_one == state_two, "Interrupts received after sensord stop!" diff --git a/sunnypilot/system/sensord/tests/ttff_test.py b/sunnypilot/system/sensord/tests/ttff_test.py new file mode 100755 index 0000000000..a9cc16d707 --- /dev/null +++ b/sunnypilot/system/sensord/tests/ttff_test.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import time +import atexit + +from cereal import messaging +from openpilot.system.manager.process_config import managed_processes + +TIMEOUT = 10*60 + +def kill(): + for proc in ['ubloxd', 'pigeond']: + managed_processes[proc].stop(retry=True, block=True) + +if __name__ == "__main__": + # start ubloxd + managed_processes['ubloxd'].start() + atexit.register(kill) + + sm = messaging.SubMaster(['ubloxGnss']) + + times = [] + for i in range(20): + # start pigeond + st = time.monotonic() + managed_processes['pigeond'].start() + + # wait for a >4 satellite fix + while True: + sm.update(0) + msg = sm['ubloxGnss'] + if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]: + report = msg.measurementReport + if report.numMeas > 4: + times.append(time.monotonic() - st) + print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m") + break + + if time.monotonic() - st > TIMEOUT: + raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m") + + time.sleep(0.1) + + # stop pigeond + managed_processes['pigeond'].stop(retry=True, block=True) + time.sleep(20) + + print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m") diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 92d5a4ef8c..b1a2fa6e13 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -173,6 +173,9 @@ procs += [ # mapd NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready), PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run), + + # locationd + NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad), ] if os.path.exists("./github_runner.sh"):