try to fix submodule

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

@@ -0,0 +1,49 @@
+
+# Serial communications classes
+
+Serial communications classes for register-based control and telemetry from SimpleFOC.
+
+## SerialASCIITelemetry
+
+:warning: unfinished, untested
+
+Telemetry class that sends telemetry as ASCII on a serial port. Similar to the classic "monitoring" functionality of SimpleFOC, but allows you to configure telemetry based on most of the defined registers.
+
+Usage:
+
+```c++
+
+SerialASCIITelemetry telemetry = SerialASCIITelemetry(); // number of float digits to display
+
+void setup() {
+    ...
+    telemetry.addMotor(&motor);
+    telemetry.setTelemetryRegisters(2, [REG_VELOCITY, REG_VOLTAGE_Q]);
+    telemetry.init();
+    ...
+}
+
+void loop() {
+    motor.move();
+    motor.loopFOC();
+    telemetry.run();
+}
+```
+
+Some options are supported:
+
+```c++
+    telemetry.floatPrecision = 4;       // send floats with 4 decimal places
+    telemetry.min_elapsed_time = 0;     // microseconds between sending telemetry
+    telemetry.downsample = 100;         // send every this many loop iterations
+```
+
+
+
+## SerialBinaryCommander
+
+:warning: unfinished, untested!
+
+Control SimpleFOC via a binary protocol over the serial port. The standard SimpleFOC registers are used.
+
+TODO document the protocol

firmware/lib/Arduino-FOC-drivers/src/comms/serial/SerialASCIITelemetry.cpp

@@ -0,0 +1,65 @@
+
+#include "./SerialASCIITelemetry.h"
+
+SerialASCIITelemetry::SerialASCIITelemetry(int floatPrecision) : Telemetry() {
+    this->floatPrecision = floatPrecision;
+};
+
+SerialASCIITelemetry::~SerialASCIITelemetry(){
+
+};
+
+void SerialASCIITelemetry::init(Print* print){
+    this->_print = print;
+    this->Telemetry::init();
+};
+
+
+
+void SerialASCIITelemetry::sendHeader() {
+    if (numRegisters > 0) {
+        writeChar('H');
+        writeChar(' ');
+        for (uint8_t i = 0; i < numRegisters; i++) {
+            writeByte(registers_motor[i]);
+            writeChar(':');
+            writeByte(registers[i]);
+            if (i < numRegisters-1)
+                writeChar(' ');
+        };
+        writeChar('\n');
+    };
+};
+
+
+
+void SerialASCIITelemetry::sendTelemetry(){
+    if (numRegisters > 0) {
+        for (uint8_t i = 0; i < numRegisters; i++) {
+            sendRegister(static_cast<SimpleFOCRegister>(registers[i]), motors[registers_motor[i]]);
+            if (i < numRegisters-1)
+                writeChar(' ');
+        };
+        writeChar('\n');
+    }
+};
+
+
+
+uint8_t SerialASCIITelemetry::writeChar(char value){
+    return _print->print(value);
+};
+
+uint8_t SerialASCIITelemetry::writeByte(uint8_t value){
+    return _print->print(value);
+};
+
+
+uint8_t SerialASCIITelemetry::writeFloat(float value){
+    return _print->print(value, floatPrecision);
+};
+
+
+uint8_t SerialASCIITelemetry::writeInt(uint32_t value){
+    return _print->print(value);    
+};

firmware/lib/Arduino-FOC-drivers/src/comms/serial/SerialASCIITelemetry.h

@@ -0,0 +1,27 @@
+
+#pragma once
+
+#include "Arduino.h"
+#include "../telemetry/Telemetry.h"
+
+class SerialASCIITelemetry : public Telemetry {
+public:
+    SerialASCIITelemetry(int floatPrecision = 2);
+    virtual ~SerialASCIITelemetry();
+
+    void init(Print* print = &Serial);
+
+protected:
+    uint8_t writeByte(uint8_t value) override;
+    uint8_t writeFloat(float value) override;
+    uint8_t writeInt(uint32_t value) override;
+    uint8_t writeChar(char value);
+
+    void sendTelemetry() override;
+    void sendHeader() override;    
+
+    Print* _print;
+    int floatPrecision = 2;
+};
+
+

firmware/lib/Arduino-FOC-drivers/src/comms/serial/SerialBinaryCommander.cpp

