try to fix submodule
This commit is contained in:
291
firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/MT6835.cpp
Normal file
291
firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/MT6835.cpp
Normal file
@@ -0,0 +1,291 @@
|
||||
|
||||
#include "./MT6835.h"
|
||||
#include "common/foc_utils.h"
|
||||
|
||||
|
||||
MT6835::MT6835(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
|
||||
// nix
|
||||
};
|
||||
|
||||
MT6835::~MT6835() {
|
||||
// nix
|
||||
};
|
||||
|
||||
|
||||
|
||||
void MT6835::init(SPIClass* _spi) {
|
||||
spi = _spi;
|
||||
if (nCS >= 0) {
|
||||
pinMode(nCS, OUTPUT);
|
||||
digitalWrite(nCS, HIGH);
|
||||
}
|
||||
spi->begin();
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
float MT6835::getCurrentAngle(){
|
||||
return readRawAngle21() / (float)MT6835_CPR * _2PI;
|
||||
};
|
||||
|
||||
|
||||
|
||||
uint32_t MT6835::readRawAngle21(){
|
||||
uint8_t data[6]; // transact 48 bits
|
||||
data[0] = (MT6835_OP_ANGLE<<4);
|
||||
data[1] = MT6835_REG_ANGLE1;
|
||||
data[2] = 0;
|
||||
data[3] = 0;
|
||||
data[4] = 0;
|
||||
data[5] = 0;
|
||||
spi->beginTransaction(settings);
|
||||
if (nCS >= 0)
|
||||
digitalWrite(nCS, LOW);
|
||||
spi->transfer(data, 6);
|
||||
if (nCS >= 0)
|
||||
digitalWrite(nCS, HIGH);
|
||||
spi->endTransaction();
|
||||
laststatus = data[4]&0x07;
|
||||
return (data[2] << 13) | (data[3] << 5) | (data[4] >> 3);
|
||||
};
|
||||
|
||||
|
||||
uint8_t MT6835::getStatus(){
|
||||
return laststatus;
|
||||
};
|
||||
|
||||
uint8_t MT6835::getCalibrationStatus(){
|
||||
uint8_t data[3] = {0};
|
||||
data[0] = 0x31; // read register 0b0011, calibration register 0x113
|
||||
data[1] = 0x13;
|
||||
spi->beginTransaction(settings);
|
||||
if(nCS >= 0)
|
||||
digitalWrite(nCS, LOW);
|
||||
spi->transfer(data, 3);
|
||||
if(nCS >= 0)
|
||||
digitalWrite(nCS, HIGH);
|
||||
spi->endTransaction();
|
||||
return data[2] >> 6; // Calibration state held in bits 6,7
|
||||
}
|
||||
|
||||
bool MT6835::setZeroFromCurrentPosition(){
|
||||
MT6835Command cmd;
|
||||
cmd.cmd = MT6835_OP_ZERO;
|
||||
cmd.addr = 0x000;
|
||||
transfer24(&cmd);
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Wait 6s after calling this method
|
||||
*/
|
||||
bool MT6835::writeEEPROM(){
|
||||
delay(1); // wait at least 1ms
|
||||
MT6835Command cmd;
|
||||
cmd.cmd = MT6835_OP_PROG;
|
||||
cmd.addr = 0x000;
|
||||
transfer24(&cmd);
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
uint8_t MT6835::getBandwidth(){
|
||||
MT6835Options5 opts = { .reg = readRegister(MT6835_REG_OPTS5) };
|
||||
return opts.bw;
|
||||
};
|
||||
void MT6835::setBandwidth(uint8_t bw){
|
||||
MT6835Options5 opts = { .reg = readRegister(MT6835_REG_OPTS5) };
|
||||
opts.bw = bw;
|
||||
writeRegister(MT6835_REG_OPTS5, opts.reg);
|
||||
};
|
||||
|
||||
uint8_t MT6835::getHysteresis(){
|
||||
MT6835Options3 opts = { .reg = getOptions3().reg };
|
||||
return opts.hyst;
|
||||
};
|
||||
void MT6835::setHysteresis(uint8_t hyst){
|
||||
MT6835Options3 opts = { .reg = getOptions3().reg };
|
||||
opts.hyst = hyst;
|
||||
setOptions3(opts);
|
||||
};
|
||||
|
||||
uint8_t MT6835::getRotationDirection(){
|
||||
MT6835Options3 opts = { .reg = getOptions3().reg };
|
||||
return opts.rot_dir;
|
||||
};
|
||||
void MT6835::setRotationDirection(uint8_t dir){
|
||||
MT6835Options3 opts = { .reg = getOptions3().reg };
|
||||
opts.rot_dir = dir;
|
||||
setOptions3(opts);
|
||||
};
|
||||
|
||||
|
||||
uint16_t MT6835::getABZResolution(){
|
||||
uint8_t hi = readRegister(MT6835_REG_ABZ_RES1);
|
||||
MT6835ABZRes lo = {
|
||||
.reg = readRegister(MT6835_REG_ABZ_RES2)
|
||||
};
|
||||
return (hi << 6) | lo.abz_res_low;
|
||||
};
|
||||
void MT6835::setABZResolution(uint16_t res){
|
||||
uint8_t hi = (res >> 6);
|
||||
MT6835ABZRes lo = {
|
||||
.reg = readRegister(MT6835_REG_ABZ_RES2)
|
||||
};
|
||||
lo.abz_res_low = (res & 0x3F);
|
||||
writeRegister(MT6835_REG_ABZ_RES1, hi);
|
||||
writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
};
|
||||
|
||||
|
||||
|
||||
bool MT6835::isABZEnabled(){
|
||||
MT6835ABZRes lo = {
|
||||
.reg = readRegister(MT6835_REG_ABZ_RES2)
|
||||
};
|
||||
return lo.abz_off==0;
|
||||
};
|
||||
void MT6835::setABZEnabled(bool enabled){
|
||||
MT6835ABZRes lo = {
|
||||
.reg = readRegister(MT6835_REG_ABZ_RES2)
|
||||
};
|
||||
lo.abz_off = enabled?0:1;
|
||||
writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
};
|
||||
|
||||
|
||||
|
||||
bool MT6835::isABSwapped(){
|
||||
MT6835ABZRes lo = {
|
||||
.reg = readRegister(MT6835_REG_ABZ_RES2)
|
||||
};
|
||||
return lo.ab_swap==1;
|
||||
};
|
||||
void MT6835::setABSwapped(bool swapped){
|
||||
MT6835ABZRes lo = {
|
||||
.reg = readRegister(MT6835_REG_ABZ_RES2)
|
||||
};
|
||||
lo.ab_swap = swapped?1:0;
|
||||
writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
};
|
||||
|
||||
|
||||
|
||||
uint16_t MT6835::getZeroPosition(){
|
||||
uint8_t hi = readRegister(MT6835_REG_ZERO1);
|
||||
MT6835Options0 lo = {
|
||||
.reg = readRegister(MT6835_REG_ZERO2)
|
||||
};
|
||||
return (hi << 4) | lo.zero_pos_low;
|
||||
};
|
||||
void MT6835::setZeroPosition(uint16_t pos){
|
||||
uint8_t hi = (pos >> 4);
|
||||
MT6835Options0 lo = {
|
||||
.reg = readRegister(MT6835_REG_ZERO2)
|
||||
};
|
||||
lo.zero_pos_low = pos & 0x0F;
|
||||
writeRegister(MT6835_REG_ZERO1, hi);
|
||||
writeRegister(MT6835_REG_ZERO2, lo.reg);
|
||||
};
|
||||
|
||||
|
||||
|
||||
MT6835Options1 MT6835::getOptions1(){
|
||||
MT6835Options1 result = {
|
||||
.reg = readRegister(MT6835_REG_OPTS1)
|
||||
};
|
||||
return result;
|
||||
};
|
||||
void MT6835::setOptions1(MT6835Options1 opts){
|
||||
writeRegister(MT6835_REG_OPTS1, opts.reg);
|
||||
};
|
||||
|
||||
|
||||
|
||||
MT6835Options2 MT6835::getOptions2(){
|
||||
MT6835Options2 result = {
|
||||
.reg = readRegister(MT6835_REG_OPTS2)
|
||||
};
|
||||
return result;
|
||||
};
|
||||
void MT6835::setOptions2(MT6835Options2 opts){
|
||||
MT6835Options2 val = getOptions2();
|
||||
val.nlc_en = opts.nlc_en;
|
||||
val.pwm_fq = opts.pwm_fq;
|
||||
val.pwm_pol = opts.pwm_pol;
|
||||
val.pwm_sel = opts.pwm_sel;
|
||||
writeRegister(MT6835_REG_OPTS2, val.reg);
|
||||
};
|
||||
|
||||
|
||||
|
||||
MT6835Options3 MT6835::getOptions3(){
|
||||
MT6835Options3 result = {
|
||||
.reg = readRegister(MT6835_REG_OPTS3)
|
||||
};
|
||||
return result;
|
||||
};
|
||||
void MT6835::setOptions3(MT6835Options3 opts){
|
||||
MT6835Options3 val = getOptions3();
|
||||
val.rot_dir = opts.rot_dir;
|
||||
val.hyst = opts.hyst;
|
||||
writeRegister(MT6835_REG_OPTS3, val.reg);
|
||||
};
|
||||
|
||||
|
||||
|
||||
MT6835Options4 MT6835::getOptions4(){
|
||||
MT6835Options4 result = {
|
||||
.reg = readRegister(MT6835_REG_OPTS4)
|
||||
};
|
||||
return result;
|
||||
};
|
||||
void MT6835::setOptions4(MT6835Options4 opts){
|
||||
MT6835Options4 val = getOptions4();
|
||||
val.gpio_ds = opts.gpio_ds;
|
||||
val.autocal_freq = opts.autocal_freq;
|
||||
writeRegister(MT6835_REG_OPTS4, val.reg);
|
||||
};
|
||||
|
||||
|
||||
|
||||
uint32_t swap_bytes(uint32_t net)
|
||||
{
|
||||
return __builtin_bswap32(net);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void MT6835::transfer24(MT6835Command* outValue) {
|
||||
uint32_t buff = swap_bytes(outValue->val);
|
||||
spi->beginTransaction(settings);
|
||||
if (nCS >= 0)
|
||||
digitalWrite(nCS, LOW);
|
||||
spi->transfer(&buff, 3);
|
||||
if (nCS >= 0)
|
||||
digitalWrite(nCS, HIGH);
|
||||
spi->endTransaction();
|
||||
outValue->val = swap_bytes(buff);
|
||||
};
|
||||
uint8_t MT6835::readRegister(uint16_t reg) {
|
||||
MT6835Command cmd;
|
||||
cmd.cmd = MT6835_OP_READ;
|
||||
cmd.addr = reg;
|
||||
transfer24(&cmd);
|
||||
return cmd.data;
|
||||
};
|
||||
bool MT6835::writeRegister(uint16_t reg, uint8_t value) {
|
||||
MT6835Command cmd;
|
||||
cmd.cmd = MT6835_OP_WRITE;
|
||||
cmd.addr = reg;
|
||||
cmd.data = value;
|
||||
transfer24(&cmd);
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
};
|
||||
217
firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/MT6835.h
Normal file
217
firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/MT6835.h
Normal file
@@ -0,0 +1,217 @@
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "SPI.h"
|
||||
|
||||
#define MT6835_OP_READ 0b0011
|
||||
#define MT6835_OP_WRITE 0b0110
|
||||
#define MT6835_OP_PROG 0b1100
|
||||
#define MT6835_OP_ZERO 0b0101
|
||||
#define MT6835_OP_ANGLE 0b1010
|
||||
|
||||
#define MT6835_CMD_MASK 0b111100000000000000000000
|
||||
#define MT6835_ADDR_MASK 0b000011111111111100000000
|
||||
#define MT6835_DATA_MASK 0b000000000000000011111111
|
||||
|
||||
#define MT6835_CPR 2097152
|
||||
|
||||
#define MT6835_STATUS_OVERSPEED 0x01
|
||||
#define MT6835_STATUS_WEAKFIELD 0x02
|
||||
#define MT6835_STATUS_UNDERVOLT 0x04
|
||||
|
||||
#define MT6835_WRITE_ACK 0x55
|
||||
|
||||
#define MT6835_REG_USERID 0x001
|
||||
|
||||
#define MT6835_REG_ANGLE1 0x003
|
||||
#define MT6835_REG_ANGLE2 0x004
|
||||
#define MT6835_REG_ANGLE3 0x005
|
||||
#define MT6835_REG_ANGLE4 0x006
|
||||
|
||||
#define MT6835_REG_ABZ_RES1 0x007
|
||||
#define MT6835_REG_ABZ_RES2 0x008
|
||||
|
||||
#define MT6835_REG_ZERO1 0x009
|
||||
#define MT6835_REG_ZERO2 0x00A
|
||||
|
||||
#define MT6835_REG_OPTS0 0x00A
|
||||
#define MT6835_REG_OPTS1 0x00B
|
||||
#define MT6835_REG_OPTS2 0x00C
|
||||
#define MT6835_REG_OPTS3 0x00D
|
||||
#define MT6835_REG_OPTS4 0x00E
|
||||
#define MT6835_REG_OPTS5 0x011
|
||||
|
||||
// NLC table, 192 bytes
|
||||
#define MT6835_REG_NLC_BASE 0x013
|
||||
|
||||
#define MT6835_REG_CAL_STATUS 0x113
|
||||
|
||||
|
||||
union MT6835ABZRes {
|
||||
struct {
|
||||
uint8_t ab_swap:1;
|
||||
uint8_t abz_off:1;
|
||||
uint8_t abz_res_low:6;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
|
||||
|
||||
union MT6835Options0 {
|
||||
struct {
|
||||
uint8_t z_pul_wid:3;
|
||||
uint8_t z_edge:1;
|
||||
uint8_t zero_pos_low:4;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
|
||||
|
||||
union MT6835Options1 {
|
||||
struct {
|
||||
uint8_t uvw_res:4;
|
||||
uint8_t uvw_off:1;
|
||||
uint8_t uvw_mux:1;
|
||||
uint8_t z_phase:2;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
|
||||
|
||||
union MT6835Options2 {
|
||||
struct {
|
||||
uint8_t pwm_sel:3;
|
||||
uint8_t pwm_pol:1;
|
||||
uint8_t pwm_fq:1;
|
||||
uint8_t nlc_en:1;
|
||||
uint8_t reserved:2;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
|
||||
|
||||
union MT6835Options3 {
|
||||
struct {
|
||||
uint8_t hyst:3;
|
||||
uint8_t rot_dir:1;
|
||||
uint8_t reserved:4;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
|
||||
|
||||
union MT6835Options4 {
|
||||
struct {
|
||||
uint8_t reserved:4;
|
||||
uint8_t autocal_freq:3;
|
||||
uint8_t gpio_ds:1;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
|
||||
|
||||
union MT6835Options5 {
|
||||
struct {
|
||||
uint8_t bw:3;
|
||||
uint8_t reserved:5;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
union MT6835Command {
|
||||
struct {
|
||||
uint32_t unused:8;
|
||||
uint32_t data:8;
|
||||
uint32_t addr:12;
|
||||
uint32_t cmd:4;
|
||||
};
|
||||
uint32_t val;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#define MT6835_BITORDER MSBFIRST
|
||||
|
||||
static SPISettings MT6835SPISettings(1000000, MT6835_BITORDER, SPI_MODE3); // @suppress("Invalid arguments")
|
||||
|
||||
|
||||
|
||||
|
||||
class MT6835 {
|
||||
public:
|
||||
MT6835(SPISettings settings = MT6835SPISettings, int nCS = -1);
|
||||
virtual ~MT6835();
|
||||
|
||||
virtual void init(SPIClass* _spi = &SPI);
|
||||
|
||||
|
||||
float getCurrentAngle(); // angle in radians, return current value
|
||||
|
||||
uint32_t readRawAngle21(); // up to 21bit precision angle value
|
||||
|
||||
|
||||
uint8_t getBandwidth();
|
||||
void setBandwidth(uint8_t bw);
|
||||
|
||||
uint8_t getHysteresis();
|
||||
void setHysteresis(uint8_t hyst);
|
||||
|
||||
uint8_t getRotationDirection();
|
||||
void setRotationDirection(uint8_t dir);
|
||||
|
||||
uint16_t getABZResolution();
|
||||
void setABZResolution(uint16_t res);
|
||||
|
||||
bool isABZEnabled();
|
||||
void setABZEnabled(bool enabled);
|
||||
|
||||
bool isABSwapped();
|
||||
void setABSwapped(bool swapped);
|
||||
|
||||
uint16_t getZeroPosition();
|
||||
void setZeroPosition(uint16_t pos);
|
||||
|
||||
MT6835Options1 getOptions1();
|
||||
void setOptions1(MT6835Options1 opts);
|
||||
|
||||
MT6835Options2 getOptions2();
|
||||
void setOptions2(MT6835Options2 opts);
|
||||
|
||||
MT6835Options3 getOptions3();
|
||||
void setOptions3(MT6835Options3 opts);
|
||||
|
||||
MT6835Options4 getOptions4();
|
||||
void setOptions4(MT6835Options4 opts);
|
||||
|
||||
uint8_t getStatus();
|
||||
|
||||
uint8_t getCalibrationStatus();
|
||||
|
||||
bool setZeroFromCurrentPosition();
|
||||
bool writeEEPROM(); // wait 6s after calling this method
|
||||
|
||||
private:
|
||||
SPIClass* spi;
|
||||
SPISettings settings;
|
||||
int nCS = -1;
|
||||
uint8_t laststatus = 0;
|
||||
|
||||
void transfer24(MT6835Command* outValue);
|
||||
uint8_t readRegister(uint16_t reg);
|
||||
bool writeRegister(uint16_t reg, uint8_t value);
|
||||
|
||||
};
|
||||
@@ -0,0 +1,24 @@
|
||||
|
||||
#include "MagneticSensorMT6835.h"
|
||||
|
||||
|
||||
MagneticSensorMT6835::MagneticSensorMT6835(int nCS, SPISettings settings) : Sensor(), MT6835(settings, nCS) {
|
||||
// nix
|
||||
};
|
||||
|
||||
|
||||
MagneticSensorMT6835::~MagneticSensorMT6835() {
|
||||
// nix
|
||||
};
|
||||
|
||||
|
||||
float MagneticSensorMT6835::getSensorAngle() {
|
||||
return getCurrentAngle();
|
||||
};
|
||||
|
||||
|
||||
void MagneticSensorMT6835::init(SPIClass* _spi) {
|
||||
this->MT6835::init(_spi);
|
||||
this->Sensor::init();
|
||||
};
|
||||
|
||||
@@ -0,0 +1,17 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "common/base_classes/Sensor.h"
|
||||
#include "./MT6835.h"
|
||||
|
||||
class MagneticSensorMT6835 : public Sensor, public MT6835 {
|
||||
public:
|
||||
MagneticSensorMT6835(int nCS = -1, SPISettings settings = MT6835SPISettings);
|
||||
virtual ~MagneticSensorMT6835();
|
||||
|
||||
virtual float getSensorAngle() override;
|
||||
|
||||
virtual void init(SPIClass* _spi = &SPI);
|
||||
};
|
||||
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
# MT6835 SimpleFOC driver
|
||||
|
||||
Driver for the MagnTek MT6835 precision magnetic rotary encoder.
|
||||
|
||||
This sensor features support for up to 21 bit resolution (!) and speeds up to 120,000RPM. While its full precision requires calibration using an external calibration system, it is impressively precise even in uncalibrated state, and it offers an internal self-calibration mode whose precision is somewhere between the other two.
|
||||
|
||||
It has ABZ, UVW and SPI interfaces, and this driver is for its SPI inteface. You can use its ABZ interface with our Encoder classes, but due to the high resolution the interrupt-based Encoder might cause high MCU load. You can use its UVW interface with our Hall Sensor classes.
|
||||
|
||||
# Hardware setup
|
||||
|
||||
Connect the sensor to an SPI bus on your MCU. Pay attention to the voltage levels needed by your PCBs. The nCS line should have a pull-up resistor.
|
||||
|
||||
# Software setup
|
||||
|
||||
Usage example:
|
||||
|
||||
```c++
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "SimpleFOC.h"
|
||||
#include "SimpleFOCDrivers.h"
|
||||
|
||||
#include "encoders/mt6835/MagneticSensorMT6835.h"
|
||||
|
||||
#define SENSOR_nCS PB6
|
||||
|
||||
SPISettings myMT6835SPISettings(1000000, MT6835_BITORDER, SPI_MODE3);
|
||||
MagneticSensorMT6835 sensor = MagneticSensorMT6835(SENSOR_nCS, myMT6835SPISettings);
|
||||
|
||||
long ts;
|
||||
|
||||
void setup() {
|
||||
sensor.init();
|
||||
ts = millis();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
sensor.update();
|
||||
long now = millis();
|
||||
if (now - ts > 1000) {
|
||||
ts = now;
|
||||
SimpleFOCDebug::print("A: ");
|
||||
SimpleFOCDebug::print(sensor.getAngle());
|
||||
SimpleFOCDebug::print(" V: ");
|
||||
SimpleFOCDebug::println(sensor.getVelocity());
|
||||
}
|
||||
delay(10);
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
Set the zero position:
|
||||
|
||||
```c++
|
||||
uint16_t pos = sensor.getZeroPosition(); // current value
|
||||
sensor.setZeroFromCurrentPosition(); // set zero to current position
|
||||
```
|
||||
|
||||
Set the ABZ resolution (needed if you want to use ABZ as the default is 1):
|
||||
|
||||
```c++
|
||||
sensor.setABZResolution(2048);
|
||||
```
|
||||
|
||||
Reference in New Issue
Block a user