try to fix submodule

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/AS5600.cpp

@@ -0,0 +1,175 @@
+
+
+#include "./AS5600.h"
+
+
+AS5600::AS5600(uint8_t address) : _address(address) {};
+AS5600::~AS5600() {};
+
+void AS5600::init(TwoWire* wire){
+    _wire = wire;
+    _wire->begin();
+    if (!closeTransactions) {
+        setAngleRegister();
+    }
+};
+
+
+void AS5600::setAngleRegister() {
+    _wire->beginTransmission(_address);
+    if (useHysteresis)
+        _wire->write(AS5600_REG_ANGLE);
+    else
+        _wire->write(AS5600_REG_ANGLE_RAW);
+    _wire->endTransmission(false);
+}
+
+
+uint16_t AS5600::angle() {
+    uint16_t result = 0;
+    if (!closeTransactions) {
+        setAngleRegister();
+    }
+    _wire->requestFrom(_address, (uint8_t)2, (uint8_t)closeTransactions);
+    result = _wire->read()<<8;
+    result |= _wire->read();
+    return result;
+};
+
+
+uint16_t AS5600::readRawAngle() {
+    return readRegister(AS5600_REG_ANGLE_RAW, 2);
+};
+
+
+uint16_t AS5600::readAngle() {
+    return readRegister(AS5600_REG_ANGLE, 2);
+};
+
+
+
+uint16_t AS5600::readMagnitude() {
+    return readRegister(AS5600_REG_MAGNITUDE, 2);
+};
+
+
+AS5600Status AS5600::readStatus() {
+    AS5600Status result;
+    result.reg = (uint8_t)readRegister(AS5600_REG_STATUS, 1); // TODO: shift bits around
+    return result;
+};
+
+
+uint8_t AS5600::readAGC() {
+    return (uint8_t)readRegister(AS5600_REG_AGC, 1);
+};
+
+
+
+AS5600Conf AS5600::readConf() {
+    AS5600Conf result;
+    result.reg = readRegister(AS5600_REG_CONF, 2);
+    return result;
+};
+
+
+uint16_t AS5600::readMang() {
+    return readRegister(AS5600_REG_MANG, 2);
+};
+
+
+uint16_t AS5600::readMPos() {
+    return readRegister(AS5600_REG_MPOS, 2);
+};
+
+
+uint16_t AS5600::readZPos() {
+    return readRegister(AS5600_REG_ZPOS, 2);
+};
+
+
+uint8_t AS5600::readZMCO() {
+    return (readRegister(AS5600_REG_ZMCO, 1)&0x03);
+};
+
+uint8_t AS5600::readI2CAddr() {
+    return (readRegister(AS5600_REG_I2CADDR, 1)>>1);
+};
+
+
+// set registers
+void AS5600::setConf(AS5600Conf value) {
+    // TODO: read before write
+    writeRegister(AS5600_REG_CONF, value.reg);
+};
+
+
+void AS5600::setMang(uint16_t value) {
+    // TODO: read before write
+    writeRegister(AS5600_REG_MANG, value);
+};
+
+
+void AS5600::setMPos(uint16_t value) {
+    // TODO: read before write
+    writeRegister(AS5600_REG_MPOS, value);
+};
+
+
+void AS5600::setZPos(uint16_t value) {
+    // TODO: read before write
+    writeRegister(AS5600_REG_ZPOS, value);
+};
+
+void AS5600::setI2CAddr(uint8_t value) {
+    uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CADDR, 1);
+    val = (value<<1) | (val&0x01);
+    writeRegister(AS5600_REG_I2CADDR, val);
+};
+
+void AS5600::setI2CUpdt(uint8_t value) {
+    uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CUPDT, 1);
+    val = (value<<1) | (val&0x01);
+    writeRegister(AS5600_REG_I2CUPDT, val);
+};
+
+
+void AS5600::burnSettings(){
+    writeRegister(AS5600_REG_BURN, 0x40, 1);
+}
+
+
+
+uint16_t AS5600::readRegister(uint8_t reg, uint8_t len){
+    uint16_t result = 0;
+    _wire->beginTransmission(_address);
+    _wire->write(reg);
+    _wire->endTransmission(false);
+    _wire->requestFrom(_address, len, (uint8_t)closeTransactions);
+    if (!closeTransactions) {
+        setAngleRegister();
+    }
+    result = _wire->read();
+    if (len == 2) {
+        result <<= 8;
+        result |= _wire->read();
+    }
+    return result;
+};
+
+
+
+void AS5600::writeRegister(uint8_t reg, uint16_t val, uint8_t len){
+    _wire->beginTransmission(_address);
+    _wire->write(reg);
+    if (len == 2) {
+        _wire->write(val>>8);
+    }
+    _wire->write(val&0xFF);
+    _wire->endTransmission(closeTransactions);
+    if (!closeTransactions) {
+        setAngleRegister();
+    }
+};
+
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/AS5600.h

