Diff: MotionFX_Manager.cpp

Revision:

0:e93a11b4e044

Child:

9:2693f9ef8ff7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionFX_Manager.cpp	Mon Oct 12 15:16:57 2015 +0000
@@ -0,0 +1,282 @@
+/**
+******************************************************************************
+* @file    MotionFX_Manager.c
+* @author  Central Lab
+* @version V1.1.0
+* @date    20-Jannuary-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.
+*
+******************************************************************************
+*/
+
+/* Includes ------------------------------------------------------------------*/
+#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)
+
+/* SF Data structure */
+static volatile osxMFX_output iDataOUT;
+static volatile osxMFX_input iDataIN;
+
+/* Private define ------------------------------------------------------------*/
+#define SAMPLETODISCARD 15
+
+/* 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;
+
+/**
+* @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)
+{
+  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; 
+  
+  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;
+  
+  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 {
+    discardedCount++;
+  }
+}
+
+
+/**
+* @brief  Initialise MotionFX engine
+* @param  None
+* @retval None
+*/
+bool MotionFX_manager_init(bool DS3_OnBoard, osxMFX_calibFactor & magOffset) 
+{
+  //  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();
+
+  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  Starts 6 axes MotionFX engine
+* @param  None
+* @retval None
+*/
+void MotionFX_manager_start_6X(void)
+{  
+  osx_MotionFX_enable_6X(OSXMFX_ENGINE_ENABLE);
+}
+
+
+/**
+* @brief  Stops 6 axes MotionFX engine
+* @param  None
+* @retval None
+*/
+void MotionFX_manager_stop_6X(void)
+{
+  osx_MotionFX_enable_6X(OSXMFX_ENGINE_DISABLE);  
+}
+
+
+/**
+* @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)
+{
+  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;
+}
+
+
+/**
+* @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)
+{
+}
+
+bool MotionFX_manager_init(bool DS3_OnBoard, osxMFX_calibFactor & magOffset) 
+{
+    return false;
+}
+
+void MotionFX_manager_start_6X(void)
+{ 
+}
+
+void MotionFX_manager_stop_6X(void)
+{
+}
+
+void MotionFX_manager_start_9X(void)
+{  
+}
+
+void MotionFX_manager_stop_9X(void)
+{
+}
+
+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****/