Merge pull request #151 from Hyp-ed/brake_pressure

SNS - Brake Pressure

Author: mifrandir

configurations/debugging/config.json

@@ -1,7 +1,8 @@
 {
   "sensors": {
     "imu_pins": [],
-    "temperature_pins": []
+    "temperature_pins": [],
+    "brake_pressure_pins": []
   },
   "debugger": {
     "output_path": "readings.json",
@@ -27,6 +28,8 @@
     "use_fake_brakes": false,
     "use_fake_controller": false,
     "use_fake_high_power": false,
+    "use_fake_brake_pressure": false,
+    "use_fake_brake_pressure_fail": false,
     "axis": 0
   },
   "fake_trajectory": {

src/debugging/can_listener.cpp

@@ -21,7 +21,8 @@ bool CanListener::hasId(uint32_t id, bool extended)
     log_.debug("received extended CAN message; skipping");
     return false;
   }
-  return ids_.contains(id);
+
+  return ids_.find(id) != ids_.end(); 
 }
 
 void CanListener::subscribe(const uint32_t id)
@@ -31,7 +32,7 @@ void CanListener::subscribe(const uint32_t id)
 
 void CanListener::unsubscribe(const uint32_t id)
 {
-  if (!ids_.contains(id)) {
+  if (ids_.find(id) == ids_.end()) {
     log_.error("tried to unsubscribe from unknown node_id");
     return;
   }

src/debugging/observer.cpp

@@ -5,6 +5,7 @@
 #include <rapidjson/istreamwrapper.h>
 
 #include <sensors/fake_temperature.hpp>
+#include <sensors/fake_brake_pressure.hpp>
 #include <sensors/imu.hpp>
 #include <sensors/main.hpp>
 #include <utils/logger.hpp>
@@ -138,6 +139,22 @@ std::optional<std::unique_ptr<Observer>> Observer::fromFile(const std::string &p
       observer->addTemperatureTask(pin);
     }
   }
+  // BRAKE_PRESSURE
+  if (system.config_.use_fake_brake_pressure) {
+    observer->addFakeBrakePressureTask(false);
+  } else if (system.config_.use_fake_brake_pressure_fail) {
+    observer->addFakeBrakePressureTask(true);
+  } else {
+    const auto pins
+      = sensors::Main::brakePressurePinsFromFile(log, system.config_.pressure_config_path);
+    if (!pins) {
+      log.error("failed to read brake pressure pin from file");
+      return std::nullopt;
+    }
+    for (const auto pin : *pins) {
+      observer->addBrakePressureTask(pin);
+    }
+  }
   return observer;
 }
 
@@ -285,4 +302,35 @@ void Observer::addFakeTemperatureTask(const bool is_fail)
   tasks_.push_back(temperature_task);
 }
 
+void Observer::addBrakePressureTask(const uint8_t pin)
+{
+  auto brake_pressure = std::make_shared<sensors::BrakePressure>(pin);
+  std::stringstream name;
+  name << "brake_pressure-" << static_cast<uint32_t>(pin);
+  Task brake_pressure_task;
+  brake_pressure_task.name    = name.str();
+  brake_pressure_task.handler = [brake_pressure](JsonWriter &json_writer) {
+    brake_pressure->run();
+    json_writer.Key("value");
+    json_writer.Uint(brake_pressure->getData());
+  };
+  tasks_.push_back(brake_pressure_task);
+}
+
+void Observer::addFakeBrakePressureTask(const bool is_fail)
+{
+  static uint32_t fake_brake_pressure_id = 0;
+  auto brake_pressure                   = std::make_shared<sensors::FakeBrakePressure>(is_fail);
+  std::stringstream name;
+  name << "fake_brake_pressure-" << fake_brake_pressure_id++;
+  Task brake_pressure_task;
+  brake_pressure_task.name    = name.str();
+  brake_pressure_task.handler = [brake_pressure](JsonWriter &json_writer) {
+    brake_pressure->run();
+    json_writer.Key("value");
+    json_writer.Uint(brake_pressure->getData());
+  };
+  tasks_.push_back(brake_pressure_task);
+}
+
 }  // namespace hyped::debugging

src/debugging/observer.hpp

@@ -41,6 +41,8 @@ class Observer : public utils::concurrent::Thread {
 
   void addTemperatureTask(const uint8_t pin);
   void addFakeTemperatureTask(const bool is_fail);
+  void addBrakePressureTask(const uint8_t pin);
+  void addFakeBrakePressureTask(const bool is_fail);
 };
 
 }  // namespace hyped::debugging

src/utils/io/gpio.cpp

