From c63bd9d2d77d8e2ebe950ac84a6aebb061d3c647 Mon Sep 17 00:00:00 2001
From: Terra <terra@clerie.de>
Date: Sun, 24 Apr 2022 14:54:40 +0200
Subject: [PATCH] Add time sync

---
 .style.yapf               |   4 +
 MultiNode/Configurator.py |  20 ++++-
 MultiNode/MNLib.cpp       |  63 +++++++++++--
 MultiNode/MNLib.h         |  14 ++-
 MultiNode/MultiNode.ino   |  41 +++++----
 RasPi/Config.toml         |   1 +
 RasPi/MultiNode.py        | 183 ++++++++++++++++++++++++++------------
 RasPi/RXTest.py           |  43 +--------
 TODO                      |   2 -
 9 files changed, 243 insertions(+), 128 deletions(-)
 create mode 100644 .style.yapf

diff --git a/.style.yapf b/.style.yapf
new file mode 100644
index 0000000..215bd83
--- /dev/null
+++ b/.style.yapf
@@ -0,0 +1,4 @@
+[style]
+based_on_style=pep8
+use_tabs=True
+column_limit=200
\ No newline at end of file
diff --git a/MultiNode/Configurator.py b/MultiNode/Configurator.py
index 390780e..7480d43 100644
--- a/MultiNode/Configurator.py
+++ b/MultiNode/Configurator.py
@@ -24,13 +24,22 @@ modem_power     = config[4]
 
 client_address  = int.from_bytes(config[5:7], byteorder="little")
 server_address  = int.from_bytes(config[7:9], byteorder="little")
-pointers = list(config[9 : 9 + n_devices])
+
+sensor_update_interval = int.from_bytes(config[9:13], byteorder="little")
+device_update_interval = int.from_bytes(config[13:17], byteorder="little")
+jitter = int.from_bytes(config[17:21], byteorder="little")
+pointers = list(config[21 : 21 + n_devices])
 
 modem_frequency = 868.0
 modem_power = 0
 client_address = 0x1234
 server_address = 0x0001
 
+# In ms
+sensor_update_interval = 3000
+device_update_interval = 7000
+jitter = 1000
+
 
 def pack_device():
     data = struct.pack("<bbb", 0, 0xFF, 0xFF)
@@ -56,8 +65,8 @@ def add_to_config(entry):
     pointer_counter += 1
     assert cfgmem_pointer < 256
 
-add_to_config(pack_analog_input(5, 15, -1000, 1000, 1, 0, False))
-add_to_config(pack_analog_input(6, 16, 3.8, 20.5, 1, 0, True))
+add_to_config(pack_analog_input(1, 15, -1000, 1000, 1, 0, False))
+add_to_config(pack_analog_input(2, 16, 3.8, 20.5, 1, 0, True))
 
 config = list(config)
 
@@ -65,7 +74,10 @@ config[0:4] = struct.pack("<f", modem_frequency)
 config[4] = modem_power
 config[5:7] = int.to_bytes(client_address, 2, byteorder="little")
 config[7:9] = int.to_bytes(server_address, 2, byteorder="little")
-config[9 : 9 + n_devices] = pointers
+config[9:13] = int.to_bytes(sensor_update_interval, 4, byteorder="little")
+config[13:17] = int.to_bytes(device_update_interval, 4, byteorder="little")
+config[17:21] = int.to_bytes(jitter, 4, byteorder="little")
+config[21 : 21 + n_devices] = pointers
 
 port.write(b"W")
 port.write(bytearray(cfgmem))
diff --git a/MultiNode/MNLib.cpp b/MultiNode/MNLib.cpp
index a64291c..263ea27 100644
--- a/MultiNode/MNLib.cpp
+++ b/MultiNode/MNLib.cpp
@@ -11,9 +11,10 @@ uint32_t message_id = 0;
 bool _is_client;
 uint8_t configuration_memory[CFGMEM];
 DeviceBase* devices[N_DEVICES];
+uint32_t last_server_message_id = 0;
 
 //void init_mn(bool is_client = true)
