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