Merge commit 'f6900b8ed92936593b39a2a060f9d1295f0f6abf'

ArduinoUtils.cpp

@@ -0,0 +1,111 @@
+#include "ArduinoUtils.h"
+#if defined(ARDUINO)
+#include <Arduino.h>
+#include <ESP8266WiFi.h>
+
+bool StartWifi(const char *wifiSsid, const char *wifiPassword,
+               bool hotspotFallback) {
+#if UNO_R4 || ARDUINO_ARCH_RP2040
+  if (WiFi.status() == WL_NO_MODULE) {
+    Serial.println("WiFi not present, WiFiSync is disabled");
+    return false;
+  }
+#endif
+
+#if ESP32
+  printf("Connecting to WiFi %s\n", wifiSsid);
+#else
+  Serial.print("Connecting to WiFi ");
+  Serial.println(wifiSsid);
+#endif
+
+  // Connect to Wifi
+  WiFi.begin(wifiSsid, wifiPassword);
+  uint32_t notConnectedCounter = 0;
+  bool connected = false;
+  bool hotSpotEnabled = false;
+
+  while (WiFi.status() != WL_CONNECTED && !hotSpotEnabled) {
+#if ESP32
+    printf(".");
+#else
+    Serial.print(".");
+#endif
+    delay(500);
+
+    notConnectedCounter++;
+    if (notConnectedCounter > 20 && hotspotFallback) {
+#if ESP32
+      printf("\nCould not connect to home network.\n");
+#else
+      Serial.println();
+      Serial.println("Could not connect to home network");
+#endif
+      WiFi.disconnect();
+      if (hotspotFallback) {
+#if ESP32
+        WiFi.mode(WIFI_OFF);
+        WiFi.mode(WIFI_AP);
+        IPAddress wifiMyIp(192, 168, 4, 1);
+        WiFi.softAPConfig(wifiMyIp, wifiMyIp, IPAddress(255, 255, 255, 0));
+
+        WiFi.softAP(hotspotSSID, hotspotPassword);
+#elif UNO_R4 || ARDUINO_ARCH_RP2040
+        WiFi.beginAP(hotspotSSID);
+#endif
+        printf("Setup WiFi hotspot...\n");
+        // printf("ssid = %s, password = %s\n", hotspotSSID, hotspotPassword);
+#if ARDUINO_ARCH_RP2040
+        String ipAddress = WiFi.localIP().toString();
+#else
+        String ipAddress = WiFi.softAPIP().toString();
+#endif
+        char buf[20];
+        ipAddress.toCharArray(buf, 20);
+        printf("IP address: %s\n", buf);
+        hotSpotEnabled = true;
+      }
+    }
+  }
+  connected = notConnectedCounter <= 20;
+
+  if (connected) {
+    char buf[20];
+    String ipAddress = WiFi.localIP().toString();
+    ipAddress.toCharArray(buf, 20);
+#if ESP32 || ESP8266
+    printf("\nWifi connected, IP address: %s\n", buf);
+#else
+    Serial.println();
+    Serial.println("Wifi connected");
+#endif
+#if ESP32
+    printf("Checking credentials in flash\n");
+    wifiPreferences.begin(PREFERENCES_NAMESPACE);
+    wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials,
+                             sizeof(credentials));
+    if (strcmp(wifiSsid, credentials.ssid) != 0 ||
+        strcmp(wifiPassword, credentials.password) != 0) {
+      printf("Updating credentials in flash...");
+      const int ssidLen = strlen(wifiSsid);
+      if (ssidLen < 32) {
+        memcpy(credentials.ssid, wifiSsid, ssidLen);
+        credentials.ssid[ssidLen] = '\0';
+      }
+
+      const int pwdLen = strlen(wifiPassword);
+      if (pwdLen < 32) {
+        memcpy(credentials.password, wifiPassword, pwdLen);
+        credentials.password[pwdLen] = '\0';
+      }
+      wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials,
+                               sizeof(credentials));
+      printf(" completed.\n");
+    }
+    wifiPreferences.end();
+#endif
+  }
+
+  return (!hotSpotEnabled);
+}
+#endif

ArduinoUtils.h

@@ -0,0 +1,4 @@
+#pragma once
+
+bool StartWifi(const char *wifiSsid, const char *wifiPassword,
+               bool hotspotFallback);

