MotionFX_Manager.cpp

00001 /**
00002   ******************************************************************************
00003   * @file        MotionFX_Manager.c
00004   * @author      MEMS Application Team
00005   * @version     V2.0.0
00006   * @date        01-May-2017
00007   * @brief       This file includes sensor fusion interface functions
00008   ******************************************************************************
00009   * @attention
00010   *
00011   * <h2><center>&copy; COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
00012   *
00013   * Redistribution and use in source and binary forms, with or without modification,
00014   * are permitted provided that the following conditions are met:
00015   *   1. Redistributions of source code must retain the above copyright notice,
00016   *      this list of conditions and the following disclaimer.
00017   *   2. Redistributions in binary form must reproduce the above copyright notice,
00018   *      this list of conditions and the following disclaimer in the documentation
00019   *      and/or other materials provided with the distribution.
00020   *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021   *      may be used to endorse or promote products derived from this software
00022   *      without specific prior written permission.
00023   *
00024   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025   * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027   * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028   * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029   * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030   * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031   * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032   * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033   * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034   *
00035   ******************************************************************************
00036   */
00037 
00038 /* Includes ------------------------------------------------------------------*/
00039 #include "mbed.h"
00040 #include "main.h"
00041 #include "MotionFX_Manager.h"
00042 
00043 #ifdef USE_SENSOR_FUSION_LIB
00044 
00045 /** @addtogroup MOTION_FX_Applications
00046   * @{
00047   */
00048 
00049 /** @addtogroup DATALOG_FUSION
00050   * @{
00051   */
00052 
00053 /** @addtogroup FX_Driver FX_Driver
00054   * @{
00055   */
00056 
00057 /* Extern variables ----------------------------------------------------------*/
00058 
00059 /* Private defines -----------------------------------------------------------*/
00060 #define SAMPLETODISCARD                 15
00061 #define GBIAS_ACC_TH_SC_6X              (2.0f*0.000765f)
00062 #define GBIAS_GYRO_TH_SC_6X             (2.0f*0.002f)
00063 #define GBIAS_MAG_TH_SC_6X              (2.0f*0.001500f)
00064 #define GBIAS_ACC_TH_SC_9X              (2.0f*0.000765f)
00065 #define GBIAS_GYRO_TH_SC_9X             (2.0f*0.002f)
00066 #define GBIAS_MAG_TH_SC_9X              (2.0f*0.001500f)
00067 
00068 /* Private variables ---------------------------------------------------------*/
00069 MFX_knobs_t iKnobs;
00070 MFX_knobs_t *ipKnobs = &iKnobs;
00071 
00072 static volatile int sampleToDiscard = SAMPLETODISCARD;
00073 static int discardedCount = 0;
00074 
00075 /* Public Functions ----------------------------------------------------------*/
00076 
00077 /**
00078   * @brief  Initialises MotionFX algorithm
00079   * @param  handle handle to gyroscope sensor
00080   * @retval none
00081   */
00082 
00083 void MotionFX_manager_init(unsigned char instance)
00084 {
00085   char LibVersion[36];
00086   
00087   __CRC_CLK_ENABLE();   
00088   MotionFX_initialize();
00089   uint8_t  len = MotionFX_GetLibVersion(LibVersion);
00090   LibVersion [len+1] = '\0';
00091   //printf ("MotionFX library Version: %s\n\r", LibVersion);
00092   
00093   MotionFX_getKnobs(ipKnobs);
00094 
00095   ipKnobs->gbias_acc_th_sc_6X = GBIAS_ACC_TH_SC_6X;
00096   ipKnobs->gbias_gyro_th_sc_6X = GBIAS_GYRO_TH_SC_6X;
00097   ipKnobs->gbias_mag_th_sc_6X = GBIAS_MAG_TH_SC_6X;
00098 
00099   ipKnobs->gbias_acc_th_sc_9X = GBIAS_ACC_TH_SC_9X;
00100   ipKnobs->gbias_gyro_th_sc_9X = GBIAS_GYRO_TH_SC_9X;
00101   ipKnobs->gbias_mag_th_sc_9X = GBIAS_MAG_TH_SC_9X;
00102   
00103 //  BSP_GYRO_Get_Instance(handle, &instance);
00104 
00105   switch (instance)
00106   {
00107 #if (defined (USE_IKS01A1))
00108     case LSM6DS0_G_0:
00109       ipKnobs->acc_orientation[0] = 'e';
00110       ipKnobs->acc_orientation[1] = 'n';
00111       ipKnobs->acc_orientation[2] = 'u';
00112 
00113       ipKnobs->gyro_orientation[0] = 'e';
00114       ipKnobs->gyro_orientation[1] = 'n';
00115       ipKnobs->gyro_orientation[2] = 'u';
00116       break;
00117 
00118     case LSM6DS3_G_0:
00119       ipKnobs->acc_orientation[0] = 'n';
00120       ipKnobs->acc_orientation[1] = 'w';
00121       ipKnobs->acc_orientation[2] = 'u';
00122 
00123       ipKnobs->gyro_orientation[0] = 'n';
00124       ipKnobs->gyro_orientation[1] = 'w';
00125       ipKnobs->gyro_orientation[2] = 'u';
00126       break;
00127 
00128     default:
00129       return;
00130 
00131 #elif (defined (USE_IKS01A2))
00132     case LSM6DSL_G_0:
00133       ipKnobs->acc_orientation[0] = 'n';
00134       ipKnobs->acc_orientation[1] = 'w';
00135       ipKnobs->acc_orientation[2] = 'u';
00136 
00137       ipKnobs->gyro_orientation[0] = 'n';
00138       ipKnobs->gyro_orientation[1] = 'w';
00139       ipKnobs->gyro_orientation[2] = 'u';
00140       break;
00141 
00142     default:
00143       return;
00144       
00145 #else
00146 #error Not supported platform
00147 #endif
00148   }
00149   
00150 #if (defined (USE_IKS01A1))
00151   ipKnobs->mag_orientation[0] = 's';
00152   ipKnobs->mag_orientation[1] = 'e';
00153   ipKnobs->mag_orientation[2] = 'u';
00154 #elif (defined (USE_IKS01A2))
00155   ipKnobs->mag_orientation[0] = 'n';
00156   ipKnobs->mag_orientation[1] = 'e';
00157   ipKnobs->mag_orientation[2] = 'u';
00158 #else
00159 #error Not supported platform
00160 #endif
00161 
00162   ipKnobs->output_type = MFX_ENGINE_OUTPUT_ENU;
00163   ipKnobs->LMode = 1;
00164   ipKnobs->modx = 1;
00165 
00166   MotionFX_setKnobs(ipKnobs);
00167 
00168   MotionFX_enable_6X(MFX_ENGINE_DISABLE);
00169   MotionFX_enable_9X(MFX_ENGINE_DISABLE);
00170 }
00171 
00172 
00173 /**
00174   * @brief  Run sensor fusion algorithm
00175   * @param  None
00176   * @retval None
00177   */
00178 void MotionFX_manager_run(MFX_input_t *data_in, MFX_output_t *data_out, float delta_time)
00179 {
00180   if(discardedCount == sampleToDiscard)
00181   {
00182     MotionFX_propagate(data_out, data_in, delta_time);
00183     MotionFX_update(data_out, data_in, delta_time, NULL);
00184   }
00185   else
00186   {
00187     discardedCount++;
00188   }
00189 }
00190 
00191 
00192 /**
00193  * @brief  Start 6 axes MotionFX engine
00194  * @param  None
00195  * @retval None
00196  */
00197 void MotionFX_manager_start_6X(void)
00198 {
00199   MotionFX_enable_6X(MFX_ENGINE_ENABLE);
00200 }
00201 
00202 
00203 /**
00204  * @brief  Stop 6 axes MotionFX engine
00205  * @param  None
00206  * @retval None
00207  */
00208 void MotionFX_manager_stop_6X(void)
00209 {
00210   MotionFX_enable_6X(MFX_ENGINE_DISABLE);
00211 }
00212 
00213 
00214 /**
00215  * @brief  Start 9 axes MotionFX engine
00216  * @param  None
00217  * @retval None
00218  */
00219 void MotionFX_manager_start_9X(void)
00220 {
00221   MotionFX_enable_9X(MFX_ENGINE_ENABLE);
00222 }
00223 
00224 
00225 /**
00226  * @brief  Stop 9 axes MotionFX engine
00227  * @param  None
00228  * @retval None
00229  */
00230 void MotionFX_manager_stop_9X(void)
00231 {
00232   MotionFX_enable_9X(MFX_ENGINE_DISABLE);
00233 }
00234 
00235 
00236 /**
00237   * @brief  Run magnetometer calibration algorithm
00238   * @param  None
00239   * @retval None
00240   */
00241 void MotionFX_manager_MagCal_run(MFX_MagCal_input_t *data_in, MFX_MagCal_output_t *data_out)
00242 {
00243    MotionFX_MagCal_run(data_in);
00244    MotionFX_MagCal_getParams(data_out);
00245 }
00246 
00247 
00248 /**
00249  * @brief  Start magnetometer calibration
00250  * @param  None
00251  * @retval None
00252  */
00253 void MotionFX_manager_MagCal_start(int sampletime)
00254 {
00255   MotionFX_MagCal_init(sampletime, 1);
00256 }
00257 
00258 
00259 /**
00260  * @brief  Stop magnetometer calibration
00261  * @param  None
00262  * @retval None
00263  */
00264 void MotionFX_manager_MagCal_stop(int sampletime)
00265 {
00266   MotionFX_MagCal_init(sampletime, 0);
00267 }
00268 
00269 
00270 /**
00271  * @brief  Load calibration parameter from memory
00272  * @param  dataSize length ot the data
00273  * @param  data pointer to the data
00274  * @retval (1) fail, (0) success
00275  */
00276 char MotionFX_LoadMagCalFromNVM(unsigned short int dataSize, unsigned int *data)
00277 {
00278 #if (defined (MOTION_FX_STORE_CALIB_FLASH))
00279   RecallCalibrationFromMemory(dataSize / 4, (uint32_t*) data);
00280   return 0;
00281 #else
00282   return 1;
00283 #endif
00284 }
00285 
00286 
00287 /**
00288  * @brief  Save calibration parameter to memory
00289  * @param  dataSize length ot the data
00290  * @param  data pointer to the data
00291  * @retval (1) fail, (0) success
00292  */
00293 char MotionFX_SaveMagCalInNVM(unsigned short int dataSize, unsigned int *data)
00294 {
00295 #if (defined (MOTION_FX_STORE_CALIB_FLASH))
00296   SaveCalibrationToMemory(dataSize / 4, (uint32_t*) data);
00297   return 0;
00298 #else
00299   return 1;
00300 #endif
00301 }
00302 
00303 /**
00304   * @}
00305   */
00306 
00307 /**
00308   * @}
00309   */
00310 
00311 /**
00312   * @}
00313   */
00314 #endif
00315 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/