try to fix submodule

2023-11-09 19:02:15 -05:00
parent c1d45aa443
commit deea94b076
366 changed files with 40228 additions and 2 deletions
--- a/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/AS5047.cpp
+++ b/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/AS5047.cpp
@@ -0,0 +1,245 @@
+/*
+ * AS5047.cpp
+ *
+ *  Created on: 2 May 2021
+ *      Author: runger
+ */
+
+#include "./AS5047.h"
+
+AS5047::AS5047(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+	// nix
+}
+
+AS5047::~AS5047() {
+}
+
+
+void AS5047::init(SPIClass* _spi) {
+	spi = _spi;
+	if (nCS>=0)
+		pinMode(nCS, OUTPUT);
+	digitalWrite(nCS, HIGH);
+	//SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices
+	spi->begin();
+	readRawAngle(); // read an angle
+}
+
+float AS5047::getCurrentAngle(){
+	readCorrectedAngle();
+	return ((float)readCorrectedAngle())/(float)AS5047_CPR * 2.0f * (float)PI;
+}
+
+float AS5047::getFastAngle(){
+	return ((float)readCorrectedAngle())/(float)AS5047_CPR * 2.0f * (float)PI;
+}
+
+
+uint16_t AS5047::readRawAngle(){
+	uint16_t command = AS5047_ANGLEUNC_REG | AS5047_RW; // set r=1 and parity=0, result is 0x7FFE
+	uint16_t lastresult = spi_transfer16(command)&AS5047_RESULT_MASK;
+	return lastresult;
+}
+uint16_t AS5047::readCorrectedAngle(){
+	uint16_t command = AS5047_ANGLECOM_REG | AS5047_PARITY | AS5047_RW; // set r=1 and parity=1, result is 0xFFFF
+	uint16_t lastresult = spi_transfer16(command)&AS5047_RESULT_MASK;
+	return lastresult;
+}
+
+
+uint16_t AS5047::readMagnitude(){
+	uint16_t command = AS5047_MAGNITUDE_REG | AS5047_RW; // set r=1, result is 0x7FFD
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop();
+	return result;
+}
+
+
+bool AS5047::isErrorFlag(){
+	return errorflag;
+}
+
+
+AS5047Error AS5047::clearErrorFlag(){
+	uint16_t command = AS5047_ERROR_REG | AS5047_RW; // set r=1, result is 0x4001
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop();
+	AS5047Error err = {
+			.framingError = ((result&0x0001)!=0x0000),
+			.commandInvalid = ((result&0x0002)!=0x0000),
+			.parityError = ((result&0x0004)!=0x0000)
+	};
+	return err;
+}
+
+
+AS5047Settings1 AS5047::readSettings1(){
+	uint16_t command = AS5047_SETTINGS1_REG | AS5047_PARITY | AS5047_RW; // set r=1, result is 0xC018
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047Settings1 result = {
+			.reg = nop()
+	};
+	return result;
+}
+
+
+void AS5047::writeSettings1(AS5047Settings1 settings){
+	uint16_t command = AS5047_SETTINGS1_REG;  // set r=0, result is 0x0018
+	spi_transfer16(command);
+	spi_transfer16(calcParity(settings.reg));
+}
+
+
+AS5047Settings2 AS5047::readSettings2(){
+	uint16_t command = AS5047_SETTINGS2_REG | AS5047_RW; // set r=1, result is 0x4019
+	spi_transfer16(command);
+	AS5047Settings2 result = {
+			.reg = nop()
+	};
+	return result;
+}
+
+
+void AS5047::writeSettings2(AS5047Settings2 settings){
+	uint16_t command = AS5047_SETTINGS2_REG | AS5047_PARITY; // set r=0, result is 0x8019
+	spi_transfer16(command);
+	spi_transfer16(calcParity(settings.reg));
+}
+
+
+
+AS5047Diagnostics AS5047::readDiagnostics(){
+	uint16_t command = AS5047_DIAGNOSTICS_REG | AS5047_PARITY | AS5047_RW; // set r=1, result is 0xFFFC
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047Diagnostics result = {
+			.reg = nop()
+	};
+	return result;
+}
+
+
+void AS5047::enablePWM(bool enable){
+	AS5047Settings1 settings = readSettings1();
+	if (settings.pwmon == enable) return; // no change
+	settings.pwmon = enable;
+	writeSettings1(settings);
+}
+
+void AS5047::enableABI(bool enable){
+	AS5047Settings1 settings = readSettings1();
+	uint8_t val = enable?0:1;
+	if (settings.uvw_abi == val) return; // no change
+	settings.uvw_abi = val;
+	writeSettings1(settings);
+}
+
+
+void AS5047::enableDEAC(bool enable){
+	AS5047Settings1 settings = readSettings1();
+	uint8_t val = enable?0:1;
+	if (settings.daecdis == val) return; // no change
+	settings.daecdis = enable?0:1;
+	writeSettings1(settings);
+}
+
+
+void AS5047::useCorrectedAngle(bool useCorrected){
+	AS5047Settings1 settings = readSettings1();
+	uint8_t val = useCorrected?0:1;
+	if (settings.dataselect == val) return; // no change
+	settings.dataselect = val;
+	writeSettings1(settings);
+}
+
+
+
+void AS5047::setHysteresis(uint8_t hyst){
+	if (hyst>3) hyst = 3;
+	uint8_t val = 3-hyst;
+	AS5047Settings2 settings = readSettings2();
+	if (settings.hys == val) return; // no change
+	settings.hys = val;
+	writeSettings2(settings);
+}
+
+
+
+
+void AS5047::setABIResolution(AS5047ABIRes res){
+	AS5047Settings1 settings = readSettings1();
+	uint8_t val = (res>>3)&0x01;
+	if (settings.abibin!=val || settings.uvw_abi!=0) {
+		settings.abibin = val;
+		settings.uvw_abi = 0;
+		writeSettings1(settings);
+	}
+	AS5047Settings2 settings2 = readSettings2();
+	val = (res&0x07);
+	if (settings2.abires!=val) {
+		settings2.abires = val;
+		writeSettings2(settings2);
+	}
+} 
+
+
+
+uint16_t AS5047::setZero(uint16_t value){
+	uint16_t command = AS5047_ZPOSL_REG | AS5047_PARITY | AS5047_RW;
+	spi_transfer16(command);
+	AS5047ZPosL posL = {
+			.reg = nop()
+	};
+	command = AS5047_ZPOSM_REG | AS5047_PARITY;
+	spi_transfer16(command);
+	spi_transfer16(calcParity((value>>6)&0x00FF));
+	command = AS5047_ZPOSL_REG;
+	posL.zposl = value&0x003F;
+	spi_transfer16(command);
+	spi_transfer16(calcParity(posL.reg));
+
+	return getZero();
+}
+
+
+uint16_t AS5047::getZero() {
+	uint16_t command = AS5047_ZPOSM_REG | AS5047_RW;
+	spi_transfer16(command);
+	command = AS5047_ZPOSL_REG | AS5047_PARITY | AS5047_RW;
+	uint16_t result = spi_transfer16(command);
+	AS5047ZPosL posL = {
+			.reg = nop()
+	};
+	return ((result&0x00FF)<<6) | posL.zposl;
+}
+
+
+uint16_t AS5047::nop(){
+	uint16_t result = spi_transfer16(0xFFFF); // using 0xFFFF as nop instead of 0x0000, then next call to fastAngle will return an angle
+	return result&AS5047_RESULT_MASK;
+}
+
+
+uint16_t AS5047::calcParity(uint16_t data){
+	data = data & 0x7FFF;
+	int sum = 0;
+	for (int i=0;i<15;i++)
+		if ((data>>i)&0x0001)
+			sum++;
+	return (sum&0x01)==0x01?(data|0x8000):data;
+}
+
+
+uint16_t AS5047::spi_transfer16(uint16_t outdata) {
+	if (nCS>=0)
+		digitalWrite(nCS, 0);
+	spi->beginTransaction(settings);
+	uint16_t result = spi->transfer16(outdata);
+	spi->endTransaction();
+	if (nCS>=0)
+		digitalWrite(nCS, 1);
+	// TODO check parity
+	errorflag = ((result&AS5047_ERRFLG)>0);
+	return result;
+}
+
+
--- a/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/AS5047.h
+++ b/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/AS5047.h
@@ -0,0 +1,167 @@
+/*
+ * AS5047.h
+ *
+ *  Created on: 2 May 2021
+ *      Author: runger
+ */
+
+#ifndef AS5047_H_
+#define AS5047_H_
+
+#include "Arduino.h"
+#include "SPI.h"
+
+
+
+
+union AS5047Diagnostics {
+	struct {
+		uint16_t agc:8;
+		uint16_t lf:1;
+		uint16_t cof:1;
+		uint16_t magh:1;
+		uint16_t magl:1;
+		uint16_t unused:4;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047ZPosM {
+	struct {
+		uint16_t zposm:8;
+		uint16_t unused:8;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047ZPosL {
+	struct {
+		uint16_t zposl:6;
+		uint16_t comp_l_error_en:1;
+		uint16_t comp_h_error_en:1;
+		uint16_t unused:8;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047Settings1 {
+	struct {
+		uint16_t reserved:1;
+		uint16_t noiseset:1;
+		uint16_t dir:1;
+		uint16_t uvw_abi:1;
+		uint16_t daecdis:1;
+		uint16_t abibin:1;
+		uint16_t dataselect:1;
+		uint16_t pwmon:1;
+		uint16_t unused:8;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047Settings2 {
+	struct {
+		uint16_t uvwpp:3;
+		uint16_t hys:2;
+		uint16_t abires:3;
+		uint16_t unused:8;
+	};
+	uint16_t reg;
+};
+
+
+struct AS5047Error {
+	bool framingError;
+	bool commandInvalid;
+	bool parityError;
+};
+
+
+enum AS5047ABIRes : uint8_t {
+	AS5047_ABI_1024 = 0b1010,
+	AS5047_ABI_2048 = 0b1001,
+	AS5047_ABI_4096 = 0b1000,
+	AS5047_ABI_100 = 0b0111,
+	AS5047_ABI_200 = 0b0110,
+	AS5047_ABI_400 = 0b0101,
+	AS5047_ABI_800 = 0b0100,
+	AS5047_ABI_1200 = 0b0011,
+	AS5047_ABI_1600 = 0b0010,
+	AS5047_ABI_2000 = 0b0001,
+	AS5047_ABI_4000 = 0b0000
+};
+
+
+#define AS5047_CPR 16384
+#define AS5047_ANGLECOM_REG 0x3FFF
+#define AS5047_ANGLEUNC_REG 0x3FFE
+#define AS5047_MAGNITUDE_REG 0x3FFD
+#define AS5047_DIAGNOSTICS_REG 0x3FFC
+#define AS5047_ERROR_REG 0x0001
+#define AS5047_PROG_REG 0x0003
+#define AS5047_ZPOSM_REG 0x0016
+#define AS5047_ZPOSL_REG 0x0017
+#define AS5047_SETTINGS1_REG 0x0018
+#define AS5047_SETTINGS2_REG 0x0019
+
+#define AS5047_PARITY 0x8000
+#define AS5047_RW 0x4000
+#define AS5047_ERRFLG 0x4000
+#define AS5047_RESULT_MASK 0x3FFF
+
+
+#define AS5047_BITORDER MSBFIRST
+
+static SPISettings AS5047SPISettings(8000000, AS5047_BITORDER, SPI_MODE1); // @suppress("Invalid arguments")
+
+
+class AS5047 {
+public:
+	AS5047(SPISettings settings = AS5047SPISettings, int nCS = -1);
+	virtual ~AS5047();
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+	float getCurrentAngle(); // angle in radians, return current value
+	float getFastAngle();	 // angle in radians, return last value and read new
+
+	uint16_t readRawAngle(); // 14bit angle value
+	uint16_t readCorrectedAngle(); // 14bit corrected angle value
+	uint16_t readMagnitude(); // 14bit magnitude value
+
+	bool isErrorFlag();
+	AS5047Error clearErrorFlag();
+
+	AS5047Diagnostics readDiagnostics();
+	AS5047Settings1 readSettings1();
+	void writeSettings1(AS5047Settings1 settings);
+	AS5047Settings2 readSettings2();
+	void writeSettings2(AS5047Settings2 settings);
+	void enablePWM(bool enable);
+	void enableABI(bool enable);
+	void setABIResolution(AS5047ABIRes res);
+	void enableDEAC(bool enable);
+	void useCorrectedAngle(bool useCorrected);
+	void setHysteresis(uint8_t hyst);
+
+	uint16_t setZero(uint16_t);
+	uint16_t getZero();
+
+	uint16_t calcParity(uint16_t data);
+
+private:
+
+	uint16_t nop();
+	uint16_t spi_transfer16(uint16_t outdata);
+	SPIClass* spi;
+	SPISettings settings;
+	bool errorflag = false;
+	int nCS = -1;
+
+};
+
+#endif /* AS5047_H_ */
--- a/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/MagneticSensorAS5047.cpp
+++ b/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/MagneticSensorAS5047.cpp
@@ -0,0 +1,29 @@
+
+#include "./MagneticSensorAS5047.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorAS5047::MagneticSensorAS5047(int nCS, bool fastMode, SPISettings settings) : AS5047(settings, nCS), fastMode(fastMode) {
+
+}
+
+
+MagneticSensorAS5047::~MagneticSensorAS5047(){ 
+
+}
+
+
+void MagneticSensorAS5047::init(SPIClass* _spi) {
+    this->AS5047::init(_spi);
+    this->Sensor::init();
+}
+
+
+float MagneticSensorAS5047::getSensorAngle() {
+    float angle_data = readRawAngle();
+    if (!fastMode) // read again to ensure current value
+        angle_data = readRawAngle();
+    angle_data = ( angle_data / (float)AS5047_CPR) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}
--- a/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/MagneticSensorAS5047.h
+++ b/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/MagneticSensorAS5047.h
@@ -0,0 +1,20 @@
+
+#ifndef __MAGNETICSENSORAS5047PD_H__
+#define __MAGNETICSENSORAS5047PD_H__
+
+#include "common/base_classes/Sensor.h"
+#include "./AS5047.h"
+
+class MagneticSensorAS5047 : public Sensor, public AS5047 {
+public:
+	MagneticSensorAS5047(int nCS = -1, bool fastMode = false, SPISettings settings = AS5047SPISettings);
+	virtual ~MagneticSensorAS5047();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+private:
+    bool fastMode = false;
+};
+
+#endif
--- a/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/README.md
+++ b/firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/README.md
@@ -0,0 +1,82 @@
+# AS5047 SimpleFOC driver
+
+While the AS5047 absolute position magnetic rotary encoder is supported by the standard MagneticSensorSPI driver included in the base distribution, this AS5047-specific driver includes some optimisations:
+
+- access to the other registers of the AS5047, including the magnitude value which can be used to check the magnet strength, and the diagnostics register
+- access to the error state of the sensor, and ability to clear errors
+- it has a fastMode setting, in which the sensor is sent only 1 command per getAngle() call - the value returned will be from previous getAngle() invocation
+
+This driver should work with AS5047P and AS5047D models. The AS5047U has it's own driver [here](../as5047u/).
+
+## Hardware setup
+
+Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver.
+
+## Software setup
+
+Its actually easier to use than the standard SPI sensor class, because it is less generic:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/as5047/MagneticSensorAS5047.h"
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorAS5047 sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+Set some options:
+
+```c++
+MagneticSensorAS5047 sensor1(SENSOR1_CS, true, mySPISettings);
+```
+
+Use another SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(SPI2);
+}
+```
+
+Here's how you can use it:
+
+```c++
+    // update the sensor (only needed if using the sensor without a motor)
+    sensor1.update();
+
+    // get the angle, in radians, including full rotations
+    float a1 = sensor1.getAngle();
+
+    // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work
+    float v1 = sensor1.getVelocity();
+
+    // get the angle, in radians, no full rotations
+    float a2 = sensor1.getCurrentAngle();
+
+    // get the raw 14 bit value
+    uint16_t raw = sensor1.readRawAngle();
+
+    // read the CORDIC magnitude value, a measure of the magnet field strength
+    float m1 = sensor1.readMagnitude();
+
+    // check for errors
+    if (sensor1.isErrorFlag()) {
+        AS5047Error error = sensor1.clearErrorFlag();
+        if (error.parityError) { // also error.framingError, error.commandInvalid
+            // etc...            
+        }
+    }
+
+    // get diagnostics
+    AS5047Diagnostics diagnostics = sensor1.readDiagnostics();
+```
+