Add MT6816 magnetic sensor SPI driver

README.md

@@ -28,6 +28,7 @@ What's here? See the sections below. Each driver or function should come with it
  - [STM32 Hardware Encoder](src/encoders/stm32hwencoder/) - Hardware timer based encoder driver for ABI type quadrature encoders.
  - [SC60228 SPI driver](src/encoders/sc60228/) - SPI driver for SemiMent SC60288 magnetic encoder IC.
  - [MA330 SPI driver](src/encoders/ma330/) - SPI driver for the MPS MagAlpha MA330 absolute position magnetic rotary encoder IC.
+ - [MT6816 SPI driver](src/encoders/mt6816/) - SPI driver for the MagnTek MT6816 absolute position magnetic rotary encoder IC.
 
 ### Communications
 

examples/encoders/mt6816/mt6816_spi/mt6816_spi.ino

@@ -0,0 +1,106 @@
+/**
+ * Comprehensive BLDC motor control example using magnetic sensor MT6816
+ *
+ * Using serial terminal user can send motor commands and configure the motor and FOC in real-time:
+ * - configure PID controller constants
+ * - change motion control loops
+ * - monitor motor variabels
+ * - set target values
+ * - check all the configuration values
+ *
+ * See more info in docs.simplefoc.com/commander_interface
+ */
+#include <SimpleFOC.h>
+#include <SimpleFOCDrivers.h>
+#include <encoders/mt6816/MagneticSensorMT6816.h>
+
+// magnetic sensor instance - MT6816 SPI mode
+MagneticSensorMT6816 sensor = MagneticSensorMT6816(5);
+
+
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(7);
+BLDCDriver3PWM driver = BLDCDriver3PWM(32, 25, 26, 33);
+
+// Inline Current Sense instance
+InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 35, 34);
+
+// commander interface
+Commander command = Commander(Serial);
+void onMotor(char* cmd){ command.motor(&motor, cmd); }
+
+void setup() {
+
+  // initialise magnetic sensor hardware
+  sensor.init();
+  // link the motor to the sensor
+  motor.linkSensor(&sensor);
+
+  // driver config
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  driver.init();
+  // link driver
+  motor.linkDriver(&driver);
+
+  // choose FOC modulation
+  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
+
+  // set control loop type to be used
+  motor.controller = MotionControlType::torque;
+
+  // contoller configuration based on the control type
+  motor.PID_velocity.P = 0.2f;
+  motor.PID_velocity.I = 20;
+  motor.PID_velocity.D = 0;
+  // default voltage_power_supply
+  motor.voltage_limit = 12;
+
+  // velocity low pass filtering time constant
+  motor.LPF_velocity.Tf = 0.01f;
+
+  // angle loop velocity limit
+  motor.velocity_limit = 50;
+
+  // use monitoring with serial for motor init
+  // monitoring port
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  current_sense.linkDriver(&driver);
+  current_sense.init();
+  current_sense.gain_b *= -1;
+  current_sense.skip_align = true;
+  motor.linkCurrentSense(&current_sense);
+
+  // initialise motor
+  motor.init();
+  // align encoder and start FOC
+  motor.initFOC();
+
+  // set the inital target value
+  motor.target = 2;
+
+  // define the motor id
+  command.add('A', onMotor, "motor");
+
+  // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
+  Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V."));
+
+  _delay(1000);
+}
+
+
+void loop() {
+  // iterative setting FOC phase voltage
+  motor.loopFOC();
+
+  // iterative function setting the outter loop target
+  // velocity, position or voltage
+  // if tatget not set in parameter uses motor.target variable
+  motor.move();
+
+  // user communication
+  command.run();
+}

src/encoders/mt6816/MT6816.cpp