-void init_mn()
+void initMN()
 {
   //_is_client = is_client;
 
@@ -196,6 +197,8 @@ void refreshConfig()
   // you can set transmitter powers from 2 to 20 dBm:
   radio.setTxPower(configuration.modem_power, false);
 
+  randomSeed(configuration.client_address);
+
   initializeDevices();
 }
 
@@ -249,7 +252,7 @@ bool send(uint8_t data[], uint8_t len)
     full_data[i + 2 + 2 + 4 + 1] = data[i];
   }
 
-  hash_generator.reset(&configuration.client_secret_key, sizeof(configuration.client_secret_key), HASH_LENGTH); // Does sizeof() work here?
+  hash_generator.reset(&configuration.client_secret_key, sizeof(configuration.client_secret_key), HASH_LENGTH);
   hash_generator.update(full_data, 2 + 2 + 4 + 1 + len);
   hash_generator.finalize(hash, HASH_LENGTH);
 
@@ -266,6 +269,45 @@ bool send(uint8_t data[], uint8_t len)
   return success;
 }
 
+bool receive()
+{
+  uint8_t buffer[RH_RF95_MAX_MESSAGE_LEN];
+  uint8_t len = sizeof(buffer);
+
+  if (radio.recv(buffer, &len))
+  {
+    uint16_t target_address = buffer[0] + ((uint16_t)buffer[1] << 8);
+    if ((target_address == configuration.client_address || target_address == 0xFFFF) && buffer[2] == (configuration.server_address & 0xFF) && buffer[3] == (configuration.server_address >> 8 & 0xFF))
+    {
+
+      hash_generator.reset(&configuration.server_secret_key, sizeof(configuration.server_secret_key), HASH_LENGTH);
+      hash_generator.update(buffer, len - HASH_LENGTH);
+      uint8_t hash[HASH_LENGTH];
+      hash_generator.finalize(hash, HASH_LENGTH);
+      for (size_t i = 0; i < HASH_LENGTH; i++)
+        if (buffer[len - HASH_LENGTH + i] != hash[i])
+          return false;
+
+      uint32_t server_message_id = buffer[4] | ((uint32_t)buffer[5] << 8) | ((uint32_t)buffer[6] << 16) | ((uint32_t)buffer[7] << 24);
+      if (server_message_id > last_server_message_id)
+      {
+        uint8_t packet_length = buffer[8];
+        MessageType message_type = (MessageType)buffer[9];
+
+        switch (message_type)
+        {
+          case MT_Time:
+            if (server_message_id > message_id) // Ensure that IDs are sequential
+              message_id = server_message_id;
+            return true;
+            break;
+        }
+      }
+    }
+  }
+  return false;
+}
+
 void initializeDevices()
 {
   for (int i = 0; i < N_DEVICES; i++)
@@ -297,12 +339,12 @@ DeviceBase* getDevice(uint8_t pointer)
   return new Device;
 }
 
-void loopSensors()
+void loopMN()
 {
+  receive();
+
   for (int i = 0; i < N_DEVICES; i++)
-  {
     devices[i]->loop();
-  }
 }
 
 void sendSensorData()
@@ -331,3 +373,14 @@ void sendSensorData()
   memcpy(&data[3], &buffer, cnt * (sizeof(float) + sizeof(uint8_t) * 2));
   send(data, sizeof(uint8_t) + sizeof(uint16_t) + cnt * (sizeof(float) + sizeof(uint8_t) * 2));
 }
+
+void sendDeviceData()
+{
+  float temperature = temperature_sensor.readInternalTemperature();
+  float voltage = batteryVoltage();
+  uint8_t data[9];
+  data[0] = MT_DeviceStatus;
+  memcpy(&data[1], &voltage, 4);
+  memcpy(&data[5], &temperature, 4);
+  send(data, 9);
+}
diff --git a/MultiNode/MNLib.h b/MultiNode/MNLib.h
index f3e578f..d7782b3 100644
--- a/MultiNode/MNLib.h
+++ b/MultiNode/MNLib.h
@@ -42,6 +42,7 @@
 
 #define N_DEVICES 15
 #define CFGMEM 256
