Bidirectional experiment with DFU code for networking functionality

src/comm.cpp

@@ -1,4 +1,5 @@
 #include "comm.h"
+#include "sensing.h"
 #include "pinout.h"
 
 WaggleNode::WaggleNode(uint8_t CE_pin, uint8_t CS_pin)
@@ -28,7 +29,8 @@ void WaggleNode::update() {
         size_t data_size = network.peek(header);
         byte *payload = new byte[data_size];
         network.read(header, payload, data_size);
-        // TODO: Callback for received payload
+        // Write data into the sensor
+        writeData(payload, data_size);
         delete[] payload;
     }
 }
@@ -49,26 +51,43 @@ uint32_t WaggleNode::get_sig_byte_() {
     return res;
 }
 
-uint8_t WaggleNode::send_telemetry(void* payload, uint8_t len, uint8_t channel) {
-    return write_(payload, channel, len);
+uint8_t WaggleNode::send_telemetry(void* payload, uint8_t len, uint8_t channel,nodeid_t dest) {
+    return write_(payload, channel, len, dest);
 }
 
-uint8_t WaggleNode::write_(void *payload, uint8_t ch, uint8_t len) {
+uint8_t WaggleNode::write_(void *payload, uint8_t ch, uint8_t len, nodeid_t dest) {
     /* Returns:
     - 0: Normal
     - 1: Send failed, test OK
     - 2: Send failed, address is lost (will block)
     - 3: Invalid send call
     */
-    auto my_id = mesh.getNodeID(mesh.mesh_address);
-    if (my_id < 0 || my_id != nodeID) mesh.renewAddress();
-    if (!mesh.write(payload, ch, len)) {
-      // If a write fails, check connectivity to the mesh network
-      if (!mesh.checkConnection()) {
-        // refresh the network address
-        Serial.println("Renewing Address");
+    if(dest == 0 ){
+        auto my_id = mesh.getNodeID(mesh.mesh_address);
+        if (my_id < 0 || my_id != nodeID) mesh.renewAddress();
+        if (!mesh.write(payload, ch, len)) {
+        // If a write fails, check connectivity to the mesh network
+        if (!mesh.checkConnection()) {
+            // refresh the network address
+            Serial.println("Renewing Address");
+            mesh.renewAddress();
+            return 2;
+        } else return 1;
+        } else return 0;
+    }
+    //dest = 1 for dfu send_packet 
+    else{
         mesh.renewAddress();
-        return 2;
-      } else return 1;
-    } else return 0;
+        if (!mesh.write(payload, ch, len)) {
+        // If a write fails, check connectivity to the mesh network
+        if (!mesh.checkConnection()) {
+            // refresh the network address
+            Serial.println("Renewing Address");
+            mesh.renewAddress();
+            return 2;
+        } else return 1;
+        } else return 0;
+
+
+    }
 }

src/comm.h

@@ -24,9 +24,9 @@ class WaggleNode {
         bool begin();
         bool begin(uint8_t radio_channel);
         void update();
-        uint8_t send_telemetry(void* payload, uint8_t len, uint8_t channel = CH_TELEMETRY);
+        uint8_t send_telemetry(void* payload, uint8_t len, uint8_t channel = CH_TELEMETRY, nodeid_t dest);
         nodeid_t nodeID;
     private:
-        uint8_t write_(void *payload, uint8_t ch, uint8_t len);
+        uint8_t write_(void *payload, uint8_t ch, uint8_t len, nodeid_t dest);
         uint32_t get_sig_byte_();
 };

src/dfu.cpp

@@ -1,5 +1,5 @@
 #include "dfu.h"
-
+#include "comm.h"
 
 #define DFU_STATE_MAIN 0
 #define DFU_STATE_SETID 1
@@ -8,8 +8,10 @@
 #define DFU_STATE_HUMAN 200
 #define DFU_STATE_HELP 201
 
+
 uint8_t dfu_state = DFU_STATE_MAIN;
 uint8_t dfu_human_mode = false;
+uint8_t dfu_update_flag = false;
 
 int8_t parse_hex_from_char(char ch) {
     if (ch >= '0' && ch <= '9')
@@ -45,21 +47,39 @@ void dfu_loop() {
         case DFU_STATE_NUKE:
             dfu_nuke();
             break;
+        case DFU_STATE_ACTIVATE:
+            dfu_activate();
+            break;
+        case DFU_STATE_RENEW_ADDRESS:
+            dfu_renew_address();
+            break;
+        case DFU_STATE_SEND_PACKET:
+            dfu_send_packet();
+            break;
     }
 }
 
+
+
+
+
 void dfu_help() {
     Serial.println(F("DFU Mode Instructions:"));
     Serial.println(F("S=setid       Set Node ID in EEPROM"));
     Serial.println(F("T=test        Test radio and print result"));
     Serial.println(F("N=nuke        Nuke the DFU for good"));
     Serial.println(F("H=human       Enter / Exit human mode; detailed response"));
+    Serial.println(F("A=activate    Activate the node"));
+    Serial.println(F("R=Renew       Renew node address"));
+    Serial.println(F("P=send        Send packet"));
     dfu_state = DFU_STATE_MAIN;
 }
 
 void dfu_main() {
     Serial.print(F("DFU>"));
-    while(!Serial.available());
+    while(!Serial.available()){
+        dfu_activate();
+    }
     char c = Serial.read();
     // Echo back the char
     Serial.println(c);
@@ -77,6 +97,12 @@ void dfu_main() {
         case 'H': case 'h':
             dfu_state = DFU_STATE_HUMAN;
             break;
+        case 'A': case 'a':
+            dfu_state = DFU_STATE_ACTIVATE;
+        case 'R': case 'r':
+            dfu_state = DFU_STATE_RENEW_ADDRESS;
+        case 'P': case 'p':
+            dfu_state = DFU_STATE_SEND_PACKET; 
         default:
             dfu_state = DFU_STATE_HELP;
             break;
@@ -108,7 +134,7 @@ void dfu_setid() {
     Serial.print(F("?>"));
     // Expect 8 characters
     while (ds_setid_counter < 8) {
-        if (!Serial.available()) continue;
+        if (!Serial.available()) dfu_activate();
         char ch = Serial.read();
         Serial.write(ch);  // Echo back this char
         int8_t data = parse_hex_from_char(ch);
@@ -162,3 +188,26 @@ void dfu_nuke() {
     // Perform a hard reset
     asm volatile (" jmp 0");
 }
+
+void dfu_activate() {
+    node.begin()
+    //set flag
+    dfu_update_flag = true;
+
+    return 0;
+}
+
+void dfu_renew_address(){ 
+    mesh.renewAddress();
+    Serial.println(F("Finish renew address"));
+    return 0;
+}
+
+void dfu_send_packet(){
+    char dest=Serial.read();
+
+
+    // payload :destination address, length, payload
+    send_telemetry(dest, strlen(dest), CH_TELEMETRY, 1)    
+
+}

src/main.cpp

@@ -1,5 +1,6 @@
 #define UNO
 
+#include <avr/wdt.h>
 #include "Ticker.h"
 #include "system.h"
 #include "sensing.h"
@@ -10,48 +11,62 @@
 
 WaggleNode node(RADIO_CE, RADIO_CSN);
 
-void reportSysStats() {
-    transmit_power_stats(node);
-}
-
 void reportTelemetry() {
+    Serial.println(F("Updating telemetry..."));
     collectData(node);
 }
 
-Ticker systemStatsTimer(reportSysStats, 10000);
+Ticker systemStatsTimer(updateSystemMonitor, 10000);
 Ticker telemetryTimer(reportTelemetry, 5000);
 
 void setup() {
+    wdt_enable(WDTO_8S);
+    signal_init();
+    signal_startup();
+    wdt_reset();
     // Setting up everything
     Serial.begin(115200);
+    Serial.print(F("RESET>\t"));
+    Serial.println(MCUSR, HEX);
+    MCUSR &= 0xf0;  // Clear the flag
     Serial.println(F("!>BOOT"));
     // Get DFU started
     if (is_dfu_available()) {
         Serial.println(F("?>D=DFU"));
         uint32_t flag = millis();
         while (millis() < flag + 1000) {
             if (Serial.available() && Serial.read() == 'D') {
-                while (1) dfu_loop();
+                while (1) {
+                    dfu_loop();
+                    wdt_reset();  // Hush doggy
+                }
             }
         }
     }
+
+    // Actually start a bunch of timeouts
+    wdt_reset();  // Hush doggy
     Serial.setTimeout(5000);
     analogReference(INTERNAL);
-    signal_init();
-    signal_startup();
     // Configure Net
     Serial.println(F(">>Starting node"));
     auto result = node.begin();
     signal_connection_begin(result);
     Serial.println(F(">>Starting sensing"));
+    wdt_reset();  // Hush doggy
     initSensing();
     transmitSensorList(node);
+    initSystemMonitor();
+    initSystemControlExperiment();
     Serial.println(F(">>Starting event loop"));
+    wdt_reset();  // Hush doggy
     systemStatsTimer.start();
     telemetryTimer.start();
+    wdt_reset();  // Hush doggy
 }
 
 void loop() {
+    wdt_reset();  // Hush doggy
     node.update();
     systemStatsTimer.update();
     telemetryTimer.update();

src/pinout.h

@@ -3,8 +3,8 @@
 #define CHRG_CHARGING 7
 #define CHRG_STANDBY 8
 
-#define BATT_SENS A6
-#define SOLAR_SENS A7
+#define SOLAR_SENS A6
+#define BATT_SENS A7
 
 #define LED_BLUE 6
 #define LED_ORANGE 4

src/sensing.cpp

@@ -0,0 +1,107 @@
+#include "sensing.h"
+
+SensorManager manager;
+
+void initSensing() {
+    manager.begin();
+}
+
+void transmitSensorList(WaggleNode& node) {
+    uint8_t msg_size = manager.getSensorCount()*8 + 7;
+    uint8_t msg[msg_size];
+    // Prepare the sysmsg header
+    // Schema: https://wagglenet.atlassian.net/wiki/spaces/SPORT/pages/13238331/Node-to-Router+Data+Format
+    *(uint32_t*)msg = node.nodeID;
+    msg[4] = 0;
+    uint8_t pos = 5;
+    for (int sensor_idx = 0; sensor_idx < manager.getSensorCount(); sensor_idx++) {
+        auto sensor = manager.getSensor(sensor_idx);
+        *(uint32_t*)(msg+pos) = (uint32_t)sensor->address;
+        pos += 4;
+        *(uint32_t*)(msg+pos) = (uint32_t)sensor->type;
+        pos += 4;
+    }
+    // Report all the sensors to router
+    node.send_telemetry(msg, msg_size, CH_SYSMSG);
+}
+
+void collectData(WaggleNode& node) {
+    for (int sensor_idx = 0; sensor_idx < manager.getSensorCount(); sensor_idx++) {
+        Serial.print(F("-!>\tCOLL\tSensor.Index\t"));
+        Serial.println(sensor_idx);
+        manager.collect(sensor_idx);
+        auto sensor = manager.getSensor(sensor_idx);
+        // FIRST PASS: Compute size of message block
+        uint8_t block_size = 15; // Headers
+        uint8_t num_entry = 0;
+        for (int entry = 0; entry < sensor->getSize(); entry++) {
+            if (sensor->hasChanged(entry)) {
+                block_size += sensor->getLength(entry) + 2;
+                num_entry ++;
+            }
+        }
+        Serial.print(F("-!>\tSEND\tMessage.Size\t"));
+        Serial.println(block_size);
+        Serial.print(F("-!>\tSEND\tMessage.Entries\t"));
+        Serial.println(num_entry);
+        if (!num_entry) return;
+        // SECOND PASS: Populate the message block
+        uint8_t* msg = new uint8_t[block_size];
+        // Populate node header
+        *(uint32_t*)msg = node.nodeID;
+        msg[4] = 0;
+        // Populate sensor header
+        *(uint32_t*)(msg+5) = sensor->address;
+        *(uint32_t*)(msg+9) = sensor->type;
+        msg[13] = 0;
+        // Populate entry number
+        msg[14] = num_entry;
+        uint8_t write_head = 15; // Write from here
+        for (int entry = 0; entry < sensor->getSize(); entry++) {
+            // if (sensor->hasChanged(entry)) {
+                if (!sensor->hasChanged(entry)) continue;
+                msg[write_head] = entry;
+                msg[write_head+1] = sensor->getLength(entry);
+                auto& msg_size = msg[write_head+1];
+                memcpy(msg+write_head+2, sensor->getData(entry), msg_size);
+                write_head += 2 + msg_size;
+                Serial.print(F("-!>\tCOLL\tCollect.Entry\t"));
+                Serial.println(entry);
+                Serial.print(F("-!>\tCOLL\tCollect.Size\t"));
+                Serial.println(msg_size);
+                // Set the entry as not changed
+                sensor->changed(entry, false);
+            // }
+        }
+        // Transmit the message
+        node.send_telemetry(msg, block_size);
+        // Clean it up
+        delete[] msg;
+    }
+}
+
+void writeData(byte* buffer, byte size) {
+    // Failure mode: If size cannot even contain header, return
+    if (size <= 15) return;
+    auto sensor_address = *(uint32_t*)(buffer+5);
+    auto sensor_type = *(uint32_t*)(buffer+9);
+    // First we validate if the type matches
+    auto sensorIdx = manager.getIndexByAddress(sensor_address);
+    // Failure mode: Target sensor not found
+    if (sensorIdx >= manager.getSensorCount()) return;
+    auto& sensor = *(manager.getSensor(sensorIdx));
+    // Now we parse out the fields
+    auto numEntries = *(byte*)(buffer+14);
+    size_t base_ptr = 15; // This is our operating head
+    for (int i = 0; i < numEntries; i++) {
+        auto entryIdx = *(byte*)(buffer+base_ptr);
+        auto entryLength = *(byte*)(buffer+base_ptr+1);
+        // Failure mode: Length mismatches
+        if (sensor.getLength(sensorIdx) != entryLength) return;
+        // Failure mode: Length overshoots message size
+        if (entryLength + 2 + base_ptr > size) return;
+        memcpy(sensor.getData(entryIdx), buffer+base_ptr+2, entryLength);
+        sensor.changed(entryIdx);
+        base_ptr += entryLength + 2;
+    }
+}