@@ -0,0 +1,104 @@
+
+#pragma once
+
+
+#include <Arduino.h>
+#include <Wire.h>
+
+
+#define AS5600_REG_ZMCO 0x00
+#define AS5600_REG_ZPOS 0x01
+#define AS5600_REG_MPOS 0x03
+#define AS5600_REG_MANG 0x05
+#define AS5600_REG_CONF 0x07
+
+#define AS5600_REG_I2CADDR 0x20
+#define AS5600_REG_I2CUPDT 0x21
+
+#define AS5600_REG_ANGLE 0x0E
+#define AS5600_REG_ANGLE_RAW 0x0C
+
+#define AS5600_REG_STATUS 0x0B
+#define AS5600_REG_AGC 0x1A
+#define AS5600_REG_MAGNITUDE 0x1B
+
+#define AS5600_REG_BURN 0xFF
+
+#define AS5600_CPR (4096.0f)
+
+
+union AS5600Conf {
+	struct {
+		uint16_t pm:2;
+		uint16_t hyst:2;
+		uint16_t outs:2;
+		uint16_t pwmf:2;
+		uint16_t sf:2;
+		uint16_t fth:3;
+		uint16_t wd:1;
+		uint16_t unused:2;
+	};
+	uint16_t reg;
+};
+
+union AS5600Status {
+	struct {
+		uint8_t unused:3;
+		uint8_t mh:1;
+		uint8_t ml:1;
+		uint8_t md:1;
+		uint8_t unused2:2;
+	};
+	uint8_t reg;
+};
+
+
+
+
+class AS5600 {
+public:
+    AS5600(uint8_t address = 0x36);
+    ~AS5600();
+
+    virtual void init(TwoWire* wire = &Wire);
+
+    // read an angle, either with or without hysteresis, depending on the useHysteresis flag
+    // and using fast mode (not closing transactions) if so configured
+    uint16_t angle();
+
+    // read registers
+    uint16_t readRawAngle();
+    uint16_t readAngle();
+
+    uint16_t readMagnitude();
+    AS5600Status readStatus();
+    uint8_t readAGC();
+
+    AS5600Conf readConf();
+    uint16_t readMang();
+    uint16_t readMPos();
+    uint16_t readZPos();
+    uint8_t readZMCO();
+    uint8_t readI2CAddr();
+
+    // set registers
+    void setConf(AS5600Conf value);
+    void setMang(uint16_t value);
+    void setMPos(uint16_t value);
+    void setZPos(uint16_t value);
+    void setI2CAddr(uint8_t value);
+    void setI2CUpdt(uint8_t value);
+
+    void burnSettings();
+
+    bool closeTransactions = true;
+    bool useHysteresis = true;
+    uint8_t _address;
+protected:
+    TwoWire* _wire;
+
+    void setAngleRegister();
+    uint16_t readRegister(uint8_t reg, uint8_t len);
+    void writeRegister(uint8_t reg, uint16_t val, uint8_t len = 2);
+};
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/MagneticSensorAS5600.cpp