+#define BROADCAST 0xFFFF
 
 #define ERROR_BLINK_HALF_INTERVAL 100 // 5 Hz
 
@@ -56,12 +57,16 @@ struct MNConfiguration
   uint16_t client_address;
   uint16_t server_address;
 
+  uint32_t sensor_update_interval;
+  uint32_t device_update_interval;
+  uint32_t jitter;
+
   uint8_t devices[N_DEVICES];
 
   uint64_t client_secret_key;
   uint64_t server_secret_key;
 } __attribute__ ((packed));
-static_assert(sizeof(MNConfiguration) == 40, "MNConfiguration has the wrong size! Please edit this in the configurator too");
+static_assert(sizeof(MNConfiguration) == 52, "MNConfiguration has the wrong size! Please edit this in the configurator too");
 
 struct DeviceBase
 {
@@ -143,6 +148,7 @@ enum MessageType
 {
   MT_DeviceStatus = 1,
   MT_SensorStatus = 2,
+  MT_Time = 2,
 };
 
 
@@ -156,7 +162,7 @@ extern DeviceBase* devices[N_DEVICES];
 
 
 //void init_mn(bool is_client);
-void init_mn();
+void initMN();
 void test();
 void printStatusReport();
 void setLoopPower(bool state);
@@ -166,8 +172,10 @@ void refreshConfig();
 float batteryVoltage();
 void errorBlink(int n); // Quickly blink n times
 bool send(uint8_t data[], uint8_t len);
+bool receive();
 void initializeDevices();
 DeviceBase* getDevice(uint8_t pointer);
-void loopSensors();
+void loopMN();
 void sendSensorData();
+void sendDeviceData();
 #endif
diff --git a/MultiNode/MultiNode.ino b/MultiNode/MultiNode.ino
index 00516ce..be32348 100644
--- a/MultiNode/MultiNode.ino
+++ b/MultiNode/MultiNode.ino
@@ -6,10 +6,10 @@
 //#define IS_SERVER
 
 
-byte LoopState = 0;
-char inByte;
-unsigned long nextTick = 0;
-unsigned int msTick = 2000;
+unsigned long tick_tracker_sensors = 0;
+unsigned long tick_tracker_device = 0;
+unsigned long next_tick_sensors = 0;
+unsigned long next_tick_device = 0;
 
 
 
@@ -17,7 +17,6 @@ unsigned int msTick = 2000;
 //RHReliableDatagram rfManager(rfm95, CLIENT_ADDRESS);
 
 // Internal on-chip Temperature sensor
-TemperatureZero TempZero = TemperatureZero();
 
 
 void setup()
@@ -29,14 +28,15 @@ void setup()
     delay(1000);
     SerialUSB.println("owo");
   }*/
-  init_mn();
+  initMN();
   /*for (int i = 0; i < 2; i++)
   {
     delay(1000);
     SerialUSB.println("awa");
   }
   printStatusReport();*/
-  nextTick = millis();
+  tick_tracker_sensors = millis();
+  tick_tracker_device = millis();
 }
 
 
@@ -111,6 +111,8 @@ void loop()
       case 'w':
         SerialUSB.readBytes(reinterpret_cast<char*>(&configuration), config_size);          
         refreshConfig();
+        tick_tracker_sensors = millis();
+        tick_tracker_device = millis();
         break;
         
       case 'R': // Read configuration memory (extended configuration for each sensor)
@@ -135,20 +137,23 @@ void loop()
     }
   }
 
-  loopSensors();
+  loopMN();
 
