1 /****************************************************************************** 2 * $Id: AKFS_APIs.c 580 2012-03-29 09:56:21Z yamada.rj $ 3 ****************************************************************************** 4 * 5 * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan 6 * 7 * Licensed under the Apache License, Version 2.0 (the "License"); 8 * you may not use this file except in compliance with the License. 9 * You may obtain a copy of the License at 10 * 11 * http://www.apache.org/licenses/LICENSE-2.0 12 * 13 * Unless required by applicable law or agreed to in writing, software 14 * distributed under the License is distributed on an "AS IS" BASIS, 15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 * See the License for the specific language governing permissions and 17 * limitations under the License. 18 */ 19 #include "AKFS_Common.h" 20 #include "AKFS_Disp.h" 21 #include "AKFS_FileIO.h" 22 #include "AKFS_APIs.h" 23 24 #ifdef WIN32 25 #include "AK8975_LinuxDriver.h" 26 #endif 27 28 static AK8975PRMS g_prms; 29 30 /*! 31 Initialize library. At first, 0 is set to all parameters. After that, some 32 parameters, which should not be 0, are set to specific value. Some of initial 33 values can be customized by editing the file \c "AKFS_CSpec.h". 34 @return The return value is #AKM_SUCCESS. 35 @param[in] hpat Specify a layout pattern number. The number is determined 36 according to the mount orientation of the magnetometer. 37 @param[in] regs[3] Specify the ASA values which are read out from 38 fuse ROM. regs[0] is ASAX, regs[1] is ASAY, regs[2] is ASAZ. 39 */ 40 int16 AKFS_Init( 41 const AKFS_PATNO hpat, 42 const uint8 regs[] 43 ) 44 { 45 AKMDATA(AKMDATA_DUMP, "%s: hpat=%d, r[0]=0x%02X, r[1]=0x%02X, r[2]=0x%02X\n", 46 __FUNCTION__, hpat, regs[0], regs[1], regs[2]); 47 48 /* Set 0 to the AK8975 structure. */ 49 memset(&g_prms, 0, sizeof(AK8975PRMS)); 50 51 /* Sensitivity */ 52 g_prms.mfv_hs.u.x = AK8975_HSENSE_DEFAULT; 53 g_prms.mfv_hs.u.y = AK8975_HSENSE_DEFAULT; 54 g_prms.mfv_hs.u.z = AK8975_HSENSE_DEFAULT; 55 g_prms.mfv_as.u.x = AK8975_ASENSE_DEFAULT; 56 g_prms.mfv_as.u.y = AK8975_ASENSE_DEFAULT; 57 g_prms.mfv_as.u.z = AK8975_ASENSE_DEFAULT; 58 59 /* Initialize variables that initial value is not 0. */ 60 g_prms.mi_hnaveV = CSPEC_HNAVE_V; 61 g_prms.mi_hnaveD = CSPEC_HNAVE_D; 62 g_prms.mi_anaveV = CSPEC_ANAVE_V; 63 g_prms.mi_anaveD = CSPEC_ANAVE_D; 64 65 /* Copy ASA values */ 66 g_prms.mi_asa.u.x = regs[0]; 67 g_prms.mi_asa.u.y = regs[1]; 68 g_prms.mi_asa.u.z = regs[2]; 69 70 /* Copy layout pattern */ 71 g_prms.m_hpat = hpat; 72 73 return AKM_SUCCESS; 74 } 75 76 /*! 77 Release resources. This function is for future expansion. 78 @return The return value is #AKM_SUCCESS. 79 */ 80 int16 AKFS_Release(void) 81 { 82 return AKM_SUCCESS; 83 } 84 85 /* 86 This function is called just before a measurement sequence starts. 87 This function reads parameters from file, then initializes algorithm 88 parameters. 89 @return The return value is #AKM_SUCCESS. 90 @param[in] path Specify a path to the settings file. 91 */ 92 int16 AKFS_Start( 93 const char* path 94 ) 95 { 96 AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path); 97 98 /* Read setting files from a file */ 99 if (AKFS_LoadParameters(&g_prms, path) != AKM_SUCCESS) { 100 AKMERROR_STR("AKFS_Load"); 101 } 102 103 /* Initialize buffer */ 104 AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hdata); 105 AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hvbuf); 106 AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_adata); 107 AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_avbuf); 108 109 /* Initialize for AOC */ 110 AKFS_InitAOC(&g_prms.m_aocv); 111 /* Initialize magnetic status */ 112 g_prms.mi_hstatus = 0; 113 114 return AKM_SUCCESS; 115 } 116 117 /*! 118 This function is called when a measurement sequence is done. 119 This fucntion writes parameters to file. 120 @return The return value is #AKM_SUCCESS. 121 @param[in] path Specify a path to the settings file. 122 */ 123 int16 AKFS_Stop( 124 const char* path 125 ) 126 { 127 AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path); 128 129 /* Write setting files to a file */ 130 if (AKFS_SaveParameters(&g_prms, path) != AKM_SUCCESS) { 131 AKMERROR_STR("AKFS_Save"); 132 } 133 134 return AKM_SUCCESS; 135 } 136 137 /*! 138 This function is called when new magnetometer data is available. The output 139 vector format and coordination system follow the Android definition. 140 @return The return value is #AKM_SUCCESS. 141 Otherwise the return value is #AKM_FAIL. 142 @param[in] mag A set of measurement data from magnetometer. X axis value 143 should be in mag[0], Y axis value should be in mag[1], Z axis value should be 144 in mag[2]. 145 @param[in] status A status of magnetometer. This status indicates the result 146 of measurement data, i.e. overflow, success or fail, etc. 147 @param[out] vx X axis value of magnetic field vector. 148 @param[out] vy Y axis value of magnetic field vector. 149 @param[out] vz Z axis value of magnetic field vector. 150 @param[out] accuracy Accuracy of magnetic field vector. 151 */ 152 int16 AKFS_Get_MAGNETIC_FIELD( 153 const int16 mag[3], 154 const int16 status, 155 AKFLOAT* vx, 156 AKFLOAT* vy, 157 AKFLOAT* vz, 158 int16* accuracy 159 ) 160 { 161 int16 akret; 162 int16 aocret; 163 AKFLOAT radius; 164 165 AKMDATA(AKMDATA_DUMP, "%s: m[0]=%d, m[1]=%d, m[2]=%d, st=%d\n", 166 __FUNCTION__, mag[0], mag[1], mag[2], status); 167 168 /* Decomposition */ 169 /* Sensitivity adjustment, i.e. multiply ASA, is done in this function. */ 170 akret = AKFS_DecompAK8975( 171 mag, 172 status, 173 &g_prms.mi_asa, 174 AKFS_HDATA_SIZE, 175 g_prms.mfv_hdata 176 ); 177 if(akret == AKFS_ERROR) { 178 AKMERROR; 179 return AKM_FAIL; 180 } 181 182 /* Adjust coordination */ 183 akret = AKFS_Rotate( 184 g_prms.m_hpat, 185 &g_prms.mfv_hdata[0] 186 ); 187 if (akret == AKFS_ERROR) { 188 AKMERROR; 189 return AKM_FAIL; 190 } 191 192 /* AOC for magnetometer */ 193 /* Offset estimation is done in this function */ 194 aocret = AKFS_AOC( 195 &g_prms.m_aocv, 196 g_prms.mfv_hdata, 197 &g_prms.mfv_ho 198 ); 199 200 /* Subtract offset */ 201 /* Then, a magnetic vector, the unit is uT, is stored in mfv_hvbuf. */ 202 akret = AKFS_VbNorm( 203 AKFS_HDATA_SIZE, 204 g_prms.mfv_hdata, 205 1, 206 &g_prms.mfv_ho, 207 &g_prms.mfv_hs, 208 AK8975_HSENSE_TARGET, 209 AKFS_HDATA_SIZE, 210 g_prms.mfv_hvbuf 211 ); 212 if(akret == AKFS_ERROR) { 213 AKMERROR; 214 return AKM_FAIL; 215 } 216 217 /* Averaging */ 218 akret = AKFS_VbAve( 219 AKFS_HDATA_SIZE, 220 g_prms.mfv_hvbuf, 221 CSPEC_HNAVE_V, 222 &g_prms.mfv_hvec 223 ); 224 if (akret == AKFS_ERROR) { 225 AKMERROR; 226 return AKM_FAIL; 227 } 228 229 /* Check the size of magnetic vector */ 230 radius = AKFS_SQRT( 231 (g_prms.mfv_hvec.u.x * g_prms.mfv_hvec.u.x) + 232 (g_prms.mfv_hvec.u.y * g_prms.mfv_hvec.u.y) + 233 (g_prms.mfv_hvec.u.z * g_prms.mfv_hvec.u.z)); 234 235 if (radius > AKFS_GEOMAG_MAX) { 236 g_prms.mi_hstatus = 0; 237 } else { 238 if (aocret) { 239 g_prms.mi_hstatus = 3; 240 } 241 } 242 243 *vx = g_prms.mfv_hvec.u.x; 244 *vy = g_prms.mfv_hvec.u.y; 245 *vz = g_prms.mfv_hvec.u.z; 246 *accuracy = g_prms.mi_hstatus; 247 248 /* Debug output */ 249 AKMDATA(AKMDATA_MAG, "Mag(%d):%8.2f, %8.2f, %8.2f\n", 250 *accuracy, *vx, *vy, *vz); 251 252 return AKM_SUCCESS; 253 } 254 255 /*! 256 This function is called when new accelerometer data is available. The output 257 vector format and coordination system follow the Android definition. 258 @return The return value is #AKM_SUCCESS when function succeeds. Otherwise 259 the return value is #AKM_FAIL. 260 @param[in] acc A set of measurement data from accelerometer. X axis value 261 should be in acc[0], Y axis value should be in acc[1], Z axis value should be 262 in acc[2]. 263 @param[in] status A status of accelerometer. This status indicates the result 264 of acceleration data, i.e. overflow, success or fail, etc. 265 @param[out] vx X axis value of acceleration vector. 266 @param[out] vy Y axis value of acceleration vector. 267 @param[out] vz Z axis value of acceleration vector. 268 @param[out] accuracy Accuracy of acceleration vector. 269 This value is always 3. 270 */ 271 int16 AKFS_Get_ACCELEROMETER( 272 const int16 acc[3], 273 const int16 status, 274 AKFLOAT* vx, 275 AKFLOAT* vy, 276 AKFLOAT* vz, 277 int16* accuracy 278 ) 279 { 280 int16 akret; 281 282 AKMDATA(AKMDATA_DUMP, "%s: a[0]=%d, a[1]=%d, a[2]=%d, st=%d\n", 283 __FUNCTION__, acc[0], acc[1], acc[2], status); 284 285 /* Save data to buffer */ 286 AKFS_BufShift( 287 AKFS_ADATA_SIZE, 288 1, 289 g_prms.mfv_adata 290 ); 291 g_prms.mfv_adata[0].u.x = acc[0]; 292 g_prms.mfv_adata[0].u.y = acc[1]; 293 g_prms.mfv_adata[0].u.z = acc[2]; 294 295 /* Subtract offset, adjust sensitivity */ 296 /* As a result, a unit of acceleration data in mfv_avbuf is '1G = 9.8' */ 297 akret = AKFS_VbNorm( 298 AKFS_ADATA_SIZE, 299 g_prms.mfv_adata, 300 1, 301 &g_prms.mfv_ao, 302 &g_prms.mfv_as, 303 AK8975_ASENSE_TARGET, 304 AKFS_ADATA_SIZE, 305 g_prms.mfv_avbuf 306 ); 307 if(akret == AKFS_ERROR) { 308 AKMERROR; 309 return AKM_FAIL; 310 } 311 312 /* Averaging */ 313 akret = AKFS_VbAve( 314 AKFS_ADATA_SIZE, 315 g_prms.mfv_avbuf, 316 CSPEC_ANAVE_V, 317 &g_prms.mfv_avec 318 ); 319 if (akret == AKFS_ERROR) { 320 AKMERROR; 321 return AKM_FAIL; 322 } 323 324 /* Adjust coordination */ 325 /* It is not needed. Because, the data from AK8975 driver is already 326 follows Android coordinate system. */ 327 328 *vx = g_prms.mfv_avec.u.x; 329 *vy = g_prms.mfv_avec.u.y; 330 *vz = g_prms.mfv_avec.u.z; 331 *accuracy = 3; 332 333 /* Debug output */ 334 AKMDATA(AKMDATA_ACC, "Acc(%d):%8.2f, %8.2f, %8.2f\n", 335 *accuracy, *vx, *vy, *vz); 336 337 return AKM_SUCCESS; 338 } 339 340 /*! 341 Get orientation sensor's elements. The vector format and coordination system 342 follow the Android definition. Before this function is called, magnetic 343 field vector and acceleration vector should be stored in the buffer by 344 calling #AKFS_Get_MAGNETIC_FIELD and #AKFS_Get_ACCELEROMETER. 345 @return The return value is #AKM_SUCCESS when function succeeds. Otherwise 346 the return value is #AKM_FAIL. 347 @param[out] azimuth Azimuthal angle in degree. 348 @param[out] pitch Pitch angle in degree. 349 @param[out] roll Roll angle in degree. 350 @param[out] accuracy Accuracy of orientation sensor. 351 */ 352 int16 AKFS_Get_ORIENTATION( 353 AKFLOAT* azimuth, 354 AKFLOAT* pitch, 355 AKFLOAT* roll, 356 int16* accuracy 357 ) 358 { 359 int16 akret; 360 361 /* Azimuth calculation */ 362 /* Coordination system follows the Android coordination. */ 363 akret = AKFS_Direction( 364 AKFS_HDATA_SIZE, 365 g_prms.mfv_hvbuf, 366 CSPEC_HNAVE_D, 367 AKFS_ADATA_SIZE, 368 g_prms.mfv_avbuf, 369 CSPEC_ANAVE_D, 370 &g_prms.mf_azimuth, 371 &g_prms.mf_pitch, 372 &g_prms.mf_roll 373 ); 374 375 if(akret == AKFS_ERROR) { 376 AKMERROR; 377 return AKM_FAIL; 378 } 379 *azimuth = g_prms.mf_azimuth; 380 *pitch = g_prms.mf_pitch; 381 *roll = g_prms.mf_roll; 382 *accuracy = g_prms.mi_hstatus; 383 384 /* Debug output */ 385 AKMDATA(AKMDATA_ORI, "Ori(%d):%8.2f, %8.2f, %8.2f\n", 386 *accuracy, *azimuth, *pitch, *roll); 387 388 return AKM_SUCCESS; 389 } 390 391
