try to fix submodule

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++;
+    }
+};
+