-  // TICK-ROUTINE
-  if (millis() > nextTick)
+  if (millis() >= next_tick_sensors)
   {
     sendSensorData();
-    
-    SerialUSB.println("s");
-    uint8_t data[5];
-    data[0] = MT_DeviceStatus;
-    float bv = batteryVoltage();
-    memcpy(&data[1], &bv, 4);
-    send(data, 5);
-    nextTick = nextTick + msTick;
+    tick_tracker_sensors = tick_tracker_sensors + configuration.sensor_update_interval;
+    next_tick_sensors = tick_tracker_sensors + random(configuration.jitter);
+    digitalWrite(LED_BUILTIN, HIGH);
+    delay(2);
+    digitalWrite(LED_BUILTIN, LOW);
+  }
+
+  if (millis() >= next_tick_device)
+  {
+    sendDeviceData();
+    tick_tracker_device = tick_tracker_device + configuration.device_update_interval;
+    next_tick_device = tick_tracker_device + random(configuration.jitter);
     digitalWrite(LED_BUILTIN, HIGH);
     delay(2);
     digitalWrite(LED_BUILTIN, LOW);
diff --git a/RasPi/Config.toml b/RasPi/Config.toml
index f7aead3..c8884b3 100644
--- a/RasPi/Config.toml
+++ b/RasPi/Config.toml
@@ -1,6 +1,7 @@
 [server]
 address = 0x0001
 secret_key = 0x2e29b257521dc792
+time_interval = 5
 
 [node]
 address = 0x1FFF
diff --git a/RasPi/MultiNode.py b/RasPi/MultiNode.py
index 53f2523..17d8c99 100644
--- a/RasPi/MultiNode.py
+++ b/RasPi/MultiNode.py
@@ -1,71 +1,140 @@
 from enum import IntEnum
-import struct
 from pyblake2 import blake2s
-import time
-import toml
+from pyLoraRFM9x import LoRa, ModemConfig
+import time, toml, math, struct
 
 HASH_LENGTH = 8
 
-with open("Config.toml", "r") as config_file:
-    config = toml.loads(config_file.read())
-
-print(config)
-devices = {}
-
 
 class MessageType(IntEnum):
-    DeviceStatus = 1
-    SensorStatus = 2
+	DeviceStatus = 1
+	SensorStatus = 2
+	Time = 2
 
 
-def decode_packet(data):
-    packet_type = data[0]
+class MultiNode:
+	sensor_type_table = {1: "V", 2: "mA"}
 
-    # match packet_type:
-    #    case MessageType.DeviceStatus:
+	def __init__(self):
+		with open("Config.toml", "r") as config_file:
+			config = toml.loads(config_file.read())
 
-    if packet_type == MessageType.DeviceStatus:
-        return {"Battery voltage": struct.unpack('<f', data[1:5])[0]}
+		print(config)
+		self.server_address = config["server"]["address"]
+		self.server_secret_key = config["server"]["secret_key"]
+		# How often to send the time
+		self.time_interval = config["server"]["time_interval"]
+		self.devices = {}
+		self.last_time_message = time.time()
 
-    if packet_type == MessageType.SensorStatus:
-        channels_raw = struct.unpack('<H', data[1:3])[0]
-        channels = []
-        for i in range(16):
-            if (channels_raw >> i) & 1:
-                channels.append(i)
-        
-        sensor_data = []
-        for i in range(len(channels)):
-            offset = i * 6
-            sensor_data.append({
-                "channel": channels[i],
-                "type": data[3 + offset],
-                "pin": data[4 + offset],
-                "value": struct.unpack('<f', data[5 + offset : 9 + offset])[0]
-            })
-        return sensor_data
+		self.lora = LoRa(
+		    0,  # SPI channel
+		    25,  # Interrupt pin
+		    255,  # Node ID
+		    reset_pin=22,
+		    modem_config=ModemConfig.Bw125Cr45Sf128,
+		    tx_power=14,
+		    freq=868,
+		    acks=False)  # , receive_all=True)
+
+		self.lora.cad_timeout = 1
+		self.lora.on_recv = self.process_packet
+		self.lora.set_mode_rx()
+
+	def loop(self):
+		if time.time() - self.last_time_message > self.time_interval:
+			self.send_packet(0xFFFF, int(MessageType.Time).to_bytes(1, "little"))
+			self.last_time_message += self.time_interval
+			#print("Sent time")
+
+	def decode_packet(self, device, data):
+		packet_type = data[0]
+
+		# match packet_type:
+		#	case MessageType.DeviceStatus:
+
+		if packet_type == MessageType.DeviceStatus:
+			device["status"] = {"battery": struct.unpack('<f', data[1:5])[0], "temperature": struct.unpack('<f', data[5:10])[0]}
+			return device["status"]
+
+		if packet_type == MessageType.SensorStatus:
+			channels_raw = struct.unpack('<H', data[1:3])[0]
+			channels = []
+			for i in range(16):
+				if (channels_raw >> i) & 1:
+					channels.append(i)
+
+			sensor_data = []
+			for i in range(len(channels)):
+				offset = i * 6
+				sensor_data.append({"channel": channels[i], "type": data[3 + offset], "pin": data[4 + offset], "value": struct.unpack('<f', data[5 + offset:9 + offset])[0]})
+			device["sensors"] = sensor_data
+			return sensor_data
+
+	def process_packet(self, payload):
+		rx_id = int.from_bytes(payload.message[0:2], byteorder="little")
+		tx_id = int.from_bytes(payload.message[2:4], byteorder="little")
+		msg_id = int.from_bytes(payload.message[4:8], byteorder="little")
+		length = payload.message[8]
+		data = payload.message[9:9 + length]
+		data_hash = payload.message[9 + length:9 + length + HASH_LENGTH]
+
+		if len(payload.message) != length + 9 + HASH_LENGTH:
+			print(f"Invalid length! Expected {length + 9 + HASH_LENGTH} actual {len(payload.message)}")
+			return
+
+		hash_function = blake2s(key=0x0.to_bytes(8, "little"), digest_size=8)
+		hash_function.update(payload.message[:-HASH_LENGTH])
+
+		if hash_function.digest() != data_hash:
+			print(f"Hash doesn't match! Expected {hash_function.digest()} got {data_hash}")
+			return
+
+		if not rx_id in self.devices:
+			self.devices[rx_id] = {"last_message_id": msg_id}
+
+		self.decode_packet(self.devices[rx_id], data)
+
+		# print(f"{tx_id} #{msg_id}: {decode_packet(data):.3f} V, {payload.rssi} dB(?) RSSI, {payload.snr} dB(?) SNR {(time.clock_gettime_ns(0)) / 1e9}")
+		# print(f"{tx_id} #{msg_id}: {data.hex()} {self.decode_packet(data)}, {payload.rssi} dB(?) RSSI, {payload.snr} dB(?) SNR {(time.clock_gettime_ns(0)) / 1e9}")
+
+	def send_packet(self, target: int, data):
+		assert len(data) < 256
+
+		payload = []
+		payload.extend(target.to_bytes(2, "little"))
+		payload.extend(self.server_address.to_bytes(2, "little"))
+		# TODO: is this the best idea? Clock change would affect it badly
+		payload.extend(int(time.time()).to_bytes(4, "little"))
+		payload.append(len(data))
+		payload.extend(data)
+
+		hash_function = blake2s(key=self.server_secret_key.to_bytes(8, "little"), digest_size=8)
+		hash_function.update(bytearray(payload))
+		payload.extend(hash_function.digest())
+		#print(payload)
+
+		self.lora.send(payload, 255)
+		self.lora.set_mode_rx()
+		self.print_data()
+
+	def print_data(self):
+		for device_id in self.devices:
+			device = self.devices[device_id]
+			print(f'{device_id}:')
+
+			if "status" in device:
+				print(f'\t{device["status"]["battery"]} V {device["status"]["temperature"]} °C')
+
+			if "sensors" in device:
+				for sensor in device["sensors"]:
+					if sensor["value"] == math.nan:
+						print(f'\tCH {sensor["channel"]} on Pin {sensor["pin"]}: ERROR')
+					else:
+						print(f'\tCH {sensor["channel"]} on Pin {sensor["pin"]}: {sensor["value"]} {self.sensor_type_table[sensor["type"]]}')
 
 
-def process_packet(payload):
-    rx_id = int.from_bytes(payload.message[0:2], byteorder="little")
-    tx_id = int.from_bytes(payload.message[2:4], byteorder="little")
-    msg_id = int.from_bytes(payload.message[4:8], byteorder="little")
-    length = payload.message[8]
-    data = payload.message[9: 9 + length]
-    data_hash = payload.message[9 + length: 9 + length + HASH_LENGTH]
-
-    if len(payload.message) != length + 9 + HASH_LENGTH:
-        print(
-            f"Invalid length! Expected {length + 9 + HASH_LENGTH} actual {len(payload.message)}")
-        return
-
-    hash_function = blake2s(key=0x0.to_bytes(8, "little"), digest_size=8)
-    hash_function.update(payload.message[: -HASH_LENGTH])
-
-    if hash_function.digest() != data_hash:
-        print(
-            f"Hash doesn't match! Expected {hash_function.digest()} got {data_hash}")
-        return
-
-    # print(f"{tx_id} #{msg_id}: {decode_packet(data):.3f} V, {payload.rssi} dB(?) RSSI, {payload.snr} dB(?) SNR {(time.clock_gettime_ns(0)) / 1e9}")
-    print(f"{tx_id} #{msg_id}: {data.hex()} {decode_packet(data)}, {payload.rssi} dB(?) RSSI, {payload.snr} dB(?) SNR {(time.clock_gettime_ns(0)) / 1e9}")
+if __name__ == "__main__":
+	multinode = MultiNode()
+	while True:
+		multinode.loop()
diff --git a/RasPi/RXTest.py b/RasPi/RXTest.py
index a93dbb8..008090b 100644
--- a/RasPi/RXTest.py
+++ b/RasPi/RXTest.py
@@ -1,45 +1,10 @@
 from pyLoraRFM9x import LoRa, ModemConfig
-import MultiNode
 
-#class MessageType(IntEnum):
-#    DeviceStatus = 1;
-#
-#def decode_packet(data):
-#    packet_type = data[0]
-#    
-#    #match packet_type:
-#    #    case MessageType.DeviceStatus:
-#    
-#    if packet_type == MessageType.DeviceStatus:
-#        return struct.unpack('f', data[1:5])[0]
-
-# This is our callback function that runs when a message is received
 def on_recv(payload):
-    #print("From:", payload.header_from)
-    #print("Received:", payload.message)
-    #print("RSSI: {}; SNR: {}".format(payload.rssi, payload.snr))
-    MultiNode.process_packet(payload)
-    #print(payload.message.hex())
-    #rx_id = int.from_bytes(payload.message[0:2], byteorder="little")
-    #tx_id = int.from_bytes(payload.message[2:4], byteorder="little")
-    #msg_id = int.from_bytes(payload.message[4:8], byteorder="little")
-    #length = payload.message[8]
-    #data = payload.message[9 : 9 + length]
-    #data_hash = payload.message[9 + length : 9 + length + HASH_LENGTH]
-    #
-    #if len(payload.message) != length + 9 + HASH_LENGTH:
-    #    print(f"Invalid length! Expected {length + 9 + HASH_LENGTH} actual {len(payload.message)}")
-    #    return
-    #
-    #hash_function = blake2s(key=0x0.to_bytes(8, "little"), digest_size=8)
-    #hash_function.update(payload.message[: -HASH_LENGTH])
-#
-    #if hash_function.digest() != data_hash:
-    #    print(f"Hash doesn't match! Expected {hash_function.digest()} got {data_hash}")
-    #    return
-    
-    #print(f"Received {struct.unpack('f', data[1:])[0]:.3f} V from {tx_id} with destination {rx_id} and {payload.rssi} dB(?) RSSI and {payload.snr} dB(?) SNR")
-    #print(f"{tx_id} #{msg_id}: {decode_packet(data):.3f} V, {payload.rssi} dB(?) RSSI, {payload.snr} dB(?) SNR {(time.clock_gettime_ns(0) - start_time) / 1e9}")
+    print("From:", payload.header_from)
+    print("Received:", payload.message)
+    print("RSSI: {}; SNR: {}".format(payload.rssi, payload.snr))
+    print(payload.message.hex())
 
 
 # Use chip select 1. GPIO pin 5 will be used for interrupts and set reset pin to 25
diff --git a/TODO b/TODO
index 639d034..7fb50c9 100644
--- a/TODO
+++ b/TODO
@@ -2,6 +2,4 @@
 - Webserver
 - More sensor types
 - Control of node outputs
-- Time(stamps)
-- Tracking of time stamps / message IDs
 - Power saving features
\ No newline at end of file