Merge pull request #407 from OUXT-Polaris/Feat/add_check_heartbeat

feat: check heart beat for estop system and phsical power cutoff

Author: wam-v-tan

firmware/mini_v/estop_driver/include/heart_beat.hpp

@@ -0,0 +1,36 @@
+#ifndef __HEART_BEAT_HPP__
+#define __HEART_BEAT_HPP__
+
+
+#include <NativeEthernet.h>
+#include <NativeEthernetUdp.h>
+
+#include "proto/hardware_communication_msgs__HeartBeat.pb.h"
+#include "pb_decode.h"
+
+
+class UDPHeartBeat {
+public:
+    UDPHeartBeat(uint16_t port, unsigned long timeout);
+    void begin();  // After define Ethernet.begin
+
+    bool verify_survival();
+    
+    auto get_msgs() -> protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat;
+
+private:
+    const uint16_t port;
+    EthernetUDP Udp;
+    uint8_t packetBuffer[UDP_TX_PACKET_MAX_SIZE];  // buffer to hold incoming packet,
+
+    protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat msg
+     = protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_init_zero;
+
+    unsigned long preTime = 0;
+    unsigned long timeout;  // ms
+
+
+    bool update_msgs();
+};
+
+#endif

firmware/mini_v/estop_driver/lib/protolink_msgs/proto/hardware_communication_msgs__HeartBeat.pb.c

@@ -0,0 +1,12 @@
+/* Automatically generated nanopb constant definitions */
+/* Generated by nanopb-1.0.0-dev */
+
+#include "proto/hardware_communication_msgs__HeartBeat.pb.h"
+#if PB_PROTO_HEADER_VERSION != 40
+#error Regenerate this file with the current version of nanopb generator.
+#endif
+
+PB_BIND(protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat, protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat, AUTO)
+
+
+

firmware/mini_v/estop_driver/lib/protolink_msgs/proto/hardware_communication_msgs__HeartBeat.pb.h

@@ -0,0 +1,48 @@
+/* Automatically generated nanopb header */
+/* Generated by nanopb-1.0.0-dev */
+
+#ifndef PB_PROTOLINK__HARDWARE_COMMUNICATION_MSGS__HEARTBEAT_PROTO_HARDWARE_COMMUNICATION_MSGS__HEARTBEAT_PB_H_INCLUDED
+#define PB_PROTOLINK__HARDWARE_COMMUNICATION_MSGS__HEARTBEAT_PROTO_HARDWARE_COMMUNICATION_MSGS__HEARTBEAT_PB_H_INCLUDED
+#include <pb.h>
+
+#if PB_PROTO_HEADER_VERSION != 40
+#error Regenerate this file with the current version of nanopb generator.
+#endif
+
+/* Struct definitions */
+typedef struct _protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat {
+    uint64_t sequence;
+} protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat;
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Initializer values for message structs */
+#define protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_init_default {0}
+#define protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_init_zero {0}
+
+/* Field tags (for use in manual encoding/decoding) */
+#define protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_sequence_tag 1
+
+/* Struct field encoding specification for nanopb */
+#define protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_FIELDLIST(X, a) \
+X(a, STATIC,   SINGULAR, UINT64,   sequence,          1)
+#define protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_CALLBACK NULL
+#define protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_DEFAULT NULL
+
+extern const pb_msgdesc_t protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_msg;
+
+/* Defines for backwards compatibility with code written before nanopb-0.4.0 */
+#define protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_fields &protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_msg
+
+/* Maximum encoded size of messages (where known) */
+#define PROTOLINK__HARDWARE_COMMUNICATION_MSGS__HEARTBEAT_PROTO_HARDWARE_COMMUNICATION_MSGS__HEARTBEAT_PB_H_MAX_SIZE protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_size
+#define protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat_size 11
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif

firmware/mini_v/estop_driver/lib/protolink_msgs/proto/hardware_communication_msgs__HeartBeat.proto

@@ -0,0 +1,6 @@
+syntax = "proto3";
+package protolink__hardware_communication_msgs__HeartBeat;
+
+message hardware_communication_msgs__HeartBeat {
+uint64 sequence = 1;
+}

firmware/mini_v/estop_driver/platformio.ini

@@ -12,4 +12,6 @@
 platform = teensy
 board = teensy41
 framework = arduino
-lib_deps = adafruit/Adafruit NeoPixel@^1.12.4
+lib_deps = 
+	adafruit/Adafruit NeoPixel@^1.12.4
+	nanopb/Nanopb@^0.4.91

firmware/mini_v/estop_driver/src/config.hpp

