Diff: MotionFX_Manager.cpp

Revision:

20:b97e14ade434

Parent:

9:2693f9ef8ff7

--- a/MotionFX_Manager.cpp	Fri Jun 09 14:00:14 2017 +0200
+++ b/MotionFX_Manager.cpp	Wed Sep 20 17:08:56 2017 +0200
@@ -1,282 +1,315 @@
 /**
-******************************************************************************
-* @file    MotionFX_Manager.c
-* @author  Central Lab
-* @version V1.1.0
-* @date    20-January-2015
-* @brief   This file includes sensor fusion interface functions
-******************************************************************************
-* @attention
-*
-* <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
-*
-* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
-* You may not use this file except in compliance with the License.
-* You may obtain a copy of the License at:
-*
-*        http://www.st.com/software_license_agreement_liberty_v2
-*
-* Redistribution and use in source and binary forms, with or without modification,
-* are permitted provided that the following conditions are met:
-*   1. Redistributions of source code must retain the above copyright notice,
-*      this list of conditions and the following disclaimer.
-*   2. Redistributions in binary form must reproduce the above copyright notice,
-*      this list of conditions and the following disclaimer in the documentation
-*      and/or other materials provided with the distribution.
-*   3. Neither the name of STMicroelectronics nor the names of its contributors
-*      may be used to endorse or promote products derived from this software
-*      without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*
-******************************************************************************
-*/
+  ******************************************************************************
+  * @file        MotionFX_Manager.c
+  * @author      MEMS Application Team
+  * @version     V2.0.0
+  * @date        01-May-2017
+  * @brief       This file includes sensor fusion interface functions
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
+  *
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. Redistributions in binary form must reproduce the above copyright notice,
+  *      this list of conditions and the following disclaimer in the documentation
+  *      and/or other materials provided with the distribution.
+  *   3. Neither the name of STMicroelectronics nor the names of its contributors
+  *      may be used to endorse or promote products derived from this software
+  *      without specific prior written permission.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  ******************************************************************************
+  */
 
 /* Includes ------------------------------------------------------------------*/
+#include "mbed.h"
+#include "main.h"
 #include "MotionFX_Manager.h"
 
 #ifdef USE_SENSOR_FUSION_LIB
 
-#include "osx_license.h"
-#define FROM_MG_TO_G    0.001
-#define FROM_MDPS_TO_DPS    0.001
-#define FROM_MGAUSS_TO_UT50 (0.1f/50.0f)
+/** @addtogroup MOTION_FX_Applications
+  * @{
+  */
+
+/** @addtogroup DATALOG_FUSION
+  * @{
+  */
+
+/** @addtogroup FX_Driver FX_Driver
+  * @{
+  */
+
+/* Extern variables ----------------------------------------------------------*/
 
-/* SF Data structure */
-static volatile osxMFX_output iDataOUT;
-static volatile osxMFX_input iDataIN;
+/* Private defines -----------------------------------------------------------*/
+#define SAMPLETODISCARD                 15
+#define GBIAS_ACC_TH_SC_6X              (2.0f*0.000765f)
+#define GBIAS_GYRO_TH_SC_6X             (2.0f*0.002f)
+#define GBIAS_MAG_TH_SC_6X              (2.0f*0.001500f)
+#define GBIAS_ACC_TH_SC_9X              (2.0f*0.000765f)
+#define GBIAS_GYRO_TH_SC_9X             (2.0f*0.002f)
+#define GBIAS_MAG_TH_SC_9X              (2.0f*0.001500f)
 
-/* Private define ------------------------------------------------------------*/
-#define SAMPLETODISCARD 15
+/* Private variables ---------------------------------------------------------*/
+MFX_knobs_t iKnobs;
+MFX_knobs_t *ipKnobs = &iKnobs;
 
-/* Private Variables -------------------------------------------------------------*/
 static volatile int sampleToDiscard = SAMPLETODISCARD;