@@ -20,7 +20,7 @@ namespace hyped {
 namespace utils {
 namespace io {
 
-#define GPIOFS 0  // used to swtich to file system method for set(), clear(), and read()
+#define GPIOFS 0  // used to switch to file system method for set(), clear(), and read()
 
 // workaround to avoid conflict with GPIO::read()
 static uint8_t readHelper(int file)

src/utils/system.cpp

@@ -91,7 +91,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.log_level_brakes = *log_level;
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.log_level_brakes' in config filet at %s; using default value",
+      "could not find field 'system.log_level_brakes' in config file at %s; using default value",
       argv[1]);
     config.log_level_brakes = config.log_level;
   }
@@ -107,7 +107,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.log_level_navigation = *log_level;
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.log_level_navigation' in config filet at %s; using default "
+      "could not find field 'system.log_level_navigation' in config file at %s; using default "
       "value",
       argv[1]);
     config.log_level_navigation = config.log_level;
@@ -124,7 +124,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.log_level_propulsion = *log_level;
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.log_level_propulsion' in config filet at %s; using default "
+      "could not find field 'system.log_level_propulsion' in config file at %s; using default "
       "value",
       argv[1]);
     config.log_level_propulsion = config.log_level;
@@ -141,7 +141,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.log_level_sensors = *log_level;
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.log_level_sensors' in config filet at %s; using default value",
+      "could not find field 'system.log_level_sensors' in config file at %s; using default value",
       argv[1]);
     config.log_level_sensors = config.log_level;
   }
@@ -157,7 +157,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.log_level_state_machine = *log_level;
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.log_level_state_machine' in config filet at %s; using default "
+      "could not find field 'system.log_level_state_machine' in config file at %s; using default "
       "value",
       argv[1]);
     config.log_level_state_machine = config.log_level;
@@ -174,7 +174,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.log_level_telemetry = *log_level;
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.log_level_telemetry' in config filet at %s; using default "
+      "could not find field 'system.log_level_telemetry' in config file at %s; using default "
       "value",
       argv[1]);
     config.log_level_telemetry = config.log_level;
@@ -199,7 +199,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_trajectory = config_object["use_fake_trajectory"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_trajectory' in config filet at %s; using default "
+      "could not find field 'system.use_fake_trajectory' in config file at %s; using default "
       "value",
       argv[1]);
     config.use_fake_trajectory = false;
@@ -209,7 +209,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_batteries = config_object["use_fake_batteries"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_batteries' in config filet at %s; using default "
+      "could not find field 'system.use_fake_batteries' in config file at %s; using default "
       "value",
       argv[0]);
     config.use_fake_batteries = false;
@@ -219,7 +219,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_batteries_fail = config_object["use_fake_batteries_fail"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_batteries_fail' in config filet at %s; using default "
+      "could not find field 'system.use_fake_batteries_fail' in config file at %s; using default "
       "value",
       argv[0]);
     config.use_fake_batteries_fail = false;
@@ -229,7 +229,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_temperature = config_object["use_fake_temperature"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_temperature' in config filet at %s; using default "
+      "could not find field 'system.use_fake_temperature' in config file at %s; using default "
       "value",
       argv[0]);
     config.use_fake_temperature = false;
@@ -239,7 +239,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_temperature_fail = config_object["use_fake_temperature_fail"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_temperature_fail' in config filet at %s; using "
+      "could not find field 'system.use_fake_temperature_fail' in config file at %s; using "
       "default "
       "value",
       argv[0]);
@@ -251,7 +251,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_ambient_pressure = config_object["use_fake_ambient_pressure"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_ambient_pressure' in config filet at %s; using "
+      "could not find field 'system.use_fake_ambient_pressure' in config file at %s; using "
       "default "
       "value",
       argv[0]);
@@ -263,7 +263,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
       = config_object["use_fake_ambient_pressure_fail"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_ambient_pressure_fail' in config filet at %s; using "
+      "could not find field 'system.use_fake_ambient_pressure_fail' in config file at %s; using "
       "default "
       "value",
       argv[0]);
@@ -275,7 +275,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_brake_pressure = config_object["use_fake_brake_pressure"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_brake_pressure' in config filet at %s; using "
+      "could not find field 'system.use_fake_brake_pressure' in config file at %s; using "
       "default "
       "value",
       argv[0]);
@@ -286,7 +286,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_ambient_pressure_fail = config_object["use_fake_brake_pressure_fail"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_brake_pressure_fail' in config filet at %s; using "
+      "could not find field 'system.use_fake_brake_pressure_fail' in config file at %s; using "
       "default "
       "value",
       argv[0]);
@@ -298,7 +298,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_brakes = config_object["use_fake_brakes"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_brakes' in config filet at %s; using default "
+      "could not find field 'system.use_fake_brakes' in config file at %s; using default "
       "value",
       argv[0]);
     config.use_fake_brakes = false;
@@ -308,7 +308,7 @@ void System::parseArgs(const int argc, const char *const *const argv)
     config.use_fake_controller = config_object["use_fake_controller"].GetBool();
   } else {
     kInitialisationErrorLogger.info(
-      "could not find field 'system.use_fake_controller' in config filet at %s; using default "
+      "could not find field 'system.use_fake_controller' in config file at %s; using default "
       "value",
       argv[0]);
     config.use_fake_controller = false;