Load: Comms integration & ported related nacelle changes

Author: BobSaidHi

include/LoadContainer.hpp

@@ -40,29 +40,75 @@ class LoadContainer {
     etl::array<TaskInfo *, NUM_OPTIONAL_TASKS> optionalTaskDescriptions = {
         &tTelnet, &tOTA};
 
-    LoadContainer(MCP23008T &loadDevice) : loadDevice(loadDevice) {}
+    LoadContainer(MCP23008T &loadDevice, LoadComms &loadComms)
+        : loadDevice(loadDevice), loadComms(loadComms) {}
     ~LoadContainer() = default;
 
-    inline bool getSafetyFlag() const { return safetyFlag; }
+    inline bool getSafetyFlag() const { return (safetyFlag); }
     inline bool isPowerPositive() const { return (INA260::current_mA > 0); }
-    inline bool isSteadyRPM() const { return false; }         // todo - steady power is actually more important
+    inline bool isSteadyRPM() const {
+        return false;
+    } // todo - steady power is actually more important
     inline bool isTargetRPMExceeded() const { return false; } // todo
 
     inline void updateSafetyFlag(bool safetyFlag) {
-        this->safetyFlag = (digitalRead(UM_PROS3::ESTOP_PIN) ==
-                            LOW) /*|| getPPCCurrent() < threshold*/; // todo
+        this->safetyFlag =
+            (digitalRead(UM_PROS3::ESTOP_PIN) == LOW) || (!isPowerPositive());
     }
     // inline void updatePowerPositive(bool powerPositive) {
     //     this->powerPositive = powerPositive;
     // }
     inline void setLoadGPIO(uint_fast8_t value) {
         loadDevice.setGPIO((uint32_t)value);
     }
+    inline void setRPM(int_fast16_t rpm) { this->currentRPM = rpm; }
 
   private:
     MCP23008T &loadDevice;
-
     bool safetyFlag = false; // todo
+    LoadComms &loadComms;
     // bool powerPositive = false;
     int_fast16_t currentRPM = 0; // todo
+
+    /**
+     * @brief Check for C++17 support, which allows us to verify if std::atomic
+     * is a acceptable (lock free) solution for shared variables
+     * @see https://stackoverflow.com/a/49915536
+     */
+    static_assert(
+        (__cplusplus >= 201703L),
+        "C++17 or higher is required for std::atomic is_always_lock_free");
+
+    /**
+     * @brief Do some basic checks regarding std::atomic and data types
+     * From C++14.2.0 atomic.h:
+     * Check Lock-free property.
+     *
+     * 0 indicates that the types are never lock-free.
+     * 1 indicates that the types are sometimes lock-free.
+     * 2 indicates that the types are always lock-free.
+     */
+    static_assert(sizeof(int) == sizeof(int_fast16_t),
+                  "Atomic Lock-free check issue");
+#if (ATOMIC_INT_LOCK_FREE == 0)
+#    error "Atomic operations on int are not lock-free on this platform."
+#elif (ATOMIC_INT_LOCK_FREE == 1)
+#    warning                                                                   \
+        "Atomic operations on int are only sometimes lock-free on this platform."
+#endif
+
+    /**
+     * @brief Check if std::atomic<int_fast16_t> is an acceptable (lock free)
+     * solution for shared variables
+     * @see https://www.reddit.com/r/embedded/comments/zn23of/comment/j0fav6o/
+     * @see
+     * https://stackoverflow.com/questions/63471387/should-volatile-still-be-used-for-sharing-data-with-isrs-in-modern-c
+     * @see https://en.cppreference.com/w/c/language/atomic.html
+     * @see https://en.cppreference.com/w/cpp/atomic/atomic.html
+     * @see https://stackoverflow.com/a/16783513
+     */
+    // static_assert(std::atomic<int_fast16_t>::is_always_lock_free,
+    //               "Atomic operations on int_fast16_t are not lock-free on "
+    //               "this platform.");
+    // std::atomic<int_fast16_t> currentRPM = 0;
 };

lib/LoadComms/LoadComms.cpp

@@ -6,16 +6,17 @@
  * Sends state, E-stop, and actuator position data while receiving RPM data.
  */
 
-#include "LoadComms.h"
+#include "LoadComms.hpp"
+#include <esp_log.h>
+
+// Initialization of static members
+QueueHandle_t LoadComms::priorityDataQueue = nullptr;
 
 /**
  * @brief MAC address of the nacelle controller.
  */
 const uint8_t NACELLE_MAC[] = {0x30, 0xED, 0xA0, 0xE0, 0x6B, 0x78};
 
-static constexpr char* TAG = "LoadboxComms";
-constexpr uint8_t wiFiChannel = 6;
-
 /**
  * @brief Pointer to instance for static callbacks.
  */
@@ -24,8 +25,9 @@ static LoadComms *s_instance = nullptr;
 LoadComms::LoadComms()
     : lastSendTime_(0),
       lastRxTime_(0),
