SNS - Pressure sensor

config/template.json

@@ -6,11 +6,16 @@
 				"pins": []
 			},
 			"can": {
-				"enabled": true,
+				"enabled": false,
 				"buses": [
 					"can1"
 				]
 			},
+			"gpio": {
+				"enabled": false,
+				"read_pins": [],
+				"write_pins": []
+			},
 			"i2c": {
 				"enabled": false,
 				"buses": []
@@ -39,13 +44,23 @@
 				"enabled": false,
 				"bus": 2,
 				"device_address": 56
+			},
+			"pressure": {
+				"enabled": false,
+				"pin": 0
 			}
 		},
 		"motors": {
 			"motor_controller": {
-				"enabled": true,
+				"enabled": false,
 				"bus": "can1"
 			}
+		},
+		"active_suspension": {
+			"enabled": false,
+			"pressure_sensor_pin": 0,
+			"lower_pressure_pin": 0,
+			"raise_pressure_pin": 0
 		}
 	}
 }

lib/debug/repl.cpp

@@ -11,7 +11,14 @@
 
 namespace hyped::debug {
 
-Repl::Repl(core::ILogger &logger) : logger_(logger), i2c_(), spi_(), pwm_(), adc_(), uart_()
+Repl::Repl(core::ILogger &logger)
+    : logger_(logger),
+      i2c_(),
+      spi_(),
+      pwm_(),
+      adc_(),
+      uart_(),
+      gpio_(io::HardwareGpio(logger))
 {
 }
 
@@ -117,6 +124,36 @@ std::optional<std::unique_ptr<Repl>> Repl::fromFile(const std::string &path)
       return std::nullopt;
     }
   }
+  if (!io.HasMember("gpio")) {
+    logger_.log(core::LogLevel::kFatal, "Missing required field 'io.gpio' in configuration file");
+    return std::nullopt;
+  }
+  const auto gpio = io["gpio"].GetObject();
+  if (!gpio.HasMember("enabled")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'io.gpio.enabled' in configuration file");
+    return std::nullopt;
+  }
+  if (gpio["enabled"].GetBool()) {
+    if (!gpio.HasMember("read_pins")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'io.gpio.read_pins' in configuration file");
+      return std::nullopt;
+    }
+    const auto read_pins = gpio["read_pins"].GetArray();
+    for (auto &pin : read_pins) {
+      repl->addGpioReadCommands(pin.GetUint());
+    }
+    if (!gpio.HasMember("write_pins")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'io.gpio.write_pins' in configuration file");
+      return std::nullopt;
+    }
+    const auto write_pins = gpio["write_pins"].GetArray();
+    for (auto &pin : write_pins) {
+      repl->addGpioWriteCommands(pin.GetUint());
+    }
+  }
   const auto buses = i2c["buses"].GetArray();
   for (auto &bus : buses) {
     repl->addI2cCommands(bus.GetUint());
@@ -257,6 +294,26 @@ std::optional<std::unique_ptr<Repl>> Repl::fromFile(const std::string &path)
     const auto bus            = temperature["bus"].GetUint();
     repl->addTemperatureCommands(bus, device_address);
   }
+  if (!sensors.HasMember("pressure")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'sensors.pressure' in configuration file");
+    return std::nullopt;
+  }
+  const auto pressure = sensors["pressure"].GetObject();
+  if (!pressure.HasMember("enabled")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'sensors.pressure.enabled' in configuration file");
+    return std::nullopt;
+  }
+  if (pressure["enabled"].GetBool()) {
+    if (!pressure.HasMember("pin")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'sensors.pressure.pin' in configuration file");
+      return std::nullopt;
+    }
+    const auto pin = pressure["pin"].GetUint();
+    repl->addPressureCommands(pin);
+  }
   if (!debugger.HasMember("motors")) {
     logger_.log(core::LogLevel::kFatal,
                 "Missing required field 'debugger.motors' in configuration file");
@@ -283,6 +340,41 @@ std::optional<std::unique_ptr<Repl>> Repl::fromFile(const std::string &path)
     const auto bus = motor_controller["bus"].GetString();
     repl->addMotorControllerCommands(bus);
   }