CMakeLists.txt

@@ -27,7 +27,7 @@ else()
         .
         LinearAlgebra
     )
-    file(GLOB srcs *.cpp Sensors/*.cpp)
+    file(GLOB srcs *.cpp Sensors/*.cpp Messages/*.cpp)
     add_library(ControlCore STATIC ${srcs})
     
     enable_testing()

LinearAlgebra/Spherical.cpp

@@ -123,7 +123,7 @@ const SphericalOf<T> SphericalOf<T>::down =
 template <typename T>
 SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
   SphericalOf<T> v = SphericalOf<T>(distance, this->direction);
-  return SphericalOf<T>();
+  return v;
 }
 
 template <typename T> SphericalOf<T> SphericalOf<T>::operator-() const {

Messages.cpp

@@ -1,6 +1,7 @@
 #include "Messages.h"
 
 #include "LowLevelMessages.h"
+// #include "Messages/CustomMsg.h"
 #include "Participant.h"
 #include "string.h"
 
@@ -106,43 +107,6 @@ unsigned char PoseMsg::Serialize(char *buffer) {
 // Pose
 #pragma endregion
 
-#pragma region CustomMsg
-
-CustomMsg::CustomMsg(char *buffer) {
-  unsigned char ix = 1;
-  this->networkId = buffer[ix++];
-  this->thingId = buffer[ix++];
-  this->bytes =
-      buffer + ix; // This is only valid because the code ensures the the msg
-                   // lifetime is shorter than the buffer lifetime...
-}
-
-CustomMsg::CustomMsg(unsigned char networkId, Thing *thing) {
-  this->networkId = networkId;
-  this->thingId = thing->id;
-  this->thing = thing;
-}
-
-unsigned char CustomMsg::Serialize(char *buffer) {
-  unsigned char ix = this->length;
-  this->thing->SendBytes(buffer, &ix);
-  if (ix <= this->length) // in this case, no data is actually sent
-    return 0;
-
-  buffer[0] = this->id;
-  buffer[1] = this->networkId;
-  buffer[2] = this->thingId;
-  return ix;
-}
-
-CustomMsg CustomMsg::Receive(char *buffer, unsigned char bufferSize) {
-  CustomMsg msg = CustomMsg(buffer);
-  return msg;
-}
-
-// CustomMsg
-#pragma endregion
-
 #pragma region DestroyMsg
 
 DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {

Messages.h

@@ -62,26 +62,6 @@ public:
   virtual unsigned char Serialize(char *buffer) override;
 };
 
-class CustomMsg : public IMessage {
-public:
-  static const unsigned char id = 0xB1;
-  static const unsigned length = 3;
-
-  unsigned char networkId;
-  unsigned char thingId;
-  Thing *thing;
-
-  unsigned char bytesSize;
-  char *bytes;
-
-  CustomMsg(char *buffer);
-  CustomMsg(unsigned char networkId, Thing *thing);
-
-  virtual unsigned char Serialize(char *buffer) override;
-
-  static CustomMsg Receive(char *buffer, unsigned char bufferSize);
-};
-
 class DestroyMsg : public IMessage {
 public:
   static const unsigned char id = 0x20;

Messages/CustomMsg.cpp

@@ -0,0 +1,41 @@
+#include "CustomMsg.h"
+
+namespace Passer {
+namespace Control {
+
+CustomMsg::CustomMsg(char *buffer) {
+  unsigned char ix = 1;
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->bytes =
+      buffer + ix; // This is only valid because the code ensures the the msg
+                   // lifetime is shorter than the buffer lifetime...
+}
+
+CustomMsg::CustomMsg(unsigned char networkId, Thing *thing) {
+  this->networkId = networkId;
+  this->thingId = thing->id;
+  this->thing = thing;
+}
+
+CustomMsg::~CustomMsg() {}
+
+unsigned char CustomMsg::Serialize(char *buffer) {
+  unsigned char ix = this->length;
+  this->thing->SendBytes(buffer, &ix);
+  if (ix <= this->length) // in this case, no data is actually sent
+    return 0;
+
+  buffer[0] = this->id;
+  buffer[1] = this->networkId;
+  buffer[2] = this->thingId;
+  return ix;
+}
+
+CustomMsg CustomMsg::Receive(char *buffer, unsigned char bufferSize) {
+  CustomMsg msg = CustomMsg(buffer);
+  return msg;
+}
+
+} // namespace Control
+} // namespace Passer

Messages/CustomMsg.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "Messages.h"
+
+namespace Passer {
+namespace Control {
+
+class CustomMsg : public IMessage {
+public:
+  static const unsigned char id = 0xB1;
+  static const unsigned length = 3;
+
+  unsigned char networkId;
+  unsigned char thingId;
+  Thing *thing;
+
+  unsigned char bytesSize;
+  char *bytes = nullptr;
+
+  CustomMsg(char *buffer);
+  CustomMsg(unsigned char networkId, Thing *thing);
+  ~CustomMsg();
+
+  virtual unsigned char Serialize(char *buffer) override;
+
+  static CustomMsg Receive(char *buffer, unsigned char bufferSize);
+};
+
+} // namespace Control
+} // namespace Passer

Participant.cpp

@@ -18,6 +18,9 @@
 #include <unistd.h>
 #endif
 
+namespace Passer {
+namespace Control {
+
 Participant::Participant() {}
 
 Participant::Participant(int port) {
@@ -55,6 +58,10 @@ void Passer::Control::Participant::SetupUDP(int localPort,
 #endif
 }
 
+#if defined(ARDUINO)
+void Participant::Update() { this->Update(millis()); }
+#endif
+
 void Participant::Update(unsigned long currentTimeMs) {
   if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
     ClientMsg *msg = new ClientMsg(this->networkId);
@@ -124,6 +131,9 @@ void Passer::Control::Participant::PublishThingInfo(Thing *thing) {
   ModelUrlMsg *modelMsg = new ModelUrlMsg(this->networkId, thing);
   this->Publish(modelMsg);
   delete modelMsg;
+  CustomMsg *customMsg = new CustomMsg(this->networkId, thing);
+  this->Publish(customMsg);
+  delete customMsg;
 }
 
 bool Participant::Send(IMessage *msg) {
@@ -243,3 +253,5 @@ void Participant::Process(CustomMsg *msg) {
 
 // Receive
 #pragma endregion
+} // namespace Control
+} // namespace Passer

Participant.h

@@ -2,6 +2,7 @@
 
 #include "ClientMsg.h"
 #include "Messages.h"
+#include "Messages/CustomMsg.h"
 #include "ModelUrlMsg.h"
 #include "NameMsg.h"
 #include "NetworkIdMsg.h"
@@ -61,6 +62,9 @@ public:
   // i.e.
   // Participant p = Participant("127.0.0.1", 8000);
 
+#if defined(ARDUINO)
+  virtual void Update();
+#endif
   virtual void Update(unsigned long currentTimeMs);
 
   void SendThingInfo(Thing *thing);

Sensors/TemperatureSensor.cpp

@@ -7,6 +7,8 @@ namespace Control {
 
 // TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
 
+TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
+
 TemperatureSensor::TemperatureSensor(unsigned char networkId,
                                      unsigned char thingId)
     : Thing(nullptr, networkId, thingId, Type::TemperatureSensor) {}

Sensors/TemperatureSensor.h

@@ -7,6 +7,7 @@ namespace Control {
 
 class TemperatureSensor : public Thing {
 public:
+  TemperatureSensor();
   TemperatureSensor(unsigned char networkId, unsigned char thingId);
 
   virtual void SetTemperature(float temp);

SiteServer.cpp

@@ -2,6 +2,9 @@
 
 #include "Sensors/TemperatureSensor.h"
 
+#include <functional>
+#include <memory>
+
 namespace Passer {
 namespace Control {
 

float16.cpp

@@ -188,6 +188,13 @@ float float16::f16tof32(uint16_t _value) const {
 }
 
 uint16_t float16::f32tof16(float f) const {
+  // untested code, but will avoid strict aliasing warning
+  // union {
+  //   float f;
+  //   uint32_t t;
+  // } u;
+  // u.f = f;
+  // uint32_t t = u.t;
   uint32_t t = *(uint32_t *)&f;
   //  man bits = 10; but we keep 11 for rounding
   uint16_t man = (t & 0x007FFFFF) >> 12;