1 /* 2 * Copyright (c) 2015, Freescale Semiconductor, Inc. 3 * Copyright 2016-2017 NXP 4 * All rights reserved. 5 * 6 * SPDX-License-Identifier: BSD-3-Clause 7 */ 8 9 /*! \file hal_frdm_fxs_mult2_b.c 10 \brief Hardware Abstraction layer for the FRDM-FXS-MULT2-B sensor shield. 11 */ 12 13 #include "sensor_fusion.h" // top level magCal and sensor fusion interfaces 14 15 // This HAL (Hardware Abstraction Layer) is applicable to: 16 // FRDM_K64F + 17 // FRDM_FXS_MULT2_B utilizing the FX0S8700 and FXAS21002 on the shield 18 // It also works for the FRDM-STBC_AGM01 board. 19 // It also works for the FRDM-STBC_AGM02 board (the driver_MAG3110.c inverts 20 // the MAG3110 Z-axis reading to enforce +Z = up, so compatibility is maintained) 21 ApplyAccelHAL(struct AccelSensor * Accel)22void ApplyAccelHAL(struct AccelSensor *Accel) 23 { 24 int8 i; // loop counter 25 26 // apply HAL to all measurements read from FIFO buffer 27 for (i = 0; i < Accel->iFIFOCount; i++) 28 { 29 // apply HAL mapping to coordinate system used 30 #if THISCOORDSYSTEM == NED 31 int16 itmp16 = Accel->iGsFIFO[i][CHX]; 32 Accel->iGsFIFO[i][CHX] = Accel->iGsFIFO[i][CHY]; 33 Accel->iGsFIFO[i][CHY] = itmp16; 34 #endif // NED 35 #if THISCOORDSYSTEM == ANDROID 36 Accel->iGsFIFO[i][CHX] = -Accel->iGsFIFO[i][CHX]; 37 Accel->iGsFIFO[i][CHY] = -Accel->iGsFIFO[i][CHY]; 38 #endif // Android 39 #if (THISCOORDSYSTEM == WIN8) 40 Accel->iGsFIFO[i][CHZ] = -Accel->iGsFIFO[i][CHZ]; 41 #endif // Win8 42 43 } // end of loop over FIFO count 44 45 return; 46 } 47 48 // function applies the hardware abstraction layer to the magnetometer readings ApplyMagHAL(struct MagSensor * Mag)49void ApplyMagHAL(struct MagSensor *Mag) 50 { 51 int8 i; // loop counter 52 53 // apply HAL to all measurements read from FIFO buffer 54 for (i = 0; i < Mag->iFIFOCount; i++) 55 { 56 // apply HAL mapping to coordinate system used 57 #if THISCOORDSYSTEM == NED 58 int16 itmp16 = Mag->iBsFIFO[i][CHX]; 59 Mag->iBsFIFO[i][CHX] = -Mag->iBsFIFO[i][CHY]; 60 Mag->iBsFIFO[i][CHY] = -itmp16; 61 Mag->iBsFIFO[i][CHZ] = -Mag->iBsFIFO[i][CHZ]; 62 #endif // NED 63 #if THISCOORDSYSTEM == ANDROID 64 Mag->iBsFIFO[i][CHX] = -Mag->iBsFIFO[i][CHX]; 65 Mag->iBsFIFO[i][CHY] = -Mag->iBsFIFO[i][CHY]; 66 #endif // Android 67 #if THISCOORDSYSTEM == WIN8 68 Mag->iBsFIFO[i][CHX] = -Mag->iBsFIFO[i][CHX]; 69 Mag->iBsFIFO[i][CHY] = -Mag->iBsFIFO[i][CHY]; 70 #endif 71 } // end of loop over FIFO count 72 73 return; 74 } 75 76 // function applies the hardware abstraction layer to the gyro readings ApplyGyroHAL(struct GyroSensor * Gyro)77void ApplyGyroHAL(struct GyroSensor *Gyro) 78 { 79 int8 i; // loop counter 80 81 // apply HAL to all measurements read from FIFO buffer 82 for (i = 0; i < Gyro->iFIFOCount; i++) 83 { 84 // apply HAL mapping to coordinate system used 85 #if THISCOORDSYSTEM == NED 86 int16 itmp16 = Gyro->iYsFIFO[i][CHX]; 87 Gyro->iYsFIFO[i][CHX] = -Gyro->iYsFIFO[i][CHY]; 88 Gyro->iYsFIFO[i][CHY] = -itmp16; 89 Gyro->iYsFIFO[i][CHZ] = -Gyro->iYsFIFO[i][CHZ]; 90 #endif // NED 91 #if THISCOORDSYSTEM == ANDROID 92 Gyro->iYsFIFO[i][CHX] = -Gyro->iYsFIFO[i][CHX]; 93 Gyro->iYsFIFO[i][CHY] = -Gyro->iYsFIFO[i][CHY]; 94 #endif // Android 95 #if THISCOORDSYSTEM == WIN8 96 Gyro->iYsFIFO[i][CHX] = -Gyro->iYsFIFO[i][CHX]; 97 Gyro->iYsFIFO[i][CHY] = -Gyro->iYsFIFO[i][CHY]; 98 #endif // Win8 99 100 } // end of loop over FIFO count 101 102 return; 103 } 104 105