+  if (!debugger.HasMember("active_suspension")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'debugger.active_suspension' in configuration file");
+    return std::nullopt;
+  }
+  const auto active_suspension = debugger["active_suspension"].GetObject();
+  if (!active_suspension.HasMember("enabled")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'active_suspension.enabled' in configuration file");
+    return std::nullopt;
+  }
+  if (active_suspension["enabled"].GetBool()) {
+    if (!active_suspension.HasMember("pressure_sensor_pin")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'active_suspension.pressure_sensor_pin' in "
+                  "configuration file");
+      return std::nullopt;
+    }
+    if (!active_suspension.HasMember("lower_pressure_pin")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'active_suspension.lower_pressure_pin' in "
+                  "configuration file");
+      return std::nullopt;
+    }
+    if (!active_suspension.HasMember("raise_pressure_pin")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'active_suspension.raise_pressure_pin' in "
+                  "configuration file");
+      return std::nullopt;
+    }
+    const auto pressure_sensor_pin = active_suspension["pressure_sensor_pin"].GetUint();
+    const auto lower_pressure_pin  = active_suspension["lower_pressure_pin"].GetUint();
+    const auto raise_pressure_pin  = active_suspension["raise_pressure_pin"].GetUint();
+    repl->addActiveSuspensionCommands(pressure_sensor_pin, lower_pressure_pin, raise_pressure_pin);
+  }
   return repl;
 }
 
@@ -393,6 +485,74 @@ void Repl::addCanCommands(const std::string &bus)
   addCommand(can_write_command);
 }
 
+void Repl::addGpioReadCommands(const std::uint8_t pin)
+{
+  const auto optional_gpio_reader = gpio_.getReader(pin);
+  if (!optional_gpio_reader) {
+    logger_.log(core::LogLevel::kFatal, "Failed to create GPIO reader on pin %d", pin);
+    return;
+  }
+  const auto gpio_reader = std::move(*optional_gpio_reader);
+  Command gpio_read_command;
+  std::stringstream identifier;
+  identifier << "gpio " << static_cast<int>(pin) << " read";
+  gpio_read_command.name = identifier.str();
+  std::stringstream description;
+  description << "Read from GPIO pin " << static_cast<int>(pin);
+  gpio_read_command.description = description.str();
+  gpio_read_command.handler     = [this, gpio_reader, pin]() {
+    const auto value = gpio_reader->read();
+    if (!value) {
+      logger_.log(core::LogLevel::kFatal, "Failed to read from GPIO pin %d", pin);
+      return;
+    }
+    logger_.log(
+      core::LogLevel::kDebug, "GPIO value from pin %d: %d", pin, static_cast<std::uint8_t>(*value));
+  };
+}
+
+void Repl::addGpioWriteCommands(const std::uint8_t pin)
+{
+  const auto optional_gpio_writer = gpio_.getWriter(pin);
+  if (!optional_gpio_writer) {
+    logger_.log(core::LogLevel::kFatal, "Failed to create GPIO writer on pin %d", pin);
+    return;
+  }
+  const auto gpio_writer = std::move(*optional_gpio_writer);
+  Command gpio_write_command;
+  std::stringstream identifier;
+  identifier << "gpio " << static_cast<int>(pin) << " write";
+  gpio_write_command.name = identifier.str();
+  std::stringstream description;
+  description << "Write to GPIO pin " << static_cast<int>(pin);
+  gpio_write_command.description = description.str();
+  gpio_write_command.handler     = [this, gpio_writer, pin]() {
+    std::cout << "Enter GPIO value: ";
+    std::uint8_t value;
+    std::cin >> std::hex >> value;
+    std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
+    core::DigitalSignal signal;
+    switch (value) {
+      case 0:
+        signal = core::DigitalSignal::kLow;
+        break;
+      case 1:
+        signal = core::DigitalSignal::kHigh;
+        break;
+      default:
+        logger_.log(core::LogLevel::kFatal, "Invalid GPIO value: %d, must be 0 or 1", value);
+        break;
+    }
+    const auto result = gpio_writer->write(signal);
+    if (result == core::Result::kFailure) {
+      logger_.log(core::LogLevel::kFatal, "Failed to write to GPIO pin %d", pin);
+      return;
+    }
+    logger_.log(core::LogLevel::kDebug, "Wrote %d to GPIO pin %d", value, pin);
+  };
+  addCommand(gpio_write_command);
+}
+
 void Repl::addI2cCommands(const std::uint8_t bus)
 {
   const auto optional_i2c = getI2c(bus);
@@ -879,6 +1039,107 @@ void Repl::addMotorControllerCommands(const std::string &bus)
   addCommand(frequency_time_command);
 }
 
