X-NUCLEO-IKS01A1 Environmental/Motion sensors data transmitted via X-NUCLEO-IDB04A1 BLE board. Compatible with iOS/Android ST BlueMS V2.1 application.
Dependencies: BLE_API X_NUCLEO_IDB0XA1 X_NUCLEO_IKS01A1 mbed
Fork of Bluemicrosystem1 by
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>© 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****/
Generated on Sun Jul 17 2022 23:51:22 by 1.7.2