MotionFX_Manager.cpp

00001 /**
00002 ******************************************************************************
00003 * @file    MotionFX_Manager.c
00004 * @author  Central Lab
00005 * @version V1.1.0
00006 * @date    20-January-2015
00007 * @brief   This file includes sensor fusion interface functions
00008 ******************************************************************************
00009 * @attention
00010 *
00011 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
00012 *
00013 * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
00014 * You may not use this file except in compliance with the License.
00015 * You may obtain a copy of the License at:
00016 *
00017 *        http://www.st.com/software_license_agreement_liberty_v2
00018 *
00019 * Redistribution and use in source and binary forms, with or without modification,
00020 * are permitted provided that the following conditions are met:
00021 *   1. Redistributions of source code must retain the above copyright notice,
00022 *      this list of conditions and the following disclaimer.
00023 *   2. Redistributions in binary form must reproduce the above copyright notice,
00024 *      this list of conditions and the following disclaimer in the documentation
00025 *      and/or other materials provided with the distribution.
00026 *   3. Neither the name of STMicroelectronics nor the names of its contributors
00027 *      may be used to endorse or promote products derived from this software
00028 *      without specific prior written permission.
00029 *
00030 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00031 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00032 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00033 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00034 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00035 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00036 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00037 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00038 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00039 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00040 *
00041 ******************************************************************************
00042 */
00043 
00044 /* Includes ------------------------------------------------------------------*/
00045 #include "MotionFX_Manager.h"
00046 
00047 #ifdef USE_SENSOR_FUSION_LIB
00048 
00049 #include "osx_license.h"
00050 #define FROM_MG_TO_G    0.001
00051 #define FROM_MDPS_TO_DPS    0.001
00052 #define FROM_MGAUSS_TO_UT50 (0.1f/50.0f)
00053 
00054 /* SF Data structure */
00055 static volatile osxMFX_output iDataOUT;
00056 static volatile osxMFX_input iDataIN;
00057 
00058 /* Private define ------------------------------------------------------------*/
00059 #define SAMPLETODISCARD 15
00060 
00061 /* Private Variables -------------------------------------------------------------*/
00062 static volatile int sampleToDiscard = SAMPLETODISCARD;
00063 static float MotionFX_engine_deltatime = DELTATIMESENSORFUSION;
00064 static int discardedCount = 0;
00065 
00066 static osxMFX_knobs iKnobs;
00067 static osxMFX_knobs* ipKnobs;
00068 
00069 /**
00070 * @brief  Run sensor fusion Algo
00071 * @param  None
00072 * @retval None
00073 */
00074 void MotionFX_manager_run(AxesRaw_t *ACC_Value, AxesRaw_t *GYR_Value, AxesRaw_t *MAG_Value, osxMFX_calibFactor & magOffset)
00075 {
00076   iDataIN.gyro[0] = GYR_Value->AXIS_X  * FROM_MDPS_TO_DPS;
00077   iDataIN.gyro[1] = GYR_Value->AXIS_Y  * FROM_MDPS_TO_DPS;
00078   iDataIN.gyro[2] = GYR_Value->AXIS_Z  * FROM_MDPS_TO_DPS; 
00079   
00080   iDataIN.acc[0] = ACC_Value->AXIS_X * FROM_MG_TO_G;
00081   iDataIN.acc[1] = ACC_Value->AXIS_Y * FROM_MG_TO_G;
00082   iDataIN.acc[2] = ACC_Value->AXIS_Z * FROM_MG_TO_G;
00083   
00084   iDataIN.mag[0] = (MAG_Value->AXIS_X - magOffset.magOffX) * FROM_MGAUSS_TO_UT50;
00085   iDataIN.mag[1] = (MAG_Value->AXIS_Y - magOffset.magOffY) * FROM_MGAUSS_TO_UT50;
00086   iDataIN.mag[2] = (MAG_Value->AXIS_Z - magOffset.magOffZ) * FROM_MGAUSS_TO_UT50;
00087   
00088   if(discardedCount == sampleToDiscard){
00089     osx_MotionFX_propagate((osxMFX_output*)&iDataOUT, (osxMFX_input*)&iDataIN, MotionFX_engine_deltatime);
00090   
00091     osx_MotionFX_update((osxMFX_output*)&iDataOUT, (osxMFX_input*)&iDataIN, MotionFX_engine_deltatime, NULL); 
00092   } else {
00093     discardedCount++;
00094   }
00095 }
00096 
00097 
00098 /**
00099 * @brief  Initialise MotionFX engine
00100 * @param  None
00101 * @retval None
00102 */
00103 bool MotionFX_manager_init(bool DS3_OnBoard, osxMFX_calibFactor & magOffset) 
00104 {
00105   //  ST MotionFX Engine Initializations        
00106   ipKnobs =&iKnobs;
00107   
00108   if(!osx_MotionFX_initialize()) {
00109     printf("Error License authentication\n\r");
00110     return false;
00111   }
00112   else {
00113     char VersionMotionFX[36];
00114     osx_MotionFX_getLibVersion(VersionMotionFX);
00115     printf("Enabled %s\n\r",VersionMotionFX);
00116   }
00117   
00118   osx_MotionFX_compass_init();
00119 
00120   osx_MotionFX_getKnobs(ipKnobs);
00121   
00122   /* Values For DS0/DS3: These values must be not changed */
00123   ipKnobs->gbias_acc_th_sc_6X = 2*0.000765;
00124   ipKnobs->gbias_gyro_th_sc_6X = 2*0.002;
00125   ipKnobs->gbias_mag_th_sc_6X = 2*0.001500;
00126   
00127   ipKnobs->gbias_acc_th_sc_9X = 2*0.000765;
00128   ipKnobs->gbias_gyro_th_sc_9X = 2*0.002;
00129   ipKnobs->gbias_mag_th_sc_9X = 2*0.001500;
00130   
00131   
00132   if(DS3_OnBoard) {
00133     /* DS3 */
00134     ipKnobs->acc_orientation[0] ='n';
00135     ipKnobs->acc_orientation[1] ='w';
00136     ipKnobs->acc_orientation[2] ='u';
00137   
00138     ipKnobs->gyro_orientation[0] = 'n';
00139     ipKnobs->gyro_orientation[1] = 'w';
00140     ipKnobs->gyro_orientation[2] = 'u';   
00141   } else {
00142     /* DS0 */
00143     ipKnobs->acc_orientation[0] ='e';
00144     ipKnobs->acc_orientation[1] ='n';
00145     ipKnobs->acc_orientation[2] ='u';
00146   
00147     ipKnobs->gyro_orientation[0] = 'e';
00148     ipKnobs->gyro_orientation[1] = 'n';
00149     ipKnobs->gyro_orientation[2] = 'u';
00150   }
00151   ipKnobs->mag_orientation[0] = 's';
00152   ipKnobs->mag_orientation[1] = 'e';
00153   ipKnobs->mag_orientation[2] = 'u';
00154   
00155   ipKnobs->output_type = OSXMFX_ENGINE_OUTPUT_ENU;
00156   
00157   ipKnobs->LMode = 1;
00158   
00159   ipKnobs->modx = 1;    
00160   
00161   osx_MotionFX_setKnobs(ipKnobs);
00162   
00163   osx_MotionFX_enable_6X(OSXMFX_ENGINE_DISABLE);
00164   
00165   osx_MotionFX_enable_9X(OSXMFX_ENGINE_DISABLE);
00166   
00167   /* Number of Sample to Discard */
00168   sampleToDiscard = SAMPLETODISCARD;
00169   discardedCount = 0;
00170   MotionFX_engine_deltatime = DELTATIMESENSORFUSION;
00171   
00172   /* Reset MagnetoOffset */
00173   magOffset.magOffX = magOffset.magOffY= magOffset.magOffZ=0;
00174   
00175       return true;
00176 }
00177 
00178 /**
00179 * @brief  Starts 6 axes MotionFX engine
00180 * @param  None
00181 * @retval None
00182 */
00183 void MotionFX_manager_start_6X(void)
00184 {  
00185   osx_MotionFX_enable_6X(OSXMFX_ENGINE_ENABLE);
00186 }
00187 
00188 
00189 /**
00190 * @brief  Stops 6 axes MotionFX engine
00191 * @param  None
00192 * @retval None
00193 */
00194 void MotionFX_manager_stop_6X(void)
00195 {
00196   osx_MotionFX_enable_6X(OSXMFX_ENGINE_DISABLE);  
00197 }
00198 
00199 
00200 /**
00201 * @brief  Starts 9 axes MotionFX engine
00202 * @param  None
00203 * @retval None
00204 */
00205 void MotionFX_manager_start_9X(void)
00206 {  
00207   sampleToDiscard = SAMPLETODISCARD;
00208   osx_MotionFX_enable_9X(OSXMFX_ENGINE_ENABLE);
00209 }
00210 
00211 
00212 /**
00213 * @brief  Stops 9 axes MotionFX engine
00214 * @param  None
00215 * @retval None
00216 */
00217 void MotionFX_manager_stop_9X(void)
00218 {
00219   osx_MotionFX_enable_9X(OSXMFX_ENGINE_DISABLE); 
00220 }
00221 
00222 
00223 /**
00224 * @brief  Get MotionFX Engine data Out
00225 * @param  None
00226 * @retval osxMFX_output *iDataOUT MotionFX Engine data Out
00227 */
00228 osxMFX_output* MotionFX_manager_getDataOUT(void)
00229 {  
00230   return (osxMFX_output*)&iDataOUT;
00231 }
00232 
00233 
00234 /**
00235 * @brief  Get MotionFX Engine data IN
00236 * @param  None
00237 * @retval osxMFX_input *iDataIN MotionFX Engine data IN
00238 */
00239 osxMFX_input* MotionFX_manager_getDataIN(void)
00240 {  
00241   return (osxMFX_input*)&iDataIN;
00242 }
00243 
00244 #else    /**  USE_SENSOR_FUSION_LIB   generate stub functions **/
00245 
00246 void MotionFX_manager_run(AxesRaw_t *ACC_Value, AxesRaw_t *GYR_Value, AxesRaw_t *MAG_Value, osxMFX_calibFactor & magOffset)
00247 {
00248 }
00249 
00250 bool MotionFX_manager_init(bool DS3_OnBoard, osxMFX_calibFactor & magOffset) 
00251 {
00252     return false;
00253 }
00254 
00255 void MotionFX_manager_start_6X(void)
00256 { 
00257 }
00258 
00259 void MotionFX_manager_stop_6X(void)
00260 {
00261 }
00262 
00263 void MotionFX_manager_start_9X(void)
00264 {  
00265 }
00266 
00267 void MotionFX_manager_stop_9X(void)
00268 {
00269 }
00270 
00271 osxMFX_output* MotionFX_manager_getDataOUT(void)
00272 {  
00273     return NULL;
00274 }
00275 
00276 osxMFX_input* MotionFX_manager_getDataIN(void)
00277 {  
00278     return NULL;    
00279 }
00280 
00281 #endif  /**   USE_SENSOR_FUSION_LIB   **/
00282 /******************* (C) COPYRIGHT 2015 STMicroelectronics *****END OF FILE****/