try to fix submodule

firmware/lib/Arduino-FOC-drivers/src/comms/telemetry/README.md

@@ -0,0 +1,42 @@
+
+# SimpleFOC Telemetry
+
+:warning: unfinished, untested
+
+A flexible abstraction for telemetry (monitoring) of SimpleFOC systems.
+
+The telemetry implementation is based on the SimpleFOC registers, and allows you to send telemetry for any (readable) register. Telemetry supports multiple motors.
+
+The concept allows you to choose registers which are then sent by the telemetry automatically, on a regular schedule.
+
+The method of sending depends on the type of telemetry you add to your program. There are telemetry drivers for:
+
+- Serial ASCII telemetry
+- Serial Binary telemetry
+- and more drivers will be added in the future
+
+Multiple motors can be added to the same telemetry, to monitor several motors at the same time. The registers reported by telemetry can be changed at run-time. Multiple instances of telemetry can be used to monitor different sets of values at different time intervals, or to send to multiple channels at the same time.
+
+## Usage
+
+Using telemetry is simple:
+
+```c++
+
+SerialASCIITelemetry telemetry = SerialASCIITelemetry();
+...
+
+void setup() {
+    ...
+    telemetry.addMotor(&motor);
+    telemetry.setTelemetryRegisters(3, { REG_TARGET, REG_ANGLE, REG_VELOCITY });
+    telemetry.init();
+    ...
+}
+
+void loop() {
+    motor.move();
+    motor.loopFOC();
+    telemetry.run();
+}
+```

firmware/lib/Arduino-FOC-drivers/src/comms/telemetry/Telemetry.cpp

@@ -0,0 +1,67 @@
+
+
+#include "./Telemetry.h"
+
+
+
+Telemetry::Telemetry() {
+    this->numRegisters = 0;
+};
+
+
+
+Telemetry::~Telemetry(){
+
+};
+
+
+
+
+
+void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors){
+    if (numRegisters<=TELEMETRY_MAX_REGISTERS) {
+        this->numRegisters = numRegisters;
+        for (uint8_t i=0; i<numRegisters; i++) {
+            this->registers[i] = registers[i];
+            if (motors!=NULL)
+                this->registers_motor[i] = motors[i];
+            else
+                this->registers_motor[i] = 0;
+        }
+    }
+};
+
+
+
+void Telemetry::init() {
+    headerSent = false;
+};
+
+
+
+void Telemetry::run() {
+    if (numRegisters<1)
+        return;
+    if (!headerSent) {
+        sendHeader();
+        headerSent = true;
+    }
+    if (downsampleCnt++ < downsample) return;
+    downsampleCnt = 0;
+    if (min_elapsed_time > 0) {
+        long now = _micros();
+        if (now - last_run_time < min_elapsed_time) return;
+        last_run_time = now;
+    }
+    sendTelemetry();
+}
+
+
+
+void Telemetry::addMotor(FOCMotor* motor) {
+    if (numMotors < TELEMETRY_MAX_MOTORS) {
+        motors[numMotors] = motor;
+        numMotors++;
+    }
+};
+

firmware/lib/Arduino-FOC-drivers/src/comms/telemetry/Telemetry.h

@@ -0,0 +1,53 @@
+
+#pragma once
+
+#include "../SimpleFOCRegisters.h"
+#include "../RegisterSender.h"
+
+#ifndef TELEMETRY_MAX_REGISTERS
+#define TELEMETRY_MAX_REGISTERS 8
+#endif
+
+#ifndef TELEMETRY_MAX_MOTORS
+#define TELEMETRY_MAX_MOTORS 4
+#endif
+
+
+#define DEF_TELEMETRY_DOWNSAMPLE 100
+
+
+typedef enum : uint8_t {
+    TELEMETRY_FRAMETYPE_DATA = 0x01,
+    TELEMETRY_FRAMETYPE_HEADER = 0x02
+} TelemetryFrameType;
+
+
+
+
+class Telemetry : public RegisterSender {
+public:
+    Telemetry();
+    virtual ~Telemetry();
+    virtual void init();
+    void addMotor(FOCMotor* motor);
+    void setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors = NULL);
+    void run();
+
+    uint16_t downsample = DEF_TELEMETRY_DOWNSAMPLE;
+    uint32_t min_elapsed_time = 0;
+protected:
+    virtual void sendTelemetry() = 0;
+    virtual void sendHeader() = 0;
+
+    FOCMotor* motors[TELEMETRY_MAX_MOTORS];
+    uint8_t numMotors = 0;    
+
+    uint8_t numRegisters;
+    uint8_t registers[TELEMETRY_MAX_REGISTERS];
+    uint8_t registers_motor[TELEMETRY_MAX_REGISTERS];
+    uint8_t frameSize;
+    bool headerSent;
+    long last_run_time = 0;
+    uint16_t downsampleCnt = 0;
+};
+