Skip to content

Commit 613fb7f

Browse files
author
Richard Unger
committed
MagnTek MT6835 angle sensor SPI driver
1 parent 61e0232 commit 613fb7f

File tree

5 files changed

+522
-0
lines changed

5 files changed

+522
-0
lines changed

‎src/encoders/mt6835/MT6835.cpp‎

Lines changed: 241 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,241 @@
1+
2+
#include "./MT6835.h"
3+
#include "common/foc_utils.h"
4+
5+
6+
7+
float MT6835::getCurrentAngle(){
8+
return readRawAngle21() / MT6835_CPR * _2PI;
9+
};
10+
11+
12+
13+
uint32_t MT6835::readRawAngle21(){
14+
uint8_t data[6]; // transact 48 bits
15+
data[0] = (MT6835_OP_ANGLE<<4) | (MT6835_REG_ANGLE1 >> 8);
16+
data[1] = MT6835_REG_ANGLE1 & 0x00FF;
17+
if (nCS >= 0)
18+
digitalWrite(nCS, LOW);
19+
spi->beginTransaction(settings);
20+
spi->transfer(data, 6);
21+
spi->endTransaction();
22+
if (nCS >= 0)
23+
digitalWrite(nCS, HIGH);
24+
return (data[2] << 12) | (data[3] << 4) | (data[4] >> 3);
25+
};
26+
27+
28+
29+
30+
bool MT6835::setZeroFromCurrentPosition(){
31+
MT6835Command cmd;
32+
cmd.cmd = MT6835_OP_ZERO;
33+
cmd.addr = 0x000;
34+
transfer24(&cmd);
35+
return cmd.data == MT6835_WRITE_ACK;
36+
};
37+
38+
39+
/**
40+
* Wait 6s after calling this method
41+
*/
42+
bool MT6835::writeEEPROM(){
43+
delay(1); // wait at least 1ms
44+
MT6835Command cmd;
45+
cmd.cmd = MT6835_OP_PROG;
46+
cmd.addr = 0x000;
47+
transfer24(&cmd);
48+
return cmd.data == MT6835_WRITE_ACK;
49+
};
50+
51+
52+
53+
54+
55+
uint8_t MT6835::getBandwidth(){
56+
MT6835Options5 opts = { .reg = readRegister(MT6835_REG_OPTS5) };
57+
return opts.bw;
58+
};
59+
void MT6835::setBandwidth(uint8_t bw){
60+
MT6835Options5 opts = { .reg = readRegister(MT6835_REG_OPTS5) };
61+
opts.bw = bw;
62+
writeRegister(MT6835_REG_OPTS5, opts.reg);
63+
};
64+
65+
uint8_t MT6835::getHysteresis(){
66+
MT6835Options3 opts = { .reg = getOptions3() };
67+
return opts.hyst;
68+
};
69+
void MT6835::setHysteresis(uint8_t hyst){
70+
MT6835Options3 opts = { .reg = getOptions3() };
71+
opts.hyst = hyst;
72+
setOptions3(opts);
73+
};
74+
75+
uint8_t MT6835::getRotationDirection(){
76+
MT6835Options3 opts = { .reg = getOptions3() };
77+
return opts.rot_dir;
78+
};
79+
void MT6835::setRotationDirection(uint8_t dir){
80+
MT6835Options3 opts = { .reg = getOptions3() };
81+
opts.rot_dir = dir;
82+
setOptions3(opts);
83+
};
84+
85+
86+
uint16_t MT6835::getABZResolution(){
87+
uint8_t hi = readRegister(MT6835_REG_ABZ_RES1);
88+
MT6835ABZRes lo = {
89+
.reg = readRegister(MT6835_REG_ABZ_RES2)
90+
};
91+
return (hi << 6) | lo.abz_res_low;
92+
};
93+
void MT6835::setABZResolution(uint16_t res){
94+
uint8_t hi = (res >> 2);
95+
MT6835ABZRes lo = {
96+
.reg = readRegister(MT6835_REG_ABZ_RES2)
97+
};
98+
lo.abz_res_low = res & 0x3F;
99+
writeRegister(MT6835_REG_ABZ_RES1, hi);
100+
writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
101+
};
102+
103+
104+
105+
bool MT6835::isABZEnabled(){
106+
MT6835ABZRes lo = {
107+
.reg = readRegister(MT6835_REG_ABZ_RES2)
108+
};
109+
return lo.abz_off==0;
110+
};
111+
void MT6835::setABZEnabled(bool enabled){
112+
MT6835ABZRes lo = {
113+
.reg = readRegister(MT6835_REG_ABZ_RES2)
114+
};
115+
lo.abz_off = enabled?0:1;
116+
writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
117+
};
118+
119+
120+
121+
bool MT6835::isABSwapped(){
122+
MT6835ABZRes lo = {
123+
.reg = readRegister(MT6835_REG_ABZ_RES2)
124+
};
125+
return lo.ab_swap==1;
126+
};
127+
void MT6835::setABSwapped(bool swapped){
128+
MT6835ABZRes lo = {
129+
.reg = readRegister(MT6835_REG_ABZ_RES2)
130+
};
131+
lo.ab_swap = swapped?1:0;
132+
writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
133+
};
134+
135+
136+
137+
uint16_t MT6835::getZeroPosition(){
138+
uint8_t hi = readRegister(MT6835_REG_ZERO1);
139+
MT6835Options0 lo = {
140+
.reg = readRegister(MT6835_REG_ZERO2)
141+
};
142+
return (hi << 4) | lo.zero_pos_low;
143+
};
144+
void MT6835::setZeroPosition(uint16_t pos){
145+
uint8_t hi = (pos >> 4);
146+
MT6835Options0 lo = {
147+
.reg = readRegister(MT6835_REG_ZERO2)
148+
};
149+
lo.zero_pos_low = pos & 0x0F;
150+
writeRegister(MT6835_REG_ZERO1, hi);
151+
writeRegister(MT6835_REG_ZERO2, lo.reg);
152+
};
153+
154+
155+
156+
MT6835Options1 MT6835::getOptions1(){
157+
MT6835Options1 result = {
158+
.reg = readRegister(MT6835_REG_OPTS1)
159+
};
160+
return result;
161+
};
162+
void MT6835::setOptions1(MT6835Options1 opts){
163+
writeRegister(MT6835_REG_OPTS1, opts.reg);
164+
};
165+
166+
167+
168+
MT6835Options2 MT6835::getOptions2(){
169+
MT6835Options2 result = {
170+
.reg = readRegister(MT6835_REG_OPTS2)
171+
};
172+
return result;
173+
};
174+
void MT6835::setOptions2(MT6835Options2 opts){
175+
MT6835Options2 val = getOptions2();
176+
val.nlc_en = opts.nlc_en;
177+
val.pwm_fq = opts.pwm_fq;
178+
val.pwm_pol = opts.pwm_pol;
179+
val.pwm_sel = opts.pwm_sel;
180+
writeRegister(MT6835_REG_OPTS2, val.reg);
181+
};
182+
183+
184+
185+
MT6835Options3 MT6835::getOptions3(){
186+
MT6835Options3 result = {
187+
.reg = readRegister(MT6835_REG_OPTS3)
188+
};
189+
return result;
190+
};
191+
void MT6835::setOptions3(MT6835Options3 opts){
192+
MT6835Options3 val = getOptions3();
193+
val.rot_dir = opts.rot_dir;
194+
val.hyst = opts.hyst;
195+
writeRegister(MT6835_REG_OPTS3, val.reg);
196+
};
197+
198+
199+
200+
MT6835Options4 MT6835::getOptions4(){
201+
MT6835Options4 result = {
202+
.reg = readRegister(MT6835_REG_OPTS4)
203+
};
204+
return result;
205+
};
206+
void MT6835::setOptions4(MT6835Options4 opts){
207+
MT6835Options4 val = getOptions4();
208+
val.gpio_ds = opts.gpio_ds;
209+
val.autocal_freq = opts.autocal_freq;
210+
writeRegister(MT6835_REG_OPTS4, val.reg);
211+
};
212+
213+
214+
215+
216+
217+
218+
void MT6835::transfer24(MT6835Command* outValue) {
219+
if (nCS >= 0)
220+
digitalWrite(nCS, LOW);
221+
spi->beginTransaction(settings);
222+
spi->transfer(outValue, 3);
223+
spi->endTransaction();
224+
if (nCS >= 0)
225+
digitalWrite(nCS, HIGH);
226+
};
227+
uint8_t MT6835::readRegister(uint16_t reg) {
228+
MT6835Command cmd;
229+
cmd.cmd = MT6835_OP_READ;
230+
cmd.addr = reg;
231+
transfer24(&cmd);
232+
return cmd.data;
233+
};
234+
bool MT6835::writeRegister(uint16_t reg, uint8_t value) {
235+
MT6835Command cmd;
236+
cmd.cmd = MT6835_OP_READ;
237+
cmd.addr = reg;
238+
cmd.data = value;
239+
transfer24(&cmd);
240+
return cmd.data == MT6835_WRITE_ACK;
241+
};

0 commit comments

Comments
 (0)