@@ -0,0 +1,153 @@
+
+#include "SerialBinaryCommander.h"
+
+
+
+SerialBinaryCommander::SerialBinaryCommander(bool echo) : Telemetry(), echo(echo) {
+
+};
+
+
+
+SerialBinaryCommander::~SerialBinaryCommander(){
+
+};
+
+
+
+void SerialBinaryCommander::init(Stream &serial) {
+    _serial = &serial;
+    this->Telemetry::init();
+};
+
+
+
+void SerialBinaryCommander::run() {
+    if (_serial->available()>2) { // TODO make this work with partial packets...
+        uint8_t command = _serial->read();
+        uint8_t registerNum = _serial->read();
+        uint8_t motorNum = _serial->read();
+        if (command==SERIALBINARY_COMMAND_READ){
+            startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); // TODO call is incorrect
+            sendRegister(static_cast<SimpleFOCRegister>(registerNum), motors[motorNum]);
+            endFrame();
+        }
+        else if (command==SERIALBINARY_COMMAND_WRITE) {
+            setRegister(static_cast<SimpleFOCRegister>(registerNum), motors[motorNum]);
+            if (echo) {
+                startFrame(registerNum, SERIALBINARY_FRAMETYPE_REGISTER); // TODO call is incorrect
+                sendRegister(static_cast<SimpleFOCRegister>(registerNum), motors[motorNum]);
+                endFrame();
+            }
+        }
+    }
+    // and handle the telemetry
+    this->Telemetry::run();
+};
+
+
+
+uint8_t SerialBinaryCommander::readBytes(void* valueToSet, uint8_t numBytes){
+    if (_serial->available()<numBytes)
+        return 0;
+    uint8_t* value = (uint8_t*)valueToSet;
+    for (uint8_t i=0; i<numBytes; i++) {
+        value[i] = _serial->read();
+    }
+    return numBytes;
+};
+
+
+
+
+uint8_t SerialBinaryCommander::writeBytes(void* valueToSend, uint8_t numBytes){
+    uint8_t* value = (uint8_t*)valueToSend;
+    for (uint8_t i=0; i<numBytes; i++) {
+        _serial->write(value[i]);
+    }
+    return numBytes;
+};
+
+
+
+
+
+
+void SerialBinaryCommander::startFrame(uint8_t frameSize, uint8_t type){
+    _serial->write(0xA5);
+    _serial->write((uint8_t)((type<<6)|frameSize));
+};
+
+
+
+
+
+void SerialBinaryCommander::endFrame(){
+    _serial->write(0x5A);
+};
+
+
+
+
+uint8_t SerialBinaryCommander::writeByte(uint8_t value){
+    return writeBytes(&value, 1);
+};
+
+
+
+uint8_t SerialBinaryCommander::writeFloat(float value){
+    return writeBytes(&value, 4);
+};
+
+
+
+uint8_t SerialBinaryCommander::writeInt(uint32_t value){
+    return writeBytes(&value, 4);
+};
+
+
+
+
+uint8_t SerialBinaryCommander::readByte(uint8_t* valueToSet){
+    return readBytes(valueToSet, 1);
+};
+
+
+
+uint8_t SerialBinaryCommander::readFloat(float* valueToSet){
+    return readBytes(valueToSet, 4);
+};
+
+
+
+uint8_t SerialBinaryCommander::readInt(uint32_t* valueToSet){
+    return readBytes(valueToSet, 4);
+};
+
+
+
+
+
+
+void SerialBinaryCommander::sendHeader() {
+    if (numRegisters > 0) {
+        startFrame(numRegisters*2, TELEMETRY_FRAMETYPE_HEADER);
+        for (uint8_t i = 0; i < numRegisters; i++) {
+            writeByte(registers[i]);
+            writeByte(registers_motor[i]);
+        };
+        endFrame();
+    };
+};
+
+
+
+void SerialBinaryCommander::sendTelemetry(){
+    if (numRegisters > 0) {
+        startFrame(frameSize, TELEMETRY_FRAMETYPE_DATA);
+        for (uint8_t i = 0; i < numRegisters; i++) {
+            sendRegister(static_cast<SimpleFOCRegister>(registers[i]), motors[registers_motor[i]]);
+        };
+        endFrame();
+    }
+};

firmware/lib/Arduino-FOC-drivers/src/comms/serial/SerialBinaryCommander.h

@@ -0,0 +1,57 @@
+
+#pragma once
+
+#include <Arduino.h>
+#include "../RegisterReceiver.h"
+#include "../telemetry/Telemetry.h"
+
+
+#ifndef COMMS_MAX_REPORT_REGISTERS
+#define COMMS_MAX_REPORT_REGISTERS 7
+#endif
+
+
+#define SERIALBINARY_FRAMETYPE_REGISTER 0x03
+#define SERIALBINARY_COMMAND_READ 0x00
+#define SERIALBINARY_COMMAND_WRITE 0x80
+
+
+class SerialBinaryCommander : public Telemetry, public RegisterReceiver {
+public:
+    SerialBinaryCommander(bool echo = false);
+    virtual ~SerialBinaryCommander();
+
+    void init(Stream &serial = Serial);
+    void run();
+
+
+    bool echo;
+protected:
+    virtual void startFrame(uint8_t frameSize, uint8_t type = TELEMETRY_FRAMETYPE_DATA);
+    virtual void endFrame();
+    uint8_t readBytes(void* valueToSet, uint8_t numBytes);
+    uint8_t writeBytes(void* valueToSend, uint8_t numBytes);
+
+    uint8_t writeByte(uint8_t value) override;
+    uint8_t writeFloat(float value) override;
+    uint8_t writeInt(uint32_t value) override;
+
+    uint8_t readByte(uint8_t* valueToSet) override;
+    uint8_t readFloat(float* valueToSet) override;
+    uint8_t readInt(uint32_t* valueToSet) override;
+
+    void sendTelemetry() override;
+    void sendHeader() override;
+
+    uint8_t curMotor = 0;
+    SimpleFOCRegister curRegister = REG_STATUS;
+    bool commanderror = false;
+    bool lastcommanderror = false;
+    uint8_t lastcommandregister = REG_STATUS;
+
+    uint8_t numReportRegisters = 0;
+    uint8_t reportMotors[COMMS_MAX_REPORT_REGISTERS];
+    SimpleFOCRegister reportRegisters[COMMS_MAX_REPORT_REGISTERS];
+
+    Stream* _serial;
+};