1 /*
2  * Author: Jon Trulson <jtrulson@ics.com>
3  * Copyright (c) 2015 Intel Corporation.
4  *
5  * Permission is hereby granted, free of charge, to any person obtaining
6  * a copy of this software and associated documentation files (the
7  * "Software"), to deal in the Software without restriction, including
8  * without limitation the rights to use, copy, modify, merge, publish,
9  * distribute, sublicense, and/or sell copies of the Software, and to
10  * permit persons to whom the Software is furnished to do so, subject to
11  * the following conditions:
12  *
13  * The above copyright notice and this permission notice shall be
14  * included in all copies or substantial portions of the Software.
15  *
16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
20  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
21  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
22  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
23  */
24 
25 #include <unistd.h>
26 #include <iostream>
27 #include <string>
28 #include <stdexcept>
29 
30 #include "ak8975.h"
31 
32 using namespace upm;
33 using namespace std;
34 
35 
AK8975(int bus,uint8_t address)36 AK8975::AK8975(int bus, uint8_t address):
37   m_i2c(bus)
38 {
39   m_addr = address;
40   m_xCoeff = 0.0;
41   m_yCoeff = 0.0;
42   m_zCoeff = 0.0;
43 
44   mraa::Result rv;
45   if ( (rv = m_i2c.address(m_addr)) != mraa::SUCCESS)
46     {
47       throw std::runtime_error(std::string(__FUNCTION__) +
48                                ": I2c.address() failed");
49       return;
50     }
51 }
52 
~AK8975()53 AK8975::~AK8975()
54 {
55 }
56 
init()57 bool AK8975::init()
58 {
59   // we put the device in 'fuse mode', and then read the compensation
60   // coefficients and store them.
61 
62   // first, set power down mode
63 
64   if (!setMode(CNTL_PWRDWN))
65     {
66       throw std::runtime_error(std::string(__FUNCTION__) +
67                                ": Unable to set PWRDWN mode");
68       return false;
69     }
70 
71   if (!setMode(CNTL_FUSE_ACCESS))
72     {
73       throw std::runtime_error(std::string(__FUNCTION__) +
74                                ": Unable to set FUSE mode");
75       return false;
76     }
77 
78   // Read each byte and store
79   m_xCoeff = (float)m_i2c.readReg(REG_ASAX);
80   m_yCoeff = (float)m_i2c.readReg(REG_ASAY);
81   m_zCoeff = (float)m_i2c.readReg(REG_ASAZ);
82 
83   // now, place back in power down mode
84   if (!setMode(CNTL_PWRDWN))
85     {
86       throw std::runtime_error(std::string(__FUNCTION__) +
87                                ": Unable to set reset PWRDWN mode");
88       return false;
89     }
90 
91   return true;
92 }
93 
setMode(CNTL_MODES_T mode)94 bool AK8975::setMode(CNTL_MODES_T mode)
95 {
96   mraa::Result rv;
97   if ((rv = m_i2c.writeReg(REG_CNTL, mode)) != mraa::SUCCESS)
98     {
99       throw std::runtime_error(std::string(__FUNCTION__) +
100                                ": I2c.writeReg() failed");
101       return false;
102     }
103 
104   // sleep at least 100us for for mode transition to complete
105   usleep(150);
106 
107   return true;
108 }
109 
isReady()110 bool AK8975::isReady()
111 {
112   uint8_t rdy = m_i2c.readReg(REG_ST1);
113 
114   if (rdy & ST1_DRDY)
115     return true;
116 
117   return false;
118 }
119 
waitforDeviceReady()120 bool AK8975::waitforDeviceReady()
121 {
122   const int maxRetries = 20;
123 
124   int retries = 0;
125 
126   while (retries < maxRetries)
127     {
128       if (isReady())
129         return true;
130 
131       usleep(5000);
132       retries++;
133     }
134 
135   throw std::runtime_error(std::string(__FUNCTION__) +
136                            ": timeout waiting for device to become ready");
137 
138   return false;
139 }
140 
update(bool selfTest)141 bool AK8975::update(bool selfTest)
142 {
143   // this flag (selfTest) is used so that we can read values without
144   // specifically taking a measurement.  For example, selfTest will
145   // pass true to this method so that the test results aren't
146   // overwritten by a measurement.
147   if (!selfTest)
148     {
149       // First set measurement mode (take a measurement)
150       if (!setMode(CNTL_MEASURE))
151         {
152           throw std::runtime_error(std::string(__FUNCTION__) +
153                                    ": Unable to set MEASURE mode");
154           return false;
155         }
156     }
157 
158   if (!waitforDeviceReady())
159     return false;
160 
161   // hope it worked.  Now read out the values and store them (uncompensated)
162   uint8_t data[6];
163   m_i2c.readBytesReg(REG_HXL, data, 6);
164 
165   int16_t x, y, z;
166   x = ( (data[1] << 8) | data[0] );
167   y = ( (data[3] << 8) | data[2] );
168   z = ( (data[5] << 8) | data[4] );
169 
170   m_xData = float(x);
171   m_yData = float(y);
172   m_zData = float(z);
173 
174   return true;
175 }
176 
selfTest()177 bool AK8975::selfTest()
178 {
179   mraa::Result rv;
180 
181   // set power down first
182   if (!setMode(CNTL_PWRDWN))
183     {
184       throw std::runtime_error(std::string(__FUNCTION__) +
185                                ": Unable to set PWRDWN mode");
186       return false;
187     }
188 
189   // enable self test bit
190   if ((rv = m_i2c.writeReg(REG_ASTC, ASTC_SELF)) != mraa::SUCCESS)
191     {
192       throw std::runtime_error(std::string(__FUNCTION__) +
193                                ": failed to enable self test");
194       return false;
195     }
196 
197   // now set self test mode
198   if (!setMode(CNTL_SELFTEST))
199     {
200       throw std::runtime_error(std::string(__FUNCTION__) +
201                                ": Unable to set SELFTEST mode");
202       return false;
203     }
204 
205   // now update current data (without enabling a measurement)
206   update(true);
207 
208   // Now, reset self test register
209   uint8_t reg = m_i2c.readReg(REG_ASTC);
210   reg &= ~ASTC_SELF;
211   if ((rv = m_i2c.writeReg(REG_ASTC, reg)) != mraa::SUCCESS)
212     {
213       throw std::runtime_error(std::string(__FUNCTION__) +
214                                ": failed to disable self test");
215       return false;
216     }
217 
218   // after self-test measurement, device transitions to power down mode
219   return true;
220 }
221 
adjustValue(float value,float adj)222 float AK8975::adjustValue(float value, float adj)
223 {
224   // apply the proper compensation to value.  This equation is taken
225   // from the AK8975 datasheet, section 8.3.11
226 
227   return ( value * ((((adj - 128.0) * 0.5) / 128.0) + 1.0) );
228 }
229 
getMagnetometer(float * x,float * y,float * z)230 void AK8975::getMagnetometer(float *x, float *y, float *z)
231 {
232   if (x)
233     *x = adjustValue(m_xData, m_xCoeff);
234   if (y)
235     *y = adjustValue(m_yData, m_yCoeff);
236   if (z)
237     *z = adjustValue(m_zData, m_zCoeff);
238 }
239 
240