Bugfix/bldc controls

components/bldc_driver/include/bldc_driver.hpp

@@ -68,13 +68,16 @@ class BldcDriver {
    *        force level.
    */
   void enable() {
-    logger_.info("Enabling");
-    for (auto g : generators_) {
-      // remove force level, and don't hold it
-      mcpwm_generator_set_force_level(g, -1, false);
+    if (enabled_) {
+      return;
     }
-    gpio_set_level((gpio_num_t)gpio_en_, 1);
+    logger_.info("Enabling");
+    mcpwm_timer_enable(timer_);
+    mcpwm_timer_start_stop(timer_, MCPWM_TIMER_START_NO_STOP);
     enabled_ = true;
+    if (gpio_en_ >= 0) {
+      gpio_set_level((gpio_num_t)gpio_en_, 1);
+    }
   }
 
   /**
@@ -83,13 +86,13 @@ class BldcDriver {
    *        low.
    */
   void disable() {
-    logger_.info("Disabling");
-    for (auto g : generators_) {
-      // set force level to 0 (gate off), and hold it
-      mcpwm_generator_set_force_level(g, 0, true);
+    if (!enabled_) {
+      return;
     }
+    logger_.info("Disabling");
+    mcpwm_timer_start_stop(timer_, MCPWM_TIMER_STOP_FULL);
+    mcpwm_timer_disable(timer_);
     enabled_ = false;
-    std::lock_guard<std::mutex> lock(en_mutex_);
     if (gpio_en_ >= 0) {
       gpio_set_level((gpio_num_t)gpio_en_, 0);
     }
@@ -107,7 +110,6 @@ class BldcDriver {
    * @return True if the driver is faulted, false otherwise.
    */
   bool is_faulted() {
-    std::lock_guard<std::mutex> lock(fault_mutex_);
     if (gpio_fault_ < 0) {
       return false;
     }
@@ -174,14 +176,6 @@ class BldcDriver {
     uint32_t b_ticks = (uint32_t)(duty_b * (float)(TICKS_PER_PERIOD / 2));
     uint32_t c_ticks = (uint32_t)(duty_c * (float)(TICKS_PER_PERIOD / 2));
 
-    static int log_counter = 0;
-    // log_counter++;
-    if (log_counter == 500) {
-      fmt::print("PWM (abc): {:.3f},{:.3f},{:.3f}\n", duty_a, duty_b, duty_c);
-      fmt::print("Ticks (abc): {},{},{}\n", a_ticks, b_ticks, c_ticks);
-      log_counter = 0;
-    }
-
     mcpwm_comparator_set_compare_value(comparators_[0], a_ticks);
     mcpwm_comparator_set_compare_value(comparators_[1], b_ticks);
     mcpwm_comparator_set_compare_value(comparators_[2], c_ticks);
@@ -223,16 +217,10 @@ class BldcDriver {
     configure_operators();
     configure_comparators();
     configure_generators();
-    // start timer
-    logger_.info("starting timer");
-    mcpwm_timer_enable(timer_);
-    mcpwm_timer_start_stop(timer_, MCPWM_TIMER_START_NO_STOP);
-    // enable
     enable();
   }
 
   void configure_enable_gpio() {
-    std::lock_guard<std::mutex> lock(en_mutex_);
     if (gpio_en_ < 0) {
       return;
     }
@@ -246,7 +234,6 @@ class BldcDriver {
   }
 
   void configure_fault_gpio() {
-    std::lock_guard<std::mutex> lock(fault_mutex_);
     if (gpio_fault_ < 0) {
       return;
     }
@@ -347,9 +334,7 @@ class BldcDriver {
   gpio_num_t gpio_bl_;
   gpio_num_t gpio_ch_;
   gpio_num_t gpio_cl_;
-  std::mutex en_mutex_;
   int gpio_en_;
-  std::mutex fault_mutex_;
   int gpio_fault_;
   std::atomic<float> power_supply_voltage_;
   std::atomic<float> limit_voltage_;

components/bldc_haptics/include/bldc_haptics.hpp

@@ -141,11 +141,42 @@ template <MotorConcept M> class BldcHaptics {
                            .log_level = Logger::Verbosity::WARN});
   }
 
+  /// @brief Destructor for the haptic motor
+  /// @note This will stop the motor if it is running
+  ~BldcHaptics() {
+    // stop the motor if it is running
+    stop();
+  }
+
+  /// @brief Check if the haptic motor is running
+  /// @return True if the haptic motor is running, false otherwise
+  bool is_running() const { return motor_task_->is_running(); }
+
   /// @brief Start the haptic motor
-  void start() { motor_task_->start(); }
+  void start() {
+    // enable the motor
+    {
+      std::unique_lock<std::mutex> lk(motor_mutex_);
+      motor_.get().enable();
+    }
+    if (is_running()) {
+      return;
+    }
+    motor_task_->start();
+  }
 
   /// @brief Stop the haptic motor
-  void stop() { motor_task_->stop(); }
+  void stop() {
+    // disable the motor
+    {
+      std::unique_lock<std::mutex> lk(motor_mutex_);
+      motor_.get().disable();
+    }
+    if (!is_running()) {
+      return;
+    }
+    motor_task_->stop();
+  }
 
   /// @brief Get the current position of the haptic motor
   /// @return Current position of the haptic motor

components/bldc_motor/include/bldc_motor.hpp

@@ -169,8 +169,12 @@ class BldcMotor {
 
     // finish the rest of init
     init();
+    // enable the motor for foc initialization
+    enable();
     // then initialize the foc (calibration etc.)
     init_foc(config.zero_electric_offset, config.sensor_direction);
+    // disable the motor to put it back into a safe state
+    disable();
   }
 
   /**
@@ -179,12 +183,18 @@ class BldcMotor {
    */
   ~BldcMotor() { disable(); }
 
+  /**
+   * @brief Check if the motor is enabled.
+   * @return True if the motor is enabled, false otherwise.
+   */
+  bool is_enabled() const { return enabled_; }
+
   /**
    * @brief Enable the controller and driver output.
    */
   void enable() {
-    enabled_ = true;
     driver_->enable();
+    enabled_ = true;
   }
 
   /**
@@ -211,6 +221,10 @@ class BldcMotor {
    * @param el_angle current electrical angle of the motor
    */
   void set_phase_voltage(float uq, float ud, float el_angle) {
+    if (!enabled_) {
+      return;
+    }
+
     float center;
     int sector;
     float _ca, _sa;
@@ -642,11 +656,7 @@ class BldcMotor {
   }
 
 protected:
-  void init() {
-    status_ = Status::INITIALIZING;
-    enable();
-    status_ = Status::UNCALIBRATED;
-  }
+  void init() { status_ = Status::UNCALIBRATED; }
 
   void init_foc(float zero_electric_offset = 0,
                 detail::SensorDirection sensor_direction = detail::SensorDirection::CLOCKWISE) {

components/cli/include/cli.hpp

@@ -68,27 +68,26 @@ class Cli : private cli::CliSession {
      * correct while APB frequency is changing in light sleep mode.
      */
     const uart_config_t uart_config = {
-            .baud_rate = CONFIG_ESP_CONSOLE_UART_BAUDRATE,
-            .data_bits = UART_DATA_8_BITS,
-            .parity = UART_PARITY_DISABLE,
-            .stop_bits = UART_STOP_BITS_1,
+      .baud_rate = CONFIG_ESP_CONSOLE_UART_BAUDRATE,
+      .data_bits = UART_DATA_8_BITS,
+      .parity = UART_PARITY_DISABLE,
+      .stop_bits = UART_STOP_BITS_1,
 #if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2
-        .source_clk = UART_SCLK_REF_TICK,
+      .source_clk = UART_SCLK_REF_TICK,
 #else
-        .source_clk = UART_SCLK_XTAL,
+      .source_clk = UART_SCLK_XTAL,
 #endif
     };
     /* Install UART driver for interrupt-driven reads and writes */
-    ESP_ERROR_CHECK( uart_driver_install(CONFIG_ESP_CONSOLE_UART_NUM,
-            256, 0, 0, NULL, 0) );
-    ESP_ERROR_CHECK( uart_param_config(CONFIG_ESP_CONSOLE_UART_NUM, &uart_config) );
+    ESP_ERROR_CHECK(uart_driver_install(CONFIG_ESP_CONSOLE_UART_NUM, 256, 0, 0, NULL, 0));
+    ESP_ERROR_CHECK(uart_param_config(CONFIG_ESP_CONSOLE_UART_NUM, &uart_config));
     /* Tell VFS to use UART driver */
     esp_vfs_dev_uart_use_driver(CONFIG_ESP_CONSOLE_UART_NUM);
     /* Minicom, screen, idf_monitor send CR when ENTER key is pressed */
     esp_vfs_dev_uart_port_set_rx_line_endings(CONFIG_ESP_CONSOLE_UART_NUM, ESP_LINE_ENDINGS_CR);
     /* Move the caret to the beginning of the next line on '\n' */
     esp_vfs_dev_uart_port_set_tx_line_endings(CONFIG_ESP_CONSOLE_UART_NUM, ESP_LINE_ENDINGS_CRLF);
-#else // CONFIG_ESP_CONSOLE_USB_SERIAL_JTAG
+#else  // CONFIG_ESP_CONSOLE_USB_SERIAL_JTAG
     usb_serial_jtag_driver_config_t cfg = USB_SERIAL_JTAG_DRIVER_CONFIG_DEFAULT();
     usb_serial_jtag_driver_install(&cfg);
     esp_vfs_usb_serial_jtag_use_driver();

components/rtsp/example/main/rtsp_example.cpp

@@ -14,8 +14,8 @@
 #include "task.hpp"
 #include "wifi_sta.hpp"
 
-#include "rtsp_server.hpp"
 #include "rtsp_client.hpp"
+#include "rtsp_server.hpp"
 
 #include "jpeg_image.hpp"
 
@@ -64,7 +64,7 @@ extern "C" void app_main(void) {
       .port = server_port,
       .path = "/mjpeg/1",
       .log_level = espp::Logger::Verbosity::INFO,
-    });
+  });
   rtsp_server.start();
 
   espp::JpegFrame jpeg_frame(jpeg_data, sizeof(jpeg_data));
@@ -77,13 +77,15 @@ extern "C" void app_main(void) {
   //! [rtsp_client_example]
   espp::RtspClient rtsp_client({
       .server_address = ip_address, // string of the form {}.{}.{}.{}
-        .rtsp_port = CONFIG_RTSP_SERVER_PORT,
-        .path = "/mjpeg/1",
-        .on_jpeg_frame = [](std::unique_ptr<espp::JpegFrame> jpeg_frame) {
-          fmt::print("Got JPEG frame of size {}x{}\n", jpeg_frame->get_width(), jpeg_frame->get_height());
-        },
-        .log_level = espp::Logger::Verbosity::ERROR,
-        });
+      .rtsp_port = CONFIG_RTSP_SERVER_PORT,
+      .path = "/mjpeg/1",
+      .on_jpeg_frame =
+          [](std::unique_ptr<espp::JpegFrame> jpeg_frame) {
+            fmt::print("Got JPEG frame of size {}x{}\n", jpeg_frame->get_width(),
+                       jpeg_frame->get_height());
+          },
+      .log_level = espp::Logger::Verbosity::ERROR,
+  });
 
   std::error_code ec;