File content as of revision 13:d00147d01d4e:

/**
******************************************************************************
* @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.
*
******************************************************************************
*/

/* 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****/