-static float MotionFX_engine_deltatime = DELTATIMESENSORFUSION;
 static int discardedCount = 0;
 
-static osxMFX_knobs iKnobs;
-static osxMFX_knobs* ipKnobs;
+/* Public Functions ----------------------------------------------------------*/
 
 /**
-* @brief  Run sensor fusion Algo
-* @param  None
-* @retval None
-*/
-void MotionFX_manager_run(AxesRaw_t *ACC_Value, AxesRaw_t *GYR_Value, AxesRaw_t *MAG_Value, osxMFX_calibFactor & magOffset)
+  * @brief  Initialises MotionFX algorithm
+  * @param  handle handle to gyroscope sensor
+  * @retval none
+  */
+
+void MotionFX_manager_init(unsigned char instance)
 {
-  iDataIN.gyro[0] = GYR_Value->AXIS_X  * FROM_MDPS_TO_DPS;
-  iDataIN.gyro[1] = GYR_Value->AXIS_Y  * FROM_MDPS_TO_DPS;
-  iDataIN.gyro[2] = GYR_Value->AXIS_Z  * FROM_MDPS_TO_DPS; 
+  char LibVersion[36];
+  
+  __CRC_CLK_ENABLE();	
+  MotionFX_initialize();
+  uint8_t  len = MotionFX_GetLibVersion(LibVersion);
+  LibVersion [len+1] = '\0';
+  //printf ("MotionFX library Version: %s\n\r", LibVersion);
+  
+  MotionFX_getKnobs(ipKnobs);
+
+  ipKnobs->gbias_acc_th_sc_6X = GBIAS_ACC_TH_SC_6X;
+  ipKnobs->gbias_gyro_th_sc_6X = GBIAS_GYRO_TH_SC_6X;
+  ipKnobs->gbias_mag_th_sc_6X = GBIAS_MAG_TH_SC_6X;
+
+  ipKnobs->gbias_acc_th_sc_9X = GBIAS_ACC_TH_SC_9X;
+  ipKnobs->gbias_gyro_th_sc_9X = GBIAS_GYRO_TH_SC_9X;
+  ipKnobs->gbias_mag_th_sc_9X = GBIAS_MAG_TH_SC_9X;
   
-  iDataIN.acc[0] = ACC_Value->AXIS_X * FROM_MG_TO_G;
-  iDataIN.acc[1] = ACC_Value->AXIS_Y * FROM_MG_TO_G;
-  iDataIN.acc[2] = ACC_Value->AXIS_Z * FROM_MG_TO_G;
+//  BSP_GYRO_Get_Instance(handle, &instance);
+
+  switch (instance)
+  {
+#if (defined (USE_IKS01A1))
+    case LSM6DS0_G_0:
+      ipKnobs->acc_orientation[0] = 'e';
+      ipKnobs->acc_orientation[1] = 'n';
+      ipKnobs->acc_orientation[2] = 'u';
+
+      ipKnobs->gyro_orientation[0] = 'e';
+      ipKnobs->gyro_orientation[1] = 'n';
+      ipKnobs->gyro_orientation[2] = 'u';
+      break;
+
+    case LSM6DS3_G_0:
+      ipKnobs->acc_orientation[0] = 'n';
+      ipKnobs->acc_orientation[1] = 'w';
+      ipKnobs->acc_orientation[2] = 'u';
+
+      ipKnobs->gyro_orientation[0] = 'n';
+      ipKnobs->gyro_orientation[1] = 'w';
+      ipKnobs->gyro_orientation[2] = 'u';
+      break;
+
+    default:
+      return;
+
+#elif (defined (USE_IKS01A2))
+    case LSM6DSL_G_0:
+      ipKnobs->acc_orientation[0] = 'n';
+      ipKnobs->acc_orientation[1] = 'w';
+      ipKnobs->acc_orientation[2] = 'u';
+
+      ipKnobs->gyro_orientation[0] = 'n';
+      ipKnobs->gyro_orientation[1] = 'w';
+      ipKnobs->gyro_orientation[2] = 'u';
+      break;
+
+    default:
+      return;
+      
+#else
+#error Not supported platform
+#endif
+  }
   
-  iDataIN.mag[0] = (MAG_Value->AXIS_X - magOffset.magOffX) * FROM_MGAUSS_TO_UT50;
-  iDataIN.mag[1] = (MAG_Value->AXIS_Y - magOffset.magOffY) * FROM_MGAUSS_TO_UT50;
-  iDataIN.mag[2] = (MAG_Value->AXIS_Z - magOffset.magOffZ) * FROM_MGAUSS_TO_UT50;
-  
-  if(discardedCount == sampleToDiscard){
-    osx_MotionFX_propagate((osxMFX_output*)&iDataOUT, (osxMFX_input*)&iDataIN, MotionFX_engine_deltatime);
-  
-    osx_MotionFX_update((osxMFX_output*)&iDataOUT, (osxMFX_input*)&iDataIN, MotionFX_engine_deltatime, NULL); 
-  } else {
+#if (defined (USE_IKS01A1))
+  ipKnobs->mag_orientation[0] = 's';
+  ipKnobs->mag_orientation[1] = 'e';
+  ipKnobs->mag_orientation[2] = 'u';
+#elif (defined (USE_IKS01A2))
+  ipKnobs->mag_orientation[0] = 'n';
+  ipKnobs->mag_orientation[1] = 'e';
+  ipKnobs->mag_orientation[2] = 'u';
+#else
+#error Not supported platform
+#endif
+
+  ipKnobs->output_type = MFX_ENGINE_OUTPUT_ENU;
+  ipKnobs->LMode = 1;
+  ipKnobs->modx = 1;
+
+  MotionFX_setKnobs(ipKnobs);
+
+  MotionFX_enable_6X(MFX_ENGINE_DISABLE);
+  MotionFX_enable_9X(MFX_ENGINE_DISABLE);
+}
+
+
+/**
+  * @brief  Run sensor fusion algorithm
+  * @param  None
+  * @retval None
+  */
+void MotionFX_manager_run(MFX_input_t *data_in, MFX_output_t *data_out, float delta_time)
+{
+  if(discardedCount == sampleToDiscard)
+  {
+    MotionFX_propagate(data_out, data_in, delta_time);
+    MotionFX_update(data_out, data_in, delta_time, NULL);
+  }
+  else
+  {
     discardedCount++;
   }
 }
 
 
 /**
-* @brief  Initialise MotionFX engine
-* @param  None
-* @retval None
-*/
-bool MotionFX_manager_init(bool DS3_OnBoard, osxMFX_calibFactor & magOffset) 
+ * @brief  Start 6 axes MotionFX engine
+ * @param  None
+ * @retval None
+ */
+void MotionFX_manager_start_6X(void)
 {
-  //  ST MotionFX Engine Initializations        
-  ipKnobs =&iKnobs;
-  
-  if(!osx_MotionFX_initialize()) {
-    printf("Error License authentication\n\r");
-    return false;
-  }
-  else {
-    char VersionMotionFX[36];
-    osx_MotionFX_getLibVersion(VersionMotionFX);
-    printf("Enabled %s\n\r",VersionMotionFX);
-  }
-  
-  osx_MotionFX_compass_init();
+  MotionFX_enable_6X(MFX_ENGINE_ENABLE);
+}
 
-  osx_MotionFX_getKnobs(ipKnobs);
-  
-  /* Values For DS0/DS3: These values must be not changed */
-  ipKnobs->gbias_acc_th_sc_6X = 2*0.000765;
-  ipKnobs->gbias_gyro_th_sc_6X = 2*0.002;
-  ipKnobs->gbias_mag_th_sc_6X = 2*0.001500;
-  
-  ipKnobs->gbias_acc_th_sc_9X = 2*0.000765;
-  ipKnobs->gbias_gyro_th_sc_9X = 2*0.002;
-  ipKnobs->gbias_mag_th_sc_9X = 2*0.001500;
-  
-  
-  if(DS3_OnBoard) {
-    /* DS3 */
-    ipKnobs->acc_orientation[0] ='n';
-    ipKnobs->acc_orientation[1] ='w';
-    ipKnobs->acc_orientation[2] ='u';
-  
-    ipKnobs->gyro_orientation[0] = 'n';
-    ipKnobs->gyro_orientation[1] = 'w';
-    ipKnobs->gyro_orientation[2] = 'u';   
-  } else {
-    /* DS0 */
-    ipKnobs->acc_orientation[0] ='e';
-    ipKnobs->acc_orientation[1] ='n';
-    ipKnobs->acc_orientation[2] ='u';
-  
-    ipKnobs->gyro_orientation[0] = 'e';
-    ipKnobs->gyro_orientation[1] = 'n';
-    ipKnobs->gyro_orientation[2] = 'u';
-  }
-  ipKnobs->mag_orientation[0] = 's';
-  ipKnobs->mag_orientation[1] = 'e';
-  ipKnobs->mag_orientation[2] = 'u';
-  
-  ipKnobs->output_type = OSXMFX_ENGINE_OUTPUT_ENU;
-  
-  ipKnobs->LMode = 1;
-  
-  ipKnobs->modx = 1;    
-  
-  osx_MotionFX_setKnobs(ipKnobs);
-  
-  osx_MotionFX_enable_6X(OSXMFX_ENGINE_DISABLE);
-  
-  osx_MotionFX_enable_9X(OSXMFX_ENGINE_DISABLE);
-  
-  /* Number of Sample to Discard */
-  sampleToDiscard = SAMPLETODISCARD;
-  discardedCount = 0;
-  MotionFX_engine_deltatime = DELTATIMESENSORFUSION;
-  
-  /* Reset MagnetoOffset */
-  magOffset.magOffX = magOffset.magOffY= magOffset.magOffZ=0;
-  
-      return true;
+
+/**
+ * @brief  Stop 6 axes MotionFX engine
+ * @param  None
+ * @retval None
+ */
+void MotionFX_manager_stop_6X(void)
+{
+  MotionFX_enable_6X(MFX_ENGINE_DISABLE);
 }
 
+
 /**
-* @brief  Starts 6 axes MotionFX engine
-* @param  None
-* @retval None
-*/
-void MotionFX_manager_start_6X(void)
-{  
-  osx_MotionFX_enable_6X(OSXMFX_ENGINE_ENABLE);
+ * @brief  Start 9 axes MotionFX engine
+ * @param  None
+ * @retval None
+ */
+void MotionFX_manager_start_9X(void)
+{
+  MotionFX_enable_9X(MFX_ENGINE_ENABLE);
+}
+
+
+/**
+ * @brief  Stop 9 axes MotionFX engine
+ * @param  None
+ * @retval None
+ */
+void MotionFX_manager_stop_9X(void)
+{
+  MotionFX_enable_9X(MFX_ENGINE_DISABLE);
+}
+
+
+/**
+  * @brief  Run magnetometer calibration algorithm
+  * @param  None
+  * @retval None
+  */
+void MotionFX_manager_MagCal_run(MFX_MagCal_input_t *data_in, MFX_MagCal_output_t *data_out)
+{
+   MotionFX_MagCal_run(data_in);
+   MotionFX_MagCal_getParams(data_out);
 }
 
 
 /**
-* @brief  Stops 6 axes MotionFX engine
-* @param  None
-* @retval None
-*/
-void MotionFX_manager_stop_6X(void)
+ * @brief  Start magnetometer calibration
+ * @param  None
+ * @retval None
+ */
+void MotionFX_manager_MagCal_start(int sampletime)
 {
-  osx_MotionFX_enable_6X(OSXMFX_ENGINE_DISABLE);  
+  MotionFX_MagCal_init(sampletime, 1);
 }
 
 
 /**
-* @brief  Starts 9 axes MotionFX engine
-* @param  None
-* @retval None
-*/
-void MotionFX_manager_start_9X(void)
-{  
-  sampleToDiscard = SAMPLETODISCARD;
-  osx_MotionFX_enable_9X(OSXMFX_ENGINE_ENABLE);
-}
-
-
-/**
-* @brief  Stops 9 axes MotionFX engine
-* @param  None
-* @retval None
-*/
-void MotionFX_manager_stop_9X(void)
+ * @brief  Stop magnetometer calibration
+ * @param  None
+ * @retval None
+ */
+void MotionFX_manager_MagCal_stop(int sampletime)
 {
-  osx_MotionFX_enable_9X(OSXMFX_ENGINE_DISABLE); 
-}
-
-
-/**
-* @brief  Get MotionFX Engine data Out
-* @param  None
-* @retval osxMFX_output *iDataOUT MotionFX Engine data Out
-*/
-osxMFX_output* MotionFX_manager_getDataOUT(void)
-{  
-  return (osxMFX_output*)&iDataOUT;
+  MotionFX_MagCal_init(sampletime, 0);
 }
 
 
 /**
-* @brief  Get MotionFX Engine data IN
-* @param  None
-* @retval osxMFX_input *iDataIN MotionFX Engine data IN
-*/
-osxMFX_input* MotionFX_manager_getDataIN(void)
-{  
-  return (osxMFX_input*)&iDataIN;
-}
-
-#else    /**  USE_SENSOR_FUSION_LIB   generate stub functions **/
-
-void MotionFX_manager_run(AxesRaw_t *ACC_Value, AxesRaw_t *GYR_Value, AxesRaw_t *MAG_Value, osxMFX_calibFactor & magOffset)
+ * @brief  Load calibration parameter from memory
+ * @param  dataSize length ot the data
+ * @param  data pointer to the data
+ * @retval (1) fail, (0) success
+ */
+char MotionFX_LoadMagCalFromNVM(unsigned short int dataSize, unsigned int *data)
 {
-}
-
-bool MotionFX_manager_init(bool DS3_OnBoard, osxMFX_calibFactor & magOffset) 
-{
-    return false;
-}
-
-void MotionFX_manager_start_6X(void)
-{ 
+#if (defined (MOTION_FX_STORE_CALIB_FLASH))
+  RecallCalibrationFromMemory(dataSize / 4, (uint32_t*) data);
+  return 0;
+#else
+  return 1;
+#endif
 }
 
-void MotionFX_manager_stop_6X(void)
+
+/**
+ * @brief  Save calibration parameter to memory
+ * @param  dataSize length ot the data
+ * @param  data pointer to the data
+ * @retval (1) fail, (0) success
+ */
+char MotionFX_SaveMagCalInNVM(unsigned short int dataSize, unsigned int *data)
 {
-}
-
-void MotionFX_manager_start_9X(void)
-{  
-}
-
-void MotionFX_manager_stop_9X(void)
-{
+#if (defined (MOTION_FX_STORE_CALIB_FLASH))
+  SaveCalibrationToMemory(dataSize / 4, (uint32_t*) data);
+  return 0;
+#else
+  return 1;
+#endif
 }
 
-osxMFX_output* MotionFX_manager_getDataOUT(void)
-{  
-    return NULL;
-}
+/**
+  * @}
+  */
 
-osxMFX_input* MotionFX_manager_getDataIN(void)
-{  
-    return NULL;    
-}
+/**
+  * @}
+  */
 
-#endif  /**   USE_SENSOR_FUSION_LIB   **/
-/******************* (C) COPYRIGHT 2015 STMicroelectronics *****END OF FILE****/
+/**
+  * @}
+  */
+#endif
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
\ No newline at end of file