@@ -0,0 +1,18 @@
+
+
+#include "./MagneticSensorAS5600.h"
+#include "common/foc_utils.h"
+
+MagneticSensorAS5600::MagneticSensorAS5600(uint8_t _address) : AS5600(_address) {};
+MagneticSensorAS5600::~MagneticSensorAS5600() {};
+
+void MagneticSensorAS5600::init(TwoWire* wire) {
+  AS5600::init(wire);
+  Sensor::init();
+};
+
+
+float MagneticSensorAS5600::getSensorAngle() {
+    uint16_t raw = readRawAngle();
+    return raw / AS5600_CPR * _2PI;
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/MagneticSensorAS5600.h

@@ -0,0 +1,22 @@
+
+#pragma once
+
+
+#include "./AS5600.h"
+#include "common/base_classes/Sensor.h"
+
+
+class MagneticSensorAS5600 : public Sensor, public AS5600 {
+
+public:
+    MagneticSensorAS5600(uint8_t _address = 0x36);
+    ~MagneticSensorAS5600();
+
+    virtual void init(TwoWire* wire = &Wire);
+
+    virtual float getSensorAngle() override;    
+
+protected:
+
+};
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/README.md

@@ -0,0 +1,131 @@
+# AS5600 SimpleFOC driver
+
+I2C protocol driver for the AMS AS5600 magnetic encoder (digital potentiometer). Also supports the newer AS5600L variant.
+
+:warning: work in progress
+
+## Hardware setup
+
+Connect the sensor to 3.3V or 5V power as appropriate, and to I2C (SDA and SCL).
+
+Important: please make sure the direction pin (DIR) is either pulled up to VDD, or down to GND. Do not leave the direction pin floating.
+
+## Software setup
+
+The sensor driver is easy to use.
+
+```c++
+#include <Arduino.h>
+#include <SimpleFOC.h>
+#include <SimpleFOCDrivers.h>
+#include "encoders/as5600/MagneticSensorAS5600.h"
+
+MagneticSensorAS5600 sensor;
+
+void setup() {
+    sensor.init();
+}
+
+long ts;
+
+void loop() {
+  sensor.update();
+  if (millis() - ts > 1000) {
+    Serial.println(sensor.getAngle(), 3);
+    ts = millis();
+  }
+  delay(1);
+}
+```
+
+You can use a different I2C bus by passing a pointer to its TwoWire object in the init method:
+
+```c++
+MagneticSensorAS5600 sensor1;
+MagneticSensorAS5600 sensor2;
+TwoWire myI2C;
+
+void setup() {
+
+    ...
+
+    sensor1.init(&myI2C);
+    sensor2.init(&Wire2);
+}
+```
+
+Using the sensor without releasing the bus should give you considerably more speed. Setting closeTransactions to false means the sensor will not release the bus between angle reads, and also will not re-write the register to the device, which boosts angle reading throughput significantly.
+
+```c++
+MagneticSensorAS5600 sensor;
+
+void setup() {
+    sensor.closeTransactions = false;
+    sensor.init();
+}
+```
+
+The sensor's other registers are exposed. Note that using the setter functions to set register values only performs a normal write, not a permanent programing of the register. Permanent programming (BURN function) is not supported by this driver.
+
+```c++
+
+MagneticSensorAS5600 sensor;
+
+void setup() {
+    sensor.closeTransactions = false;
+    sensor.init();
+
+    uint16_t magnitude = sensor.readMagnitude();
+    AS5600Status status = sensor.readStatus();
+    // print the values or something
+}
+
+```
+
+The AS5600L has a default address of 0x40, and you can set the I2C address. To temporarily change the address (resets to default on restart):
+
+```c++
+
+MagneticSensorAS5600 sensor(0x40); // default address of AS5600L is 0x40
+
+void setup() {
+    Serial.begin(115200);
+    delay(2000);
+
+    sensor.closeTransactions = true; // use normal transactons
+    sensor.init();
+    sensor.setI2CAddr(0x41); // set address to 0x41
+    sensor.setI2CUpdt(0x41); // i2c address is now 0x41
+    sensor._address = 0x41;
+    delay(10);
+    // sensor is using new address
+    uint16_t magnitude = sensor.readMagnitude();
+}
+```
+
+
+You can program an I2C address permanently (AS5600L only). The address will remain after reset. This programming operation can only be done once.
+
+```c++
+
+MagneticSensorAS5600 sensor(0x40); // default address of AS5600L is 0x40
+
+void setup() {
+    Serial.begin(115200);
+    delay(2000);
+
+    sensor.closeTransactions = true; // use normal transactons
+    sensor.init();
+
+    uint8_t addr = sensor.readI2CAddr(); // read I2C address, should be 0x40
+    Serial.print("Current Address: ");
+    Serial.println(addr);
+
+    sensor.setI2CAddr(0x41); // set address to 0x41
+    sensor.burnSettings(); // permanently set new address
+    Serial.println("Permanently changed address to 0x41. Restart the system.");
+    while (1);
+}
+
+
+```