+void Repl::addPressureCommands(const std::uint8_t pin)
+{
+  const auto optional_adc = getAdc(pin);
+  if (!optional_adc) {
+    logger_.log(core::LogLevel::kFatal, "Failed to create adc instance");
+    return;
+  }
+  const auto adc             = std::move(*optional_adc);
+  const auto pressure_sensor = std::make_shared<sensors::Pressure>(logger_, adc);
+  Command pressure_sensor_read_command;
+  pressure_sensor_read_command.name        = "pressure sensor read";
+  pressure_sensor_read_command.description = "Read the pressure sensor";
+  pressure_sensor_read_command.handler     = [this, pressure_sensor]() {
+    const auto pressure = pressure_sensor->read();
+    if (!pressure) {
+      logger_.log(core::LogLevel::kFatal, "Failed to read pressure sensor");
+      return;
+    }
+    logger_.log(core::LogLevel::kInfo, "Pressure: %f", *pressure);
+  };
+  addCommand(pressure_sensor_read_command);
+}
+
+void Repl::addActiveSuspensionCommands(const std::uint8_t adc_pin,
+                                       const std::uint8_t lower_pressure_pin,
+                                       const std::uint8_t raise_pressure_pin)
+{
+  const auto optional_adc = getAdc(adc_pin);
+  if (!optional_adc) {
+    logger_.log(core::LogLevel::kFatal, "Failed to create adc instance");
+    return;
+  }
+  const auto adc                          = std::move(*optional_adc);
+  const auto optional_raise_pressure_gpio = gpio_.getWriter(raise_pressure_pin);
+  if (!optional_raise_pressure_gpio) {
+    logger_.log(core::LogLevel::kFatal,
+                "Failed to create gpio writer for raise pressure pin %d",
+                raise_pressure_pin);
+    return;
+  }
+  const auto raise_pressure_gpio          = std::move(*optional_raise_pressure_gpio);
+  const auto optional_lower_pressure_gpio = gpio_.getWriter(lower_pressure_pin);
+  if (!optional_lower_pressure_gpio) {
+    logger_.log(core::LogLevel::kFatal,
+                "Failed to create gpio writer for lower pressure pin %d",
+                lower_pressure_pin);
+    return;
+  }
+  const auto lower_pressure_gpio = std::move(*optional_lower_pressure_gpio);
+  const auto pressure_sensor     = std::make_shared<sensors::Pressure>(logger_, adc);
+  Command active_suspension_set_command;
+  active_suspension_set_command.name        = "active suspension set";
+  active_suspension_set_command.description = "Set the active suspension to specified pressure";
+  active_suspension_set_command.handler
+    = [this, pressure_sensor, lower_pressure_gpio, raise_pressure_gpio]() {
+        std::cout << "Enter pressure to set (bar)" << std::endl;
+        core::Float pressure;
+        std::cin >> pressure;
+        std::cout << "Enter precision (%)" << std::endl;
+        core::Float precision;
+        std::cin >> precision;
+        std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
+        const core::Float lower_bound = pressure * (1 - precision / 100);
+        const core::Float upper_bound = pressure * (1 + precision / 100);
+        std::uint8_t in_range_count   = 0;
+        while (1) {
+          const auto optional_current_pressure = pressure_sensor->read();
+          if (!optional_current_pressure) {
+            logger_.log(core::LogLevel::kFatal, "Failed to read pressure sensor");
+            return;
+          }
+          const core::Float current_pressure = *optional_current_pressure;
+          if (in_range_count == 100) {
+            logger_.log(core::LogLevel::kInfo, "Pressure set to %f", current_pressure);
+            return;
+          }
+          if (current_pressure >= lower_bound && current_pressure <= upper_bound) {
+            logger_.log(
+              core::LogLevel::kInfo, "Pressure in range, current pressure %f", current_pressure);
+            raise_pressure_gpio->write(core::DigitalSignal::kLow);
+            lower_pressure_gpio->write(core::DigitalSignal::kLow);
+            ++in_range_count;
+            continue;
+          }
+          in_range_count = 0;
+          if (current_pressure < lower_bound) {
+            logger_.log(
+              core::LogLevel::kInfo, "Lowering pressure, current pressure %f", current_pressure);
+            raise_pressure_gpio->write(core::DigitalSignal::kHigh);
+            lower_pressure_gpio->write(core::DigitalSignal::kLow);
+          }
+          if (current_pressure > upper_bound) {
+            logger_.log(
+              core::LogLevel::kInfo, "Raising pressure, current pressure %f", current_pressure);
+            raise_pressure_gpio->write(core::DigitalSignal::kLow);
+            lower_pressure_gpio->write(core::DigitalSignal::kHigh);
+          }
+        }
+      };
+}
+
 std::optional<std::shared_ptr<io::IAdc>> Repl::getAdc(const std::uint8_t bus)
 {
   const auto adc = adc_.find(bus);

lib/debug/repl.hpp

@@ -24,6 +24,7 @@
 #include <motors/frequency_calculator.hpp>
 #include <motors/time_frequency_calculator.hpp>
 #include <sensors/accelerometer.hpp>
+#include <sensors/pressure.hpp>
 #include <sensors/temperature.hpp>
 
 namespace hyped::debug {
@@ -49,13 +50,19 @@ class Repl {
   void addHelpCommand();
   void addAdcCommands(const std::uint8_t pin);
   void addCanCommands(const std::string &bus);
+  void addGpioReadCommands(const std::uint8_t pin);
+  void addGpioWriteCommands(const std::uint8_t pin);
   void addI2cCommands(const std::uint8_t bus);
   void addPwmCommands(const std::uint8_t module, const std::uint32_t period);
   void addSpiCommands(const std::uint8_t bus);
   void addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t device_address);
   void addTemperatureCommands(const std::uint8_t bus, const std::uint8_t device_address);
   void addUartCommands(const std::uint8_t bus);
   void addMotorControllerCommands(const std::string &bus);
+  void addPressureCommands(const std::uint8_t pin);
+  void addActiveSuspensionCommands(const std::uint8_t pressure_sensor_pin,
+                                   const std::uint8_t lower_pressure_pin,
+                                   const std::uint8_t raise_pressure_pin);
 
   /**
    * @brief Get the Adc object associated with the given pin or create a new one if it doesn't exist
@@ -125,8 +132,9 @@ class Repl {
 
   core::ILogger &logger_;
   std::map<std::string, Command> command_map_;
-  std::unordered_map<std::string, std::shared_ptr<io::ICan>> can_;
   std::unordered_map<std::uint8_t, std::shared_ptr<io::IAdc>> adc_;
+  std::unordered_map<std::string, std::shared_ptr<io::ICan>> can_;
+  io::HardwareGpio gpio_;
   std::unordered_map<std::uint8_t, std::shared_ptr<io::II2c>> i2c_;
   std::unordered_map<io::PwmModule, std::shared_ptr<io::Pwm>> pwm_;
   std::unordered_map<io::SpiBus, std::shared_ptr<io::ISpi>> spi_;