-      linkAlive_(false),
-      nacelleRPM_(0.0f) {
+      linkAlive_(false) //,
+      // nacelleRPM_(0.0f) 
+      {
   s_instance = this;
 }
 
@@ -51,41 +53,59 @@ bool LoadComms::begin() {
     }
 
   if (esp_now_init() != ESP_OK) {
-    Serial.println("ESP-NOW init failed");
+    ESP_LOGE(TAG, "ESP-NOW init failed");
+    return false;
+  }
+
+  if(esp_now_register_send_cb(onDataSent_) != ESP_OK) {
+    ESP_LOGE(TAG, "Failed to register tx cb");
     return false;
   }
 
-  esp_now_register_send_cb(onDataSent_);
-  esp_now_register_recv_cb(onDataRecv_);
+  if(esp_now_register_recv_cb(onDataRecv_) != ESP_OK) {
+    ESP_LOGE(TAG, "Failed to register rx cb");
+    return false;
+  }
 
-  setupPeer_();
+  if( setupPeer_() != ESP_OK) {
+    // Logging already handled
+    return false;
+  }
 
   Serial.println("Loadbox ready");
   return true;
 }
 
-void LoadComms::setupPeer_() {
+esp_err_t LoadComms::setupPeer_() {
   esp_now_peer_info_t peerInfo = {};
   memcpy(peerInfo.peer_addr, NACELLE_MAC, 6);
   peerInfo.channel = 0;
   peerInfo.encrypt = false;
 
-  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
-    Serial.println("Failed to add peer");
-    return;
+  esp_err_t result = esp_now_add_peer(&peerInfo);
+  if (result != ESP_OK) {
+    ESP_LOGE(TAG, "Failed to add peer: %d at %02x:%02x:%02x:%02x:%02x:%02x", result,
+             NACELLE_MAC[0], NACELLE_MAC[1], NACELLE_MAC[2],
+             NACELLE_MAC[3], NACELLE_MAC[4], NACELLE_MAC[5]);
+  } else{
+    ESP_LOGI(TAG, "Peer added: %02X:%02X:%02X:%02X:%02X:%02X",
+           NACELLE_MAC[0], NACELLE_MAC[1], NACELLE_MAC[2],
+           NACELLE_MAC[3], NACELLE_MAC[4], NACELLE_MAC[5]);
   }
 
-  Serial.println("Peer added");
+  return result;
 }
 
 void LoadComms::onDataSent_(const wifi_tx_info_t *tx_info, esp_now_send_status_t status) {
-  (void)tx_info;
-  Serial.print("Send status: ");
-  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Fail");
+  // (void)tx_info;
+  // Serial.print("Send status: ");
+  // Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Fail");
+  // This is check elsewhere
 }
 
 void LoadComms::onDataRecv_(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len) {
   if (s_instance == nullptr) {
+    ESP_LOGE(TAG, "Rx CB: Invalid instance");
     return;
   }
 
@@ -95,47 +115,53 @@ void LoadComms::onDataRecv_(const esp_now_recv_info_t *recv_info, const uint8_t
                 mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
 
   if (len == sizeof(NacellePacket)) {
-    memcpy(&s_instance->incomingPacket_, data, sizeof(NacellePacket));
+    // memcpy(&s_instance->incomingPacket_, data, sizeof(NacellePacket));
 
     s_instance->lastRxTime_ = millis();
     s_instance->linkAlive_ = true;
 
-    s_instance->nacelleRPM_ = s_instance->incomingPacket_.rpm;
+    // s_instance->nacelleRPM_ = s_instance->incomingPacket_.rpm;
 
-    Serial.println("Received NacellePacket:");
-    printNacellePacket(s_instance->incomingPacket_, Serial);
+    // Serial.println("Received NacellePacket:");
+    // printNacellePacket(s_instance->incomingPacket_, Serial);
+
+    (void)xQueueOverwrite(priorityDataQueue, data); // Allegedly cannot fail
   } else {
-    Serial.print("Unexpected packet length: ");
-    Serial.println(len);
+    ESP_LOGE(TAG, "Rx invalid len: %d", len);
   }
 }
 
-void LoadComms::sendLoadboxData(int8_t state, int8_t estop, int16_t actuatorPos) {
-  unsigned long now = millis();
-
-  if (now - lastSendTime_ >= LOAD_COMMS_SEND_PERIOD_MS) {
-    lastSendTime_ = now;
-    makeLoadboxPacket(outgoingPacket_, state, estop, actuatorPos);
-    esp_now_send(NACELLE_MAC, (uint8_t *)&outgoingPacket_, sizeof(outgoingPacket_));
-  }
-
-  if (now - lastRxTime_ > LOAD_COMMS_TIMEOUT_MS) {
+bool LoadComms::sendLoadboxData(uint8_t estop) {
+  // if (now - lastSendTime_ >= LOAD_COMMS_SEND_PERIOD_MS) {
+  makeLoadboxPacket(outgoingPacket_, estop);
+  esp_err_t result = esp_now_send(NACELLE_MAC, (uint8_t *)&outgoingPacket_, sizeof(outgoingPacket_));
+  if(result == ESP_OK) {
+    lastSendTime_ = millis();
+    linkAlive_ = true;
+  } else {
     linkAlive_ = false;
+    ESP_LOGE(TAG, "Tx failed");
   }
 
-  if (!linkAlive_) {
-    Serial.println("WARNING: nacelle comms timeout");
-  }
-}
+  // }
+  // if (now - lastRxTime_ > LOAD_COMMS_TIMEOUT_MS) {
+  //   linkAlive_ = false;
+  // }
 
-void LoadComms::process() {
-  // This method can be used for future expansion (e.g., periodic checks)
+  // if (!linkAlive_) {
+  //   Serial.println("WARNING: nacelle comms timeout");
+  // }
+  return linkAlive_;
 }
 
+// void LoadComms::process() {
+//   // This method can be used for future expansion (e.g., periodic checks)
+// }
+
 bool LoadComms::isLinkAlive() const {
   return linkAlive_;
 }
 
-float LoadComms::getNacelleRPM() const {
-  return nacelleRPM_;
-}
+// float LoadComms::getNacelleRPM() const {
+//   return nacelleRPM_;
+// }

lib/LoadComms/LoadComms.h lib/LoadComms/LoadComms.hpplib/LoadComms/LoadComms.h renamed to lib/LoadComms/LoadComms.hpp

@@ -13,13 +13,13 @@
  * Sends state, E-stop, and actuator position data while receiving RPM data.
  */
 
-#ifndef LOAD_COMMS_H
-#define LOAD_COMMS_H
+#ifndef LOAD_COMMS_HPP
+#define LOAD_COMMS_HPP
 
 #include <Arduino.h>
 #include <WiFi.h>
 #include <esp_now.h>
-#include <TurbinePacket.h>
+#include <2026Core/TurbinePacket/TurbinePacket.hpp>
 
 /**
  * @brief MAC address of the nacelle controller.
@@ -29,19 +29,24 @@ extern const uint8_t NACELLE_MAC[];
 /**
  * @brief Communication timeout threshold in milliseconds.
  */
-const unsigned long LOAD_COMMS_TIMEOUT_MS = 1500;
+// const unsigned long LOAD_COMMS_TIMEOUT_MS = 1500;
 
 /**
  * @brief Transmission period in milliseconds.
  */
-const unsigned long LOAD_COMMS_SEND_PERIOD_MS = 100;
+// const unsigned long LOAD_COMMS_SEND_PERIOD_MS = 100;
 
 /**
  * @class LoadComms
  * @brief ESP-NOW communication handler for load box controller.
  */
 class LoadComms {
 public:
+  static constexpr char* TAG = "LCO";
+  static constexpr uint8_t wiFiChannel = 6;
+
+  static QueueHandle_t priorityDataQueue;
+
   /**
    * @brief Construct a new LoadComms object.
    */
@@ -55,17 +60,16 @@ class LoadComms {
 
   /**
    * @brief Send load box data to nacelle.
-   * @param state State value to send.
-   * @param estop E-stop value to send.
-   * @param actuatorPos Actuator position to send.
+   * @param safety Safety value to send.
    */
-  void sendLoadboxData(int8_t state, int8_t estop, int16_t actuatorPos);
+  bool sendLoadboxData(uint8_t safety);
 
   /**
    * @brief Process communication - call in main loop.
    *        Handles periodic sending and link health monitoring.
+   * @deprecated WIll call sendLoadboxData directly from main
    */
-  void process();
+  // void process();
 
   /**
    * @brief Check if communication link is active.
@@ -76,21 +80,23 @@ class LoadComms {
   /**
    * @brief Get the latest received RPM from nacelle.
    * @return Current RPM value.
+   * @deprecated Giving deferred processing in main a try
    */
-  float getNacelleRPM() const;
+  // float getNacelleRPM() const;
 
 private:
   NacellePacket incomingPacket_;  ///< Received packet from nacelle.
   LoadboxPacket outgoingPacket_;   ///< Outgoing packet to send.
   unsigned long lastSendTime_;    ///< Timestamp of last transmission.
   unsigned long lastRxTime_;      ///< Timestamp of last received packet.
   bool linkAlive_;                ///< Link health status.
-  float nacelleRPM_;                     ///< Cached RPM value.
+  // float nacelleRPM_;                     ///< Cached RPM value.
 
   /**
    * @brief Configure ESP-NOW peer.
+   * @returns ESP_OK if peer setup successful, error code otherwise.
    */
-  void setupPeer_();
+  esp_err_t setupPeer_();
 
   /**
    * @brief Callback executed after data is sent.
@@ -108,4 +114,4 @@ class LoadComms {
   static void onDataRecv_(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len);
 };
 
-#endif // LOAD_COMMS_H
+#endif // LOAD_COMMS_HPP