@@ -0,0 +1,56 @@
+
+#include "MT6816.h"
+
+MT6816::MT6816(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+};
+
+MT6816::~MT6816() {
+};
+
+void MT6816::init(SPIClass* _spi) {
+    spi = _spi;
+    if (nCS >= 0) {
+        pinMode(nCS, OUTPUT);
+        digitalWrite(nCS, HIGH);
+        spi->begin();
+    }
+};
+
+uint16_t MT6816::readRawAngle()  {
+    uint16_t angle_data = 0;
+    angle_data = spi_transfer16(MT6816_READ_REG_03) << 8;
+    angle_data |= spi_transfer16(MT6816_READ_REG_04);
+
+    if ((angle_data & MT6816_NO_MAGNET_WARNING_BIT) == MT6816_NO_MAGNET_WARNING_BIT) {
+        this->no_magnetic_reading = true;
+    } else {
+        this->no_magnetic_reading = false;
+    }
+
+    if (!this->parityCheck(angle_data)) {
+        return 0;
+    }
+
+    return (angle_data >> 2);
+}
+
+bool MT6816::parityCheck(uint16_t data) {
+    data ^= data >> 8;
+    data ^= data >> 4;
+    data ^= data >> 2;
+    data ^= data >> 1;
+
+    return (~data) & 1;
+}
+
+uint16_t MT6816::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);
+
+    return result;
+}

src/encoders/mt6816/MT6816.h

@@ -0,0 +1,40 @@
+
+#ifndef MT6816_H
+#define MT6816_H
+
+#include "Arduino.h"
+#include "SPI.h"
+
+#define _2PI                                6.28318530718f
+#define MT6816_CPR                          16384.0f
+
+#define MT6816_READ_REG_03                  0x8300
+#define MT6816_READ_REG_04                  0x8400
+
+#define MT6816_NO_MAGNET_WARNING_BIT        0x0002
+#define MT6816_BITORDER                     MSBFIRST
+
+
+static SPISettings MT6816SPISettings(1000000, MT6816_BITORDER, SPI_MODE3);
+
+class MT6816 {
+public:
+    MT6816(SPISettings settings = MT6816SPISettings, int nCS = -1);
+    virtual ~MT6816();
+
+    virtual void init(SPIClass* _spi = &SPI);
+    uint16_t readRawAngle();
+    bool isNoMagneticReading() {
+        return no_magnetic_reading;
+    }
+
+private:
+    bool parityCheck(uint16_t data);
+    uint16_t spi_transfer16(uint16_t outdata);
+    SPIClass* spi;
+    SPISettings settings;
+    bool no_magnetic_reading = false;
+    int nCS = -1;
+};
+
+#endif /* MT6816_H */

src/encoders/mt6816/MagneticSensorMT6816.cpp

@@ -0,0 +1,26 @@
+
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "MagneticSensorMT6816.h"
+
+
+MagneticSensorMT6816::MagneticSensorMT6816(int nCS, SPISettings settings) : MT6816(settings, nCS) {
+}
+
+MagneticSensorMT6816::~MagneticSensorMT6816(){
+}
+
+void MagneticSensorMT6816::init(SPIClass* _spi) {
+    this->MT6816::init(_spi);
+    this->Sensor::init();
+}
+
+float MagneticSensorMT6816::getSensorAngle() {
+    uint16_t raw_angle_data = readRawAngle();
+
+    if (this->MT6816::isNoMagneticReading()) {
+        return 0;
+    }
+
+    return static_cast<float>(raw_angle_data) / MT6816_CPR * _2PI;
+}

src/encoders/mt6816/MagneticSensorMT6816.h

@@ -0,0 +1,17 @@
+
+#ifndef MAGNETICSENSOR_MT6816_H
+#define MAGNETICSENSOR_MT6816_H
+
+#include "common/base_classes/Sensor.h"
+#include "MT6816.h"
+
+class MagneticSensorMT6816 : public Sensor, public MT6816 {
+public:
+    MagneticSensorMT6816(int nCS = -1, SPISettings settings = MT6816SPISettings);
+    virtual ~MagneticSensorMT6816();
+
+    virtual float getSensorAngle() override;
+    virtual void init(SPIClass* _spi = &SPI);
+};
+
+#endif /* MAGNETICSENSOR_MT6816_H */