@@ -0,0 +1,15 @@
+#ifndef __CONFIG_HPP__
+#define __CONFIG_HPP__
+
+
+const unsigned long TIMEOUT = 1000;
+
+
+byte MAC[] = {
+    0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAD
+};
+const IPAddress IP(192, 168, 0, 103);
+const unsigned int LOCALPORT_HEART = 4000;  // local port to listen on
+
+
+#endif

firmware/mini_v/estop_driver/src/heart_beat.cpp

@@ -0,0 +1,53 @@
+#include "heart_beat.hpp"
+
+UDPHeartBeat::UDPHeartBeat(u_int16_t _port, unsigned long _timeout = 1000) : port(_port), timeout(_timeout) {
+    
+}
+
+void UDPHeartBeat::begin(){
+    Udp.begin(port);
+}
+
+bool UDPHeartBeat::update_msgs()
+{
+    int packetSize = Udp.parsePacket();
+    if (packetSize) {
+        Serial.print("Received packet of size ");
+        Serial.print(packetSize);
+        Serial.print("  From ");
+        IPAddress remote = Udp.remoteIP();
+        for (int i=0; i < 4; i++) {
+        Serial.print(remote[i], DEC);
+        if (i < 3) Serial.print(".");
+        }
+        Serial.print(", port ");
+        Serial.print(Udp.remotePort());
+        Serial.print(" heart");
+        const size_t num_bytes = Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
+        
+        for (auto c : packetBuffer){
+            Serial.print(" ");
+            Serial.print(c);
+        }
+        Serial.println();
+        return true;
+    }
+    return false;
+}
+
+bool UDPHeartBeat::verify_survival()
+{
+    if (update_msgs()) {
+        preTime = millis();
+        return true;
+    }
+    else if (timeout > millis() - preTime) {
+        return true;
+    }
+    return false;
+}
+
+protolink__hardware_communication_msgs__HeartBeat_hardware_communication_msgs__HeartBeat UDPHeartBeat::get_msgs()
+{
+    return msg;
+}

firmware/mini_v/estop_driver/src/main.cpp

@@ -1,8 +1,13 @@
 #include <Arduino.h>
-
+#include <NativeEthernet.h>
+#include <NativeEthernetUdp.h>
 #include <Adafruit_NeoPixel.h>
 
-#define ESTOP_BUTTON_PIN 41
+#include "config.hpp"
+#include "heart_beat.hpp"
+
+#define ESTOP_BUTTON_IN_PIN 41
+#define ESTOP_BUTTON_OUT_PIN 37
 
 #define NUMPIXELS        60
 #define BRIGHTNESS      100
@@ -17,6 +22,9 @@ Adafruit_NeoPixel pixels[pixels_n] = {
   Adafruit_NeoPixel(NUMPIXELS, 17, NEO_GRB + NEO_KHZ800),
 };
 
+// Heart Beat
+UDPHeartBeat UdpHeart(LOCALPORT_HEART, TIMEOUT);
+
 unsigned long pre_time = 0;
 unsigned long pre_saver_time = 0;
 bool pre_estop_state = false;
@@ -42,7 +50,8 @@ void setup()
 {
   Serial.begin(9600);
 
-  pinMode(ESTOP_BUTTON_PIN, INPUT);
+  pinMode(ESTOP_BUTTON_IN_PIN, INPUT);
+  pinMode(ESTOP_BUTTON_OUT_PIN, OUTPUT);
 
   // Neopixel initalize
   for (int i = 0; i < pixels_n; i++)
@@ -52,13 +61,32 @@ void setup()
     pixels[i].clear();
     pixels[i].show();
   }
+
+  Ethernet.begin(MAC, IP);
+  if (Ethernet.hardwareStatus() == EthernetNoHardware) {
+    Serial.println("Ethernet shield was not found.  Sorry, can't run without hardware. :(");
+    while (true) {
+      delay(1); // do nothing, no point running without Ethernet hardware
+    }
+  }
+  if (Ethernet.linkStatus() == LinkOFF) {
+    Serial.println("Ethernet cable is not connected.");
+  }
+  // start UDP
+  UdpHeart.begin();
 }
 
 
 void loop()
 {
-  bool estop_state = !digitalRead(ESTOP_BUTTON_PIN);
+  // Check heart beat and signal on/off
+  if (UdpHeart.verify_survival()) digitalWrite(ESTOP_BUTTON_OUT_PIN, HIGH);
+  else digitalWrite(ESTOP_BUTTON_OUT_PIN, LOW);
+
+  // Check e-stop button state
+  bool estop_state = !digitalRead(ESTOP_BUTTON_IN_PIN);
 
+  // Determination state
   if (estop_state == true && millis() - pre_saver_time < SAVER)
     state = 2;
   else if (millis() - pre_time > INTERVAL)