diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index dbfcebe75b..d1bd91357c 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -6,7 +6,9 @@ if arch == "aarch64": lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', cereal, messaging, 'capnp', 'zmq', 'kj']) else: sensors = [ + 'sensors/file_sensor.cc', 'sensors/i2c_sensor.cc', + 'sensors/light_sensor.cc', 'sensors/bmx055_accel.cc', 'sensors/bmx055_gyro.cc', 'sensors/bmx055_magn.cc', diff --git a/selfdrive/sensord/sensors/file_sensor.cc b/selfdrive/sensord/sensors/file_sensor.cc new file mode 100644 index 0000000000..6d03ef1b1f --- /dev/null +++ b/selfdrive/sensord/sensors/file_sensor.cc @@ -0,0 +1,15 @@ +#include +#include + +#include "file_sensor.hpp" + +FileSensor::FileSensor(std::string filename) : file(filename) { +} + +int FileSensor::init() { + return file.is_open() ? 0 : 1; +} + +FileSensor::~FileSensor(){ + file.close(); +} diff --git a/selfdrive/sensord/sensors/file_sensor.hpp b/selfdrive/sensord/sensors/file_sensor.hpp new file mode 100644 index 0000000000..25a6f203c8 --- /dev/null +++ b/selfdrive/sensord/sensors/file_sensor.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include +#include + +#include "cereal/gen/cpp/log.capnp.h" +#include "sensors/sensor.hpp" + + +class FileSensor : public Sensor { +protected: + std::ifstream file; + +public: + FileSensor(std::string filename); + ~FileSensor(); + int init(); + virtual void get_event(cereal::SensorEventData::Builder &event) = 0; +}; diff --git a/selfdrive/sensord/sensors/i2c_sensor.hpp b/selfdrive/sensord/sensors/i2c_sensor.hpp index 37d7568736..39ef79cf83 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.hpp +++ b/selfdrive/sensord/sensors/i2c_sensor.hpp @@ -3,13 +3,14 @@ #include #include "cereal/gen/cpp/log.capnp.h" #include "common/i2c.h" +#include "sensors/sensor.hpp" #include "sensors/constants.hpp" int16_t read_12_bit(uint8_t lsb, uint8_t msb); int16_t read_16_bit(uint8_t lsb, uint8_t msb); -class I2CSensor { +class I2CSensor : public Sensor { private: I2CBus *bus; virtual uint8_t get_device_address() = 0; diff --git a/selfdrive/sensord/sensors/light_sensor.cc b/selfdrive/sensord/sensors/light_sensor.cc new file mode 100644 index 0000000000..eb3eb05f17 --- /dev/null +++ b/selfdrive/sensord/sensors/light_sensor.cc @@ -0,0 +1,23 @@ +#include +#include + +#include "common/timing.h" + +#include "light_sensor.hpp" +#include "constants.hpp" + +void LightSensor::get_event(cereal::SensorEventData::Builder &event){ + uint64_t start_time = nanos_since_boot(); + file.clear(); + file.seekg(0); + + int value; + file >> value; + + event.setSource(cereal::SensorEventData::SensorSource::RPR0521); + event.setVersion(1); + event.setSensor(SENSOR_LIGHT); + event.setType(SENSOR_TYPE_LIGHT); + event.setTimestamp(start_time); + event.setLight(value); +} diff --git a/selfdrive/sensord/sensors/light_sensor.hpp b/selfdrive/sensord/sensors/light_sensor.hpp new file mode 100644 index 0000000000..7c98cb29cf --- /dev/null +++ b/selfdrive/sensord/sensors/light_sensor.hpp @@ -0,0 +1,8 @@ +#pragma once +#include "file_sensor.hpp" + +class LightSensor : public FileSensor { +public: + LightSensor(std::string filename) : FileSensor(filename){}; + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/sensor.hpp b/selfdrive/sensord/sensors/sensor.hpp new file mode 100644 index 0000000000..91f4179081 --- /dev/null +++ b/selfdrive/sensord/sensors/sensor.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include "cereal/gen/cpp/log.capnp.h" + +class Sensor { +public: + virtual int init() = 0; + virtual void get_event(cereal::SensorEventData::Builder &event) = 0; +}; diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index 0ef8b2a0ca..61dda58111 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -9,10 +9,12 @@ #include "common/timing.h" #include "common/swaglog.h" +#include "sensors/sensor.hpp" #include "sensors/constants.hpp" #include "sensors/bmx055_accel.hpp" #include "sensors/bmx055_gyro.hpp" #include "sensors/bmx055_magn.hpp" +#include "sensors/light_sensor.hpp" volatile sig_atomic_t do_exit = 0; @@ -36,15 +38,17 @@ int sensor_loop() { BMX055_Accel accel(i2c_bus_imu); BMX055_Gyro gyro(i2c_bus_imu); BMX055_Magn magn(i2c_bus_imu); + LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw"); // Sensor init - std::vector sensors; + std::vector sensors; sensors.push_back(&accel); sensors.push_back(&gyro); + sensors.push_back(&light); // sensors.push_back(&magn); - for (I2CSensor * sensor : sensors){ + for (Sensor * sensor : sensors){ int err = sensor->init(); if (err < 0){ LOGE("Error initializing sensors");