commit a321990a6bed75c43fbeb02c8ba8a87a1894be47 Author: fize Date: Tue Dec 6 17:34:02 2022 +0800 框架 diff --git a/.cproject b/.cproject new file mode 100644 index 0000000..6082574 --- /dev/null +++ b/.cproject @@ -0,0 +1,242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.mxproject b/.mxproject new file mode 100644 index 0000000..d2eff14 --- /dev/null +++ b/.mxproject @@ -0,0 +1,25 @@ +[PreviousLibFiles] +LibFiles=Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_spi.h;Drivers\STM32F1xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_def.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_rcc.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_rcc_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_bus.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_rcc.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_system.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_utils.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_gpio.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_gpio_ex.h;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio_ex.c;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_gpio.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_dma_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_dma.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_dma.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_cortex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_cortex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_pwr.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_pwr.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_flash.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_flash_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_exti.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_exti.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_tim.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_tim_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_tim.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_uart.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_usart.h;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_spi.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_dma.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_cortex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_pwr.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_exti.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_uart.c;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_spi.h;Drivers\STM32F1xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_def.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_rcc.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_rcc_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_bus.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_rcc.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_system.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_utils.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_gpio.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_gpio_ex.h;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio_ex.c;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_gpio.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_dma_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_dma.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_dma.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_cortex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_cortex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_pwr.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_pwr.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_flash.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_flash_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_exti.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_exti.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_tim.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_tim_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_tim.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_uart.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_usart.h;Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f103xb.h;Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f1xx.h;Drivers\CMSIS\Device\ST\STM32F1xx\Include\system_stm32f1xx.h;Drivers\CMSIS\Device\ST\STM32F1xx\Source\Templates\system_stm32f1xx.c;Drivers\CMSIS\Include\cmsis_armcc.h;Drivers\CMSIS\Include\cmsis_armclang.h;Drivers\CMSIS\Include\cmsis_compiler.h;Drivers\CMSIS\Include\cmsis_gcc.h;Drivers\CMSIS\Include\cmsis_iccarm.h;Drivers\CMSIS\Include\cmsis_version.h;Drivers\CMSIS\Include\core_armv8mbl.h;Drivers\CMSIS\Include\core_armv8mml.h;Drivers\CMSIS\Include\core_cm0.h;Drivers\CMSIS\Include\core_cm0plus.h;Drivers\CMSIS\Include\core_cm1.h;Drivers\CMSIS\Include\core_cm23.h;Drivers\CMSIS\Include\core_cm3.h;Drivers\CMSIS\Include\core_cm33.h;Drivers\CMSIS\Include\core_cm4.h;Drivers\CMSIS\Include\core_cm7.h;Drivers\CMSIS\Include\core_sc000.h;Drivers\CMSIS\Include\core_sc300.h;Drivers\CMSIS\Include\mpu_armv7.h;Drivers\CMSIS\Include\mpu_armv8.h;Drivers\CMSIS\Include\tz_context.h; + +[PreviousUsedCubeIDEFiles] +SourceFiles=Core\Src\main.c;Core\Src\stm32f1xx_it.c;Core\Src\stm32f1xx_hal_msp.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_spi.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_dma.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_cortex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_pwr.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_exti.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_uart.c;Drivers\CMSIS\Device\ST\STM32F1xx\Source\Templates\system_stm32f1xx.c;Core\Src\system_stm32f1xx.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_spi.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_dma.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_cortex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_pwr.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_exti.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_uart.c;Drivers\CMSIS\Device\ST\STM32F1xx\Source\Templates\system_stm32f1xx.c;Core\Src\system_stm32f1xx.c;;; +HeaderPath=Drivers\STM32F1xx_HAL_Driver\Inc;Drivers\STM32F1xx_HAL_Driver\Inc\Legacy;Drivers\CMSIS\Device\ST\STM32F1xx\Include;Drivers\CMSIS\Include;Core\Inc; +CDefines=USE_HAL_DRIVER;STM32F103xB;USE_HAL_DRIVER;USE_HAL_DRIVER; + +[PreviousGenFiles] +AdvancedFolderStructure=true +HeaderFileListSize=3 +HeaderFiles#0=..\Core\Inc\stm32f1xx_it.h +HeaderFiles#1=..\Core\Inc\stm32f1xx_hal_conf.h +HeaderFiles#2=..\Core\Inc\main.h +HeaderFolderListSize=1 +HeaderPath#0=..\Core\Inc +HeaderFiles=; +SourceFileListSize=3 +SourceFiles#0=..\Core\Src\stm32f1xx_it.c +SourceFiles#1=..\Core\Src\stm32f1xx_hal_msp.c +SourceFiles#2=..\Core\Src\main.c +SourceFolderListSize=1 +SourcePath#0=..\Core\Src +SourceFiles=; + diff --git a/.project b/.project new file mode 100644 index 0000000..1adf765 --- /dev/null +++ b/.project @@ -0,0 +1,32 @@ + + + IMU_DUAL + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + com.st.stm32cube.ide.mcu.MCUProjectNature + com.st.stm32cube.ide.mcu.MCUCubeProjectNature + org.eclipse.cdt.core.cnature + com.st.stm32cube.ide.mcu.MCUCubeIdeServicesRevAev2ProjectNature + com.st.stm32cube.ide.mcu.MCUAdvancedStructureProjectNature + com.st.stm32cube.ide.mcu.MCUSingleCpuProjectNature + com.st.stm32cube.ide.mcu.MCURootProjectNature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + + diff --git a/.settings/com.st.stm32cube.ide.mcu.sfr.prefs b/.settings/com.st.stm32cube.ide.mcu.sfr.prefs new file mode 100644 index 0000000..9a8cd0a --- /dev/null +++ b/.settings/com.st.stm32cube.ide.mcu.sfr.prefs @@ -0,0 +1,3 @@ +eclipse.preferences.version=1 +svd_custom_file_path= +svd_file_path=platform\:/plugin/com.st.stm32cube.ide.mcu.productdb.debug/resources/cmsis/STMicroelectronics_CMSIS_SVD/STM32F103.svd diff --git a/.settings/com.st.stm32cube.ide.mcu.sfrview.prefs b/.settings/com.st.stm32cube.ide.mcu.sfrview.prefs new file mode 100644 index 0000000..98a69fc --- /dev/null +++ b/.settings/com.st.stm32cube.ide.mcu.sfrview.prefs @@ -0,0 +1,2 @@ +eclipse.preferences.version=1 +sfrviewstate={"fFavorites"\:{"fLists"\:{}},"fProperties"\:{"fNodeProperties"\:{}}} diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml new file mode 100644 index 0000000..381264a --- /dev/null +++ b/.settings/language.settings.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.settings/org.eclipse.cdt.codan.core.prefs b/.settings/org.eclipse.cdt.codan.core.prefs new file mode 100644 index 0000000..2e279ce --- /dev/null +++ b/.settings/org.eclipse.cdt.codan.core.prefs @@ -0,0 +1,74 @@ +com.st.stm32cube.ide.mcu.ide.oss.source.checker.libnano.problem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Float formatting support\\")"} +eclipse.preferences.version=1 +org.eclipse.cdt.codan.checkers.errnoreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No return\\")",implicit\=>false} +org.eclipse.cdt.codan.checkers.errreturnvalue.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused return value\\")"} +org.eclipse.cdt.codan.checkers.localvarreturn=-Warning +org.eclipse.cdt.codan.checkers.localvarreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Returning the address of a local variable\\")"} +org.eclipse.cdt.codan.checkers.nocommentinside.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Nesting comments\\")"} +org.eclipse.cdt.codan.checkers.nolinecomment.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Line comments\\")"} +org.eclipse.cdt.codan.checkers.noreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No return value\\")",implicit\=>false} +org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Abstract class cannot be instantiated\\")"} +org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Ambiguous problem\\")"} +org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Assignment in condition\\")"} +org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Assignment to itself\\")"} +org.eclipse.cdt.codan.internal.checkers.BlacklistProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.BlacklistProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Function or method is blacklisted\\")",blacklist\=>()} +org.eclipse.cdt.codan.internal.checkers.CStyleCastProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.CStyleCastProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"C-Style cast instead of C++ cast\\")",checkMacro\=>true} +org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"No break at end of case\\")",no_break_comment\=>"no break",last_case_param\=>false,empty_case_param\=>false,enable_fallthrough_quickfix_param\=>false} +org.eclipse.cdt.codan.internal.checkers.CatchByReference.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Catching by reference is recommended\\")",unknown\=>false,exceptions\=>()} +org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Circular inheritance\\")"} +org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Class members should be properly initialized\\")",skip\=>true} +org.eclipse.cdt.codan.internal.checkers.CopyrightProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.CopyrightProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Lack of copyright information\\")",regex\=>".*Copyright.*"} +org.eclipse.cdt.codan.internal.checkers.DecltypeAutoProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid 'decltype(auto)' specifier\\")"} +org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Field cannot be resolved\\")"} +org.eclipse.cdt.codan.internal.checkers.FloatCompareProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.FloatCompareProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Direct float comparison\\")"} +org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Function cannot be resolved\\")"} +org.eclipse.cdt.codan.internal.checkers.GotoStatementProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.GotoStatementProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Goto statement used\\")"} +org.eclipse.cdt.codan.internal.checkers.InvalidArguments.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid arguments\\")"} +org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid template argument\\")"} +org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Label statement not found\\")"} +org.eclipse.cdt.codan.internal.checkers.MagicNumberProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.MagicNumberProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Avoid magic numbers\\")",checkArray\=>true,checkOperatorParen\=>true,exceptions\=>(1,0,-1,2,1.0,0.0,-1.0)} +org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Member declaration not found\\")"} +org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Method cannot be resolved\\")"} +org.eclipse.cdt.codan.internal.checkers.MissCaseProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.MissCaseProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Missing cases in switch\\")"} +org.eclipse.cdt.codan.internal.checkers.MissDefaultProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.MissDefaultProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Missing default in switch\\")",defaultWithAllEnums\=>false} +org.eclipse.cdt.codan.internal.checkers.MissReferenceProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.MissReferenceProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Missing reference return value in assignment operator\\")"} +org.eclipse.cdt.codan.internal.checkers.MissSelfCheckProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.MissSelfCheckProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Missing self check in assignment operator\\")"} +org.eclipse.cdt.codan.internal.checkers.MultipleDeclarationsProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.MultipleDeclarationsProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Multiple variable declaration\\")"} +org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Name convention for function\\")",pattern\=>"^[a-z]",macro\=>true,exceptions\=>()} +org.eclipse.cdt.codan.internal.checkers.NoDiscardProblem=Warning +org.eclipse.cdt.codan.internal.checkers.NoDiscardProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Return value not evaluated\\")",macro\=>true} +org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Class has a virtual method and non-virtual destructor\\")"} +org.eclipse.cdt.codan.internal.checkers.OverloadProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid overload\\")"} +org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid redeclaration\\")"} +org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Invalid redefinition\\")"} +org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Return with parenthesis\\")"} +org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Format String Vulnerability\\")"} +org.eclipse.cdt.codan.internal.checkers.ShallowCopyProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.ShallowCopyProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Miss copy constructor or assignment operator\\")",onlynew\=>false} +org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Statement has no effect\\")",macro\=>true,exceptions\=>()} +org.eclipse.cdt.codan.internal.checkers.StaticVariableInHeaderProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.StaticVariableInHeaderProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Static variable in header file\\")"} +org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Suggested parenthesis around expression\\")",paramNot\=>false} +org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Suspicious semicolon\\")",else\=>false,afterelse\=>false} +org.eclipse.cdt.codan.internal.checkers.SymbolShadowingProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.SymbolShadowingProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Symbol shadowing\\")",paramFuncParameters\=>true} +org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Type cannot be resolved\\")"} +org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused function declaration\\")",macro\=>true} +org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused static function\\")",macro\=>true} +org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Unused variable declaration in file scope\\")",macro\=>true,exceptions\=>("@(\#)","$Id")} +org.eclipse.cdt.codan.internal.checkers.UsingInHeaderProblem=-Warning +org.eclipse.cdt.codan.internal.checkers.UsingInHeaderProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Using directive in header\\")"} +org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Symbol is not resolved\\")"} +org.eclipse.cdt.codan.internal.checkers.VirtualMethodCallProblem=-Error +org.eclipse.cdt.codan.internal.checkers.VirtualMethodCallProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},suppression_comment\=>"@suppress(\\"Virtual method call in constructor/destructor\\")"} diff --git a/.settings/org.eclipse.cdt.core.prefs b/.settings/org.eclipse.cdt.core.prefs new file mode 100644 index 0000000..c8ec5df --- /dev/null +++ b/.settings/org.eclipse.cdt.core.prefs @@ -0,0 +1,6 @@ +doxygen/doxygen_new_line_after_brief=true +doxygen/doxygen_use_brief_tag=false +doxygen/doxygen_use_javadoc_tags=true +doxygen/doxygen_use_pre_tag=false +doxygen/doxygen_use_structural_commands=false +eclipse.preferences.version=1 diff --git a/.settings/stm32cubeide.project.prefs b/.settings/stm32cubeide.project.prefs new file mode 100644 index 0000000..4554f09 --- /dev/null +++ b/.settings/stm32cubeide.project.prefs @@ -0,0 +1,4 @@ +66BE74F758C12D739921AEA421D593D3=2 +8DF89ED150041C4CBC7CB9A9CAA90856=E08A4A42B43BAA3E4C79F58D26E83ECC +DC22A860405A8BF2F2C095E5B6529F12=E08A4A42B43BAA3E4C79F58D26E83ECC +eclipse.preferences.version=1 diff --git a/ASM330LHH/asm330lhh_reg.c b/ASM330LHH/asm330lhh_reg.c new file mode 100644 index 0000000..71738fa --- /dev/null +++ b/ASM330LHH/asm330lhh_reg.c @@ -0,0 +1,4887 @@ +/** + ****************************************************************************** + * @file asm330lhh_reg.c + * @author Sensors Software Solution Team + * @brief ASM330LHH driver file + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2021 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +#include "asm330lhh_reg.h" + +/** + * @defgroup ASM330LHH + * @brief This file provides a set of functions needed to drive the + * asm330lhh enhanced inertial module. + * @{ + * + */ + +/** + * @defgroup ASM330LHH_Interfaces_Functions + * @brief This section provide a set of functions used to read and + * write a generic register of the device. + * MANDATORY: return 0 -> no Error. + * @{ + * + */ + +/** + * @brief Read generic device register + * + * @param ctx read / write interface definitions(ptr) + * @param reg register to read + * @param data pointer to buffer that store the data read(ptr) + * @param len number of consecutive register to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t asm330lhh_read_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len) +{ + int32_t ret; + ret = ctx->read_reg(ctx->handle, reg, data, len); + + return ret; +} + +/** + * @brief Write generic device register + * + * @param ctx read / write interface definitions(ptr) + * @param reg register to write + * @param data pointer to data to write in register reg(ptr) + * @param len number of consecutive register to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t asm330lhh_write_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len) +{ + int32_t ret; + + ret = ctx->write_reg(ctx->handle, reg, data, len); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_Sensitivity + * @brief These functions convert raw-data into engineering units. + * @{ + * + */ + +float_t asm330lhh_from_fs2g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.061f); +} + +float_t asm330lhh_from_fs4g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.122f); +} + +float_t asm330lhh_from_fs8g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.244f); +} + +float_t asm330lhh_from_fs16g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.488f); +} + +float_t asm330lhh_from_fs125dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 4.375f); +} + +float_t asm330lhh_from_fs250dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 8.75f); +} + +float_t asm330lhh_from_fs500dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 17.50f); +} + +float_t asm330lhh_from_fs1000dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 35.0f); +} + +float_t asm330lhh_from_fs2000dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 70.0f); +} + +float_t asm330lhh_from_fs4000dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 140.0f); +} + +float_t asm330lhh_from_lsb_to_celsius(int16_t lsb) +{ + return (((float_t)lsb / 256.0f) + 25.0f); +} + +float_t asm330lhh_from_lsb_to_nsec(int32_t lsb) +{ + return ((float_t)lsb * 25000.0f); +} + +/** + * @} + * + */ + +/** + * @defgroup LSM9DS1_Data_generation + * @brief This section groups all the functions concerning data + * generation + * @{ + * + */ + +/** + * @brief Accelerometer full-scale selection[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of fs_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_full_scale_set(stmdev_ctx_t *ctx, + asm330lhh_fs_xl_t val) +{ + asm330lhh_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.fs_xl = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer full-scale selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of fs_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_full_scale_get(stmdev_ctx_t *ctx, + asm330lhh_fs_xl_t *val) +{ + asm330lhh_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.fs_xl) + { + case ASM330LHH_2g: + *val = ASM330LHH_2g; + break; + + case ASM330LHH_16g: + *val = ASM330LHH_16g; + break; + + case ASM330LHH_4g: + *val = ASM330LHH_4g; + break; + + case ASM330LHH_8g: + *val = ASM330LHH_8g; + break; + + default: + *val = ASM330LHH_2g; + break; + } + + return ret; +} + +/** + * @brief Accelerometer UI data rate selection.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of odr_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_data_rate_set(stmdev_ctx_t *ctx, + asm330lhh_odr_xl_t val) +{ + asm330lhh_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.odr_xl = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer UI data rate selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of odr_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_data_rate_get(stmdev_ctx_t *ctx, + asm330lhh_odr_xl_t *val) +{ + asm330lhh_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.odr_xl) + { + case ASM330LHH_XL_ODR_OFF: + *val = ASM330LHH_XL_ODR_OFF; + break; + + case ASM330LHH_XL_ODR_12Hz5: + *val = ASM330LHH_XL_ODR_12Hz5; + break; + + case ASM330LHH_XL_ODR_26Hz: + *val = ASM330LHH_XL_ODR_26Hz; + break; + + case ASM330LHH_XL_ODR_52Hz: + *val = ASM330LHH_XL_ODR_52Hz; + break; + + case ASM330LHH_XL_ODR_104Hz: + *val = ASM330LHH_XL_ODR_104Hz; + break; + + case ASM330LHH_XL_ODR_208Hz: + *val = ASM330LHH_XL_ODR_208Hz; + break; + + case ASM330LHH_XL_ODR_417Hz: + *val = ASM330LHH_XL_ODR_417Hz; + break; + + case ASM330LHH_XL_ODR_833Hz: + *val = ASM330LHH_XL_ODR_833Hz; + break; + + case ASM330LHH_XL_ODR_1667Hz: + *val = ASM330LHH_XL_ODR_1667Hz; + break; + + case ASM330LHH_XL_ODR_3333Hz: + *val = ASM330LHH_XL_ODR_3333Hz; + break; + + case ASM330LHH_XL_ODR_6667Hz: + *val = ASM330LHH_XL_ODR_6667Hz; + break; + + default: + *val = ASM330LHH_XL_ODR_OFF; + break; + } + + return ret; +} + +/** + * @brief Gyroscope UI chain full-scale selection.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of fs_g in reg CTRL2_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_full_scale_set(stmdev_ctx_t *ctx, + asm330lhh_fs_g_t val) +{ + asm330lhh_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + + if (ret == 0) + { + ctrl2_g.fs_g = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + } + + return ret; +} + +/** + * @brief Gyroscope UI chain full-scale selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of fs_g in reg CTRL2_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_full_scale_get(stmdev_ctx_t *ctx, + asm330lhh_fs_g_t *val) +{ + asm330lhh_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + + switch (ctrl2_g.fs_g) + { + case ASM330LHH_125dps: + *val = ASM330LHH_125dps; + break; + + case ASM330LHH_250dps: + *val = ASM330LHH_250dps; + break; + + case ASM330LHH_500dps: + *val = ASM330LHH_500dps; + break; + + case ASM330LHH_1000dps: + *val = ASM330LHH_1000dps; + break; + + case ASM330LHH_2000dps: + *val = ASM330LHH_2000dps; + break; + + case ASM330LHH_4000dps: + *val = ASM330LHH_4000dps; + break; + + default: + *val = ASM330LHH_125dps; + break; + } + + return ret; +} + +/** + * @brief Gyroscope data rate.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of odr_g in reg CTRL2_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_data_rate_set(stmdev_ctx_t *ctx, + asm330lhh_odr_g_t val) +{ + asm330lhh_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + + if (ret == 0) + { + ctrl2_g.odr_g = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + } + + return ret; +} + +/** + * @brief Gyroscope data rate.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of odr_g in reg CTRL2_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_data_rate_get(stmdev_ctx_t *ctx, + asm330lhh_odr_g_t *val) +{ + asm330lhh_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + + switch (ctrl2_g.odr_g) + { + case ASM330LHH_GY_ODR_OFF: + *val = ASM330LHH_GY_ODR_OFF; + break; + + case ASM330LHH_GY_ODR_12Hz5: + *val = ASM330LHH_GY_ODR_12Hz5; + break; + + case ASM330LHH_GY_ODR_26Hz: + *val = ASM330LHH_GY_ODR_26Hz; + break; + + case ASM330LHH_GY_ODR_52Hz: + *val = ASM330LHH_GY_ODR_52Hz; + break; + + case ASM330LHH_GY_ODR_104Hz: + *val = ASM330LHH_GY_ODR_104Hz; + break; + + case ASM330LHH_GY_ODR_208Hz: + *val = ASM330LHH_GY_ODR_208Hz; + break; + + case ASM330LHH_GY_ODR_417Hz: + *val = ASM330LHH_GY_ODR_417Hz; + break; + + case ASM330LHH_GY_ODR_833Hz: + *val = ASM330LHH_GY_ODR_833Hz; + break; + + case ASM330LHH_GY_ODR_1667Hz: + *val = ASM330LHH_GY_ODR_1667Hz; + break; + + case ASM330LHH_GY_ODR_3333Hz: + *val = ASM330LHH_GY_ODR_3333Hz; + break; + + case ASM330LHH_GY_ODR_6667Hz: + *val = ASM330LHH_GY_ODR_6667Hz; + break; + + default: + *val = ASM330LHH_GY_ODR_OFF; + break; + } + + return ret; +} + +/** + * @brief Block data update.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of bdu in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_block_data_update_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.bdu = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Block data update.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of bdu in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_block_data_update_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + *val = ctrl3_c.bdu; + + return ret; +} + +/** + * @brief Weight of XL user offset bits of registers X_OFS_USR (73h), + * Y_OFS_USR (74h), Z_OFS_USR (75h).[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of usr_off_w in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_offset_weight_set(stmdev_ctx_t *ctx, + asm330lhh_usr_off_w_t val) +{ + asm330lhh_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.usr_off_w = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + } + + return ret; +} + +/** + * @brief Weight of XL user offset bits of registers X_OFS_USR (73h), + * Y_OFS_USR (74h), Z_OFS_USR (75h).[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of usr_off_w in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_offset_weight_get(stmdev_ctx_t *ctx, + asm330lhh_usr_off_w_t *val) +{ + asm330lhh_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + switch (ctrl6_c.usr_off_w) + { + case ASM330LHH_LSb_1mg: + *val = ASM330LHH_LSb_1mg; + break; + + case ASM330LHH_LSb_16mg: + *val = ASM330LHH_LSb_16mg; + break; + + default: + *val = ASM330LHH_LSb_1mg; + break; + } + + return ret; +} + +/** + * @brief Read all the interrupt flag of the device. + *[get] + * @param ctx Read / write interface definitions.(ptr) + * @param val Get registers ALL_INT_SRC; WAKE_UP_SRC; + * D6D_SRC; STATUS_REG; + * EMB_FUNC_STATUS; FSM_STATUS_A/B + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_all_sources_get(stmdev_ctx_t *ctx, + asm330lhh_all_sources_t *val) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_ALL_INT_SRC, + (uint8_t *)&val->all_int_src, 1); + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_SRC, + (uint8_t *)&val->wake_up_src, 1); + } + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_D6D_SRC, + (uint8_t *)&val->d6d_src, 1); + } + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_STATUS_REG, + (uint8_t *)&val->status_reg, 1); + } + + return ret; +} + +/** + * @brief The STATUS_REG register is read by the primary interface.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get register STATUS_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_status_reg_get(stmdev_ctx_t *ctx, + asm330lhh_status_reg_t *val) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_STATUS_REG, (uint8_t *) val, 1); + + return ret; +} + +/** + * @brief Accelerometer new data available.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of xlda in reg STATUS_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_status_reg_t status_reg; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = status_reg.xlda; + + return ret; +} + +/** + * @brief Gyroscope new data available.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of gda in reg STATUS_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_status_reg_t status_reg; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = status_reg.gda; + + return ret; +} + +/** + * @brief Temperature new data available.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of tda in reg STATUS_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_temp_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_status_reg_t status_reg; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = status_reg.tda; + + return ret; +} + +/** + * @brief Accelerometer X-axis user offset correction expressed in two’s + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_x_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = asm330lhh_write_reg(ctx, ASM330LHH_X_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer X-axis user offset correction expressed in two’s + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_x_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_X_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer Y-axis user offset correction expressed in two’s + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_y_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = asm330lhh_write_reg(ctx, ASM330LHH_Y_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer Y-axis user offset correction expressed in two’s + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_y_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_Y_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer Z-axis user offset correction expressed in two’s + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_z_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = asm330lhh_write_reg(ctx, ASM330LHH_Z_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer X-axis user offset correction expressed in two’s + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_z_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_Z_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Enables user offset on out.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of usr_off_on_out in reg CTRL7_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.usr_off_on_out = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + } + + return ret; +} + +/** + * @brief Get user offset on out flag.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get values of usr_off_on_out in reg CTRL7_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + *val = ctrl7_g.usr_off_on_out; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_Timestamp + * @brief This section groups all the functions that manage the + * timestamp generation. + * @{ + * + */ + +/** + * @brief Reset timestamp counter.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_timestamp_rst(stmdev_ctx_t *ctx) +{ + uint8_t rst_val = 0xAA; + return asm330lhh_write_reg(ctx, ASM330LHH_TIMESTAMP2, &rst_val, 1); +} + +/** + * @brief Enables timestamp counter.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of timestamp_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_timestamp_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.timestamp_en = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Enables timestamp counter.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of timestamp_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + *val = ctrl10_c.timestamp_en; + + return ret; +} + +/** + * @brief Timestamp first data output register (r). + * The value is expressed as a 32-bit word and the bit resolution + * is 25 μs.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val) +{ + uint8_t buff[4]; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_TIMESTAMP0, buff, 4); + *val = buff[3]; + *val = (*val * 256U) + buff[2]; + *val = (*val * 256U) + buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_Data output + * @brief This section groups all the data output functions. + * @{ + * + */ + +/** + * @brief Circular burst-mode (rounding) read of the output registers.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of rounding in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_rounding_mode_set(stmdev_ctx_t *ctx, + asm330lhh_rounding_t val) +{ + asm330lhh_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.rounding = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Gyroscope UI chain full-scale selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of rounding in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_rounding_mode_get(stmdev_ctx_t *ctx, + asm330lhh_rounding_t *val) +{ + asm330lhh_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.rounding) + { + case ASM330LHH_NO_ROUND: + *val = ASM330LHH_NO_ROUND; + break; + + case ASM330LHH_ROUND_XL: + *val = ASM330LHH_ROUND_XL; + break; + + case ASM330LHH_ROUND_GY: + *val = ASM330LHH_ROUND_GY; + break; + + case ASM330LHH_ROUND_GY_XL: + *val = ASM330LHH_ROUND_GY_XL; + break; + + default: + *val = ASM330LHH_NO_ROUND; + break; + } + + return ret; +} + +/** + * @brief Temperature data output register (r). + * L and H registers together express a 16-bit word in two’s + * complement.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_OUT_TEMP_L, buff, 2); + *val = (int16_t)buff[1]; + *val = (*val * 256) + (int16_t)buff[0]; + + return ret; +} + +/** + * @brief Angular rate sensor. The value is expressed as a 16-bit + * word in two’s complement.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_angular_rate_raw_get(stmdev_ctx_t *ctx, + int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_OUTX_L_G, buff, 6); + val[0] = (int16_t)buff[1]; + val[0] = (val[0] * 256) + (int16_t)buff[0]; + val[1] = (int16_t)buff[3]; + val[1] = (val[1] * 256) + (int16_t)buff[2]; + val[2] = (int16_t)buff[5]; + val[2] = (val[2] * 256) + (int16_t)buff[4]; + + return ret; +} + +/** + * @brief Linear acceleration output register. The value is expressed as a + * 16-bit word in two’s complement.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_acceleration_raw_get(stmdev_ctx_t *ctx, + int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_OUTX_L_A, buff, 6); + val[0] = (int16_t)buff[1]; + val[0] = (val[0] * 256) + (int16_t)buff[0]; + val[1] = (int16_t)buff[3]; + val[1] = (val[1] * 256) + (int16_t)buff[2]; + val[2] = (int16_t)buff[5]; + val[2] = (val[2] * 256) + (int16_t)buff[4]; + + return ret; +} + +/** + * @brief FIFO data output.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_out_raw_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_DATA_OUT_X_L, val, 6); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_common + * @brief This section groups common useful functions. + * @{ + * + */ + +/** + * @brief DEVICE_CONF bit configuration[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of device_conf in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_device_conf_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.device_conf = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEVICE_CONF bit configuration[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of device_conf in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_device_conf_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + *val = ctrl9_xl.device_conf; + + return ret; +} + +/** + * @brief Difference in percentage of the effective ODR (and timestamp rate) + * with respect to the typical.[set] + * Step: 0.15%. 8-bit format, 2's complement. + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of freq_fine in reg INTERNAL_FREQ_FINE + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_odr_cal_reg_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_internal_freq_fine_t internal_freq_fine; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INTERNAL_FREQ_FINE, + (uint8_t *)&internal_freq_fine, 1); + + if (ret == 0) + { + internal_freq_fine.freq_fine = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_INTERNAL_FREQ_FINE, + (uint8_t *)&internal_freq_fine, 1); + } + + return ret; +} + +/** + * @brief Difference in percentage of the effective ODR (and timestamp rate) + * with respect to the typical.[get] + * Step: 0.15%. 8-bit format, 2's complement. + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of freq_fine in reg INTERNAL_FREQ_FINE + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_odr_cal_reg_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_internal_freq_fine_t internal_freq_fine; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INTERNAL_FREQ_FINE, + (uint8_t *)&internal_freq_fine, 1); + *val = internal_freq_fine.freq_fine; + + return ret; +} + +/** + * @brief Data-ready pulsed / letched mode.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of dataready_pulsed in + * reg COUNTER_BDR_REG1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_data_ready_mode_set(stmdev_ctx_t *ctx, + asm330lhh_dataready_pulsed_t val) +{ + asm330lhh_counter_bdr_reg1_t counter_bdr_reg1; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + + if (ret == 0) + { + counter_bdr_reg1.dataready_pulsed = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + } + + return ret; +} + +/** + * @brief Data-ready pulsed / letched mode.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of dataready_pulsed in + * reg COUNTER_BDR_REG1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_data_ready_mode_get(stmdev_ctx_t *ctx, + asm330lhh_dataready_pulsed_t *val) +{ + asm330lhh_counter_bdr_reg1_t counter_bdr_reg1; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + + switch (counter_bdr_reg1.dataready_pulsed) + { + case ASM330LHH_DRDY_LATCHED: + *val = ASM330LHH_DRDY_LATCHED; + break; + + case ASM330LHH_DRDY_PULSED: + *val = ASM330LHH_DRDY_PULSED; + break; + + default: + *val = ASM330LHH_DRDY_LATCHED; + break; + } + + return ret; +} + +/** + * @brief Device Who am I.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_device_id_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WHO_AM_I, buff, 1); + + return ret; +} + +/** + * @brief Software reset. Restore the default values in user registers.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sw_reset in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_reset_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.sw_reset = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Software reset. Restore the default values in user registers.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sw_reset in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_reset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + *val = ctrl3_c.sw_reset; + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple byte + * access with a serial interface.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of if_inc in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.if_inc = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple byte + * access with a serial interface.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of if_inc in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + *val = ctrl3_c.if_inc; + + return ret; +} + +/** + * @brief Reboot memory content. Reload the calibration parameters.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of boot in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_boot_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.boot = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Reboot memory content. Reload the calibration parameters.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of boot in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_boot_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + *val = ctrl3_c.boot; + + return ret; +} + + + +/** + * @brief Linear acceleration sensor self-test enable.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of st_xl in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_self_test_set(stmdev_ctx_t *ctx, + asm330lhh_st_xl_t val) +{ + asm330lhh_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.st_xl = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Linear acceleration sensor self-test enable.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of st_xl in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_self_test_get(stmdev_ctx_t *ctx, + asm330lhh_st_xl_t *val) +{ + asm330lhh_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.st_xl) + { + case ASM330LHH_XL_ST_DISABLE: + *val = ASM330LHH_XL_ST_DISABLE; + break; + + case ASM330LHH_XL_ST_POSITIVE: + *val = ASM330LHH_XL_ST_POSITIVE; + break; + + case ASM330LHH_XL_ST_NEGATIVE: + *val = ASM330LHH_XL_ST_NEGATIVE; + break; + + default: + *val = ASM330LHH_XL_ST_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Angular rate sensor self-test enable.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of st_g in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_self_test_set(stmdev_ctx_t *ctx, + asm330lhh_st_g_t val) +{ + asm330lhh_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.st_g = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Angular rate sensor self-test enable.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of st_g in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_self_test_get(stmdev_ctx_t *ctx, + asm330lhh_st_g_t *val) +{ + asm330lhh_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.st_g) + { + case ASM330LHH_GY_ST_DISABLE: + *val = ASM330LHH_GY_ST_DISABLE; + break; + + case ASM330LHH_GY_ST_POSITIVE: + *val = ASM330LHH_GY_ST_POSITIVE; + break; + + case ASM330LHH_GY_ST_NEGATIVE: + *val = ASM330LHH_GY_ST_NEGATIVE; + break; + + default: + *val = ASM330LHH_GY_ST_DISABLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_filters + * @brief This section group all the functions concerning the + * filters configuration + * @{ + * + */ + +/** + * @brief Accelerometer output from LPF2 filtering stage selection.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of lpf2_xl_en in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_filter_lp2_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.lpf2_xl_en = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer output from LPF2 filtering stage selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of lpf2_xl_en in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_filter_lp2_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + *val = ctrl1_xl.lpf2_xl_en; + + return ret; +} + +/** + * @brief Enables gyroscope digital LPF1 if auxiliary SPI is disabled; + * the bandwidth can be selected through FTYPE [2:0] in CTRL6_C.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of lpf1_sel_g in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_filter_lp1_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.lpf1_sel_g = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope digital LPF1 if auxiliary SPI is disabled; + * the bandwidth can be selected through FTYPE [2:0] in CTRL6_C.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of lpf1_sel_g in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_filter_lp1_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = ctrl4_c.lpf1_sel_g; + + return ret; +} + +/** + * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends + * (XL and Gyro independently masked).[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of drdy_mask in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_filter_settling_mask_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.drdy_mask = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends + * (XL and Gyro independently masked).[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of drdy_mask in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_filter_settling_mask_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = ctrl4_c.drdy_mask; + + return ret; +} + +/** + * @brief Gyroscope low pass filter 1 bandwidth.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of ftype in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + asm330lhh_ftype_t val) +{ + asm330lhh_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.ftype = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + } + + return ret; +} + +/** + * @brief Gyroscope low pass filter 1 bandwidth.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of ftype in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + asm330lhh_ftype_t *val) +{ + asm330lhh_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + switch (ctrl6_c.ftype) + { + case ASM330LHH_ULTRA_LIGHT: + *val = ASM330LHH_ULTRA_LIGHT; + break; + + case ASM330LHH_VERY_LIGHT: + *val = ASM330LHH_VERY_LIGHT; + break; + + case ASM330LHH_LIGHT: + *val = ASM330LHH_LIGHT; + break; + + case ASM330LHH_MEDIUM: + *val = ASM330LHH_MEDIUM; + break; + + case ASM330LHH_STRONG: + *val = ASM330LHH_STRONG; + break; + + case ASM330LHH_VERY_STRONG: + *val = ASM330LHH_VERY_STRONG; + break; + + case ASM330LHH_AGGRESSIVE: + *val = ASM330LHH_AGGRESSIVE; + break; + + case ASM330LHH_XTREME: + *val = ASM330LHH_XTREME; + break; + + default: + *val = ASM330LHH_ULTRA_LIGHT; + break; + } + + return ret; +} + +/** + * @brief Low pass filter 2 on 6D function selection.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of low_pass_on_6d in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_lp2_on_6d_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.low_pass_on_6d = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief Low pass filter 2 on 6D function selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of low_pass_on_6d in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_lp2_on_6d_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + *val = ctrl8_xl.low_pass_on_6d; + + return ret; +} + +/** + * @brief Accelerometer slope filter / high-pass filter selection + * on output.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of hp_slope_xl_en in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_hp_path_on_out_set(stmdev_ctx_t *ctx, + asm330lhh_hp_slope_xl_en_t val) +{ + asm330lhh_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.hp_slope_xl_en = (((uint8_t)val & 0x10U) >> 4); + ctrl8_xl.hp_ref_mode_xl = (((uint8_t)val & 0x20U) >> 5); + ctrl8_xl.hpcf_xl = (uint8_t)val & 0x07U; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer slope filter / high-pass filter selection on + * output.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of hp_slope_xl_en in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_hp_path_on_out_get(stmdev_ctx_t *ctx, + asm330lhh_hp_slope_xl_en_t *val) +{ + asm330lhh_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + switch (((ctrl8_xl.hp_ref_mode_xl << 5) + (ctrl8_xl.hp_slope_xl_en << + 4) + + ctrl8_xl.hpcf_xl)) + { + case ASM330LHH_HP_PATH_DISABLE_ON_OUT: + *val = ASM330LHH_HP_PATH_DISABLE_ON_OUT; + break; + + case ASM330LHH_SLOPE_ODR_DIV_4: + *val = ASM330LHH_SLOPE_ODR_DIV_4; + break; + + case ASM330LHH_HP_ODR_DIV_10: + *val = ASM330LHH_HP_ODR_DIV_10; + break; + + case ASM330LHH_HP_ODR_DIV_20: + *val = ASM330LHH_HP_ODR_DIV_20; + break; + + case ASM330LHH_HP_ODR_DIV_45: + *val = ASM330LHH_HP_ODR_DIV_45; + break; + + case ASM330LHH_HP_ODR_DIV_100: + *val = ASM330LHH_HP_ODR_DIV_100; + break; + + case ASM330LHH_HP_ODR_DIV_200: + *val = ASM330LHH_HP_ODR_DIV_200; + break; + + case ASM330LHH_HP_ODR_DIV_400: + *val = ASM330LHH_HP_ODR_DIV_400; + break; + + case ASM330LHH_HP_ODR_DIV_800: + *val = ASM330LHH_HP_ODR_DIV_800; + break; + + case ASM330LHH_HP_REF_MD_ODR_DIV_10: + *val = ASM330LHH_HP_REF_MD_ODR_DIV_10; + break; + + case ASM330LHH_HP_REF_MD_ODR_DIV_20: + *val = ASM330LHH_HP_REF_MD_ODR_DIV_20; + break; + + case ASM330LHH_HP_REF_MD_ODR_DIV_45: + *val = ASM330LHH_HP_REF_MD_ODR_DIV_45; + break; + + case ASM330LHH_HP_REF_MD_ODR_DIV_100: + *val = ASM330LHH_HP_REF_MD_ODR_DIV_100; + break; + + case ASM330LHH_HP_REF_MD_ODR_DIV_200: + *val = ASM330LHH_HP_REF_MD_ODR_DIV_200; + break; + + case ASM330LHH_HP_REF_MD_ODR_DIV_400: + *val = ASM330LHH_HP_REF_MD_ODR_DIV_400; + break; + + case ASM330LHH_HP_REF_MD_ODR_DIV_800: + *val = ASM330LHH_HP_REF_MD_ODR_DIV_800; + break; + + case ASM330LHH_LP_ODR_DIV_10: + *val = ASM330LHH_LP_ODR_DIV_10; + break; + + case ASM330LHH_LP_ODR_DIV_20: + *val = ASM330LHH_LP_ODR_DIV_20; + break; + + case ASM330LHH_LP_ODR_DIV_45: + *val = ASM330LHH_LP_ODR_DIV_45; + break; + + case ASM330LHH_LP_ODR_DIV_100: + *val = ASM330LHH_LP_ODR_DIV_100; + break; + + case ASM330LHH_LP_ODR_DIV_200: + *val = ASM330LHH_LP_ODR_DIV_200; + break; + + case ASM330LHH_LP_ODR_DIV_400: + *val = ASM330LHH_LP_ODR_DIV_400; + break; + + case ASM330LHH_LP_ODR_DIV_800: + *val = ASM330LHH_LP_ODR_DIV_800; + break; + + default: + *val = ASM330LHH_HP_PATH_DISABLE_ON_OUT; + break; + } + + return ret; +} + +/** + * @brief Enables accelerometer LPF2 and HPF fast-settling mode. + * The filter sets the second samples after writing this bit. + * Active only during device exit from powerdown mode.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of fastsettl_mode_xl in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.fastsettl_mode_xl = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief Enables accelerometer LPF2 and HPF fast-settling mode. + * The filter sets the second samples after writing + * this bit. Active only during device exit from powerdown mode.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of fastsettl_mode_xl in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_fast_settling_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + *val = ctrl8_xl.fastsettl_mode_xl; + + return ret; +} + +/** + * @brief HPF or SLOPE filter selection on wake-up and Activity/Inactivity + * functions.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of slope_fds in reg INT_CFG0 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_hp_path_internal_set(stmdev_ctx_t *ctx, + asm330lhh_slope_fds_t val) +{ + asm330lhh_int_cfg0_t int_cfg0; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + + if (ret == 0) + { + int_cfg0.slope_fds = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + } + + return ret; +} + +/** + * @brief HPF or SLOPE filter selection on wake-up and Activity/Inactivity + * functions.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of slope_fds in reg INT_CFG0 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_hp_path_internal_get(stmdev_ctx_t *ctx, + asm330lhh_slope_fds_t *val) +{ + asm330lhh_int_cfg0_t int_cfg0; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + + switch (int_cfg0.slope_fds) + { + case ASM330LHH_USE_SLOPE: + *val = ASM330LHH_USE_SLOPE; + break; + + case ASM330LHH_USE_HPF: + *val = ASM330LHH_USE_HPF; + break; + + default: + *val = ASM330LHH_USE_SLOPE; + break; + } + + return ret; +} + +/** + * @brief Enables gyroscope digital high-pass filter. The filter is enabled + * only if the gyro is in HP mode.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of hp_en_g and hp_en_g in reg CTRL7_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_hp_path_internal_set(stmdev_ctx_t *ctx, + asm330lhh_hpm_g_t val) +{ + asm330lhh_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.hp_en_g = (((uint8_t)val & 0x80U) >> 7); + ctrl7_g.hpm_g = (uint8_t)val & 0x03U; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope digital high-pass filter. The filter is + * enabled only if the gyro is in HP mode.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of hp_en_g and hp_en_g in reg CTRL7_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_hp_path_internal_get(stmdev_ctx_t *ctx, + asm330lhh_hpm_g_t *val) +{ + asm330lhh_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + switch ((ctrl7_g.hp_en_g << 7) + ctrl7_g.hpm_g) + { + case ASM330LHH_HP_FILTER_NONE: + *val = ASM330LHH_HP_FILTER_NONE; + break; + + case ASM330LHH_HP_FILTER_16mHz: + *val = ASM330LHH_HP_FILTER_16mHz; + break; + + case ASM330LHH_HP_FILTER_65mHz: + *val = ASM330LHH_HP_FILTER_65mHz; + break; + + case ASM330LHH_HP_FILTER_260mHz: + *val = ASM330LHH_HP_FILTER_260mHz; + break; + + case ASM330LHH_HP_FILTER_1Hz04: + *val = ASM330LHH_HP_FILTER_1Hz04; + break; + + default: + *val = ASM330LHH_HP_FILTER_NONE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_ serial_interface + * @brief This section groups all the functions concerning main + * serial interface management (not auxiliary) + * @{ + * + */ + +/** + * @brief Connect/Disconnect SDO/SA0 internal pull-up.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sdo_pu_en in reg PIN_CTRL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_sdo_sa0_mode_set(stmdev_ctx_t *ctx, + asm330lhh_sdo_pu_en_t val) +{ + asm330lhh_pin_ctrl_t pin_ctrl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_PIN_CTRL, + (uint8_t *)&pin_ctrl, 1); + + if (ret == 0) + { + pin_ctrl.sdo_pu_en = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_PIN_CTRL, + (uint8_t *)&pin_ctrl, 1); + } + + return ret; +} + +/** + * @brief Connect/Disconnect SDO/SA0 internal pull-up.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of sdo_pu_en in reg PIN_CTRL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_sdo_sa0_mode_get(stmdev_ctx_t *ctx, + asm330lhh_sdo_pu_en_t *val) +{ + asm330lhh_pin_ctrl_t pin_ctrl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_PIN_CTRL, + (uint8_t *)&pin_ctrl, 1); + + switch (pin_ctrl.sdo_pu_en) + { + case ASM330LHH_PULL_UP_DISC: + *val = ASM330LHH_PULL_UP_DISC; + break; + + case ASM330LHH_PULL_UP_CONNECT: + *val = ASM330LHH_PULL_UP_CONNECT; + break; + + default: + *val = ASM330LHH_PULL_UP_DISC; + break; + } + + return ret; +} + +/** + * @brief SPI Serial Interface Mode selection.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sim in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_spi_mode_set(stmdev_ctx_t *ctx, asm330lhh_sim_t val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.sim = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief SPI Serial Interface Mode selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of sim in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_spi_mode_get(stmdev_ctx_t *ctx, + asm330lhh_sim_t *val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.sim) + { + case ASM330LHH_SPI_4_WIRE: + *val = ASM330LHH_SPI_4_WIRE; + break; + + case ASM330LHH_SPI_3_WIRE: + *val = ASM330LHH_SPI_3_WIRE; + break; + + default: + *val = ASM330LHH_SPI_4_WIRE; + break; + } + + return ret; +} + +/** + * @brief Disable / Enable I2C interface.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of i2c_disable in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_i2c_interface_set(stmdev_ctx_t *ctx, + asm330lhh_i2c_disable_t val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.i2c_disable = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Disable / Enable I2C interface.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of i2c reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_i2c_interface_get(stmdev_ctx_t *ctx, + asm330lhh_i2c_disable_t *val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + switch (ctrl4_c.i2c_disable) + { + case ASM330LHH_I2C_ENABLE: + *val = ASM330LHH_I2C_ENABLE; + break; + + case ASM330LHH_I2C_DISABLE: + *val = ASM330LHH_I2C_DISABLE; + break; + + default: + *val = ASM330LHH_I2C_ENABLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_interrupt_pins + * @brief This section groups all the functions that manage + * interrupt pins + * @{ + * + */ + +/** + * @brief Select the signal that need to route on int1 pad.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Structure of registers: INT1_CTRL,MD1_CFG, + * EMB_FUNC_INT1, FSM_INT1_A, FSM_INT1_B + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_pin_int1_route_set(stmdev_ctx_t *ctx, + asm330lhh_pin_int1_route_t *val) +{ + asm330lhh_int_cfg1_t tap_cfg2; + int32_t ret; + + ret = asm330lhh_write_reg(ctx, ASM330LHH_INT1_CTRL, + (uint8_t *)&val->int1_ctrl, 1); + + if (ret == 0) + { + ret = asm330lhh_write_reg(ctx, ASM330LHH_MD1_CFG, + (uint8_t *)&val->md1_cfg, 1); + } + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG1, + (uint8_t *)&tap_cfg2, 1); + + if ((val->int1_ctrl.den_drdy_flag | + val->int1_ctrl.int1_boot | + val->int1_ctrl.int1_cnt_bdr | + val->int1_ctrl.int1_drdy_g | + val->int1_ctrl.int1_drdy_xl | + val->int1_ctrl.int1_fifo_full | + val->int1_ctrl.int1_fifo_ovr | + val->int1_ctrl.int1_fifo_th | + val->md1_cfg.int1_6d | + val->md1_cfg.int1_ff | + val->md1_cfg.int1_wu | + val->md1_cfg.int1_sleep_change) != PROPERTY_DISABLE) + { + tap_cfg2.interrupts_enable = PROPERTY_ENABLE; + } + + else + { + tap_cfg2.interrupts_enable = PROPERTY_DISABLE; + } + } + + if (ret == 0) + { + ret = asm330lhh_write_reg(ctx, ASM330LHH_INT_CFG1, + (uint8_t *)&tap_cfg2, 1); + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int1 pad.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Structure of registers: INT1_CTRL, MD1_CFG, + * EMB_FUNC_INT1, FSM_INT1_A, FSM_INT1_B.(ptr) + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_pin_int1_route_get(stmdev_ctx_t *ctx, + asm330lhh_pin_int1_route_t *val) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT1_CTRL, + (uint8_t *)&val->int1_ctrl, 1); + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_MD1_CFG, + (uint8_t *)&val->md1_cfg, 1); + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int2 pad[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Structure of registers INT2_CTRL, MD2_CFG, + * EMB_FUNC_INT2, FSM_INT2_A, FSM_INT2_B + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_pin_int2_route_set(stmdev_ctx_t *ctx, + asm330lhh_pin_int2_route_t *val) +{ + asm330lhh_int_cfg1_t tap_cfg2; + int32_t ret; + + ret = asm330lhh_write_reg(ctx, ASM330LHH_INT2_CTRL, + (uint8_t *)&val->int2_ctrl, 1); + + if (ret == 0) + { + ret = asm330lhh_write_reg(ctx, ASM330LHH_MD2_CFG, + (uint8_t *)&val->md2_cfg, 1); + } + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG1, + (uint8_t *)&tap_cfg2, 1); + } + + if (ret == 0) + { + if ((val->int2_ctrl.int2_drdy_xl | + val->int2_ctrl.int2_drdy_g | + val->int2_ctrl.int2_drdy_temp | + val->int2_ctrl.int2_fifo_th | + val->int2_ctrl.int2_fifo_ovr | + val->int2_ctrl.int2_fifo_full | + val->int2_ctrl.int2_cnt_bdr | + val->md2_cfg.int2_6d | + val->md2_cfg.int2_ff | + val->md2_cfg.int2_wu | + val->md2_cfg.int2_sleep_change) != PROPERTY_DISABLE) + { + tap_cfg2.interrupts_enable = PROPERTY_ENABLE; + } + + else + { + tap_cfg2.interrupts_enable = PROPERTY_DISABLE; + } + + ret = asm330lhh_write_reg(ctx, ASM330LHH_INT_CFG1, + (uint8_t *)&tap_cfg2, 1); + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int2 pad.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Structure of registers INT2_CTRL, MD2_CFG, + * EMB_FUNC_INT2, FSM_INT2_A, FSM_INT2_B.[get] + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_pin_int2_route_get(stmdev_ctx_t *ctx, + asm330lhh_pin_int2_route_t *val) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT2_CTRL, + (uint8_t *)&val->int2_ctrl, 1); + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_MD2_CFG, + (uint8_t *)&val->md2_cfg, 1); + } + + return ret; +} + +/** + * @brief Push-pull/open drain selection on interrupt pads.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of pp_od in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_pin_mode_set(stmdev_ctx_t *ctx, + asm330lhh_pp_od_t val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.pp_od = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Push-pull/open drain selection on interrupt pads.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of pp_od in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_pin_mode_get(stmdev_ctx_t *ctx, + asm330lhh_pp_od_t *val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.pp_od) + { + case ASM330LHH_PUSH_PULL: + *val = ASM330LHH_PUSH_PULL; + break; + + case ASM330LHH_OPEN_DRAIN: + *val = ASM330LHH_OPEN_DRAIN; + break; + + default: + *val = ASM330LHH_PUSH_PULL; + break; + } + + return ret; +} + +/** + * @brief Interrupt active-high/low.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of h_lactive in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_pin_polarity_set(stmdev_ctx_t *ctx, + asm330lhh_h_lactive_t val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.h_lactive = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Interrupt active-high/low.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of h_lactive in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_pin_polarity_get(stmdev_ctx_t *ctx, + asm330lhh_h_lactive_t *val) +{ + asm330lhh_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.h_lactive) + { + case ASM330LHH_ACTIVE_HIGH: + *val = ASM330LHH_ACTIVE_HIGH; + break; + + case ASM330LHH_ACTIVE_LOW: + *val = ASM330LHH_ACTIVE_LOW; + break; + + default: + *val = ASM330LHH_ACTIVE_HIGH; + break; + } + + return ret; +} + +/** + * @brief All interrupt signals become available on INT1 pin.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of int2_on_int1 in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_all_on_int1_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.int2_on_int1 = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief All interrupt signals become available on INT1 pin.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of int2_on_int1 in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_all_on_int1_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = ctrl4_c.int2_on_int1; + + return ret; +} + +/** + * @brief All interrupt signals notification mode.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of lir in reg INT_CFG0 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_int_notification_set(stmdev_ctx_t *ctx, + asm330lhh_lir_t val) +{ + asm330lhh_int_cfg0_t int_cfg0; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + + if (ret == 0) + { + int_cfg0.lir = (uint8_t)val & 0x01U; + int_cfg0.int_clr_on_read = (uint8_t)val & 0x01U; + ret = asm330lhh_write_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + } + + return ret; +} + +/** + * @brief All interrupt signals notification mode.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of lir in reg INT_CFG0 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_int_notification_get(stmdev_ctx_t *ctx, + asm330lhh_lir_t *val) +{ + asm330lhh_int_cfg0_t int_cfg0; + int32_t ret; + + *val = ASM330LHH_ALL_INT_PULSED; + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + + switch ((int_cfg0.lir << 1) + int_cfg0.int_clr_on_read) + { + case ASM330LHH_ALL_INT_PULSED: + *val = ASM330LHH_ALL_INT_PULSED; + break; + + case ASM330LHH_ALL_INT_LATCHED: + *val = ASM330LHH_ALL_INT_LATCHED; + break; + + default: + *val = ASM330LHH_ALL_INT_PULSED; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_Wake_Up_event + * @brief This section groups all the functions that manage the + * Wake Up event generation. + * @{ + * + */ + +/** + * @brief Weight of 1 LSB of wakeup threshold.[set] + * 0: 1 LSB =FS_XL / 64 + * 1: 1 LSB = FS_XL / 256 + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of wake_ths_w in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_wkup_ths_weight_set(stmdev_ctx_t *ctx, + asm330lhh_wake_ths_w_t val) +{ + asm330lhh_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.wake_ths_w = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Weight of 1 LSB of wakeup threshold.[get] + * 0: 1 LSB =FS_XL / 64 + * 1: 1 LSB = FS_XL / 256 + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of wake_ths_w in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_wkup_ths_weight_get(stmdev_ctx_t *ctx, + asm330lhh_wake_ths_w_t *val) +{ + asm330lhh_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + switch (wake_up_dur.wake_ths_w) + { + case ASM330LHH_LSb_FS_DIV_64: + *val = ASM330LHH_LSb_FS_DIV_64; + break; + + case ASM330LHH_LSb_FS_DIV_256: + *val = ASM330LHH_LSb_FS_DIV_256; + break; + + default: + *val = ASM330LHH_LSb_FS_DIV_64; + break; + } + + return ret; +} + +/** + * @brief Threshold for wakeup: 1 LSB weight depends on WAKE_THS_W in + * WAKE_UP_DUR.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of wk_ths in reg WAKE_UP_THS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_wkup_threshold_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + if (ret == 0) + { + wake_up_ths.wk_ths = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Threshold for wakeup: 1 LSB weight depends on WAKE_THS_W in + * WAKE_UP_DUR.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of wk_ths in reg WAKE_UP_THS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_wkup_threshold_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + *val = wake_up_ths.wk_ths; + + return ret; +} + +/** + * @brief Wake up duration event( 1LSb = 1 / ODR ).[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of usr_off_on_wu in reg WAKE_UP_THS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_on_wkup_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + asm330lhh_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + if (ret == 0) + { + wake_up_ths.usr_off_on_wu = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Wake up duration event( 1LSb = 1 / ODR ).[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of usr_off_on_wu in reg WAKE_UP_THS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_xl_usr_offset_on_wkup_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + *val = wake_up_ths.usr_off_on_wu; + + return ret; +} + +/** + * @brief Wake up duration event(1LSb = 1 / ODR).[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of wake_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_wkup_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.wake_dur = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Wake up duration event(1LSb = 1 / ODR).[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of wake_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_wkup_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + *val = wake_up_dur.wake_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_ Activity/Inactivity_detection + * @brief This section groups all the functions concerning + * activity/inactivity detection. + * @{ + * + */ + +/** + * @brief Enables gyroscope Sleep mode.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sleep_g in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_sleep_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.sleep_g = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope Sleep mode.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sleep_g in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_gy_sleep_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = ctrl4_c.sleep_g; + + return ret; +} + +/** + * @brief Drives the sleep status instead of sleep change on INT pins + * (only if INT1_SLEEP_CHANGE or INT2_SLEEP_CHANGE bits + * are enabled).[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sleep_status_on_int in reg INT_CFG0 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_act_pin_notification_set(stmdev_ctx_t *ctx, + asm330lhh_sleep_status_on_int_t val) +{ + asm330lhh_int_cfg0_t int_cfg0; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + + if (ret == 0) + { + int_cfg0. sleep_status_on_int = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + } + + return ret; +} + +/** + * @brief Drives the sleep status instead of sleep change on INT pins + * (only if INT1_SLEEP_CHANGE or INT2_SLEEP_CHANGE bits + * are enabled).[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of sleep_status_on_int in reg INT_CFG0 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_act_pin_notification_get(stmdev_ctx_t *ctx, + asm330lhh_sleep_status_on_int_t *val) +{ + asm330lhh_int_cfg0_t int_cfg0; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG0, + (uint8_t *)&int_cfg0, 1); + + switch (int_cfg0. sleep_status_on_int) + { + case ASM330LHH_DRIVE_SLEEP_CHG_EVENT: + *val = ASM330LHH_DRIVE_SLEEP_CHG_EVENT; + break; + + case ASM330LHH_DRIVE_SLEEP_STATUS: + *val = ASM330LHH_DRIVE_SLEEP_STATUS; + break; + + default: + *val = ASM330LHH_DRIVE_SLEEP_CHG_EVENT; + break; + } + + return ret; +} + +/** + * @brief Enable inactivity function.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of inact_en in reg INT_CFG1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_act_mode_set(stmdev_ctx_t *ctx, + asm330lhh_inact_en_t val) +{ + asm330lhh_int_cfg1_t int_cfg1; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG1, + (uint8_t *)&int_cfg1, 1); + + if (ret == 0) + { + int_cfg1.inact_en = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_INT_CFG1, + (uint8_t *)&int_cfg1, 1); + } + + return ret; +} + +/** + * @brief Enable inactivity function.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of inact_en in reg INT_CFG1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_act_mode_get(stmdev_ctx_t *ctx, + asm330lhh_inact_en_t *val) +{ + asm330lhh_int_cfg1_t int_cfg1; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_INT_CFG1, + (uint8_t *)&int_cfg1, 1); + + switch (int_cfg1.inact_en) + { + case ASM330LHH_XL_AND_GY_NOT_AFFECTED: + *val = ASM330LHH_XL_AND_GY_NOT_AFFECTED; + break; + + case ASM330LHH_XL_12Hz5_GY_NOT_AFFECTED: + *val = ASM330LHH_XL_12Hz5_GY_NOT_AFFECTED; + break; + + case ASM330LHH_XL_12Hz5_GY_SLEEP: + *val = ASM330LHH_XL_12Hz5_GY_SLEEP; + break; + + case ASM330LHH_XL_12Hz5_GY_PD: + *val = ASM330LHH_XL_12Hz5_GY_PD; + break; + + default: + *val = ASM330LHH_XL_AND_GY_NOT_AFFECTED; + break; + } + + return ret; +} + +/** + * @brief Duration to go in sleep mode (1 LSb = 512 / ODR).[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sleep_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_act_sleep_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.sleep_dur = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Duration to go in sleep mode.(1 LSb = 512 / ODR).[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sleep_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_act_sleep_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + *val = wake_up_dur.sleep_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_ Six_position_detection(6D/4D) + * @brief This section groups all the functions concerning six + * position detection (6D). + * @{ + * + */ + +/** + * @brief Threshold for 4D/6D function.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of sixd_ths in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_6d_threshold_set(stmdev_ctx_t *ctx, + asm330lhh_sixd_ths_t val) +{ + asm330lhh_ths_6d_t ths_6d; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_THS_6D, + (uint8_t *)&ths_6d, 1); + + if (ret == 0) + { + ths_6d.sixd_ths = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_THS_6D, + (uint8_t *)&ths_6d, 1); + } + + return ret; +} + +/** + * @brief Threshold for 4D/6D function.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of sixd_ths in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_6d_threshold_get(stmdev_ctx_t *ctx, + asm330lhh_sixd_ths_t *val) +{ + asm330lhh_ths_6d_t ths_6d; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_THS_6D, + (uint8_t *)&ths_6d, 1); + + switch (ths_6d.sixd_ths) + { + case ASM330LHH_DEG_80: + *val = ASM330LHH_DEG_80; + break; + + case ASM330LHH_DEG_70: + *val = ASM330LHH_DEG_70; + break; + + case ASM330LHH_DEG_60: + *val = ASM330LHH_DEG_60; + break; + + case ASM330LHH_DEG_50: + *val = ASM330LHH_DEG_50; + break; + + default: + *val = ASM330LHH_DEG_80; + break; + } + + return ret; +} + +/** + * @brief 4D orientation detection enable.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of d4d_en in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ths_6d_t ths_6d; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_THS_6D, + (uint8_t *)&ths_6d, 1); + + if (ret == 0) + { + ths_6d.d4d_en = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_THS_6D, + (uint8_t *)&ths_6d, 1); + } + + return ret; +} + +/** + * @brief 4D orientation detection enable.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of d4d_en in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ths_6d_t ths_6d; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_THS_6D, + (uint8_t *)&ths_6d, 1); + *val = ths_6d.d4d_en; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_free_fall + * @brief This section group all the functions concerning the free + * fall detection. + * @{ + * + */ + +/** + * @brief Free fall threshold setting.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of ff_ths in reg FREE_FALL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_ff_threshold_set(stmdev_ctx_t *ctx, + asm330lhh_ff_ths_t val) +{ + asm330lhh_free_fall_t free_fall; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FREE_FALL, + (uint8_t *)&free_fall, 1); + + if (ret == 0) + { + free_fall.ff_ths = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FREE_FALL, + (uint8_t *)&free_fall, 1); + } + + return ret; +} + +/** + * @brief Free fall threshold setting.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of ff_ths in reg FREE_FALL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_ff_threshold_get(stmdev_ctx_t *ctx, + asm330lhh_ff_ths_t *val) +{ + asm330lhh_free_fall_t free_fall; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FREE_FALL, + (uint8_t *)&free_fall, 1); + + switch (free_fall.ff_ths) + { + case ASM330LHH_FF_TSH_156mg: + *val = ASM330LHH_FF_TSH_156mg; + break; + + case ASM330LHH_FF_TSH_219mg: + *val = ASM330LHH_FF_TSH_219mg; + break; + + case ASM330LHH_FF_TSH_250mg: + *val = ASM330LHH_FF_TSH_250mg; + break; + + case ASM330LHH_FF_TSH_312mg: + *val = ASM330LHH_FF_TSH_312mg; + break; + + case ASM330LHH_FF_TSH_344mg: + *val = ASM330LHH_FF_TSH_344mg; + break; + + case ASM330LHH_FF_TSH_406mg: + *val = ASM330LHH_FF_TSH_406mg; + break; + + case ASM330LHH_FF_TSH_469mg: + *val = ASM330LHH_FF_TSH_469mg; + break; + + case ASM330LHH_FF_TSH_500mg: + *val = ASM330LHH_FF_TSH_500mg; + break; + + default: + *val = ASM330LHH_FF_TSH_156mg; + break; + } + + return ret; +} + +/** + * @brief Free-fall duration event(1LSb = 1 / ODR).[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of ff_dur in reg FREE_FALL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_ff_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_wake_up_dur_t wake_up_dur; + asm330lhh_free_fall_t free_fall; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.ff_dur = (val & 0x20U) >> 5; + ret = asm330lhh_write_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_FREE_FALL, + (uint8_t *)&free_fall, 1); + } + + if (ret == 0) + { + free_fall.ff_dur = val & 0x1FU; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FREE_FALL, + (uint8_t *)&free_fall, 1); + } + + return ret; +} + +/** + * @brief Free-fall duration event(1LSb = 1 / ODR).[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of ff_dur in reg FREE_FALL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_ff_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_wake_up_dur_t wake_up_dur; + asm330lhh_free_fall_t free_fall; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_FREE_FALL, + (uint8_t *)&free_fall, 1); + } + + *val = (wake_up_dur.ff_dur << 5) + free_fall.ff_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_fifo + * @brief This section group all the functions concerning + * the fifo usage + * @{ + * + */ + +/** + * @brief FIFO watermark level selection.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of wtm in reg FIFO_CTRL1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_watermark_set(stmdev_ctx_t *ctx, uint16_t val) +{ + asm330lhh_fifo_ctrl1_t fifo_ctrl1; + asm330lhh_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2.wtm = (uint8_t)((val / 256U) & 0x01U); + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + if (ret == 0) + { + fifo_ctrl1.wtm = (uint8_t)(val - (fifo_ctrl2.wtm * 256U)); + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL1, + (uint8_t *)&fifo_ctrl1, 1); + } + + return ret; +} + +/** + * @brief FIFO watermark level selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of wtm in reg FIFO_CTRL1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_watermark_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + asm330lhh_fifo_ctrl1_t fifo_ctrl1; + asm330lhh_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL1, + (uint8_t *)&fifo_ctrl1, 1); + } + + *val = fifo_ctrl2.wtm; + *val = (*val * 256U) + fifo_ctrl1.wtm; + + return ret; +} +/** + * @brief Enables ODR CHANGE virtual sensor to be batched in FIFO.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of odrchg_en in reg FIFO_CTRL2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + asm330lhh_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2.odrchg_en = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief Enables ODR CHANGE virtual sensor to be batched in FIFO.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of odrchg_en in reg FIFO_CTRL2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + *val = fifo_ctrl2.odrchg_en; + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at threshold + * level.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of stop_on_wtm in reg FIFO_CTRL2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2.stop_on_wtm = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at threshold + * level.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of stop_on_wtm in reg FIFO_CTRL2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + *val = fifo_ctrl2.stop_on_wtm; + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for accelerometer data.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of bdr_xl in reg FIFO_CTRL3 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_xl_batch_set(stmdev_ctx_t *ctx, + asm330lhh_bdr_xl_t val) +{ + asm330lhh_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + if (ret == 0) + { + fifo_ctrl3.bdr_xl = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for accelerometer data.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of bdr_xl in reg FIFO_CTRL3 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_xl_batch_get(stmdev_ctx_t *ctx, + asm330lhh_bdr_xl_t *val) +{ + asm330lhh_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + switch (fifo_ctrl3.bdr_xl) + { + case ASM330LHH_XL_NOT_BATCHED: + *val = ASM330LHH_XL_NOT_BATCHED; + break; + + case ASM330LHH_XL_BATCHED_AT_12Hz5: + *val = ASM330LHH_XL_BATCHED_AT_12Hz5; + break; + + case ASM330LHH_XL_BATCHED_AT_26Hz: + *val = ASM330LHH_XL_BATCHED_AT_26Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_52Hz: + *val = ASM330LHH_XL_BATCHED_AT_52Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_104Hz: + *val = ASM330LHH_XL_BATCHED_AT_104Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_208Hz: + *val = ASM330LHH_XL_BATCHED_AT_208Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_417Hz: + *val = ASM330LHH_XL_BATCHED_AT_417Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_833Hz: + *val = ASM330LHH_XL_BATCHED_AT_833Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_1667Hz: + *val = ASM330LHH_XL_BATCHED_AT_1667Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_3333Hz: + *val = ASM330LHH_XL_BATCHED_AT_3333Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_6667Hz: + *val = ASM330LHH_XL_BATCHED_AT_6667Hz; + break; + + case ASM330LHH_XL_BATCHED_AT_6Hz5: + *val = ASM330LHH_XL_BATCHED_AT_6Hz5; + break; + + default: + *val = ASM330LHH_XL_NOT_BATCHED; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for gyroscope data.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of bdr_gy in reg FIFO_CTRL3 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_gy_batch_set(stmdev_ctx_t *ctx, + asm330lhh_bdr_gy_t val) +{ + asm330lhh_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + if (ret == 0) + { + fifo_ctrl3.bdr_gy = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for gyroscope data.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of bdr_gy in reg FIFO_CTRL3 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_gy_batch_get(stmdev_ctx_t *ctx, + asm330lhh_bdr_gy_t *val) +{ + asm330lhh_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + switch (fifo_ctrl3.bdr_gy) + { + case ASM330LHH_GY_NOT_BATCHED: + *val = ASM330LHH_GY_NOT_BATCHED; + break; + + case ASM330LHH_GY_BATCHED_AT_12Hz5: + *val = ASM330LHH_GY_BATCHED_AT_12Hz5; + break; + + case ASM330LHH_GY_BATCHED_AT_26Hz: + *val = ASM330LHH_GY_BATCHED_AT_26Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_52Hz: + *val = ASM330LHH_GY_BATCHED_AT_52Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_104Hz: + *val = ASM330LHH_GY_BATCHED_AT_104Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_208Hz: + *val = ASM330LHH_GY_BATCHED_AT_208Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_417Hz: + *val = ASM330LHH_GY_BATCHED_AT_417Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_833Hz: + *val = ASM330LHH_GY_BATCHED_AT_833Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_1667Hz: + *val = ASM330LHH_GY_BATCHED_AT_1667Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_3333Hz: + *val = ASM330LHH_GY_BATCHED_AT_3333Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_6667Hz: + *val = ASM330LHH_GY_BATCHED_AT_6667Hz; + break; + + case ASM330LHH_GY_BATCHED_AT_6Hz5: + *val = ASM330LHH_GY_BATCHED_AT_6Hz5; + break; + + default: + *val = ASM330LHH_GY_NOT_BATCHED; + break; + } + + return ret; +} + +/** + * @brief FIFO mode selection.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of fifo_mode in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_mode_set(stmdev_ctx_t *ctx, + asm330lhh_fifo_mode_t val) +{ + asm330lhh_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.fifo_mode = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief FIFO mode selection.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of fifo_mode in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_mode_get(stmdev_ctx_t *ctx, + asm330lhh_fifo_mode_t *val) +{ + asm330lhh_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + switch (fifo_ctrl4.fifo_mode) + { + case ASM330LHH_BYPASS_MODE: + *val = ASM330LHH_BYPASS_MODE; + break; + + case ASM330LHH_FIFO_MODE: + *val = ASM330LHH_FIFO_MODE; + break; + + case ASM330LHH_STREAM_TO_FIFO_MODE: + *val = ASM330LHH_STREAM_TO_FIFO_MODE; + break; + + case ASM330LHH_BYPASS_TO_STREAM_MODE: + *val = ASM330LHH_BYPASS_TO_STREAM_MODE; + break; + + case ASM330LHH_STREAM_MODE: + *val = ASM330LHH_STREAM_MODE; + break; + + case ASM330LHH_BYPASS_TO_FIFO_MODE: + *val = ASM330LHH_BYPASS_TO_FIFO_MODE; + break; + + default: + *val = ASM330LHH_BYPASS_MODE; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for temperature data.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of odr_t_batch in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_temp_batch_set(stmdev_ctx_t *ctx, + asm330lhh_odr_t_batch_t val) +{ + asm330lhh_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.odr_t_batch = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for temperature data.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of odr_t_batch in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_temp_batch_get(stmdev_ctx_t *ctx, + asm330lhh_odr_t_batch_t *val) +{ + asm330lhh_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + switch (fifo_ctrl4.odr_t_batch) + { + case ASM330LHH_TEMP_NOT_BATCHED: + *val = ASM330LHH_TEMP_NOT_BATCHED; + break; + + case ASM330LHH_TEMP_BATCHED_AT_52Hz: + *val = ASM330LHH_TEMP_BATCHED_AT_52Hz; + break; + + case ASM330LHH_TEMP_BATCHED_AT_12Hz5: + *val = ASM330LHH_TEMP_BATCHED_AT_12Hz5; + break; + + case ASM330LHH_TEMP_BATCHED_AT_1Hz6: + *val = ASM330LHH_TEMP_BATCHED_AT_1Hz6; + break; + + default: + *val = ASM330LHH_TEMP_NOT_BATCHED; + break; + } + + return ret; +} + +/** + * @brief Selects decimation for timestamp batching in FIFO. + * Writing rate will be the maximum rate between XL and + * GYRO BDR divided by decimation decoder.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of odr_ts_batch in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_timestamp_decimation_set(stmdev_ctx_t *ctx, + asm330lhh_odr_ts_batch_t val) +{ + asm330lhh_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.dec_ts_batch = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Selects decimation for timestamp batching in FIFO. + * Writing rate will be the maximum rate between XL and + * GYRO BDR divided by decimation decoder.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of odr_ts_batch in reg + * FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_timestamp_decimation_get(stmdev_ctx_t *ctx, + asm330lhh_odr_ts_batch_t *val) +{ + asm330lhh_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + switch (fifo_ctrl4.dec_ts_batch) + { + case ASM330LHH_NO_DECIMATION: + *val = ASM330LHH_NO_DECIMATION; + break; + + case ASM330LHH_DEC_1: + *val = ASM330LHH_DEC_1; + break; + + case ASM330LHH_DEC_8: + *val = ASM330LHH_DEC_8; + break; + + case ASM330LHH_DEC_32: + *val = ASM330LHH_DEC_32; + break; + + default: + *val = ASM330LHH_NO_DECIMATION; + break; + } + + return ret; +} + +/** + * @brief Selects the trigger for the internal counter of batching events + * between XL and gyro.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of trig_counter_bdr in + * reg COUNTER_BDR_REG1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_cnt_event_batch_set(stmdev_ctx_t *ctx, + asm330lhh_trig_counter_bdr_t val) +{ + asm330lhh_counter_bdr_reg1_t counter_bdr_reg1; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + + if (ret == 0) + { + counter_bdr_reg1.trig_counter_bdr = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + } + + return ret; +} + +/** + * @brief Selects the trigger for the internal counter of batching events + * between XL and gyro.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of trig_counter_bdr + * in reg COUNTER_BDR_REG1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_cnt_event_batch_get(stmdev_ctx_t *ctx, + asm330lhh_trig_counter_bdr_t *val) +{ + asm330lhh_counter_bdr_reg1_t counter_bdr_reg1; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + + switch (counter_bdr_reg1.trig_counter_bdr) + { + case ASM330LHH_XL_BATCH_EVENT: + *val = ASM330LHH_XL_BATCH_EVENT; + break; + + case ASM330LHH_GYRO_BATCH_EVENT: + *val = ASM330LHH_GYRO_BATCH_EVENT; + break; + + default: + *val = ASM330LHH_XL_BATCH_EVENT; + break; + } + + return ret; +} + +/** + * @brief Resets the internal counter of batching events for a single sensor. + * This bit is automatically reset to zero if it was set to ‘1’.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of rst_counter_bdr in reg COUNTER_BDR_REG1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_rst_batch_counter_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + asm330lhh_counter_bdr_reg1_t counter_bdr_reg1; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + + if (ret == 0) + { + counter_bdr_reg1.rst_counter_bdr = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + } + + return ret; +} + +/** + * @brief Resets the internal counter of batching events for a single sensor. + * This bit is automatically reset to zero if it was set to ‘1’.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of rst_counter_bdr in reg COUNTER_BDR_REG1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_rst_batch_counter_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + asm330lhh_counter_bdr_reg1_t counter_bdr_reg1; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + *val = counter_bdr_reg1.rst_counter_bdr; + + return ret; +} + +/** + * @brief Batch data rate counter.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of cnt_bdr_th in reg COUNTER_BDR_REG2 + * and COUNTER_BDR_REG1. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_batch_counter_threshold_set(stmdev_ctx_t *ctx, + uint16_t val) +{ + asm330lhh_counter_bdr_reg2_t counter_bdr_reg1; + asm330lhh_counter_bdr_reg2_t counter_bdr_reg2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + + if (ret == 0) + { + counter_bdr_reg1.cnt_bdr_th = (uint8_t)((val / 256U) & 0x03U); + ret = asm330lhh_write_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + } + + if (ret == 0) + { + counter_bdr_reg2.cnt_bdr_th = (uint8_t)(val - + (counter_bdr_reg1.cnt_bdr_th * 256U)); + ret = asm330lhh_write_reg(ctx, ASM330LHH_COUNTER_BDR_REG2, + (uint8_t *)&counter_bdr_reg2, 1); + } + + return ret; +} + +/** + * @brief Batch data rate counter.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of cnt_bdr_th in reg COUNTER_BDR_REG2 + * and COUNTER_BDR_REG1. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_batch_counter_threshold_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + asm330lhh_counter_bdr_reg1_t counter_bdr_reg1; + asm330lhh_counter_bdr_reg2_t counter_bdr_reg2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_COUNTER_BDR_REG2, + (uint8_t *)&counter_bdr_reg2, 1); + } + + *val = counter_bdr_reg1.cnt_bdr_th; + *val = (*val * 256U) + counter_bdr_reg2.cnt_bdr_th; + + return ret; +} + +/** + * @brief Number of unread sensor data (TAG + 6 bytes) stored in FIFO.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of diff_fifo in reg FIFO_STATUS1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_data_level_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + asm330lhh_fifo_status1_t fifo_status1; + asm330lhh_fifo_status2_t fifo_status2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_STATUS1, + (uint8_t *)&fifo_status1, 1); + + if (ret == 0) + { + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = fifo_status2.diff_fifo; + *val = (*val * 256U) + fifo_status1.diff_fifo; + } + + return ret; +} + +/** + * @brief Smart FIFO status.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Registers FIFO_STATUS2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_status_get(stmdev_ctx_t *ctx, + asm330lhh_fifo_status2_t *val) +{ + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_STATUS2, (uint8_t *)val, 1); + + return ret; +} + +/** + * @brief Smart FIFO full status.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of fifo_full_ia in reg FIFO_STATUS2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_full_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_fifo_status2_t fifo_status2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = fifo_status2.fifo_full_ia; + + return ret; +} + +/** + * @brief FIFO overrun status.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of fifo_over_run_latched in + * reg FIFO_STATUS2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_ovr_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_fifo_status2_t fifo_status2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = fifo_status2. fifo_ovr_ia; + + return ret; +} + +/** + * @brief FIFO watermark status.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of fifo_wtm_ia in reg FIFO_STATUS2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_wtm_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_fifo_status2_t fifo_status2; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = fifo_status2.fifo_wtm_ia; + + return ret; +} + +/** + * @brief Identifies the sensor in FIFO_DATA_OUT.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of tag_sensor in reg FIFO_DATA_OUT_TAG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_fifo_sensor_tag_get(stmdev_ctx_t *ctx, + asm330lhh_fifo_tag_t *val) +{ + asm330lhh_fifo_data_out_tag_t fifo_data_out_tag; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_FIFO_DATA_OUT_TAG, + (uint8_t *)&fifo_data_out_tag, 1); + + switch (fifo_data_out_tag.tag_sensor) + { + case ASM330LHH_GYRO_NC_TAG: + *val = ASM330LHH_GYRO_NC_TAG; + break; + + case ASM330LHH_XL_NC_TAG: + *val = ASM330LHH_XL_NC_TAG; + break; + + case ASM330LHH_TEMPERATURE_TAG: + *val = ASM330LHH_TEMPERATURE_TAG; + break; + + case ASM330LHH_TIMESTAMP_TAG: + *val = ASM330LHH_TIMESTAMP_TAG; + break; + + case ASM330LHH_CFG_CHANGE_TAG: + *val = ASM330LHH_CFG_CHANGE_TAG; + break; + + default: + *val = ASM330LHH_CFG_CHANGE_TAG; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup ASM330LHH_DEN_functionality + * @brief This section groups all the functions concerning + * DEN functionality. + * @{ + * + */ + +/** + * @brief DEN functionality marking mode.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_mode in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_mode_set(stmdev_ctx_t *ctx, + asm330lhh_den_mode_t val) +{ + asm330lhh_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.den_mode = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + } + + return ret; +} + +/** + * @brief DEN functionality marking mode.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of den_mode in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_mode_get(stmdev_ctx_t *ctx, + asm330lhh_den_mode_t *val) +{ + asm330lhh_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + switch (ctrl6_c.den_mode) + { + case ASM330LHH_DEN_DISABLE: + *val = ASM330LHH_DEN_DISABLE; + break; + + case ASM330LHH_LEVEL_FIFO: + *val = ASM330LHH_LEVEL_FIFO; + break; + + case ASM330LHH_LEVEL_LETCHED: + *val = ASM330LHH_LEVEL_LETCHED; + break; + + case ASM330LHH_LEVEL_TRIGGER: + *val = ASM330LHH_LEVEL_TRIGGER; + break; + + case ASM330LHH_EDGE_TRIGGER: + *val = ASM330LHH_EDGE_TRIGGER; + break; + + default: + *val = ASM330LHH_DEN_DISABLE; + break; + } + + return ret; +} + +/** + * @brief DEN active level configuration.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_lh in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_polarity_set(stmdev_ctx_t *ctx, + asm330lhh_den_lh_t val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_lh = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEN active level configuration.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of den_lh in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_polarity_get(stmdev_ctx_t *ctx, + asm330lhh_den_lh_t *val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + switch (ctrl9_xl.den_lh) + { + case ASM330LHH_DEN_ACT_LOW: + *val = ASM330LHH_DEN_ACT_LOW; + break; + + case ASM330LHH_DEN_ACT_HIGH: + *val = ASM330LHH_DEN_ACT_HIGH; + break; + + default: + *val = ASM330LHH_DEN_ACT_LOW; + break; + } + + return ret; +} + +/** + * @brief DEN configuration.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_xl_g in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_enable_set(stmdev_ctx_t *ctx, + asm330lhh_den_xl_g_t val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_xl_g = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEN configuration.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Get the values of den_xl_g in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_enable_get(stmdev_ctx_t *ctx, + asm330lhh_den_xl_g_t *val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + switch (ctrl9_xl.den_xl_g) + { + case ASM330LHH_STAMP_IN_GY_DATA: + *val = ASM330LHH_STAMP_IN_GY_DATA; + break; + + case ASM330LHH_STAMP_IN_XL_DATA: + *val = ASM330LHH_STAMP_IN_XL_DATA; + break; + + case ASM330LHH_STAMP_IN_GY_XL_DATA: + *val = ASM330LHH_STAMP_IN_GY_XL_DATA; + break; + + default: + *val = ASM330LHH_STAMP_IN_GY_DATA; + break; + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of X-axis.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_z in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_mark_axis_x_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_z = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of X-axis.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_z in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_mark_axis_x_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + *val = ctrl9_xl.den_z; + + return ret; +} + +/** + * @brief DEN value stored in LSB of Y-axis.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_y in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_mark_axis_y_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_y = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of Y-axis.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_y in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_mark_axis_y_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + *val = ctrl9_xl.den_y; + + return ret; +} + +/** + * @brief DEN value stored in LSB of Z-axis.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_x in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_mark_axis_z_set(stmdev_ctx_t *ctx, uint8_t val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_x = (uint8_t)val; + ret = asm330lhh_write_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of Z-axis.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val Change the values of den_x in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t asm330lhh_den_mark_axis_z_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + asm330lhh_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = asm330lhh_read_reg(ctx, ASM330LHH_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + *val = ctrl9_xl.den_x; + + return ret; +} + +/** + * @} + * + */ + +/** + * @} + * + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/ASM330LHH/asm330lhh_reg.h b/ASM330LHH/asm330lhh_reg.h new file mode 100644 index 0000000..af65ba7 --- /dev/null +++ b/ASM330LHH/asm330lhh_reg.h @@ -0,0 +1,1532 @@ +/** + ****************************************************************************** + * @file asm330lhh_reg.h + * @author Sensors Software Solution Team + * @brief This file contains all the functions prototypes for the + * asm330lhh_reg.c driver. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2021 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef ASM330LHH_REGS_H +#define ASM330LHH_REGS_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include +#include +#include + +/** @addtogroup ASM330LHH + * @{ + * + */ + +/** @defgroup Endianness definitions + * @{ + * + */ + +#ifndef DRV_BYTE_ORDER +#ifndef __BYTE_ORDER__ + +#define DRV_LITTLE_ENDIAN 1234 +#define DRV_BIG_ENDIAN 4321 + +/** if _BYTE_ORDER is not defined, choose the endianness of your architecture + * by uncommenting the define which fits your platform endianness + */ +//#define DRV_BYTE_ORDER DRV_BIG_ENDIAN +#define DRV_BYTE_ORDER DRV_LITTLE_ENDIAN + +#else /* defined __BYTE_ORDER__ */ + +#define DRV_LITTLE_ENDIAN __ORDER_LITTLE_ENDIAN__ +#define DRV_BIG_ENDIAN __ORDER_BIG_ENDIAN__ +#define DRV_BYTE_ORDER __BYTE_ORDER__ + +#endif /* __BYTE_ORDER__*/ +#endif /* DRV_BYTE_ORDER */ + +/** + * @} + * + */ + +/** @defgroup STMicroelectronics sensors common types + * @{ + * + */ + +#ifndef MEMS_SHARED_TYPES +#define MEMS_SHARED_TYPES + +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t bit0 : 1; + uint8_t bit1 : 1; + uint8_t bit2 : 1; + uint8_t bit3 : 1; + uint8_t bit4 : 1; + uint8_t bit5 : 1; + uint8_t bit6 : 1; + uint8_t bit7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t bit7 : 1; + uint8_t bit6 : 1; + uint8_t bit5 : 1; + uint8_t bit4 : 1; + uint8_t bit3 : 1; + uint8_t bit2 : 1; + uint8_t bit1 : 1; + uint8_t bit0 : 1; +#endif /* DRV_BYTE_ORDER */ +} bitwise_t; + +#define PROPERTY_DISABLE (0U) +#define PROPERTY_ENABLE (1U) + +/** @addtogroup Interfaces_Functions + * @brief This section provide a set of functions used to read and + * write a generic register of the device. + * MANDATORY: return 0 -> no Error. + * @{ + * + */ + +typedef int32_t (*stmdev_write_ptr)(void *, uint8_t, const uint8_t *, uint16_t); +typedef int32_t (*stmdev_read_ptr)(void *, uint8_t, uint8_t *, uint16_t); +typedef void (*stmdev_mdelay_ptr)(uint32_t millisec); + +typedef struct +{ + /** Component mandatory fields **/ + stmdev_write_ptr write_reg; + stmdev_read_ptr read_reg; + /** Component optional fields **/ + stmdev_mdelay_ptr mdelay; + /** Customizable optional pointer **/ + void *handle; +} stmdev_ctx_t; + +/** + * @} + * + */ + +#endif /* MEMS_SHARED_TYPES */ + +#ifndef MEMS_UCF_SHARED_TYPES +#define MEMS_UCF_SHARED_TYPES + +/** @defgroup Generic address-data structure definition + * @brief This structure is useful to load a predefined configuration + * of a sensor. + * You can create a sensor configuration by your own or using + * Unico / Unicleo tools available on STMicroelectronics + * web site. + * + * @{ + * + */ + +typedef struct +{ + uint8_t address; + uint8_t data; +} ucf_line_t; + +/** + * @} + * + */ + +#endif /* MEMS_UCF_SHARED_TYPES */ + +/** + * @} + * + */ + +/** @defgroup ASM330LHH Infos + * @{ + * + */ + +/** I2C Device Address 8 bit format if SA0=0 -> D5 if SA0=1 -> D7 **/ +#define ASM330LHH_I2C_ADD_L 0xD5U +#define ASM330LHH_I2C_ADD_H 0xD7U + +/** Device Identification (Who am I) **/ +#define ASM330LHH_ID 0x6BU + +/** + * @} + * + */ + +#define ASM330LHH_PIN_CTRL 0x02U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 6; + uint8_t sdo_pu_en : 1; + uint8_t not_used_02 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 1; + uint8_t sdo_pu_en : 1; + uint8_t not_used_01 : 6; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_pin_ctrl_t; + +#define ASM330LHH_FIFO_CTRL1 0x07U +typedef struct +{ + uint8_t wtm : 8; +} asm330lhh_fifo_ctrl1_t; + +#define ASM330LHH_FIFO_CTRL2 0x08U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t wtm : 1; + uint8_t not_used_01 : 3; + uint8_t odrchg_en : 1; + uint8_t not_used_02 : 2; + uint8_t stop_on_wtm : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t stop_on_wtm : 1; + uint8_t not_used_02 : 2; + uint8_t odrchg_en : 1; + uint8_t not_used_01 : 3; + uint8_t wtm : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_fifo_ctrl2_t; + +#define ASM330LHH_FIFO_CTRL3 0x09U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t bdr_xl : 4; + uint8_t bdr_gy : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t bdr_gy : 4; + uint8_t bdr_xl : 4; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_fifo_ctrl3_t; + +#define ASM330LHH_FIFO_CTRL4 0x0AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_mode : 3; + uint8_t not_used_01 : 1; + uint8_t odr_t_batch : 2; + uint8_t dec_ts_batch : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t dec_ts_batch : 2; + uint8_t odr_t_batch : 2; + uint8_t not_used_01 : 1; + uint8_t fifo_mode : 3; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_fifo_ctrl4_t; + +#define ASM330LHH_COUNTER_BDR_REG1 0x0BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t cnt_bdr_th : 2; + uint8_t not_used_01 : 3; + uint8_t trig_counter_bdr : 1; + uint8_t rst_counter_bdr : 1; + uint8_t dataready_pulsed : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t dataready_pulsed : 1; + uint8_t rst_counter_bdr : 1; + uint8_t trig_counter_bdr : 1; + uint8_t not_used_01 : 3; + uint8_t cnt_bdr_th : 2; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_counter_bdr_reg1_t; + +#define ASM330LHH_COUNTER_BDR_REG2 0x0CU +typedef struct +{ + uint8_t cnt_bdr_th : 8; +} asm330lhh_counter_bdr_reg2_t; + +#define ASM330LHH_INT1_CTRL 0x0DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_drdy_xl : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_boot : 1; + uint8_t int1_fifo_th : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_fifo_full : 1; + uint8_t int1_cnt_bdr : 1; + uint8_t den_drdy_flag : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_drdy_flag : 1; + uint8_t int1_cnt_bdr : 1; + uint8_t int1_fifo_full : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_fifo_th : 1; + uint8_t int1_boot : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_int1_ctrl_t; + +#define ASM330LHH_INT2_CTRL 0x0EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_drdy_xl : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_temp : 1; + uint8_t int2_fifo_th : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_fifo_full : 1; + uint8_t int2_cnt_bdr : 1; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t int2_cnt_bdr : 1; + uint8_t int2_fifo_full : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_fifo_th : 1; + uint8_t int2_drdy_temp : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_int2_ctrl_t; + +#define ASM330LHH_WHO_AM_I 0x0FU +#define ASM330LHH_CTRL1_XL 0x10U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t lpf2_xl_en : 1; + uint8_t fs_xl : 2; + uint8_t odr_xl : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_xl : 4; + uint8_t fs_xl : 2; + uint8_t lpf2_xl_en : 1; + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl1_xl_t; + +#define ASM330LHH_CTRL2_G 0x11U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fs_g : 4; /* fs_4000 + fs_125 + fs_g */ + uint8_t odr_g : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_g : 4; + uint8_t fs_g : 4; /* fs_4000 + fs_125 + fs_g */ +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl2_g_t; + +#define ASM330LHH_CTRL3_C 0x12U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sw_reset : 1; + uint8_t not_used_01 : 1; + uint8_t if_inc : 1; + uint8_t sim : 1; + uint8_t pp_od : 1; + uint8_t h_lactive : 1; + uint8_t bdu : 1; + uint8_t boot : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t boot : 1; + uint8_t bdu : 1; + uint8_t h_lactive : 1; + uint8_t pp_od : 1; + uint8_t sim : 1; + uint8_t if_inc : 1; + uint8_t not_used_01 : 1; + uint8_t sw_reset : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl3_c_t; + +#define ASM330LHH_CTRL4_C 0x13U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t lpf1_sel_g : 1; + uint8_t i2c_disable : 1; + uint8_t drdy_mask : 1; + uint8_t not_used_02 : 1; + uint8_t int2_on_int1 : 1; + uint8_t sleep_g : 1; + uint8_t not_used_03 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_03 : 1; + uint8_t sleep_g : 1; + uint8_t int2_on_int1 : 1; + uint8_t not_used_02 : 1; + uint8_t drdy_mask : 1; + uint8_t i2c_disable : 1; + uint8_t lpf1_sel_g : 1; + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl4_c_t; + +#define ASM330LHH_CTRL5_C 0x14U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_xl : 2; + uint8_t st_g : 2; + uint8_t not_used_01 : 1; + uint8_t rounding : 2; + uint8_t not_used_02 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 1; + uint8_t rounding : 2; + uint8_t not_used_01 : 1; + uint8_t st_g : 2; + uint8_t st_xl : 2; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl5_c_t; + +#define ASM330LHH_CTRL6_C 0x15U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ftype : 3; + uint8_t usr_off_w : 1; + uint8_t not_used_01 : 1; + uint8_t den_mode : 3; /* trig_en + lvl1_en + lvl2_en */ +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_mode : 3; /* trig_en + lvl1_en + lvl2_en */ + uint8_t not_used_01 : 1; + uint8_t usr_off_w : 1; + uint8_t ftype : 3; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl6_c_t; + +#define ASM330LHH_CTRL7_G 0x16U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t usr_off_on_out : 1; + uint8_t not_used_02 : 2; + uint8_t hpm_g : 2; + uint8_t hp_en_g : 1; + uint8_t not_used_03 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_03 : 1; + uint8_t hp_en_g : 1; + uint8_t hpm_g : 2; + uint8_t not_used_02 : 2; + uint8_t usr_off_on_out : 1; + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl7_g_t; + +#define ASM330LHH_CTRL8_XL 0x17U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t low_pass_on_6d : 1; + uint8_t not_used_01 : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t fastsettl_mode_xl : 1; + uint8_t hp_ref_mode_xl : 1; + uint8_t hpcf_xl : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t hpcf_xl : 3; + uint8_t hp_ref_mode_xl : 1; + uint8_t fastsettl_mode_xl : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t not_used_01 : 1; + uint8_t low_pass_on_6d : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl8_xl_t; + +#define ASM330LHH_CTRL9_XL 0x18U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t device_conf : 1; + uint8_t den_lh : 1; + uint8_t den_xl_g : 2; /* den_xl_en + den_xl_g */ + uint8_t den_z : 1; + uint8_t den_y : 1; + uint8_t den_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_x : 1; + uint8_t den_y : 1; + uint8_t den_z : 1; + uint8_t den_xl_g : 2; /* den_xl_en + den_xl_g */ + uint8_t den_lh : 1; + uint8_t device_conf : 1; + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl9_xl_t; + +#define ASM330LHH_CTRL10_C 0x19U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 5; + uint8_t timestamp_en : 1; + uint8_t not_used_02 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 2; + uint8_t timestamp_en : 1; + uint8_t not_used_01 : 5; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ctrl10_c_t; + +#define ASM330LHH_ALL_INT_SRC 0x1AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ff_ia : 1; + uint8_t wu_ia : 1; + uint8_t not_used_01 : 2; + uint8_t d6d_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t not_used_02 : 1; + uint8_t timestamp_endcount : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timestamp_endcount : 1; + uint8_t not_used_02 : 1; + uint8_t sleep_change_ia : 1; + uint8_t d6d_ia : 1; + uint8_t not_used_01 : 2; + uint8_t wu_ia : 1; + uint8_t ff_ia : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_all_int_src_t; + +#define ASM330LHH_WAKE_UP_SRC 0x1BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t z_wu : 1; + uint8_t y_wu : 1; + uint8_t x_wu : 1; + uint8_t wu_ia : 1; + uint8_t sleep_state : 1; + uint8_t ff_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t sleep_change_ia : 1; + uint8_t ff_ia : 1; + uint8_t sleep_state : 1; + uint8_t wu_ia : 1; + uint8_t x_wu : 1; + uint8_t y_wu : 1; + uint8_t z_wu : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_wake_up_src_t; + +#define ASM330LHH_D6D_SRC 0x1DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xl : 1; + uint8_t xh : 1; + uint8_t yl : 1; + uint8_t yh : 1; + uint8_t zl : 1; + uint8_t zh : 1; + uint8_t d6d_ia : 1; + uint8_t den_drdy : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_drdy : 1; + uint8_t d6d_ia : 1; + uint8_t zh : 1; + uint8_t zl : 1; + uint8_t yh : 1; + uint8_t yl : 1; + uint8_t xh : 1; + uint8_t xl : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_d6d_src_t; + +#define ASM330LHH_STATUS_REG 0x1EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xlda : 1; + uint8_t gda : 1; + uint8_t tda : 1; + uint8_t not_used_01 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 5; + uint8_t tda : 1; + uint8_t gda : 1; + uint8_t xlda : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_status_reg_t; + +#define ASM330LHH_OUT_TEMP_L 0x20U +#define ASM330LHH_OUT_TEMP_H 0x21U +#define ASM330LHH_OUTX_L_G 0x22U +#define ASM330LHH_OUTX_H_G 0x23U +#define ASM330LHH_OUTY_L_G 0x24U +#define ASM330LHH_OUTY_H_G 0x25U +#define ASM330LHH_OUTZ_L_G 0x26U +#define ASM330LHH_OUTZ_H_G 0x27U +#define ASM330LHH_OUTX_L_A 0x28U +#define ASM330LHH_OUTX_H_A 0x29U +#define ASM330LHH_OUTY_L_A 0x2AU +#define ASM330LHH_OUTY_H_A 0x2BU +#define ASM330LHH_OUTZ_L_A 0x2CU +#define ASM330LHH_OUTZ_H_A 0x2DU +#define ASM330LHH_FIFO_STATUS1 0x3AU +typedef struct +{ + uint8_t diff_fifo : 8; +} asm330lhh_fifo_status1_t; + +#define ASM330LHH_FIFO_STATUS2 0x3BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t diff_fifo : 2; + uint8_t not_used_01 : 1; + uint8_t fifo_ovr_latched : 1; + uint8_t counter_bdr_ia : 1; + uint8_t fifo_full_ia : 1; + uint8_t fifo_ovr_ia : 1; + uint8_t fifo_wtm_ia : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_wtm_ia : 1; + uint8_t fifo_ovr_ia : 1; + uint8_t fifo_full_ia : 1; + uint8_t counter_bdr_ia : 1; + uint8_t fifo_ovr_latched : 1; + uint8_t not_used_01 : 1; + uint8_t diff_fifo : 2; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_fifo_status2_t; + +#define ASM330LHH_TIMESTAMP0 0x40U +#define ASM330LHH_TIMESTAMP1 0x41U +#define ASM330LHH_TIMESTAMP2 0x42U +#define ASM330LHH_TIMESTAMP3 0x43U +#define ASM330LHH_INT_CFG0 0x56U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t lir : 1; + uint8_t not_used_01 : 3; + uint8_t slope_fds : 1; + uint8_t sleep_status_on_int : 1; + uint8_t int_clr_on_read : 1; + uint8_t not_used_02 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 1; + uint8_t int_clr_on_read : 1; + uint8_t sleep_status_on_int : 1; + uint8_t slope_fds : 1; + uint8_t not_used_01 : 3; + uint8_t lir : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_int_cfg0_t; + +#define ASM330LHH_INT_CFG1 0x58U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 5; + uint8_t inact_en : 2; + uint8_t interrupts_enable : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t interrupts_enable : 1; + uint8_t inact_en : 2; + uint8_t not_used_01 : 5; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_int_cfg1_t; + +#define ASM330LHH_THS_6D 0x59U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 5; + uint8_t sixd_ths : 2; + uint8_t d4d_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t d4d_en : 1; + uint8_t sixd_ths : 2; + uint8_t not_used_01 : 5; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_ths_6d_t; + +#define ASM330LHH_WAKE_UP_THS 0x5BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t wk_ths : 6; + uint8_t usr_off_on_wu : 1; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t usr_off_on_wu : 1; + uint8_t wk_ths : 6; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_wake_up_ths_t; + +#define ASM330LHH_WAKE_UP_DUR 0x5CU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sleep_dur : 4; + uint8_t wake_ths_w : 1; + uint8_t wake_dur : 2; + uint8_t ff_dur : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ff_dur : 1; + uint8_t wake_dur : 2; + uint8_t wake_ths_w : 1; + uint8_t sleep_dur : 4; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_wake_up_dur_t; + +#define ASM330LHH_FREE_FALL 0x5DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ff_ths : 3; + uint8_t ff_dur : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ff_dur : 5; + uint8_t ff_ths : 3; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_free_fall_t; + +#define ASM330LHH_MD1_CFG 0x5EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 2; + uint8_t int1_6d : 1; + uint8_t not_used_02 : 1; + uint8_t int1_ff : 1; + uint8_t int1_wu : 1; + uint8_t not_used_03 : 1; + uint8_t int1_sleep_change : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_sleep_change : 1; + uint8_t not_used_03 : 1; + uint8_t int1_wu : 1; + uint8_t int1_ff : 1; + uint8_t not_used_02 : 1; + uint8_t int1_6d : 1; + uint8_t not_used_01 : 2; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_md1_cfg_t; + +#define ASM330LHH_MD2_CFG 0x5FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_timestamp : 1; + uint8_t not_used_01 : 1; + uint8_t int2_6d : 1; + uint8_t not_used_02 : 1; + uint8_t int2_ff : 1; + uint8_t int2_wu : 1; + uint8_t not_used_03 : 1; + uint8_t int2_sleep_change : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_sleep_change : 1; + uint8_t not_used_03 : 1; + uint8_t int2_wu : 1; + uint8_t int2_ff : 1; + uint8_t not_used_02 : 1; + uint8_t int2_6d : 1; + uint8_t not_used_01 : 1; + uint8_t int2_timestamp : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_md2_cfg_t; + +#define ASM330LHH_INTERNAL_FREQ_FINE 0x63U +typedef struct +{ + uint8_t freq_fine : 8; +} asm330lhh_internal_freq_fine_t; + +#define ASM330LHH_X_OFS_USR 0x73U +#define ASM330LHH_Y_OFS_USR 0x74U +#define ASM330LHH_Z_OFS_USR 0x75U +#define ASM330LHH_FIFO_DATA_OUT_TAG 0x78U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tag_parity : 1; + uint8_t tag_cnt : 2; + uint8_t tag_sensor : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t tag_sensor : 5; + uint8_t tag_cnt : 2; + uint8_t tag_parity : 1; +#endif /* DRV_BYTE_ORDER */ +} asm330lhh_fifo_data_out_tag_t; + +#define ASM330LHH_FIFO_DATA_OUT_X_L 0x79U +#define ASM330LHH_FIFO_DATA_OUT_X_H 0x7AU +#define ASM330LHH_FIFO_DATA_OUT_Y_L 0x7BU +#define ASM330LHH_FIFO_DATA_OUT_Y_H 0x7CU +#define ASM330LHH_FIFO_DATA_OUT_Z_L 0x7DU +#define ASM330LHH_FIFO_DATA_OUT_Z_H 0x7EU + +/** + * @defgroup ASM330LHH_Register_Union + * @brief This union group all the registers having a bit-field + * description. + * This union is useful but it's not needed by the driver. + * + * REMOVING this union you are compliant with: + * MISRA-C 2012 [Rule 19.2] -> " Union are not allowed " + * + * @{ + * + */ +typedef union +{ + asm330lhh_pin_ctrl_t pin_ctrl; + asm330lhh_fifo_ctrl1_t fifo_ctrl1; + asm330lhh_fifo_ctrl2_t fifo_ctrl2; + asm330lhh_fifo_ctrl3_t fifo_ctrl3; + asm330lhh_fifo_ctrl4_t fifo_ctrl4; + asm330lhh_counter_bdr_reg1_t counter_bdr_reg1; + asm330lhh_counter_bdr_reg2_t counter_bdr_reg2; + asm330lhh_int1_ctrl_t int1_ctrl; + asm330lhh_int2_ctrl_t int2_ctrl; + asm330lhh_ctrl1_xl_t ctrl1_xl; + asm330lhh_ctrl2_g_t ctrl2_g; + asm330lhh_ctrl3_c_t ctrl3_c; + asm330lhh_ctrl4_c_t ctrl4_c; + asm330lhh_ctrl5_c_t ctrl5_c; + asm330lhh_ctrl6_c_t ctrl6_c; + asm330lhh_ctrl7_g_t ctrl7_g; + asm330lhh_ctrl8_xl_t ctrl8_xl; + asm330lhh_ctrl9_xl_t ctrl9_xl; + asm330lhh_ctrl10_c_t ctrl10_c; + asm330lhh_all_int_src_t all_int_src; + asm330lhh_wake_up_src_t wake_up_src; + asm330lhh_d6d_src_t d6d_src; + asm330lhh_status_reg_t status_reg; + asm330lhh_fifo_status1_t fifo_status1; + asm330lhh_fifo_status2_t fifo_status2; + asm330lhh_int_cfg0_t int_cfg0; + asm330lhh_int_cfg1_t int_cfg1; + asm330lhh_ths_6d_t ths_6d; + asm330lhh_wake_up_ths_t wake_up_ths; + asm330lhh_wake_up_dur_t wake_up_dur; + asm330lhh_free_fall_t free_fall; + asm330lhh_md1_cfg_t md1_cfg; + asm330lhh_md2_cfg_t md2_cfg; + asm330lhh_internal_freq_fine_t internal_freq_fine; + asm330lhh_fifo_data_out_tag_t fifo_data_out_tag; + bitwise_t bitwise; + uint8_t byte; +} asm330lhh_reg_t; + +/** + * @} + * + */ + +int32_t asm330lhh_read_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); +int32_t asm330lhh_write_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); + +float_t asm330lhh_from_fs2g_to_mg(int16_t lsb); +float_t asm330lhh_from_fs4g_to_mg(int16_t lsb); +float_t asm330lhh_from_fs8g_to_mg(int16_t lsb); +float_t asm330lhh_from_fs16g_to_mg(int16_t lsb); + +float_t asm330lhh_from_fs125dps_to_mdps(int16_t lsb); +float_t asm330lhh_from_fs250dps_to_mdps(int16_t lsb); +float_t asm330lhh_from_fs500dps_to_mdps(int16_t lsb); +float_t asm330lhh_from_fs1000dps_to_mdps(int16_t lsb); +float_t asm330lhh_from_fs2000dps_to_mdps(int16_t lsb); +float_t asm330lhh_from_fs4000dps_to_mdps(int16_t lsb); + +float_t asm330lhh_from_lsb_to_celsius(int16_t lsb); + +float_t asm330lhh_from_lsb_to_nsec(int32_t lsb); + +typedef enum +{ + ASM330LHH_2g = 0, + ASM330LHH_16g = 1, /* if XL_FS_MODE = '1' -> ASM330LHH_2g */ + ASM330LHH_4g = 2, + ASM330LHH_8g = 3, +} asm330lhh_fs_xl_t; +int32_t asm330lhh_xl_full_scale_set(stmdev_ctx_t *ctx, + asm330lhh_fs_xl_t val); +int32_t asm330lhh_xl_full_scale_get(stmdev_ctx_t *ctx, + asm330lhh_fs_xl_t *val); + +typedef enum +{ + ASM330LHH_XL_ODR_OFF = 0, + ASM330LHH_XL_ODR_12Hz5 = 1, + ASM330LHH_XL_ODR_26Hz = 2, + ASM330LHH_XL_ODR_52Hz = 3, + ASM330LHH_XL_ODR_104Hz = 4, + ASM330LHH_XL_ODR_208Hz = 5, + ASM330LHH_XL_ODR_417Hz = 6, + ASM330LHH_XL_ODR_833Hz = 7, + ASM330LHH_XL_ODR_1667Hz = 8, + ASM330LHH_XL_ODR_3333Hz = 9, + ASM330LHH_XL_ODR_6667Hz = 10, +} asm330lhh_odr_xl_t; +int32_t asm330lhh_xl_data_rate_set(stmdev_ctx_t *ctx, + asm330lhh_odr_xl_t val); +int32_t asm330lhh_xl_data_rate_get(stmdev_ctx_t *ctx, + asm330lhh_odr_xl_t *val); + +typedef enum +{ + ASM330LHH_125dps = 2, + ASM330LHH_250dps = 0, + ASM330LHH_500dps = 4, + ASM330LHH_1000dps = 8, + ASM330LHH_2000dps = 12, + ASM330LHH_4000dps = 1, +} asm330lhh_fs_g_t; +int32_t asm330lhh_gy_full_scale_set(stmdev_ctx_t *ctx, + asm330lhh_fs_g_t val); +int32_t asm330lhh_gy_full_scale_get(stmdev_ctx_t *ctx, + asm330lhh_fs_g_t *val); + +typedef enum +{ + ASM330LHH_GY_ODR_OFF = 0, + ASM330LHH_GY_ODR_12Hz5 = 1, + ASM330LHH_GY_ODR_26Hz = 2, + ASM330LHH_GY_ODR_52Hz = 3, + ASM330LHH_GY_ODR_104Hz = 4, + ASM330LHH_GY_ODR_208Hz = 5, + ASM330LHH_GY_ODR_417Hz = 6, + ASM330LHH_GY_ODR_833Hz = 7, + ASM330LHH_GY_ODR_1667Hz = 8, + ASM330LHH_GY_ODR_3333Hz = 9, + ASM330LHH_GY_ODR_6667Hz = 10, +} asm330lhh_odr_g_t; +int32_t asm330lhh_gy_data_rate_set(stmdev_ctx_t *ctx, + asm330lhh_odr_g_t val); +int32_t asm330lhh_gy_data_rate_get(stmdev_ctx_t *ctx, + asm330lhh_odr_g_t *val); + +int32_t asm330lhh_block_data_update_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t asm330lhh_block_data_update_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + ASM330LHH_LSb_1mg = 0, + ASM330LHH_LSb_16mg = 1, +} asm330lhh_usr_off_w_t; +int32_t asm330lhh_xl_offset_weight_set(stmdev_ctx_t *ctx, + asm330lhh_usr_off_w_t val); +int32_t asm330lhh_xl_offset_weight_get(stmdev_ctx_t *ctx, + asm330lhh_usr_off_w_t *val); + +typedef struct +{ + asm330lhh_all_int_src_t all_int_src; + asm330lhh_wake_up_src_t wake_up_src; + asm330lhh_d6d_src_t d6d_src; + asm330lhh_status_reg_t status_reg; +} asm330lhh_all_sources_t; +int32_t asm330lhh_all_sources_get(stmdev_ctx_t *ctx, + asm330lhh_all_sources_t *val); + +int32_t asm330lhh_status_reg_get(stmdev_ctx_t *ctx, + asm330lhh_status_reg_t *val); + +int32_t asm330lhh_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t asm330lhh_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t asm330lhh_temp_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t asm330lhh_xl_usr_offset_x_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t asm330lhh_xl_usr_offset_x_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t asm330lhh_xl_usr_offset_y_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t asm330lhh_xl_usr_offset_y_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t asm330lhh_xl_usr_offset_z_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t asm330lhh_xl_usr_offset_z_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t asm330lhh_xl_usr_offset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_xl_usr_offset_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_timestamp_rst(stmdev_ctx_t *ctx); + +int32_t asm330lhh_timestamp_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val); + +typedef enum +{ + ASM330LHH_NO_ROUND = 0, + ASM330LHH_ROUND_XL = 1, + ASM330LHH_ROUND_GY = 2, + ASM330LHH_ROUND_GY_XL = 3, +} asm330lhh_rounding_t; +int32_t asm330lhh_rounding_mode_set(stmdev_ctx_t *ctx, + asm330lhh_rounding_t val); +int32_t asm330lhh_rounding_mode_get(stmdev_ctx_t *ctx, + asm330lhh_rounding_t *val); + +int32_t asm330lhh_temperature_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t asm330lhh_angular_rate_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t asm330lhh_acceleration_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t asm330lhh_fifo_out_raw_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_device_conf_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_device_conf_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_odr_cal_reg_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_odr_cal_reg_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + ASM330LHH_DRDY_LATCHED = 0, + ASM330LHH_DRDY_PULSED = 1, +} asm330lhh_dataready_pulsed_t; +int32_t asm330lhh_data_ready_mode_set(stmdev_ctx_t *ctx, + asm330lhh_dataready_pulsed_t val); +int32_t asm330lhh_data_ready_mode_get(stmdev_ctx_t *ctx, + asm330lhh_dataready_pulsed_t *val); + +int32_t asm330lhh_device_id_get(stmdev_ctx_t *ctx, uint8_t *buff); + +int32_t asm330lhh_reset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_reset_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_boot_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_boot_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + ASM330LHH_XL_ST_DISABLE = 0, + ASM330LHH_XL_ST_POSITIVE = 1, + ASM330LHH_XL_ST_NEGATIVE = 2, +} asm330lhh_st_xl_t; +int32_t asm330lhh_xl_self_test_set(stmdev_ctx_t *ctx, + asm330lhh_st_xl_t val); +int32_t asm330lhh_xl_self_test_get(stmdev_ctx_t *ctx, + asm330lhh_st_xl_t *val); + +typedef enum +{ + ASM330LHH_GY_ST_DISABLE = 0, + ASM330LHH_GY_ST_POSITIVE = 1, + ASM330LHH_GY_ST_NEGATIVE = 3, +} asm330lhh_st_g_t; +int32_t asm330lhh_gy_self_test_set(stmdev_ctx_t *ctx, + asm330lhh_st_g_t val); +int32_t asm330lhh_gy_self_test_get(stmdev_ctx_t *ctx, + asm330lhh_st_g_t *val); + +int32_t asm330lhh_xl_filter_lp2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_xl_filter_lp2_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_gy_filter_lp1_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_gy_filter_lp1_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_filter_settling_mask_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t asm330lhh_filter_settling_mask_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + ASM330LHH_ULTRA_LIGHT = 0, + ASM330LHH_VERY_LIGHT = 1, + ASM330LHH_LIGHT = 2, + ASM330LHH_MEDIUM = 3, + ASM330LHH_STRONG = 4, + ASM330LHH_VERY_STRONG = 5, + ASM330LHH_AGGRESSIVE = 6, + ASM330LHH_XTREME = 7, +} asm330lhh_ftype_t; +int32_t asm330lhh_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + asm330lhh_ftype_t val); +int32_t asm330lhh_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + asm330lhh_ftype_t *val); + +int32_t asm330lhh_xl_lp2_on_6d_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_xl_lp2_on_6d_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + ASM330LHH_HP_PATH_DISABLE_ON_OUT = 0x00, + ASM330LHH_SLOPE_ODR_DIV_4 = 0x10, + ASM330LHH_HP_ODR_DIV_10 = 0x11, + ASM330LHH_HP_ODR_DIV_20 = 0x12, + ASM330LHH_HP_ODR_DIV_45 = 0x13, + ASM330LHH_HP_ODR_DIV_100 = 0x14, + ASM330LHH_HP_ODR_DIV_200 = 0x15, + ASM330LHH_HP_ODR_DIV_400 = 0x16, + ASM330LHH_HP_ODR_DIV_800 = 0x17, + ASM330LHH_HP_REF_MD_ODR_DIV_10 = 0x31, + ASM330LHH_HP_REF_MD_ODR_DIV_20 = 0x32, + ASM330LHH_HP_REF_MD_ODR_DIV_45 = 0x33, + ASM330LHH_HP_REF_MD_ODR_DIV_100 = 0x34, + ASM330LHH_HP_REF_MD_ODR_DIV_200 = 0x35, + ASM330LHH_HP_REF_MD_ODR_DIV_400 = 0x36, + ASM330LHH_HP_REF_MD_ODR_DIV_800 = 0x37, + ASM330LHH_LP_ODR_DIV_10 = 0x01, + ASM330LHH_LP_ODR_DIV_20 = 0x02, + ASM330LHH_LP_ODR_DIV_45 = 0x03, + ASM330LHH_LP_ODR_DIV_100 = 0x04, + ASM330LHH_LP_ODR_DIV_200 = 0x05, + ASM330LHH_LP_ODR_DIV_400 = 0x06, + ASM330LHH_LP_ODR_DIV_800 = 0x07, +} asm330lhh_hp_slope_xl_en_t; +int32_t asm330lhh_xl_hp_path_on_out_set(stmdev_ctx_t *ctx, + asm330lhh_hp_slope_xl_en_t val); +int32_t asm330lhh_xl_hp_path_on_out_get(stmdev_ctx_t *ctx, + asm330lhh_hp_slope_xl_en_t *val); + +int32_t asm330lhh_xl_fast_settling_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t asm330lhh_xl_fast_settling_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + ASM330LHH_USE_SLOPE = 0, + ASM330LHH_USE_HPF = 1, +} asm330lhh_slope_fds_t; +int32_t asm330lhh_xl_hp_path_internal_set(stmdev_ctx_t *ctx, + asm330lhh_slope_fds_t val); +int32_t asm330lhh_xl_hp_path_internal_get(stmdev_ctx_t *ctx, + asm330lhh_slope_fds_t *val); + +typedef enum +{ + ASM330LHH_HP_FILTER_NONE = 0x00, + ASM330LHH_HP_FILTER_16mHz = 0x80, + ASM330LHH_HP_FILTER_65mHz = 0x81, + ASM330LHH_HP_FILTER_260mHz = 0x82, + ASM330LHH_HP_FILTER_1Hz04 = 0x83, +} asm330lhh_hpm_g_t; +int32_t asm330lhh_gy_hp_path_internal_set(stmdev_ctx_t *ctx, + asm330lhh_hpm_g_t val); +int32_t asm330lhh_gy_hp_path_internal_get(stmdev_ctx_t *ctx, + asm330lhh_hpm_g_t *val); + +typedef enum +{ + ASM330LHH_PULL_UP_DISC = 0, + ASM330LHH_PULL_UP_CONNECT = 1, +} asm330lhh_sdo_pu_en_t; +int32_t asm330lhh_sdo_sa0_mode_set(stmdev_ctx_t *ctx, + asm330lhh_sdo_pu_en_t val); +int32_t asm330lhh_sdo_sa0_mode_get(stmdev_ctx_t *ctx, + asm330lhh_sdo_pu_en_t *val); + +typedef enum +{ + ASM330LHH_SPI_4_WIRE = 0, + ASM330LHH_SPI_3_WIRE = 1, +} asm330lhh_sim_t; +int32_t asm330lhh_spi_mode_set(stmdev_ctx_t *ctx, + asm330lhh_sim_t val); +int32_t asm330lhh_spi_mode_get(stmdev_ctx_t *ctx, + asm330lhh_sim_t *val); + +typedef enum +{ + ASM330LHH_I2C_ENABLE = 0, + ASM330LHH_I2C_DISABLE = 1, +} asm330lhh_i2c_disable_t; +int32_t asm330lhh_i2c_interface_set(stmdev_ctx_t *ctx, + asm330lhh_i2c_disable_t val); +int32_t asm330lhh_i2c_interface_get(stmdev_ctx_t *ctx, + asm330lhh_i2c_disable_t *val); + +typedef struct +{ + asm330lhh_int1_ctrl_t int1_ctrl; + asm330lhh_md1_cfg_t md1_cfg; +} asm330lhh_pin_int1_route_t; +int32_t asm330lhh_pin_int1_route_set(stmdev_ctx_t *ctx, + asm330lhh_pin_int1_route_t *val); +int32_t asm330lhh_pin_int1_route_get(stmdev_ctx_t *ctx, + asm330lhh_pin_int1_route_t *val); + +typedef struct +{ + asm330lhh_int2_ctrl_t int2_ctrl; + asm330lhh_md2_cfg_t md2_cfg; +} asm330lhh_pin_int2_route_t; +int32_t asm330lhh_pin_int2_route_set(stmdev_ctx_t *ctx, + asm330lhh_pin_int2_route_t *val); +int32_t asm330lhh_pin_int2_route_get(stmdev_ctx_t *ctx, + asm330lhh_pin_int2_route_t *val); + +typedef enum +{ + ASM330LHH_PUSH_PULL = 0, + ASM330LHH_OPEN_DRAIN = 1, +} asm330lhh_pp_od_t; +int32_t asm330lhh_pin_mode_set(stmdev_ctx_t *ctx, + asm330lhh_pp_od_t val); +int32_t asm330lhh_pin_mode_get(stmdev_ctx_t *ctx, + asm330lhh_pp_od_t *val); + +typedef enum +{ + ASM330LHH_ACTIVE_HIGH = 0, + ASM330LHH_ACTIVE_LOW = 1, +} asm330lhh_h_lactive_t; +int32_t asm330lhh_pin_polarity_set(stmdev_ctx_t *ctx, + asm330lhh_h_lactive_t val); +int32_t asm330lhh_pin_polarity_get(stmdev_ctx_t *ctx, + asm330lhh_h_lactive_t *val); + +int32_t asm330lhh_all_on_int1_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_all_on_int1_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + ASM330LHH_ALL_INT_PULSED = 0, + ASM330LHH_ALL_INT_LATCHED = 3, +} asm330lhh_lir_t; +int32_t asm330lhh_int_notification_set(stmdev_ctx_t *ctx, + asm330lhh_lir_t val); +int32_t asm330lhh_int_notification_get(stmdev_ctx_t *ctx, + asm330lhh_lir_t *val); + +typedef enum +{ + ASM330LHH_LSb_FS_DIV_64 = 0, + ASM330LHH_LSb_FS_DIV_256 = 1, +} asm330lhh_wake_ths_w_t; +int32_t asm330lhh_wkup_ths_weight_set(stmdev_ctx_t *ctx, + asm330lhh_wake_ths_w_t val); +int32_t asm330lhh_wkup_ths_weight_get(stmdev_ctx_t *ctx, + asm330lhh_wake_ths_w_t *val); + +int32_t asm330lhh_wkup_threshold_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_wkup_threshold_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_xl_usr_offset_on_wkup_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t asm330lhh_xl_usr_offset_on_wkup_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t asm330lhh_wkup_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_wkup_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_gy_sleep_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_gy_sleep_mode_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + ASM330LHH_DRIVE_SLEEP_CHG_EVENT = 0, + ASM330LHH_DRIVE_SLEEP_STATUS = 1, +} asm330lhh_sleep_status_on_int_t; +int32_t asm330lhh_act_pin_notification_set(stmdev_ctx_t *ctx, + asm330lhh_sleep_status_on_int_t val); +int32_t asm330lhh_act_pin_notification_get(stmdev_ctx_t *ctx, + asm330lhh_sleep_status_on_int_t *val); + +typedef enum +{ + ASM330LHH_XL_AND_GY_NOT_AFFECTED = 0, + ASM330LHH_XL_12Hz5_GY_NOT_AFFECTED = 1, + ASM330LHH_XL_12Hz5_GY_SLEEP = 2, + ASM330LHH_XL_12Hz5_GY_PD = 3, +} asm330lhh_inact_en_t; +int32_t asm330lhh_act_mode_set(stmdev_ctx_t *ctx, + asm330lhh_inact_en_t val); +int32_t asm330lhh_act_mode_get(stmdev_ctx_t *ctx, + asm330lhh_inact_en_t *val); + +int32_t asm330lhh_act_sleep_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_act_sleep_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + ASM330LHH_DEG_80 = 0, + ASM330LHH_DEG_70 = 1, + ASM330LHH_DEG_60 = 2, + ASM330LHH_DEG_50 = 3, +} asm330lhh_sixd_ths_t; +int32_t asm330lhh_6d_threshold_set(stmdev_ctx_t *ctx, + asm330lhh_sixd_ths_t val); +int32_t asm330lhh_6d_threshold_get(stmdev_ctx_t *ctx, + asm330lhh_sixd_ths_t *val); + +int32_t asm330lhh_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + ASM330LHH_FF_TSH_156mg = 0, + ASM330LHH_FF_TSH_219mg = 1, + ASM330LHH_FF_TSH_250mg = 2, + ASM330LHH_FF_TSH_312mg = 3, + ASM330LHH_FF_TSH_344mg = 4, + ASM330LHH_FF_TSH_406mg = 5, + ASM330LHH_FF_TSH_469mg = 6, + ASM330LHH_FF_TSH_500mg = 7, +} asm330lhh_ff_ths_t; +int32_t asm330lhh_ff_threshold_set(stmdev_ctx_t *ctx, + asm330lhh_ff_ths_t val); +int32_t asm330lhh_ff_threshold_get(stmdev_ctx_t *ctx, + asm330lhh_ff_ths_t *val); + +int32_t asm330lhh_ff_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_ff_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_fifo_watermark_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t asm330lhh_fifo_watermark_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t asm330lhh_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t asm330lhh_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t asm330lhh_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t asm330lhh_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + ASM330LHH_XL_NOT_BATCHED = 0, + ASM330LHH_XL_BATCHED_AT_12Hz5 = 1, + ASM330LHH_XL_BATCHED_AT_26Hz = 2, + ASM330LHH_XL_BATCHED_AT_52Hz = 3, + ASM330LHH_XL_BATCHED_AT_104Hz = 4, + ASM330LHH_XL_BATCHED_AT_208Hz = 5, + ASM330LHH_XL_BATCHED_AT_417Hz = 6, + ASM330LHH_XL_BATCHED_AT_833Hz = 7, + ASM330LHH_XL_BATCHED_AT_1667Hz = 8, + ASM330LHH_XL_BATCHED_AT_3333Hz = 9, + ASM330LHH_XL_BATCHED_AT_6667Hz = 10, + ASM330LHH_XL_BATCHED_AT_6Hz5 = 11, +} asm330lhh_bdr_xl_t; +int32_t asm330lhh_fifo_xl_batch_set(stmdev_ctx_t *ctx, + asm330lhh_bdr_xl_t val); +int32_t asm330lhh_fifo_xl_batch_get(stmdev_ctx_t *ctx, + asm330lhh_bdr_xl_t *val); + +typedef enum +{ + ASM330LHH_GY_NOT_BATCHED = 0, + ASM330LHH_GY_BATCHED_AT_12Hz5 = 1, + ASM330LHH_GY_BATCHED_AT_26Hz = 2, + ASM330LHH_GY_BATCHED_AT_52Hz = 3, + ASM330LHH_GY_BATCHED_AT_104Hz = 4, + ASM330LHH_GY_BATCHED_AT_208Hz = 5, + ASM330LHH_GY_BATCHED_AT_417Hz = 6, + ASM330LHH_GY_BATCHED_AT_833Hz = 7, + ASM330LHH_GY_BATCHED_AT_1667Hz = 8, + ASM330LHH_GY_BATCHED_AT_3333Hz = 9, + ASM330LHH_GY_BATCHED_AT_6667Hz = 10, + ASM330LHH_GY_BATCHED_AT_6Hz5 = 11, +} asm330lhh_bdr_gy_t; +int32_t asm330lhh_fifo_gy_batch_set(stmdev_ctx_t *ctx, + asm330lhh_bdr_gy_t val); +int32_t asm330lhh_fifo_gy_batch_get(stmdev_ctx_t *ctx, + asm330lhh_bdr_gy_t *val); + +typedef enum +{ + ASM330LHH_BYPASS_MODE = 0, + ASM330LHH_FIFO_MODE = 1, + ASM330LHH_STREAM_TO_FIFO_MODE = 3, + ASM330LHH_BYPASS_TO_STREAM_MODE = 4, + ASM330LHH_STREAM_MODE = 6, + ASM330LHH_BYPASS_TO_FIFO_MODE = 7, +} asm330lhh_fifo_mode_t; +int32_t asm330lhh_fifo_mode_set(stmdev_ctx_t *ctx, + asm330lhh_fifo_mode_t val); +int32_t asm330lhh_fifo_mode_get(stmdev_ctx_t *ctx, + asm330lhh_fifo_mode_t *val); + +typedef enum +{ + ASM330LHH_TEMP_NOT_BATCHED = 0, + ASM330LHH_TEMP_BATCHED_AT_52Hz = 1, + ASM330LHH_TEMP_BATCHED_AT_12Hz5 = 2, + ASM330LHH_TEMP_BATCHED_AT_1Hz6 = 3, +} asm330lhh_odr_t_batch_t; +int32_t asm330lhh_fifo_temp_batch_set(stmdev_ctx_t *ctx, + asm330lhh_odr_t_batch_t val); +int32_t asm330lhh_fifo_temp_batch_get(stmdev_ctx_t *ctx, + asm330lhh_odr_t_batch_t *val); + +typedef enum +{ + ASM330LHH_NO_DECIMATION = 0, + ASM330LHH_DEC_1 = 1, + ASM330LHH_DEC_8 = 2, + ASM330LHH_DEC_32 = 3, +} asm330lhh_odr_ts_batch_t; +int32_t asm330lhh_fifo_timestamp_decimation_set(stmdev_ctx_t *ctx, + asm330lhh_odr_ts_batch_t val); +int32_t asm330lhh_fifo_timestamp_decimation_get(stmdev_ctx_t *ctx, + asm330lhh_odr_ts_batch_t *val); + +typedef enum +{ + ASM330LHH_XL_BATCH_EVENT = 0, + ASM330LHH_GYRO_BATCH_EVENT = 1, +} asm330lhh_trig_counter_bdr_t; +int32_t asm330lhh_fifo_cnt_event_batch_set(stmdev_ctx_t *ctx, + asm330lhh_trig_counter_bdr_t val); +int32_t asm330lhh_fifo_cnt_event_batch_get(stmdev_ctx_t *ctx, + asm330lhh_trig_counter_bdr_t *val); + +int32_t asm330lhh_rst_batch_counter_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t asm330lhh_rst_batch_counter_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t asm330lhh_batch_counter_threshold_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t asm330lhh_batch_counter_threshold_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t asm330lhh_fifo_data_level_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t asm330lhh_fifo_status_get(stmdev_ctx_t *ctx, + asm330lhh_fifo_status2_t *val); + +int32_t asm330lhh_fifo_full_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_fifo_ovr_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t asm330lhh_fifo_wtm_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + ASM330LHH_GYRO_NC_TAG = 1, + ASM330LHH_XL_NC_TAG, + ASM330LHH_TEMPERATURE_TAG, + ASM330LHH_TIMESTAMP_TAG, + ASM330LHH_CFG_CHANGE_TAG, +} asm330lhh_fifo_tag_t; +int32_t asm330lhh_fifo_sensor_tag_get(stmdev_ctx_t *ctx, + asm330lhh_fifo_tag_t *val); + +typedef enum +{ + ASM330LHH_DEN_DISABLE = 0, + ASM330LHH_LEVEL_FIFO = 6, + ASM330LHH_LEVEL_LETCHED = 3, + ASM330LHH_LEVEL_TRIGGER = 2, + ASM330LHH_EDGE_TRIGGER = 4, +} asm330lhh_den_mode_t; +int32_t asm330lhh_den_mode_set(stmdev_ctx_t *ctx, + asm330lhh_den_mode_t val); +int32_t asm330lhh_den_mode_get(stmdev_ctx_t *ctx, + asm330lhh_den_mode_t *val); + +typedef enum +{ + ASM330LHH_DEN_ACT_LOW = 0, + ASM330LHH_DEN_ACT_HIGH = 1, +} asm330lhh_den_lh_t; +int32_t asm330lhh_den_polarity_set(stmdev_ctx_t *ctx, + asm330lhh_den_lh_t val); +int32_t asm330lhh_den_polarity_get(stmdev_ctx_t *ctx, + asm330lhh_den_lh_t *val); + +typedef enum +{ + ASM330LHH_STAMP_IN_GY_DATA = 0, + ASM330LHH_STAMP_IN_XL_DATA = 1, + ASM330LHH_STAMP_IN_GY_XL_DATA = 2, +} asm330lhh_den_xl_g_t; +int32_t asm330lhh_den_enable_set(stmdev_ctx_t *ctx, + asm330lhh_den_xl_g_t val); +int32_t asm330lhh_den_enable_get(stmdev_ctx_t *ctx, + asm330lhh_den_xl_g_t *val); + +int32_t asm330lhh_den_mark_axis_x_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_den_mark_axis_x_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t asm330lhh_den_mark_axis_y_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_den_mark_axis_y_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t asm330lhh_den_mark_axis_z_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t asm330lhh_den_mark_axis_z_get(stmdev_ctx_t *ctx, + uint8_t *val); + +/** + *@} + * + */ + +#ifdef __cplusplus +} +#endif + +#endif /* ASM330LHH_REGS_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/ASM330LHH/user_ams330lhh.c b/ASM330LHH/user_ams330lhh.c new file mode 100644 index 0000000..109fb34 --- /dev/null +++ b/ASM330LHH/user_ams330lhh.c @@ -0,0 +1,54 @@ + +#include "user_asm330lhh.h" + + + +stmdev_ctx_t spi1_dev ; +extern SPI_HandleTypeDef hspi1; + +static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp,uint16_t len) +{ + GPIO_TypeDef *GPIOx; + uint16_t GPIO_Pin; + if(handle == &hspi1) + { + GPIOx = SPI1_CS_PORT; + GPIO_Pin= SPI1_CS_PIN; + } + HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET); + HAL_SPI_Transmit(handle, ®, 1, 1000); + HAL_SPI_Transmit(handle, (uint8_t*) bufp, len, 1000); + HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET); + return 0; +} + +static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp,uint16_t len) +{ + uint8_t temp=reg; + temp |= 0x80; +// GPIO_TypeDef *GPIOx; +// uint16_t GPIO_Pin; +// if (handle == &hspi1) { +// GPIOx = SPI1_CS_PORT; +// GPIO_Pin = SPI1_CS_PIN; +// } +// HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET); +// HAL_SPI_Transmit(handle, &temp, 1, 1000); +// HAL_SPI_Receive(handle, bufp, len, 1000); +// HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET); + + + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET); + HAL_SPI_Transmit(handle, &temp, 1, 1000); + HAL_SPI_Receive(handle, bufp, len, 1000); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); + return 0; +} + +void user_dev_init() +{ + spi1_dev.handle=&hspi1; + spi1_dev.mdelay=HAL_Delay; + spi1_dev.write_reg=platform_write; + spi1_dev.read_reg=platform_read; +} diff --git a/ASM330LHH/user_asm330lhh.h b/ASM330LHH/user_asm330lhh.h new file mode 100644 index 0000000..fe42b2c --- /dev/null +++ b/ASM330LHH/user_asm330lhh.h @@ -0,0 +1,24 @@ +#ifndef USER_ASM330LHH +#define USER_ASM330LHH + +#define SPI1_CS_PORT GPIOA +#define SPI1_CS_PIN GPIO_PIN_4 + +#include +#include +#include "asm330lhh_reg.h" +#include "stm32f1xx_hal.h" +#include "rtklib.h" + +#define IMU_READY 1 +#define IMU_NOT_READY 1 +#define IMU_LOCK 1 +#define IMU_UNLOCK 0 + + +void user_dev_init(); +extern uint8_t rx_uart2; + +extern stmdev_ctx_t spi1_dev; + +#endif diff --git a/Core/Inc/main.h b/Core/Inc/main.h new file mode 100644 index 0000000..503dd4c --- /dev/null +++ b/Core/Inc/main.h @@ -0,0 +1,69 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.h + * @brief : Header for main.c file. + * This file contains the common defines of the application. + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __MAIN_H +#define __MAIN_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f1xx_hal.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Exported types ------------------------------------------------------------*/ +/* USER CODE BEGIN ET */ + +/* USER CODE END ET */ + +/* Exported constants --------------------------------------------------------*/ +/* USER CODE BEGIN EC */ + +/* USER CODE END EC */ + +/* Exported macro ------------------------------------------------------------*/ +/* USER CODE BEGIN EM */ + +/* USER CODE END EM */ + +/* Exported functions prototypes ---------------------------------------------*/ +void Error_Handler(void); + +/* USER CODE BEGIN EFP */ + +/* USER CODE END EFP */ + +/* Private defines -----------------------------------------------------------*/ + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +#ifdef __cplusplus +} +#endif + +#endif /* __MAIN_H */ diff --git a/Core/Inc/stm32f1xx_hal_conf.h b/Core/Inc/stm32f1xx_hal_conf.h new file mode 100644 index 0000000..e5ea88b --- /dev/null +++ b/Core/Inc/stm32f1xx_hal_conf.h @@ -0,0 +1,391 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f1xx_hal_conf.h + * @brief HAL configuration file. + ****************************************************************************** + * @attention + * + * Copyright (c) 2017 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F1xx_HAL_CONF_H +#define __STM32F1xx_HAL_CONF_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/* ########################## Module Selection ############################## */ +/** + * @brief This is the list of modules to be used in the HAL driver + */ + +#define HAL_MODULE_ENABLED + /*#define HAL_ADC_MODULE_ENABLED */ +/*#define HAL_CRYP_MODULE_ENABLED */ +/*#define HAL_CAN_MODULE_ENABLED */ +/*#define HAL_CAN_LEGACY_MODULE_ENABLED */ +/*#define HAL_CEC_MODULE_ENABLED */ +/*#define HAL_CORTEX_MODULE_ENABLED */ +/*#define HAL_CRC_MODULE_ENABLED */ +/*#define HAL_DAC_MODULE_ENABLED */ +/*#define HAL_DMA_MODULE_ENABLED */ +/*#define HAL_ETH_MODULE_ENABLED */ +/*#define HAL_FLASH_MODULE_ENABLED */ +#define HAL_GPIO_MODULE_ENABLED +/*#define HAL_I2C_MODULE_ENABLED */ +/*#define HAL_I2S_MODULE_ENABLED */ +/*#define HAL_IRDA_MODULE_ENABLED */ +/*#define HAL_IWDG_MODULE_ENABLED */ +/*#define HAL_NOR_MODULE_ENABLED */ +/*#define HAL_NAND_MODULE_ENABLED */ +/*#define HAL_PCCARD_MODULE_ENABLED */ +/*#define HAL_PCD_MODULE_ENABLED */ +/*#define HAL_HCD_MODULE_ENABLED */ +/*#define HAL_PWR_MODULE_ENABLED */ +/*#define HAL_RCC_MODULE_ENABLED */ +/*#define HAL_RTC_MODULE_ENABLED */ +/*#define HAL_SD_MODULE_ENABLED */ +/*#define HAL_MMC_MODULE_ENABLED */ +/*#define HAL_SDRAM_MODULE_ENABLED */ +/*#define HAL_SMARTCARD_MODULE_ENABLED */ +#define HAL_SPI_MODULE_ENABLED +/*#define HAL_SRAM_MODULE_ENABLED */ +#define HAL_TIM_MODULE_ENABLED +#define HAL_UART_MODULE_ENABLED +/*#define HAL_USART_MODULE_ENABLED */ +/*#define HAL_WWDG_MODULE_ENABLED */ + +#define HAL_CORTEX_MODULE_ENABLED +#define HAL_DMA_MODULE_ENABLED +#define HAL_FLASH_MODULE_ENABLED +#define HAL_EXTI_MODULE_ENABLED +#define HAL_GPIO_MODULE_ENABLED +#define HAL_PWR_MODULE_ENABLED +#define HAL_RCC_MODULE_ENABLED + +/* ########################## Oscillator Values adaptation ####################*/ +/** + * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSE is used as system clock source, directly or through the PLL). + */ +#if !defined (HSE_VALUE) + #define HSE_VALUE 8000000U /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined (HSE_STARTUP_TIMEOUT) + #define HSE_STARTUP_TIMEOUT 100U /*!< Time out for HSE start up, in ms */ +#endif /* HSE_STARTUP_TIMEOUT */ + +/** + * @brief Internal High Speed oscillator (HSI) value. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSI is used as system clock source, directly or through the PLL). + */ +#if !defined (HSI_VALUE) + #define HSI_VALUE 8000000U /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ + +/** + * @brief Internal Low Speed oscillator (LSI) value. + */ +#if !defined (LSI_VALUE) + #define LSI_VALUE 40000U /*!< LSI Typical Value in Hz */ +#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz + The real value may vary depending on the variations + in voltage and temperature. */ + +/** + * @brief External Low Speed oscillator (LSE) value. + * This value is used by the UART, RTC HAL module to compute the system frequency + */ +#if !defined (LSE_VALUE) + #define LSE_VALUE 32768U /*!< Value of the External oscillator in Hz*/ +#endif /* LSE_VALUE */ + +#if !defined (LSE_STARTUP_TIMEOUT) + #define LSE_STARTUP_TIMEOUT 5000U /*!< Time out for LSE start up, in ms */ +#endif /* LSE_STARTUP_TIMEOUT */ + +/* Tip: To avoid modifying this file each time you need to use different HSE, + === you can define the HSE value in your toolchain compiler preprocessor. */ + +/* ########################### System Configuration ######################### */ +/** + * @brief This is the HAL system configuration section + */ +#define VDD_VALUE 3300U /*!< Value of VDD in mv */ +#define TICK_INT_PRIORITY 15U /*!< tick interrupt priority (lowest by default) */ +#define USE_RTOS 0U +#define PREFETCH_ENABLE 1U + +#define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ +#define USE_HAL_CAN_REGISTER_CALLBACKS 0U /* CAN register callback disabled */ +#define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ +#define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ +#define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */ +#define USE_HAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */ +#define USE_HAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */ +#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */ +#define USE_HAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */ +#define USE_HAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */ +#define USE_HAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */ +#define USE_HAL_PCCARD_REGISTER_CALLBACKS 0U /* PCCARD register callback disabled */ +#define USE_HAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */ +#define USE_HAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */ +#define USE_HAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */ +#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */ +#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */ +#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */ +#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */ +#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */ +#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */ +#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */ +#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */ + +/* ########################## Assert Selection ############################## */ +/** + * @brief Uncomment the line below to expanse the "assert_param" macro in the + * HAL drivers code + */ +/* #define USE_FULL_ASSERT 1U */ + +/* ################## Ethernet peripheral configuration ##################### */ + +/* Section 1 : Ethernet peripheral configuration */ + +/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */ +#define MAC_ADDR0 2U +#define MAC_ADDR1 0U +#define MAC_ADDR2 0U +#define MAC_ADDR3 0U +#define MAC_ADDR4 0U +#define MAC_ADDR5 0U + +/* Definition of the Ethernet driver buffers size and count */ +#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ +#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ +#define ETH_RXBUFNB 8U /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ +#define ETH_TXBUFNB 4U /* 4 Tx buffers of size ETH_TX_BUF_SIZE */ + +/* Section 2: PHY configuration section */ + +/* DP83848_PHY_ADDRESS Address*/ +#define DP83848_PHY_ADDRESS 0x01U +/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/ +#define PHY_RESET_DELAY 0x000000FFU +/* PHY Configuration delay */ +#define PHY_CONFIG_DELAY 0x00000FFFU + +#define PHY_READ_TO 0x0000FFFFU +#define PHY_WRITE_TO 0x0000FFFFU + +/* Section 3: Common PHY Registers */ + +#define PHY_BCR ((uint16_t)0x00) /*!< Transceiver Basic Control Register */ +#define PHY_BSR ((uint16_t)0x01) /*!< Transceiver Basic Status Register */ + +#define PHY_RESET ((uint16_t)0x8000) /*!< PHY Reset */ +#define PHY_LOOPBACK ((uint16_t)0x4000) /*!< Select loop-back mode */ +#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100) /*!< Set the full-duplex mode at 100 Mb/s */ +#define PHY_HALFDUPLEX_100M ((uint16_t)0x2000) /*!< Set the half-duplex mode at 100 Mb/s */ +#define PHY_FULLDUPLEX_10M ((uint16_t)0x0100) /*!< Set the full-duplex mode at 10 Mb/s */ +#define PHY_HALFDUPLEX_10M ((uint16_t)0x0000) /*!< Set the half-duplex mode at 10 Mb/s */ +#define PHY_AUTONEGOTIATION ((uint16_t)0x1000) /*!< Enable auto-negotiation function */ +#define PHY_RESTART_AUTONEGOTIATION ((uint16_t)0x0200) /*!< Restart auto-negotiation function */ +#define PHY_POWERDOWN ((uint16_t)0x0800) /*!< Select the power down mode */ +#define PHY_ISOLATE ((uint16_t)0x0400) /*!< Isolate PHY from MII */ + +#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020) /*!< Auto-Negotiation process completed */ +#define PHY_LINKED_STATUS ((uint16_t)0x0004) /*!< Valid link established */ +#define PHY_JABBER_DETECTION ((uint16_t)0x0002) /*!< Jabber condition detected */ + +/* Section 4: Extended PHY Registers */ +#define PHY_SR ((uint16_t)0x10U) /*!< PHY status register Offset */ + +#define PHY_SPEED_STATUS ((uint16_t)0x0002U) /*!< PHY Speed mask */ +#define PHY_DUPLEX_STATUS ((uint16_t)0x0004U) /*!< PHY Duplex mask */ + +/* ################## SPI peripheral configuration ########################## */ + +/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver +* Activated: CRC code is present inside driver +* Deactivated: CRC code cleaned from driver +*/ + +#define USE_SPI_CRC 0U + +/* Includes ------------------------------------------------------------------*/ +/** + * @brief Include module's header file + */ + +#ifdef HAL_RCC_MODULE_ENABLED +#include "stm32f1xx_hal_rcc.h" +#endif /* HAL_RCC_MODULE_ENABLED */ + +#ifdef HAL_GPIO_MODULE_ENABLED +#include "stm32f1xx_hal_gpio.h" +#endif /* HAL_GPIO_MODULE_ENABLED */ + +#ifdef HAL_EXTI_MODULE_ENABLED +#include "stm32f1xx_hal_exti.h" +#endif /* HAL_EXTI_MODULE_ENABLED */ + +#ifdef HAL_DMA_MODULE_ENABLED +#include "stm32f1xx_hal_dma.h" +#endif /* HAL_DMA_MODULE_ENABLED */ + +#ifdef HAL_ETH_MODULE_ENABLED +#include "stm32f1xx_hal_eth.h" +#endif /* HAL_ETH_MODULE_ENABLED */ + +#ifdef HAL_CAN_MODULE_ENABLED +#include "stm32f1xx_hal_can.h" +#endif /* HAL_CAN_MODULE_ENABLED */ + +#ifdef HAL_CAN_LEGACY_MODULE_ENABLED + #include "Legacy/stm32f1xx_hal_can_legacy.h" +#endif /* HAL_CAN_LEGACY_MODULE_ENABLED */ + +#ifdef HAL_CEC_MODULE_ENABLED +#include "stm32f1xx_hal_cec.h" +#endif /* HAL_CEC_MODULE_ENABLED */ + +#ifdef HAL_CORTEX_MODULE_ENABLED +#include "stm32f1xx_hal_cortex.h" +#endif /* HAL_CORTEX_MODULE_ENABLED */ + +#ifdef HAL_ADC_MODULE_ENABLED +#include "stm32f1xx_hal_adc.h" +#endif /* HAL_ADC_MODULE_ENABLED */ + +#ifdef HAL_CRC_MODULE_ENABLED +#include "stm32f1xx_hal_crc.h" +#endif /* HAL_CRC_MODULE_ENABLED */ + +#ifdef HAL_DAC_MODULE_ENABLED +#include "stm32f1xx_hal_dac.h" +#endif /* HAL_DAC_MODULE_ENABLED */ + +#ifdef HAL_FLASH_MODULE_ENABLED +#include "stm32f1xx_hal_flash.h" +#endif /* HAL_FLASH_MODULE_ENABLED */ + +#ifdef HAL_SRAM_MODULE_ENABLED +#include "stm32f1xx_hal_sram.h" +#endif /* HAL_SRAM_MODULE_ENABLED */ + +#ifdef HAL_NOR_MODULE_ENABLED +#include "stm32f1xx_hal_nor.h" +#endif /* HAL_NOR_MODULE_ENABLED */ + +#ifdef HAL_I2C_MODULE_ENABLED +#include "stm32f1xx_hal_i2c.h" +#endif /* HAL_I2C_MODULE_ENABLED */ + +#ifdef HAL_I2S_MODULE_ENABLED +#include "stm32f1xx_hal_i2s.h" +#endif /* HAL_I2S_MODULE_ENABLED */ + +#ifdef HAL_IWDG_MODULE_ENABLED +#include "stm32f1xx_hal_iwdg.h" +#endif /* HAL_IWDG_MODULE_ENABLED */ + +#ifdef HAL_PWR_MODULE_ENABLED +#include "stm32f1xx_hal_pwr.h" +#endif /* HAL_PWR_MODULE_ENABLED */ + +#ifdef HAL_RTC_MODULE_ENABLED +#include "stm32f1xx_hal_rtc.h" +#endif /* HAL_RTC_MODULE_ENABLED */ + +#ifdef HAL_PCCARD_MODULE_ENABLED +#include "stm32f1xx_hal_pccard.h" +#endif /* HAL_PCCARD_MODULE_ENABLED */ + +#ifdef HAL_SD_MODULE_ENABLED +#include "stm32f1xx_hal_sd.h" +#endif /* HAL_SD_MODULE_ENABLED */ + +#ifdef HAL_NAND_MODULE_ENABLED +#include "stm32f1xx_hal_nand.h" +#endif /* HAL_NAND_MODULE_ENABLED */ + +#ifdef HAL_SPI_MODULE_ENABLED +#include "stm32f1xx_hal_spi.h" +#endif /* HAL_SPI_MODULE_ENABLED */ + +#ifdef HAL_TIM_MODULE_ENABLED +#include "stm32f1xx_hal_tim.h" +#endif /* HAL_TIM_MODULE_ENABLED */ + +#ifdef HAL_UART_MODULE_ENABLED +#include "stm32f1xx_hal_uart.h" +#endif /* HAL_UART_MODULE_ENABLED */ + +#ifdef HAL_USART_MODULE_ENABLED +#include "stm32f1xx_hal_usart.h" +#endif /* HAL_USART_MODULE_ENABLED */ + +#ifdef HAL_IRDA_MODULE_ENABLED +#include "stm32f1xx_hal_irda.h" +#endif /* HAL_IRDA_MODULE_ENABLED */ + +#ifdef HAL_SMARTCARD_MODULE_ENABLED +#include "stm32f1xx_hal_smartcard.h" +#endif /* HAL_SMARTCARD_MODULE_ENABLED */ + +#ifdef HAL_WWDG_MODULE_ENABLED +#include "stm32f1xx_hal_wwdg.h" +#endif /* HAL_WWDG_MODULE_ENABLED */ + +#ifdef HAL_PCD_MODULE_ENABLED +#include "stm32f1xx_hal_pcd.h" +#endif /* HAL_PCD_MODULE_ENABLED */ + +#ifdef HAL_HCD_MODULE_ENABLED +#include "stm32f1xx_hal_hcd.h" +#endif /* HAL_HCD_MODULE_ENABLED */ + +#ifdef HAL_MMC_MODULE_ENABLED +#include "stm32f1xx_hal_mmc.h" +#endif /* HAL_MMC_MODULE_ENABLED */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ +#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ +void assert_failed(uint8_t* file, uint32_t line); +#else +#define assert_param(expr) ((void)0U) +#endif /* USE_FULL_ASSERT */ + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F1xx_HAL_CONF_H */ + diff --git a/Core/Inc/stm32f1xx_it.h b/Core/Inc/stm32f1xx_it.h new file mode 100644 index 0000000..94424fd --- /dev/null +++ b/Core/Inc/stm32f1xx_it.h @@ -0,0 +1,70 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f1xx_it.h + * @brief This file contains the headers of the interrupt handlers. + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F1xx_IT_H +#define __STM32F1xx_IT_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Exported types ------------------------------------------------------------*/ +/* USER CODE BEGIN ET */ + +/* USER CODE END ET */ + +/* Exported constants --------------------------------------------------------*/ +/* USER CODE BEGIN EC */ + +/* USER CODE END EC */ + +/* Exported macro ------------------------------------------------------------*/ +/* USER CODE BEGIN EM */ + +/* USER CODE END EM */ + +/* Exported functions prototypes ---------------------------------------------*/ +void NMI_Handler(void); +void HardFault_Handler(void); +void MemManage_Handler(void); +void BusFault_Handler(void); +void UsageFault_Handler(void); +void SVC_Handler(void); +void DebugMon_Handler(void); +void PendSV_Handler(void); +void SysTick_Handler(void); +void EXTI0_IRQHandler(void); +void TIM1_BRK_IRQHandler(void); +void USART1_IRQHandler(void); +void USART2_IRQHandler(void); +/* USER CODE BEGIN EFP */ + +/* USER CODE END EFP */ + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F1xx_IT_H */ diff --git a/Core/Src/inter_callback.c b/Core/Src/inter_callback.c new file mode 100644 index 0000000..95a0303 --- /dev/null +++ b/Core/Src/inter_callback.c @@ -0,0 +1,77 @@ +#include "main.h" +#include "user_asm330lhh.h" +#include "asm330lhh_reg.h" +#include "rtklib.h" + +extern uint32_t time_100us; +extern SPI_HandleTypeDef hspi1; + +extern TIM_HandleTypeDef htim1; + +extern UART_HandleTypeDef huart1; +extern UART_HandleTypeDef huart2; +extern nmea_t nmea; + + + + +IMU_mng IMU_mng_st={0}; +IMU_mng IMU_mng_tdk={0}; +extern void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) +{ + if(htim->Instance == htim1.Instance) + { + if(nmea.sol.utctime.sec+0.0001<1) + { + nmea.sol.utctime.sec+=0.0001; + } + if(nmea.sol.time.sec+0.0001<1) + { + nmea.sol.time.sec+=0.0001; + } + } +} +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) +{ + if(GPIO_Pin == GPIO_PIN_1) + { + if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1)==GPIO_PIN_RESET)//PPS impulse + { + nmea.sol.utctime.sec=0; + nmea.sol.time.sec=0;/* time (GPST) */ + nmea.sol.utctime.time++; + nmea.sol.time.time++;/* time (GPST) */ + } + } + if(GPIO_Pin == GPIO_PIN_0) + { + if(HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_0)==GPIO_PIN_RESET)//INI1 from ST IMU + { + if((!IMU_mng_st.lock)&&(!IMU_mng_st.ready)) + { + IMU_mng_st.lock=IMU_LOCK; + IMU_mng_st.time= nmea.sol.time; + IMU_mng_st.utctime= nmea.sol.utctime; + IMU_mng_st.ready=IMU_READY; + IMU_mng_st.lock=IMU_UNLOCK; + } + + } + } +} +uint8_t rx_uart2=0; +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) +{ + if(huart->Instance==huart2.Instance) + { + if(input_nmea(&nmea, rx_uart2)!=1) + { + HAL_UART_Receive_IT(&huart2, &rx_uart2, 1); + } + else + { + HAL_TIM_Base_Start(&htim1); + } + } + +} diff --git a/Core/Src/main.c b/Core/Src/main.c new file mode 100644 index 0000000..773e7cf --- /dev/null +++ b/Core/Src/main.c @@ -0,0 +1,399 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +#include "user_asm330lhh.h" +#include "asm330lhh_reg.h" +#include "rtklib.h" +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +SPI_HandleTypeDef hspi1; + +TIM_HandleTypeDef htim1; + +UART_HandleTypeDef huart1; +UART_HandleTypeDef huart2; + +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +void SystemClock_Config(void); +static void MX_GPIO_Init(void); +static void MX_SPI1_Init(void); +static void MX_USART1_UART_Init(void); +static void MX_TIM1_Init(void); +static void MX_USART2_UART_Init(void); +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ +uint8_t temp[1000]={0}; +uint32_t time_100us=0; +nmea_t nmea={0}; +extern IMU_mng IMU_mng_st; +extern IMU_mng IMU_mng_tdk; +/* USER CODE END 0 */ + +/** + * @brief The application entry point. + * @retval int + */ +int main(void) +{ + /* USER CODE BEGIN 1 */ + + /* USER CODE END 1 */ + + /* MCU Configuration--------------------------------------------------------*/ + + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ + HAL_Init(); + + /* USER CODE BEGIN Init */ + + /* USER CODE END Init */ + + /* Configure the system clock */ + SystemClock_Config(); + + /* USER CODE BEGIN SysInit */ + + /* USER CODE END SysInit */ + + /* Initialize all configured peripherals */ + MX_GPIO_Init(); + MX_SPI1_Init(); + MX_USART1_UART_Init(); + MX_TIM1_Init(); + MX_USART2_UART_Init(); + /* USER CODE BEGIN 2 */ + user_dev_init(); + HAL_Delay(1000); + asm330lhh_device_id_get(&spi1_dev, temp); + + + HAL_UART_Receive_IT(&huart2, &rx_uart2, 1); + /* USER CODE END 2 */ + + /* Infinite loop */ + /* USER CODE BEGIN WHILE */ + while (1) + { + if(IMU_mng_st.ready) + { + //spi read + + + + + IMU_mng_st.ready=IMU_NOT_READY; + } + /* USER CODE END WHILE */ + + /* USER CODE BEGIN 3 */ + } + /* USER CODE END 3 */ +} + +/** + * @brief System Clock Configuration + * @retval None + */ +void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2; + RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) + { + Error_Handler(); + } +} + +/** + * @brief SPI1 Initialization Function + * @param None + * @retval None + */ +static void MX_SPI1_Init(void) +{ + + /* USER CODE BEGIN SPI1_Init 0 */ + + /* USER CODE END SPI1_Init 0 */ + + /* USER CODE BEGIN SPI1_Init 1 */ + + /* USER CODE END SPI1_Init 1 */ + /* SPI1 parameter configuration*/ + hspi1.Instance = SPI1; + hspi1.Init.Mode = SPI_MODE_MASTER; + hspi1.Init.Direction = SPI_DIRECTION_2LINES; + hspi1.Init.DataSize = SPI_DATASIZE_8BIT; + hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH; + hspi1.Init.CLKPhase = SPI_PHASE_2EDGE; + hspi1.Init.NSS = SPI_NSS_SOFT; + hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; + hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB; + hspi1.Init.TIMode = SPI_TIMODE_DISABLE; + hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + hspi1.Init.CRCPolynomial = 10; + if (HAL_SPI_Init(&hspi1) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN SPI1_Init 2 */ + + /* USER CODE END SPI1_Init 2 */ + +} + +/** + * @brief TIM1 Initialization Function + * @param None + * @retval None + */ +static void MX_TIM1_Init(void) +{ + + /* USER CODE BEGIN TIM1_Init 0 */ + + /* USER CODE END TIM1_Init 0 */ + + TIM_ClockConfigTypeDef sClockSourceConfig = {0}; + TIM_MasterConfigTypeDef sMasterConfig = {0}; + + /* USER CODE BEGIN TIM1_Init 1 */ + + /* USER CODE END TIM1_Init 1 */ + htim1.Instance = TIM1; + htim1.Init.Prescaler = 0; + htim1.Init.CounterMode = TIM_COUNTERMODE_UP; + htim1.Init.Period = 6400; + htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim1.Init.RepetitionCounter = 0; + htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + if (HAL_TIM_Base_Init(&htim1) != HAL_OK) + { + Error_Handler(); + } + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK) + { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM1_Init 2 */ + + /* USER CODE END TIM1_Init 2 */ + +} + +/** + * @brief USART1 Initialization Function + * @param None + * @retval None + */ +static void MX_USART1_UART_Init(void) +{ + + /* USER CODE BEGIN USART1_Init 0 */ + + /* USER CODE END USART1_Init 0 */ + + /* USER CODE BEGIN USART1_Init 1 */ + + /* USER CODE END USART1_Init 1 */ + huart1.Instance = USART1; + huart1.Init.BaudRate = 460800; + huart1.Init.WordLength = UART_WORDLENGTH_8B; + huart1.Init.StopBits = UART_STOPBITS_1; + huart1.Init.Parity = UART_PARITY_NONE; + huart1.Init.Mode = UART_MODE_TX_RX; + huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart1.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart1) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART1_Init 2 */ + + /* USER CODE END USART1_Init 2 */ + +} + +/** + * @brief USART2 Initialization Function + * @param None + * @retval None + */ +static void MX_USART2_UART_Init(void) +{ + + /* USER CODE BEGIN USART2_Init 0 */ + + /* USER CODE END USART2_Init 0 */ + + /* USER CODE BEGIN USART2_Init 1 */ + + /* USER CODE END USART2_Init 1 */ + huart2.Instance = USART2; + huart2.Init.BaudRate = 115200; + huart2.Init.WordLength = UART_WORDLENGTH_8B; + huart2.Init.StopBits = UART_STOPBITS_1; + huart2.Init.Parity = UART_PARITY_NONE; + huart2.Init.Mode = UART_MODE_TX_RX; + huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart2.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart2) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART2_Init 2 */ + + /* USER CODE END USART2_Init 2 */ + +} + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ +static void MX_GPIO_Init(void) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); + + /*Configure GPIO pin : PA1 */ + GPIO_InitStruct.Pin = GPIO_PIN_1; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /*Configure GPIO pin : PA4 */ + GPIO_InitStruct.Pin = GPIO_PIN_4; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_PULLUP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /*Configure GPIO pin : PB0 */ + GPIO_InitStruct.Pin = GPIO_PIN_0; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* EXTI interrupt init*/ + HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(EXTI0_IRQn); + +} + +/* USER CODE BEGIN 4 */ + +/* USER CODE END 4 */ + +/** + * @brief This function is executed in case of error occurrence. + * @retval None + */ +void Error_Handler(void) +{ + /* USER CODE BEGIN Error_Handler_Debug */ + /* User can add his own implementation to report the HAL error return state */ + __disable_irq(); + while (1) + { + } + /* USER CODE END Error_Handler_Debug */ +} + +#ifdef USE_FULL_ASSERT +/** + * @brief Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * @param file: pointer to the source file name + * @param line: assert_param error line source number + * @retval None + */ +void assert_failed(uint8_t *file, uint32_t line) +{ + /* USER CODE BEGIN 6 */ + /* User can add his own implementation to report the file name and line number, + ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ + /* USER CODE END 6 */ +} +#endif /* USE_FULL_ASSERT */ diff --git a/Core/Src/stm32f1xx_hal_msp.c b/Core/Src/stm32f1xx_hal_msp.c new file mode 100644 index 0000000..2eaa192 --- /dev/null +++ b/Core/Src/stm32f1xx_hal_msp.c @@ -0,0 +1,328 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f1xx_hal_msp.c + * @brief This file provides code for the MSP Initialization + * and de-Initialization codes. + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN TD */ + +/* USER CODE END TD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN Define */ + +/* USER CODE END Define */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN Macro */ + +/* USER CODE END Macro */ + +/* Private variables ---------------------------------------------------------*/ +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* External functions --------------------------------------------------------*/ +/* USER CODE BEGIN ExternalFunctions */ + +/* USER CODE END ExternalFunctions */ + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ +/** + * Initializes the Global MSP. + */ +void HAL_MspInit(void) +{ + /* USER CODE BEGIN MspInit 0 */ + + /* USER CODE END MspInit 0 */ + + __HAL_RCC_AFIO_CLK_ENABLE(); + __HAL_RCC_PWR_CLK_ENABLE(); + + /* System interrupt init*/ + + /** NOJTAG: JTAG-DP Disabled and SW-DP Enabled + */ + __HAL_AFIO_REMAP_SWJ_NOJTAG(); + + /* USER CODE BEGIN MspInit 1 */ + + /* USER CODE END MspInit 1 */ +} + +/** +* @brief SPI MSP Initialization +* This function configures the hardware resources used in this example +* @param hspi: SPI handle pointer +* @retval None +*/ +void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(hspi->Instance==SPI1) + { + /* USER CODE BEGIN SPI1_MspInit 0 */ + + /* USER CODE END SPI1_MspInit 0 */ + /* Peripheral clock enable */ + __HAL_RCC_SPI1_CLK_ENABLE(); + + __HAL_RCC_GPIOA_CLK_ENABLE(); + /**SPI1 GPIO Configuration + PA5 ------> SPI1_SCK + PA6 ------> SPI1_MISO + PA7 ------> SPI1_MOSI + */ + GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_7; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = GPIO_PIN_6; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USER CODE BEGIN SPI1_MspInit 1 */ + + /* USER CODE END SPI1_MspInit 1 */ + } + +} + +/** +* @brief SPI MSP De-Initialization +* This function freeze the hardware resources used in this example +* @param hspi: SPI handle pointer +* @retval None +*/ +void HAL_SPI_MspDeInit(SPI_HandleTypeDef* hspi) +{ + if(hspi->Instance==SPI1) + { + /* USER CODE BEGIN SPI1_MspDeInit 0 */ + + /* USER CODE END SPI1_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_SPI1_CLK_DISABLE(); + + /**SPI1 GPIO Configuration + PA5 ------> SPI1_SCK + PA6 ------> SPI1_MISO + PA7 ------> SPI1_MOSI + */ + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7); + + /* USER CODE BEGIN SPI1_MspDeInit 1 */ + + /* USER CODE END SPI1_MspDeInit 1 */ + } + +} + +/** +* @brief TIM_Base MSP Initialization +* This function configures the hardware resources used in this example +* @param htim_base: TIM_Base handle pointer +* @retval None +*/ +void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base) +{ + if(htim_base->Instance==TIM1) + { + /* USER CODE BEGIN TIM1_MspInit 0 */ + + /* USER CODE END TIM1_MspInit 0 */ + /* Peripheral clock enable */ + __HAL_RCC_TIM1_CLK_ENABLE(); + /* TIM1 interrupt Init */ + HAL_NVIC_SetPriority(TIM1_BRK_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(TIM1_BRK_IRQn); + /* USER CODE BEGIN TIM1_MspInit 1 */ + + /* USER CODE END TIM1_MspInit 1 */ + } + +} + +/** +* @brief TIM_Base MSP De-Initialization +* This function freeze the hardware resources used in this example +* @param htim_base: TIM_Base handle pointer +* @retval None +*/ +void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base) +{ + if(htim_base->Instance==TIM1) + { + /* USER CODE BEGIN TIM1_MspDeInit 0 */ + + /* USER CODE END TIM1_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_TIM1_CLK_DISABLE(); + + /* TIM1 interrupt DeInit */ + HAL_NVIC_DisableIRQ(TIM1_BRK_IRQn); + /* USER CODE BEGIN TIM1_MspDeInit 1 */ + + /* USER CODE END TIM1_MspDeInit 1 */ + } + +} + +/** +* @brief UART MSP Initialization +* This function configures the hardware resources used in this example +* @param huart: UART handle pointer +* @retval None +*/ +void HAL_UART_MspInit(UART_HandleTypeDef* huart) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(huart->Instance==USART1) + { + /* USER CODE BEGIN USART1_MspInit 0 */ + + /* USER CODE END USART1_MspInit 0 */ + /* Peripheral clock enable */ + __HAL_RCC_USART1_CLK_ENABLE(); + + __HAL_RCC_GPIOA_CLK_ENABLE(); + /**USART1 GPIO Configuration + PA9 ------> USART1_TX + PA10 ------> USART1_RX + */ + GPIO_InitStruct.Pin = GPIO_PIN_9; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = GPIO_PIN_10; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USART1 interrupt Init */ + HAL_NVIC_SetPriority(USART1_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(USART1_IRQn); + /* USER CODE BEGIN USART1_MspInit 1 */ + + /* USER CODE END USART1_MspInit 1 */ + } + else if(huart->Instance==USART2) + { + /* USER CODE BEGIN USART2_MspInit 0 */ + + /* USER CODE END USART2_MspInit 0 */ + /* Peripheral clock enable */ + __HAL_RCC_USART2_CLK_ENABLE(); + + __HAL_RCC_GPIOA_CLK_ENABLE(); + /**USART2 GPIO Configuration + PA2 ------> USART2_TX + PA3 ------> USART2_RX + */ + GPIO_InitStruct.Pin = GPIO_PIN_2; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = GPIO_PIN_3; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USART2 interrupt Init */ + HAL_NVIC_SetPriority(USART2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(USART2_IRQn); + /* USER CODE BEGIN USART2_MspInit 1 */ + + /* USER CODE END USART2_MspInit 1 */ + } + +} + +/** +* @brief UART MSP De-Initialization +* This function freeze the hardware resources used in this example +* @param huart: UART handle pointer +* @retval None +*/ +void HAL_UART_MspDeInit(UART_HandleTypeDef* huart) +{ + if(huart->Instance==USART1) + { + /* USER CODE BEGIN USART1_MspDeInit 0 */ + + /* USER CODE END USART1_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_USART1_CLK_DISABLE(); + + /**USART1 GPIO Configuration + PA9 ------> USART1_TX + PA10 ------> USART1_RX + */ + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10); + + /* USART1 interrupt DeInit */ + HAL_NVIC_DisableIRQ(USART1_IRQn); + /* USER CODE BEGIN USART1_MspDeInit 1 */ + + /* USER CODE END USART1_MspDeInit 1 */ + } + else if(huart->Instance==USART2) + { + /* USER CODE BEGIN USART2_MspDeInit 0 */ + + /* USER CODE END USART2_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_USART2_CLK_DISABLE(); + + /**USART2 GPIO Configuration + PA2 ------> USART2_TX + PA3 ------> USART2_RX + */ + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_2|GPIO_PIN_3); + + /* USART2 interrupt DeInit */ + HAL_NVIC_DisableIRQ(USART2_IRQn); + /* USER CODE BEGIN USART2_MspDeInit 1 */ + + /* USER CODE END USART2_MspDeInit 1 */ + } + +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ diff --git a/Core/Src/stm32f1xx_it.c b/Core/Src/stm32f1xx_it.c new file mode 100644 index 0000000..acef5a7 --- /dev/null +++ b/Core/Src/stm32f1xx_it.c @@ -0,0 +1,261 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f1xx_it.c + * @brief Interrupt Service Routines. + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" +#include "stm32f1xx_it.h" +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN TD */ + +/* USER CODE END TD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/* External variables --------------------------------------------------------*/ +extern TIM_HandleTypeDef htim1; +extern UART_HandleTypeDef huart1; +extern UART_HandleTypeDef huart2; +/* USER CODE BEGIN EV */ + +/* USER CODE END EV */ + +/******************************************************************************/ +/* Cortex-M3 Processor Interruption and Exception Handlers */ +/******************************************************************************/ +/** + * @brief This function handles Non maskable interrupt. + */ +void NMI_Handler(void) +{ + /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ + + /* USER CODE END NonMaskableInt_IRQn 0 */ + /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ + while (1) + { + } + /* USER CODE END NonMaskableInt_IRQn 1 */ +} + +/** + * @brief This function handles Hard fault interrupt. + */ +void HardFault_Handler(void) +{ + /* USER CODE BEGIN HardFault_IRQn 0 */ + + /* USER CODE END HardFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_HardFault_IRQn 0 */ + /* USER CODE END W1_HardFault_IRQn 0 */ + } +} + +/** + * @brief This function handles Memory management fault. + */ +void MemManage_Handler(void) +{ + /* USER CODE BEGIN MemoryManagement_IRQn 0 */ + + /* USER CODE END MemoryManagement_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */ + /* USER CODE END W1_MemoryManagement_IRQn 0 */ + } +} + +/** + * @brief This function handles Prefetch fault, memory access fault. + */ +void BusFault_Handler(void) +{ + /* USER CODE BEGIN BusFault_IRQn 0 */ + + /* USER CODE END BusFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_BusFault_IRQn 0 */ + /* USER CODE END W1_BusFault_IRQn 0 */ + } +} + +/** + * @brief This function handles Undefined instruction or illegal state. + */ +void UsageFault_Handler(void) +{ + /* USER CODE BEGIN UsageFault_IRQn 0 */ + + /* USER CODE END UsageFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_UsageFault_IRQn 0 */ + /* USER CODE END W1_UsageFault_IRQn 0 */ + } +} + +/** + * @brief This function handles System service call via SWI instruction. + */ +void SVC_Handler(void) +{ + /* USER CODE BEGIN SVCall_IRQn 0 */ + + /* USER CODE END SVCall_IRQn 0 */ + /* USER CODE BEGIN SVCall_IRQn 1 */ + + /* USER CODE END SVCall_IRQn 1 */ +} + +/** + * @brief This function handles Debug monitor. + */ +void DebugMon_Handler(void) +{ + /* USER CODE BEGIN DebugMonitor_IRQn 0 */ + + /* USER CODE END DebugMonitor_IRQn 0 */ + /* USER CODE BEGIN DebugMonitor_IRQn 1 */ + + /* USER CODE END DebugMonitor_IRQn 1 */ +} + +/** + * @brief This function handles Pendable request for system service. + */ +void PendSV_Handler(void) +{ + /* USER CODE BEGIN PendSV_IRQn 0 */ + + /* USER CODE END PendSV_IRQn 0 */ + /* USER CODE BEGIN PendSV_IRQn 1 */ + + /* USER CODE END PendSV_IRQn 1 */ +} + +/** + * @brief This function handles System tick timer. + */ +void SysTick_Handler(void) +{ + /* USER CODE BEGIN SysTick_IRQn 0 */ + + /* USER CODE END SysTick_IRQn 0 */ + HAL_IncTick(); + /* USER CODE BEGIN SysTick_IRQn 1 */ + + /* USER CODE END SysTick_IRQn 1 */ +} + +/******************************************************************************/ +/* STM32F1xx Peripheral Interrupt Handlers */ +/* Add here the Interrupt Handlers for the used peripherals. */ +/* For the available peripheral interrupt handler names, */ +/* please refer to the startup file (startup_stm32f1xx.s). */ +/******************************************************************************/ + +/** + * @brief This function handles EXTI line0 interrupt. + */ +void EXTI0_IRQHandler(void) +{ + /* USER CODE BEGIN EXTI0_IRQn 0 */ + + /* USER CODE END EXTI0_IRQn 0 */ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0); + /* USER CODE BEGIN EXTI0_IRQn 1 */ + + /* USER CODE END EXTI0_IRQn 1 */ +} + +/** + * @brief This function handles TIM1 break interrupt. + */ +void TIM1_BRK_IRQHandler(void) +{ + /* USER CODE BEGIN TIM1_BRK_IRQn 0 */ + + /* USER CODE END TIM1_BRK_IRQn 0 */ + HAL_TIM_IRQHandler(&htim1); + /* USER CODE BEGIN TIM1_BRK_IRQn 1 */ + + /* USER CODE END TIM1_BRK_IRQn 1 */ +} + +/** + * @brief This function handles USART1 global interrupt. + */ +void USART1_IRQHandler(void) +{ + /* USER CODE BEGIN USART1_IRQn 0 */ + + /* USER CODE END USART1_IRQn 0 */ + HAL_UART_IRQHandler(&huart1); + /* USER CODE BEGIN USART1_IRQn 1 */ + + /* USER CODE END USART1_IRQn 1 */ +} + +/** + * @brief This function handles USART2 global interrupt. + */ +void USART2_IRQHandler(void) +{ + /* USER CODE BEGIN USART2_IRQn 0 */ + + /* USER CODE END USART2_IRQn 0 */ + HAL_UART_IRQHandler(&huart2); + /* USER CODE BEGIN USART2_IRQn 1 */ + + /* USER CODE END USART2_IRQn 1 */ +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ diff --git a/Core/Src/syscalls.c b/Core/Src/syscalls.c new file mode 100644 index 0000000..f4278b7 --- /dev/null +++ b/Core/Src/syscalls.c @@ -0,0 +1,176 @@ +/** + ****************************************************************************** + * @file syscalls.c + * @author Auto-generated by STM32CubeIDE + * @brief STM32CubeIDE Minimal System calls file + * + * For more information about which c-functions + * need which of these lowlevel functions + * please consult the Newlib libc-manual + ****************************************************************************** + * @attention + * + * Copyright (c) 2020-2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Includes */ +#include +#include +#include +#include +#include +#include +#include +#include + + +/* Variables */ +extern int __io_putchar(int ch) __attribute__((weak)); +extern int __io_getchar(void) __attribute__((weak)); + + +char *__env[1] = { 0 }; +char **environ = __env; + + +/* Functions */ +void initialise_monitor_handles() +{ +} + +int _getpid(void) +{ + return 1; +} + +int _kill(int pid, int sig) +{ + (void)pid; + (void)sig; + errno = EINVAL; + return -1; +} + +void _exit (int status) +{ + _kill(status, -1); + while (1) {} /* Make sure we hang here */ +} + +__attribute__((weak)) int _read(int file, char *ptr, int len) +{ + (void)file; + int DataIdx; + + for (DataIdx = 0; DataIdx < len; DataIdx++) + { + *ptr++ = __io_getchar(); + } + + return len; +} + +__attribute__((weak)) int _write(int file, char *ptr, int len) +{ + (void)file; + int DataIdx; + + for (DataIdx = 0; DataIdx < len; DataIdx++) + { + __io_putchar(*ptr++); + } + return len; +} + +int _close(int file) +{ + (void)file; + return -1; +} + + +int _fstat(int file, struct stat *st) +{ + (void)file; + st->st_mode = S_IFCHR; + return 0; +} + +int _isatty(int file) +{ + (void)file; + return 1; +} + +int _lseek(int file, int ptr, int dir) +{ + (void)file; + (void)ptr; + (void)dir; + return 0; +} + +int _open(char *path, int flags, ...) +{ + (void)path; + (void)flags; + /* Pretend like we always fail */ + return -1; +} + +int _wait(int *status) +{ + (void)status; + errno = ECHILD; + return -1; +} + +int _unlink(char *name) +{ + (void)name; + errno = ENOENT; + return -1; +} + +int _times(struct tms *buf) +{ + (void)buf; + return -1; +} + +int _stat(char *file, struct stat *st) +{ + (void)file; + st->st_mode = S_IFCHR; + return 0; +} + +int _link(char *old, char *new) +{ + (void)old; + (void)new; + errno = EMLINK; + return -1; +} + +int _fork(void) +{ + errno = EAGAIN; + return -1; +} + +int _execve(char *name, char **argv, char **env) +{ + (void)name; + (void)argv; + (void)env; + errno = ENOMEM; + return -1; +} diff --git a/Core/Src/sysmem.c b/Core/Src/sysmem.c new file mode 100644 index 0000000..54081ac --- /dev/null +++ b/Core/Src/sysmem.c @@ -0,0 +1,79 @@ +/** + ****************************************************************************** + * @file sysmem.c + * @author Generated by STM32CubeIDE + * @brief STM32CubeIDE System Memory calls file + * + * For more information about which C functions + * need which of these lowlevel functions + * please consult the newlib libc manual + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Includes */ +#include +#include + +/** + * Pointer to the current high watermark of the heap usage + */ +static uint8_t *__sbrk_heap_end = NULL; + +/** + * @brief _sbrk() allocates memory to the newlib heap and is used by malloc + * and others from the C library + * + * @verbatim + * ############################################################################ + * # .data # .bss # newlib heap # MSP stack # + * # # # # Reserved by _Min_Stack_Size # + * ############################################################################ + * ^-- RAM start ^-- _end _estack, RAM end --^ + * @endverbatim + * + * This implementation starts allocating at the '_end' linker symbol + * The '_Min_Stack_Size' linker symbol reserves a memory for the MSP stack + * The implementation considers '_estack' linker symbol to be RAM end + * NOTE: If the MSP stack, at any point during execution, grows larger than the + * reserved size, please increase the '_Min_Stack_Size'. + * + * @param incr Memory size + * @return Pointer to allocated memory + */ +void *_sbrk(ptrdiff_t incr) +{ + extern uint8_t _end; /* Symbol defined in the linker script */ + extern uint8_t _estack; /* Symbol defined in the linker script */ + extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */ + const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size; + const uint8_t *max_heap = (uint8_t *)stack_limit; + uint8_t *prev_heap_end; + + /* Initialize heap end at first call */ + if (NULL == __sbrk_heap_end) + { + __sbrk_heap_end = &_end; + } + + /* Protect heap from growing into the reserved MSP stack */ + if (__sbrk_heap_end + incr > max_heap) + { + errno = ENOMEM; + return (void *)-1; + } + + prev_heap_end = __sbrk_heap_end; + __sbrk_heap_end += incr; + + return (void *)prev_heap_end; +} diff --git a/Core/Src/system_stm32f1xx.c b/Core/Src/system_stm32f1xx.c new file mode 100644 index 0000000..bc96aae --- /dev/null +++ b/Core/Src/system_stm32f1xx.c @@ -0,0 +1,408 @@ +/** + ****************************************************************************** + * @file system_stm32f1xx.c + * @author MCD Application Team + * @brief CMSIS Cortex-M3 Device Peripheral Access Layer System Source File. + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * factors, AHB/APBx prescalers and Flash settings). + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f1xx_xx.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f1xx_xx.s" file, to + * configure the system clock before to branch to main program. + * + * 4. The default value of HSE crystal is set to 8 MHz (or 25 MHz, depending on + * the product used), refer to "HSE_VALUE". + * When HSE is used as system clock source, directly or through PLL, and you + * are using different crystal you have to adapt the HSE value to your own + * configuration. + * + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2017 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f1xx_system + * @{ + */ + +/** @addtogroup STM32F1xx_System_Private_Includes + * @{ + */ + +#include "stm32f1xx.h" + +/** + * @} + */ + +/** @addtogroup STM32F1xx_System_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F1xx_System_Private_Defines + * @{ + */ + +#if !defined (HSE_VALUE) + #define HSE_VALUE 8000000U /*!< Default value of the External oscillator in Hz. + This value can be provided and adapted by the user application. */ +#endif /* HSE_VALUE */ + +#if !defined (HSI_VALUE) + #define HSI_VALUE 8000000U /*!< Default value of the Internal oscillator in Hz. + This value can be provided and adapted by the user application. */ +#endif /* HSI_VALUE */ + +/*!< Uncomment the following line if you need to use external SRAM */ +#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG) +/* #define DATA_IN_ExtSRAM */ +#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */ + +/* Note: Following vector table addresses must be defined in line with linker + configuration. */ +/*!< Uncomment the following line if you need to relocate the vector table + anywhere in Flash or Sram, else the vector table is kept at the automatic + remap of boot address selected */ +/* #define USER_VECT_TAB_ADDRESS */ + +#if defined(USER_VECT_TAB_ADDRESS) +/*!< Uncomment the following line if you need to relocate your vector Table + in Sram else user remap will be done in Flash. */ +/* #define VECT_TAB_SRAM */ +#if defined(VECT_TAB_SRAM) +#define VECT_TAB_BASE_ADDRESS SRAM_BASE /*!< Vector Table base address field. + This value must be a multiple of 0x200. */ +#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +#else +#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field. + This value must be a multiple of 0x200. */ +#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +#endif /* VECT_TAB_SRAM */ +#endif /* USER_VECT_TAB_ADDRESS */ + +/******************************************************************************/ + +/** + * @} + */ + +/** @addtogroup STM32F1xx_System_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F1xx_System_Private_Variables + * @{ + */ + + /* This variable is updated in three ways: + 1) by calling CMSIS function SystemCoreClockUpdate() + 2) by calling HAL API function HAL_RCC_GetHCLKFreq() + 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency + Note: If you use this function to configure the system clock; then there + is no need to call the 2 first functions listed above, since SystemCoreClock + variable is updated automatically. + */ +uint32_t SystemCoreClock = 16000000; +const uint8_t AHBPrescTable[16U] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; +const uint8_t APBPrescTable[8U] = {0, 0, 0, 0, 1, 2, 3, 4}; + +/** + * @} + */ + +/** @addtogroup STM32F1xx_System_Private_FunctionPrototypes + * @{ + */ + +#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG) +#ifdef DATA_IN_ExtSRAM + static void SystemInit_ExtMemCtl(void); +#endif /* DATA_IN_ExtSRAM */ +#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */ + +/** + * @} + */ + +/** @addtogroup STM32F1xx_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemCoreClock variable. + * @note This function should be used only after reset. + * @param None + * @retval None + */ +void SystemInit (void) +{ +#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG) + #ifdef DATA_IN_ExtSRAM + SystemInit_ExtMemCtl(); + #endif /* DATA_IN_ExtSRAM */ +#endif + + /* Configure the Vector Table location -------------------------------------*/ +#if defined(USER_VECT_TAB_ADDRESS) + SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#endif /* USER_VECT_TAB_ADDRESS */ +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f1xx.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f1xx.h file (default value + * 8 MHz or 25 MHz, depending on the product used), user has to ensure + * that HSE_VALUE is same as the real frequency of the crystal used. + * Otherwise, this function may have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * @param None + * @retval None + */ +void SystemCoreClockUpdate (void) +{ + uint32_t tmp = 0U, pllmull = 0U, pllsource = 0U; + +#if defined(STM32F105xC) || defined(STM32F107xC) + uint32_t prediv1source = 0U, prediv1factor = 0U, prediv2factor = 0U, pll2mull = 0U; +#endif /* STM32F105xC */ + +#if defined(STM32F100xB) || defined(STM32F100xE) + uint32_t prediv1factor = 0U; +#endif /* STM32F100xB or STM32F100xE */ + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00U: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04U: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08U: /* PLL used as system clock */ + + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + +#if !defined(STM32F105xC) && !defined(STM32F107xC) + pllmull = ( pllmull >> 18U) + 2U; + + if (pllsource == 0x00U) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1U) * pllmull; + } + else + { + #if defined(STM32F100xB) || defined(STM32F100xE) + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1U; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + #else + /* HSE selected as PLL clock entry */ + if ((RCC->CFGR & RCC_CFGR_PLLXTPRE) != (uint32_t)RESET) + {/* HSE oscillator clock divided by 2 */ + SystemCoreClock = (HSE_VALUE >> 1U) * pllmull; + } + else + { + SystemCoreClock = HSE_VALUE * pllmull; + } + #endif + } +#else + pllmull = pllmull >> 18U; + + if (pllmull != 0x0DU) + { + pllmull += 2U; + } + else + { /* PLL multiplication factor = PLL input clock * 6.5 */ + pllmull = 13U / 2U; + } + + if (pllsource == 0x00U) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1U) * pllmull; + } + else + {/* PREDIV1 selected as PLL clock entry */ + + /* Get PREDIV1 clock source and division factor */ + prediv1source = RCC->CFGR2 & RCC_CFGR2_PREDIV1SRC; + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1U; + + if (prediv1source == 0U) + { + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + else + {/* PLL2 clock selected as PREDIV1 clock entry */ + + /* Get PREDIV2 division factor and PLL2 multiplication factor */ + prediv2factor = ((RCC->CFGR2 & RCC_CFGR2_PREDIV2) >> 4U) + 1U; + pll2mull = ((RCC->CFGR2 & RCC_CFGR2_PLL2MUL) >> 8U) + 2U; + SystemCoreClock = (((HSE_VALUE / prediv2factor) * pll2mull) / prediv1factor) * pllmull; + } + } +#endif /* STM32F105xC */ + break; + + default: + SystemCoreClock = HSI_VALUE; + break; + } + + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4U)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG) +/** + * @brief Setup the external memory controller. Called in startup_stm32f1xx.s + * before jump to __main + * @param None + * @retval None + */ +#ifdef DATA_IN_ExtSRAM +/** + * @brief Setup the external memory controller. + * Called in startup_stm32f1xx_xx.s/.c before jump to main. + * This function configures the external SRAM mounted on STM3210E-EVAL + * board (STM32 High density devices). This SRAM will be used as program + * data memory (including heap and stack). + * @param None + * @retval None + */ +void SystemInit_ExtMemCtl(void) +{ + __IO uint32_t tmpreg; + /*!< FSMC Bank1 NOR/SRAM3 is used for the STM3210E-EVAL, if another Bank is + required, then adjust the Register Addresses */ + + /* Enable FSMC clock */ + RCC->AHBENR = 0x00000114U; + + /* Delay after an RCC peripheral clock enabling */ + tmpreg = READ_BIT(RCC->AHBENR, RCC_AHBENR_FSMCEN); + + /* Enable GPIOD, GPIOE, GPIOF and GPIOG clocks */ + RCC->APB2ENR = 0x000001E0U; + + /* Delay after an RCC peripheral clock enabling */ + tmpreg = READ_BIT(RCC->APB2ENR, RCC_APB2ENR_IOPDEN); + + (void)(tmpreg); + +/* --------------- SRAM Data lines, NOE and NWE configuration ---------------*/ +/*---------------- SRAM Address lines configuration -------------------------*/ +/*---------------- NOE and NWE configuration --------------------------------*/ +/*---------------- NE3 configuration ----------------------------------------*/ +/*---------------- NBL0, NBL1 configuration ---------------------------------*/ + + GPIOD->CRL = 0x44BB44BBU; + GPIOD->CRH = 0xBBBBBBBBU; + + GPIOE->CRL = 0xB44444BBU; + GPIOE->CRH = 0xBBBBBBBBU; + + GPIOF->CRL = 0x44BBBBBBU; + GPIOF->CRH = 0xBBBB4444U; + + GPIOG->CRL = 0x44BBBBBBU; + GPIOG->CRH = 0x444B4B44U; + +/*---------------- FSMC Configuration ---------------------------------------*/ +/*---------------- Enable FSMC Bank1_SRAM Bank ------------------------------*/ + + FSMC_Bank1->BTCR[4U] = 0x00001091U; + FSMC_Bank1->BTCR[5U] = 0x00110212U; +} +#endif /* DATA_IN_ExtSRAM */ +#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/Core/Startup/startup_stm32f103c8tx.s b/Core/Startup/startup_stm32f103c8tx.s new file mode 100644 index 0000000..f2b7fbe --- /dev/null +++ b/Core/Startup/startup_stm32f103c8tx.s @@ -0,0 +1,365 @@ +/** + *************** (C) COPYRIGHT 2017 STMicroelectronics ************************ + * @file startup_stm32f103xb.s + * @author MCD Application Team + * @brief STM32F103xB Devices vector table for Atollic toolchain. + * This module performs: + * - Set the initial SP + * - Set the initial PC == Reset_Handler, + * - Set the vector table entries with the exceptions ISR address + * - Configure the clock system + * - Branches to main in the C library (which eventually + * calls main()). + * After Reset the Cortex-M3 processor is in Thread mode, + * priority is Privileged, and the Stack is set to Main. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2017 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + + .syntax unified + .cpu cortex-m3 + .fpu softvfp + .thumb + +.global g_pfnVectors +.global Default_Handler + +/* start address for the initialization values of the .data section. +defined in linker script */ +.word _sidata +/* start address for the .data section. defined in linker script */ +.word _sdata +/* end address for the .data section. defined in linker script */ +.word _edata +/* start address for the .bss section. defined in linker script */ +.word _sbss +/* end address for the .bss section. defined in linker script */ +.word _ebss + +.equ BootRAM, 0xF108F85F +/** + * @brief This is the code that gets called when the processor first + * starts execution following a reset event. Only the absolutely + * necessary set is performed, after which the application + * supplied main() routine is called. + * @param None + * @retval : None +*/ + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function +Reset_Handler: + +/* Copy the data segment initializers from flash to SRAM */ + ldr r0, =_sdata + ldr r1, =_edata + ldr r2, =_sidata + movs r3, #0 + b LoopCopyDataInit + +CopyDataInit: + ldr r4, [r2, r3] + str r4, [r0, r3] + adds r3, r3, #4 + +LoopCopyDataInit: + adds r4, r0, r3 + cmp r4, r1 + bcc CopyDataInit + +/* Zero fill the bss segment. */ + ldr r2, =_sbss + ldr r4, =_ebss + movs r3, #0 + b LoopFillZerobss + +FillZerobss: + str r3, [r2] + adds r2, r2, #4 + +LoopFillZerobss: + cmp r2, r4 + bcc FillZerobss + +/* Call the clock system intitialization function.*/ + bl SystemInit +/* Call static constructors */ + bl __libc_init_array +/* Call the application's entry point.*/ + bl main + bx lr +.size Reset_Handler, .-Reset_Handler + +/** + * @brief This is the code that gets called when the processor receives an + * unexpected interrupt. This simply enters an infinite loop, preserving + * the system state for examination by a debugger. + * + * @param None + * @retval : None +*/ + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop + .size Default_Handler, .-Default_Handler +/****************************************************************************** +* +* The minimal vector table for a Cortex M3. Note that the proper constructs +* must be placed on this to ensure that it ends up at physical address +* 0x0000.0000. +* +******************************************************************************/ + .section .isr_vector,"a",%progbits + .type g_pfnVectors, %object + .size g_pfnVectors, .-g_pfnVectors + + +g_pfnVectors: + + .word _estack + .word Reset_Handler + .word NMI_Handler + .word HardFault_Handler + .word MemManage_Handler + .word BusFault_Handler + .word UsageFault_Handler + .word 0 + .word 0 + .word 0 + .word 0 + .word SVC_Handler + .word DebugMon_Handler + .word 0 + .word PendSV_Handler + .word SysTick_Handler + .word WWDG_IRQHandler + .word PVD_IRQHandler + .word TAMPER_IRQHandler + .word RTC_IRQHandler + .word FLASH_IRQHandler + .word RCC_IRQHandler + .word EXTI0_IRQHandler + .word EXTI1_IRQHandler + .word EXTI2_IRQHandler + .word EXTI3_IRQHandler + .word EXTI4_IRQHandler + .word DMA1_Channel1_IRQHandler + .word DMA1_Channel2_IRQHandler + .word DMA1_Channel3_IRQHandler + .word DMA1_Channel4_IRQHandler + .word DMA1_Channel5_IRQHandler + .word DMA1_Channel6_IRQHandler + .word DMA1_Channel7_IRQHandler + .word ADC1_2_IRQHandler + .word USB_HP_CAN1_TX_IRQHandler + .word USB_LP_CAN1_RX0_IRQHandler + .word CAN1_RX1_IRQHandler + .word CAN1_SCE_IRQHandler + .word EXTI9_5_IRQHandler + .word TIM1_BRK_IRQHandler + .word TIM1_UP_IRQHandler + .word TIM1_TRG_COM_IRQHandler + .word TIM1_CC_IRQHandler + .word TIM2_IRQHandler + .word TIM3_IRQHandler + .word TIM4_IRQHandler + .word I2C1_EV_IRQHandler + .word I2C1_ER_IRQHandler + .word I2C2_EV_IRQHandler + .word I2C2_ER_IRQHandler + .word SPI1_IRQHandler + .word SPI2_IRQHandler + .word USART1_IRQHandler + .word USART2_IRQHandler + .word USART3_IRQHandler + .word EXTI15_10_IRQHandler + .word RTC_Alarm_IRQHandler + .word USBWakeUp_IRQHandler + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word BootRAM /* @0x108. This is for boot in RAM mode for + STM32F10x Medium Density devices. */ + +/******************************************************************************* +* +* Provide weak aliases for each Exception handler to the Default_Handler. +* As they are weak aliases, any function with the same name will override +* this definition. +* +*******************************************************************************/ + + .weak NMI_Handler + .thumb_set NMI_Handler,Default_Handler + + .weak HardFault_Handler + .thumb_set HardFault_Handler,Default_Handler + + .weak MemManage_Handler + .thumb_set MemManage_Handler,Default_Handler + + .weak BusFault_Handler + .thumb_set BusFault_Handler,Default_Handler + + .weak UsageFault_Handler + .thumb_set UsageFault_Handler,Default_Handler + + .weak SVC_Handler + .thumb_set SVC_Handler,Default_Handler + + .weak DebugMon_Handler + .thumb_set DebugMon_Handler,Default_Handler + + .weak PendSV_Handler + .thumb_set PendSV_Handler,Default_Handler + + .weak SysTick_Handler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDG_IRQHandler + .thumb_set WWDG_IRQHandler,Default_Handler + + .weak PVD_IRQHandler + .thumb_set PVD_IRQHandler,Default_Handler + + .weak TAMPER_IRQHandler + .thumb_set TAMPER_IRQHandler,Default_Handler + + .weak RTC_IRQHandler + .thumb_set RTC_IRQHandler,Default_Handler + + .weak FLASH_IRQHandler + .thumb_set FLASH_IRQHandler,Default_Handler + + .weak RCC_IRQHandler + .thumb_set RCC_IRQHandler,Default_Handler + + .weak EXTI0_IRQHandler + .thumb_set EXTI0_IRQHandler,Default_Handler + + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA1_Channel1_IRQHandler + .thumb_set DMA1_Channel1_IRQHandler,Default_Handler + + .weak DMA1_Channel2_IRQHandler + .thumb_set DMA1_Channel2_IRQHandler,Default_Handler + + .weak DMA1_Channel3_IRQHandler + .thumb_set DMA1_Channel3_IRQHandler,Default_Handler + + .weak DMA1_Channel4_IRQHandler + .thumb_set DMA1_Channel4_IRQHandler,Default_Handler + + .weak DMA1_Channel5_IRQHandler + .thumb_set DMA1_Channel5_IRQHandler,Default_Handler + + .weak DMA1_Channel6_IRQHandler + .thumb_set DMA1_Channel6_IRQHandler,Default_Handler + + .weak DMA1_Channel7_IRQHandler + .thumb_set DMA1_Channel7_IRQHandler,Default_Handler + + .weak ADC1_2_IRQHandler + .thumb_set ADC1_2_IRQHandler,Default_Handler + + .weak USB_HP_CAN1_TX_IRQHandler + .thumb_set USB_HP_CAN1_TX_IRQHandler,Default_Handler + + .weak USB_LP_CAN1_RX0_IRQHandler + .thumb_set USB_LP_CAN1_RX0_IRQHandler,Default_Handler + + .weak CAN1_RX1_IRQHandler + .thumb_set CAN1_RX1_IRQHandler,Default_Handler + + .weak CAN1_SCE_IRQHandler + .thumb_set CAN1_SCE_IRQHandler,Default_Handler + + .weak EXTI9_5_IRQHandler + .thumb_set EXTI9_5_IRQHandler,Default_Handler + + .weak TIM1_BRK_IRQHandler + .thumb_set TIM1_BRK_IRQHandler,Default_Handler + + .weak TIM1_UP_IRQHandler + .thumb_set TIM1_UP_IRQHandler,Default_Handler + + .weak TIM1_TRG_COM_IRQHandler + .thumb_set TIM1_TRG_COM_IRQHandler,Default_Handler + + .weak TIM1_CC_IRQHandler + .thumb_set TIM1_CC_IRQHandler,Default_Handler + + .weak TIM2_IRQHandler + .thumb_set TIM2_IRQHandler,Default_Handler + + .weak TIM3_IRQHandler + .thumb_set TIM3_IRQHandler,Default_Handler + + .weak TIM4_IRQHandler + .thumb_set TIM4_IRQHandler,Default_Handler + + .weak I2C1_EV_IRQHandler + .thumb_set I2C1_EV_IRQHandler,Default_Handler + + .weak I2C1_ER_IRQHandler + .thumb_set I2C1_ER_IRQHandler,Default_Handler + + .weak I2C2_EV_IRQHandler + .thumb_set I2C2_EV_IRQHandler,Default_Handler + + .weak I2C2_ER_IRQHandler + .thumb_set I2C2_ER_IRQHandler,Default_Handler + + .weak SPI1_IRQHandler + .thumb_set SPI1_IRQHandler,Default_Handler + + .weak SPI2_IRQHandler + .thumb_set SPI2_IRQHandler,Default_Handler + + .weak USART1_IRQHandler + .thumb_set USART1_IRQHandler,Default_Handler + + .weak USART2_IRQHandler + .thumb_set USART2_IRQHandler,Default_Handler + + .weak USART3_IRQHandler + .thumb_set USART3_IRQHandler,Default_Handler + + .weak EXTI15_10_IRQHandler + .thumb_set EXTI15_10_IRQHandler,Default_Handler + + .weak RTC_Alarm_IRQHandler + .thumb_set RTC_Alarm_IRQHandler,Default_Handler + + .weak USBWakeUp_IRQHandler + .thumb_set USBWakeUp_IRQHandler,Default_Handler + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + diff --git a/IMU_DUAL Debug.launch b/IMU_DUAL Debug.launch new file mode 100644 index 0000000..725661c --- /dev/null +++ b/IMU_DUAL Debug.launch @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/IMU_DUAL.ioc b/IMU_DUAL.ioc new file mode 100644 index 0000000..50a9322 --- /dev/null +++ b/IMU_DUAL.ioc @@ -0,0 +1,162 @@ +#MicroXplorer Configuration settings - do not modify +CAD.formats= +CAD.pinconfig= +CAD.provider= +File.Version=6 +GPIO.groupedBy=Group By Peripherals +KeepUserPlacement=false +Mcu.CPN=STM32F103C8T6 +Mcu.Family=STM32F1 +Mcu.IP0=NVIC +Mcu.IP1=RCC +Mcu.IP2=SPI1 +Mcu.IP3=SYS +Mcu.IP4=TIM1 +Mcu.IP5=USART1 +Mcu.IP6=USART2 +Mcu.IPNb=7 +Mcu.Name=STM32F103C(8-B)Tx +Mcu.Package=LQFP48 +Mcu.Pin0=PA1 +Mcu.Pin1=PA2 +Mcu.Pin10=PA13 +Mcu.Pin11=PA14 +Mcu.Pin12=VP_SYS_VS_Systick +Mcu.Pin13=VP_TIM1_VS_ClockSourceINT +Mcu.Pin2=PA3 +Mcu.Pin3=PA4 +Mcu.Pin4=PA5 +Mcu.Pin5=PA6 +Mcu.Pin6=PA7 +Mcu.Pin7=PB0 +Mcu.Pin8=PA9 +Mcu.Pin9=PA10 +Mcu.PinsNb=14 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32F103C8Tx +MxCube.Version=6.7.0 +MxDb.Version=DB.6.0.70 +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.EXTI0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false +NVIC.TIM1_BRK_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.USART1_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.USART2_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +PA1.GPIOParameters=GPIO_ModeDefaultEXTI +PA1.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING +PA1.Locked=true +PA1.Signal=GPXTI1 +PA10.Mode=Asynchronous +PA10.Signal=USART1_RX +PA13.Mode=Serial_Wire +PA13.Signal=SYS_JTMS-SWDIO +PA14.Mode=Serial_Wire +PA14.Signal=SYS_JTCK-SWCLK +PA2.Mode=Asynchronous +PA2.Signal=USART2_TX +PA3.Mode=Asynchronous +PA3.Signal=USART2_RX +PA4.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd +PA4.GPIO_PuPd=GPIO_PULLUP +PA4.GPIO_Speed=GPIO_SPEED_FREQ_HIGH +PA4.Locked=true +PA4.PinState=GPIO_PIN_SET +PA4.Signal=GPIO_Output +PA5.Mode=Full_Duplex_Master +PA5.Signal=SPI1_SCK +PA6.Mode=Full_Duplex_Master +PA6.Signal=SPI1_MISO +PA7.Mode=Full_Duplex_Master +PA7.Signal=SPI1_MOSI +PA9.Mode=Asynchronous +PA9.Signal=USART1_TX +PB0.GPIOParameters=GPIO_ModeDefaultEXTI +PB0.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING +PB0.Locked=true +PB0.Signal=GPXTI0 +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=false +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32F103C8Tx +ProjectManager.FirmwarePackage=STM32Cube FW_F1 V1.8.4 +ProjectManager.FreePins=false +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x200 +ProjectManager.KeepUserCode=true +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=1 +ProjectManager.MainLocation=Core/Src +ProjectManager.NoMain=false +ProjectManager.PreviousToolchain= +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=IMU_DUAL.ioc +ProjectManager.ProjectName=IMU_DUAL +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x400 +ProjectManager.TargetToolchain=STM32CubeIDE +ProjectManager.ToolChainLocation= +ProjectManager.UnderRoot=true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_SPI1_Init-SPI1-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,5-MX_TIM1_Init-TIM1-false-HAL-true,6-MX_USART2_UART_Init-USART2-false-HAL-true +RCC.ADCFreqValue=32000000 +RCC.AHBFreq_Value=64000000 +RCC.APB1CLKDivider=RCC_HCLK_DIV2 +RCC.APB1Freq_Value=32000000 +RCC.APB1TimFreq_Value=64000000 +RCC.APB2Freq_Value=64000000 +RCC.APB2TimFreq_Value=64000000 +RCC.FCLKCortexFreq_Value=64000000 +RCC.FamilyName=M +RCC.HCLKFreq_Value=64000000 +RCC.IPParameters=ADCFreqValue,AHBFreq_Value,APB1CLKDivider,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,MCOFreq_Value,PLLCLKFreq_Value,PLLMCOFreq_Value,PLLMUL,SYSCLKFreq_VALUE,SYSCLKSource,TimSysFreq_Value,USBFreq_Value +RCC.MCOFreq_Value=64000000 +RCC.PLLCLKFreq_Value=64000000 +RCC.PLLMCOFreq_Value=32000000 +RCC.PLLMUL=RCC_PLL_MUL16 +RCC.SYSCLKFreq_VALUE=64000000 +RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.TimSysFreq_Value=64000000 +RCC.USBFreq_Value=64000000 +SH.GPXTI0.0=GPIO_EXTI0 +SH.GPXTI0.ConfNb=1 +SH.GPXTI1.0=GPIO_EXTI1 +SH.GPXTI1.ConfNb=1 +SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_8 +SPI1.CLKPhase=SPI_PHASE_2EDGE +SPI1.CLKPolarity=SPI_POLARITY_HIGH +SPI1.CalculateBaudRate=8.0 MBits/s +SPI1.Direction=SPI_DIRECTION_2LINES +SPI1.IPParameters=VirtualType,Mode,Direction,BaudRatePrescaler,CalculateBaudRate,CLKPolarity,CLKPhase +SPI1.Mode=SPI_MODE_MASTER +SPI1.VirtualType=VM_MASTER +TIM1.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE +TIM1.ClockDivision=TIM_CLOCKDIVISION_DIV1 +TIM1.IPParameters=ClockDivision,AutoReloadPreload,Prescaler,Period +TIM1.Period=6400 +TIM1.Prescaler=0 +USART1.BaudRate=460800 +USART1.IPParameters=VirtualMode,BaudRate +USART1.VirtualMode=VM_ASYNC +USART2.IPParameters=VirtualMode +USART2.VirtualMode=VM_ASYNC +VP_SYS_VS_Systick.Mode=SysTick +VP_SYS_VS_Systick.Signal=SYS_VS_Systick +VP_TIM1_VS_ClockSourceINT.Mode=Internal +VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT +board=custom +isbadioc=false diff --git a/STM32F103C8TX_FLASH.ld b/STM32F103C8TX_FLASH.ld new file mode 100644 index 0000000..e71e7e4 --- /dev/null +++ b/STM32F103C8TX_FLASH.ld @@ -0,0 +1,185 @@ +/* +****************************************************************************** +** +** @file : LinkerScript.ld +** +** @author : Auto-generated by STM32CubeIDE +** +** @brief : Linker script for STM32F103C8Tx Device from STM32F1 series +** 64Kbytes FLASH +** 20Kbytes RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used +** +** Target : STMicroelectronics STM32 +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +****************************************************************************** +** @attention +** +** Copyright (c) 2022 STMicroelectronics. +** All rights reserved. +** +** This software is licensed under terms that can be found in the LICENSE file +** in the root directory of this software component. +** If no LICENSE file comes with this software, it is provided AS-IS. +** +****************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of "RAM" Ram type memory */ + +_Min_Heap_Size = 0x200; /* required amount of heap */ +_Min_Stack_Size = 0x400; /* required amount of stack */ + +/* Memories definition */ +MEMORY +{ + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K + FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 64K +} + +/* Sections */ +SECTIONS +{ + /* The startup code into "FLASH" Rom type memory */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data into "FLASH" Rom type memory */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data into "FLASH" Rom type memory */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab : { + . = ALIGN(4); + *(.ARM.extab* .gnu.linkonce.armextab.*) + . = ALIGN(4); + } >FLASH + + .ARM : { + . = ALIGN(4); + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + . = ALIGN(4); + } >FLASH + + .preinit_array : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + . = ALIGN(4); + } >FLASH + + .init_array : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + . = ALIGN(4); + } >FLASH + + .fini_array : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + . = ALIGN(4); + } >FLASH + + /* Used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections into "RAM" Ram type memory */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + *(.RamFunc) /* .RamFunc sections */ + *(.RamFunc*) /* .RamFunc* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + + } >RAM AT> FLASH + + /* Uninitialized data section into "RAM" Ram type memory */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss section */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + /* Remove information from the compiler libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/nmea/nmea_zjut.c b/nmea/nmea_zjut.c new file mode 100644 index 0000000..55440c0 --- /dev/null +++ b/nmea/nmea_zjut.c @@ -0,0 +1,398 @@ +#include "main.h" +#include "time.h" +#include "rtklib.h" + +/* 在rtklib 中定义结构体 */ + + + +#define MAXFIELD 64 /* max number of fields in a record */ + +typedef enum fsm_err +{ + FSM_EOK = 0, + FSM_ERROR, + FSM_EINVAL, +} fsm_err_t; + +enum +{ + FSM_STATE_INIT = 0, + FSM_STATE_ABORT, + FSM_STATE_COMPLETE, + FSM_STATE_NAME_RECV, + FSM_STATE_ARGS_RECV, + FSM_STATE_CSUM_RECV, + FSM_STATE_CRLF_RECV, + FSM_STATE_MAX, +}; + +typedef enum +{ + FSM_EVENT_DOLLAR_CHAR = 0, + FSM_EVENT_ORDINARY_CHAR, + FSM_EVENT_COMMA_CHAR, + FSM_EVENT_ASTERISK_CHAR, + FSM_EVENT_CR_CHAR, + FSM_EVENT_LF_CHAR, + FSM_EVENT_NUL_CHAR, + FSM_EVENT_MAX, + FSM_EVENT_G_CHAR, + FSM_EVENT_DATA, + FSM_EVENT_CHEAK1, + FSM_EVENT_CHEAK2, + FSM_EVENT_FINISH, + FSM_EVENT_IDLE, +} fsm_event_t; + +enum +{ + FSM_ERROR_DOLLAR_REPEAT = 1, + FSM_ERROR_ORDINARY_EARLY, + FSM_ERROR_ORDINARY_REPEAT, + FSM_ERROR_COMMA_EARLY, + FSM_ERROR_COMMA_REPEAT, + FSM_ERROR_ASTERISK_EARLY, + FSM_ERROR_ASTERISK_REPEAT, + FSM_ERROR_CR_EARLY, + FSM_ERROR_CR_REPEAT, + FSM_ERROR_LF_EARLY, + FSM_ERROR_LF_REPEAT, + FSM_ERROR_NUL_EARLY, +}; + +static int nmea_xor(uint8_t *buff, int len) +{ + int i = 0; + int result = buff[1]; + int ans = 0; + for (i = 2; buff[i] != '*' && i < len - 2; i++) + { + result ^= buff[i]; + } + if (buff[++i] > '9') + { + ans += 10 + buff[i] - 'A'; + } + else + { + ans += buff[i] - '0'; + } + ans *= 16; + if (buff[++i] > '9') + { + ans += 10 + buff[i] - 'A'; + } + else + { + ans += buff[i] - '0'; + } + if (ans == result) + return 1; + else + return 0; +} +/* convert calendar day/time to time ------------------------------------------- +* convert calendar day/time to gtime_t struct +* args : double *ep I day/time {year,month,day,hour,min,sec} +* return : gtime_t struct +* notes : proper in 1970-2037 or 1970-2099 (64bit time_t) +*-----------------------------------------------------------------------------*/ +extern gtime_t epoch2time(const double *ep) +{ + const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335}; + gtime_t time={0}; + int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2]; + + if (year<1970||2099=3?1:0); + sec=(int)floor(ep[5]); + time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec; + time.sec=ep[5]-sec; + return time; +} + +/* convert time in nmea format to time ---------------------------------------*/ +static void septime(double t, double *t1, double *t2, double *t3) +{ + *t1 = floor(t / 10000.0); + t -= *t1 * 10000.0; + *t2 = floor(t / 100.0); + *t3 = t - *t2 * 100.0; +} + +/* convert ddmm.mm in nmea format to deg -------------------------------------*/ +static double dmm2deg(double dmm) +{ + return floor(dmm / 100.0) + fmod(dmm, 100.0) / 60.0; +} + +/* decode NMEA RMC (Recommended Minumum Specific GNSS Data) sentence ---------*/ +static int decode_nmearmc(char **val, int n, sol_t *sol) +{ + double tod = 0.0, lat = 0.0, lon = 0.0, vel = 0.0, dir = 0.0, date = 0.0, ang = 0.0, ep[6]; + double pos[3] = {0}; + char act = ' ', ns = 'N', ew = 'E', mew = 'E', mode = 'A'; + int i; + + // trace(4, "decode_nmearmc: n=%d\n", n); + + for (i = 0; i < n; i++) + { + switch (i) + { + case 0: + tod = atof(val[i]); + break; /* time in utc (hhmmss) */ + case 1: + act = *val[i]; + break; /* A=active,V=void */ + case 2: + lat = atof(val[i]); + break; /* latitude (ddmm.mmm) */ + case 3: + ns = *val[i]; + break; /* N=north,S=south */ + case 4: + lon = atof(val[i]); + break; /* longitude (dddmm.mmm) */ + case 5: + ew = *val[i]; + break; /* E=east,W=west */ + case 6: + vel = atof(val[i]); + break; /* speed (knots) */ + case 7: + dir = atof(val[i]); + break; /* track angle (deg) */ + case 8: + date = atof(val[i]); + break; /* date (ddmmyy) */ + case 9: + ang = atof(val[i]); + break; /* magnetic variation */ + case 10: + mew = *val[i]; + break; /* E=east,W=west */ + case 11: + mode = *val[i]; + break; /* mode indicator (>nmea 2) */ + /* A=autonomous,D=differential */ + /* E=estimated,N=not valid,S=simulator */ + } + } + if ((act != 'A' && act != 'V') || (ns != 'N' && ns != 'S') || (ew != 'E' && ew != 'W')) + { + // trace(3, "invalid nmea rmc format\n"); + return 0; + } + pos[0] = (ns == 'S' ? -1.0 : 1.0) * dmm2deg(lat) * D2R; + pos[1] = (ew == 'W' ? -1.0 : 1.0) * dmm2deg(lon) * D2R; + septime(date, ep + 2, ep + 1, ep); + septime(tod, ep + 3, ep + 4, ep + 5); + ep[0] += ep[0] < 80.0 ? 2000.0 : 1900.0; + sol->utctime=epoch2time(ep); + sol->time = utc2gpst(sol->utctime); + pos2ecef(pos, sol->rr); + sol->stat = mode == 'D' ? SOLQ_DGPS : SOLQ_SINGLE; + sol->ns = 0; + + sol->type = 0; /* postion type = xyz */ + + // trace(1, "decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d vel=%.2f dir=%.0f ang=%.0f mew=%c mode=%c \r\n", + // time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns, + // vel, dir, ang, mew, mode); + + // trace(3, "decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d \r\n", + // time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns); + + return 2; /* update time */ +} +static int decode_nmea(char *buff, sol_t *sol) +{ + char *p, *q, *val[MAXFIELD]; + int n = 0; + + // trace(4, "decode_nmea: buff=%s\n", buff); + + /* parse fields */ + for (p = buff; *p && n < MAXFIELD; p = q + 1) + { + if ((q = strchr(p, ',')) || (q = strchr(p, '*'))) + { + val[n++] = p; + *q = '\0'; + } + else + break; + } + if (n < 1) + { + return 0; + } + if (!strcmp(val[0] + 3, "RMC")) + { /* $xxRMC */ + return decode_nmearmc(val + 1, n - 1, sol); + } + // else if (!strcmp(val[0] + 3, "ZDA")) + //{ /* $xxZDA */ + // return decode_nmeazda(val + 1, n - 1, sol); + // } + // else if (!strcmp(val[0] + 3, "GGA")) + //{ /* $xxGGA */ + // return decode_nmeagga(val + 1, n - 1, sol); + // } + return 0; +} + +// extern int input_nmea(nmea_t *nmea, uint8_t data) +// { +// static uint8_t state = 0; +// int8_t ans = 0; +// trace(5, "input_nmea: data=%02x\n", data); + +// if (nmea->nbyte == 0 && data == '$') +// { +// state = 1; +// } +// else if (nmea->nbyte != 0 && data == '*') +// { +// state = 2; +// } +// else if (nmea->len != 0) +// { +// state = 3; +// } + +// if (state == 1) +// { +// nmea->buff[nmea->nbyte++] = data; +// if(nmea->nbyte == 2 && nmea->buff[1] != 'G') +// { +// ans = -1; +// } +// } +// else if (state == 2) +// { +// nmea->buff[nmea->nbyte++] = data; +// nmea->len = nmea->nbyte + 2; +// } +// else if (state == 3) +// { +// nmea->buff[nmea->nbyte++] = data; +// if (nmea->nbyte == nmea->len) /* �����ռ���� ����У�� */ +// { +// if (0 == nmea_xor(nmea->buff, nmea->len)) +// { +// trace(1, "nmea parity error: len=%d \n", nmea->len); +// ans = -1; +// } +// else +// { +// ans = 1; +// } +// } +// } +// if (ans == 1) +// { +// return decode_nmea((char *)nmea->buff, &(nmea->sol)); +// } +// if(nmea->nbyte >=255) /* Խ�� */ +// { +// ans = -1; +// } +// if (ans != 0) +// { +// nmea->len = 0; +// nmea->nbyte = 0; +// state = 0; +// } + +// return ans; +// } + +extern int input_nmea(nmea_t *nmea, const uint8_t data) +{ + static fsm_event_t event = 0; + int8_t ans = 0; + // trace(5, "input_nmea: data=%02x\n", data); + + switch (data) + { + case '$': + event = FSM_EVENT_DOLLAR_CHAR; + break; + case '*': + if(event == FSM_EVENT_DATA) + event = FSM_EVENT_ASTERISK_CHAR; + else + event = FSM_EVENT_IDLE; + break; + default: + break; + } + + switch (event) + { + case FSM_EVENT_DOLLAR_CHAR: + nmea->nbyte = 0; + nmea->len = 0; + nmea->buff[nmea->nbyte++] = data; + event = FSM_EVENT_G_CHAR; + break; + case FSM_EVENT_G_CHAR: + if (data == 'G') + { + nmea->buff[nmea->nbyte++] = data; + event = FSM_EVENT_DATA; + } + else + { + event = FSM_EVENT_IDLE; + } + break; + case FSM_EVENT_DATA: + if (nmea->nbyte > 168) + { + event = FSM_EVENT_IDLE; + } + else + { + nmea->buff[nmea->nbyte++] = data; + } + break; + case FSM_EVENT_ASTERISK_CHAR: + nmea->buff[nmea->nbyte++] = data; + nmea->len = nmea->nbyte + 2; + event = FSM_EVENT_CHEAK1; + break; + case FSM_EVENT_CHEAK1: + nmea->buff[nmea->nbyte++] = data; + event = FSM_EVENT_CHEAK2; + break; + case FSM_EVENT_CHEAK2: + nmea->buff[nmea->nbyte++] = data; + event = FSM_EVENT_FINISH; + break; + default: + break; + } + if (event == FSM_EVENT_FINISH) + { + event = FSM_EVENT_IDLE; + if (0 == nmea_xor(nmea->buff, nmea->len)) + { + // trace(3, "nmea parity error: len=%d \r\n", nmea->len); + ans = -1; + } + else + { + ans = 1; + return decode_nmea((char *)nmea->buff, &(nmea->sol)); + } + } + + return ans; +} diff --git a/nmea/rtkcmn.c b/nmea/rtkcmn.c new file mode 100644 index 0000000..a9b317d --- /dev/null +++ b/nmea/rtkcmn.c @@ -0,0 +1,3901 @@ +//#define _POSIX_C_SOURCE 199506 +#include "rtklib.h" +// +//#include +//#include +//#include +// +////#include "brd_timer.h" +////#include +///* constants -----------------------------------------------------------------*/ +// +//#define POLYCRC32 0xEDB88320u /* CRC32 polynomial */ +//#define POLYCRC24Q 0x1864CFBu /* CRC24Q polynomial */ +// +//#define SQR(x) ((x) * (x)) +//#define MAX_VAR_EPH SQR(300.0) /* max variance eph to reject satellite (m^2) */ +// +//static const double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */ +//static const double gst0[] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */ +//static const double bdt0[] = {2006, 1, 1, 0, 0, 0}; /* beidou time reference */ +// +static double leaps[MAXLEAPS + 1][7] = {/* leap seconds (y,m,d,h,m,s,utc-gpst) */ + {2017, 1, 1, 0, 0, 0, -18}, + {2015, 7, 1, 0, 0, 0, -17}, + {2012, 7, 1, 0, 0, 0, -16}, + {2009, 1, 1, 0, 0, 0, -15}, + {2006, 1, 1, 0, 0, 0, -14}, + {1999, 1, 1, 0, 0, 0, -13}, + {1997, 7, 1, 0, 0, 0, -12}, + {1996, 1, 1, 0, 0, 0, -11}, + {1994, 7, 1, 0, 0, 0, -10}, + {1993, 7, 1, 0, 0, 0, -9}, + {1992, 7, 1, 0, 0, 0, -8}, + {1991, 1, 1, 0, 0, 0, -7}, + {1990, 1, 1, 0, 0, 0, -6}, + {1988, 1, 1, 0, 0, 0, -5}, + {1985, 7, 1, 0, 0, 0, -4}, + {1983, 7, 1, 0, 0, 0, -3}, + {1982, 7, 1, 0, 0, 0, -2}, + {1981, 7, 1, 0, 0, 0, -1}, + {0}}; +//const double chisqr[100] = {/* chi-sqr(n) (alpha=0.001) */ +// 10.8, 13.8, 16.3, 18.5, 20.5, 22.5, 24.3, 26.1, 27.9, 29.6, +// 31.3, 32.9, 34.5, 36.1, 37.7, 39.3, 40.8, 42.3, 43.8, 45.3, +// 46.8, 48.3, 49.7, 51.2, 52.6, 54.1, 55.5, 56.9, 58.3, 59.7, +// 61.1, 62.5, 63.9, 65.2, 66.6, 68.0, 69.3, 70.7, 72.1, 73.4, +// 74.7, 76.0, 77.3, 78.6, 80.0, 81.3, 82.6, 84.0, 85.4, 86.7, +// 88.0, 89.3, 90.6, 91.9, 93.3, 94.7, 96.0, 97.4, 98.7, 100, +// 101, 102, 103, 104, 105, 107, 108, 109, 110, 112, +// 113, 114, 115, 116, 118, 119, 120, 122, 123, 125, +// 126, 127, 128, 129, 131, 132, 133, 134, 135, 137, +// 138, 139, 140, 142, 143, 144, 145, 147, 148, 149}; +//// const char *formatstrs[32] = { /* stream format strings */ +//// "RTCM 2", /* 0 */ +//// "RTCM 3", /* 1 */ +//// "NovAtel OEM7", /* 2 */ +//// "ComNav", /* 3 */ +//// "u-blox UBX", /* 4 */ +//// "Swift Navigation SBP", /* 5 */ +//// "Hemisphere", /* 6 */ +//// "SkyTraq", /* 7 */ +//// "Javad GREIS", /* 8 */ +//// "NVS BINR", /* 9 */ +//// "BINEX", /* 10 */ +//// "Trimble RT17", /* 11 */ +//// "Septentrio SBF", /* 12 */ +//// "Tersus", /* 13 */ +//// "RINEX", /* 14 */ +//// "SP3", /* 15 */ +//// "RINEX CLK", /* 16 */ +//// "SBAS", /* 17 */ +//// "NMEA 0183", /* 18 */ +//// "TERSUS", /* 19 */ +//// NULL}; +// +//static char *obscodes[] = { +// /* observation code strings */ +// +// "", "1C", "1P", "1W", "1Y", "1M", "1N", "1S", "1L", "1E", /* 0- 9 */ +// "1A", "1B", "1X", "1Z", "2C", "2D", "2S", "2L", "2X", "2P", /* 10-19 */ +// "2W", "2Y", "2M", "2N", "5I", "5Q", "5X", "7I", "7Q", "7X", /* 20-29 */ +// "6A", "6B", "6C", "6X", "6Z", "6S", "6L", "8L", "8Q", "8X", /* 30-39 */ +// "2I", "2Q", "6I", "6Q", "3I", "3Q", "3X", "1I", "1Q", "5A", /* 40-49 */ +// "5B", "5C", "9A", "9B", "9C", "9X", "1D", "5D", "5P", "5Z", /* 50-59 */ +// "6E", "7D", "7P", "7Z", "8D", "8P", "4A", "4B", "4X", "" /* 60-69 */ +//}; +//#ifdef L5_TO_L2 +///*L1 + L5˫Ƶ���ջ� ����L5���ȼ���L2ǰ��*/ +//static char codepris[7][MAXFREQ][16] = { +// /* code priority for each freq-index */ +// /* L1/E1/B1 L2/E5b/B2b L5/E5a/B2a E6/LEX/B3 E5(a+b) */ +// {"CPYWMNSL", "IQX", "CPYWMNDLSX", "", "", ""}, /* GPS */ +// {"CPABX", "IQX", "CPABX", "", "", ""}, /* GLO */ +// {"CABXZ", "XIQ", "XIQ", "ABCXZ", "IQX", ""}, /* GAL */ +// {"CLSXZ", "IQXDPZ", "LSX", "LSXEZ", "", ""}, /* QZS */ +// {"C", "IQX", "", "", "", ""}, /* SBS */ +// {"IQXDPAN", "DPX", "IQXDPZ", "IQXA", "DPX", ""}, /* BDS */ +// {"ABCX", "ABCX", "", "", "", ""} /* IRN */ +//}; +//#else +////ԭ���ȼ� +//static char codepris[7][MAXFREQ][16] = { +// /* code priority for each freq-index */ +// /* L1/E1/B1 L2/E5b/B2b L5/E5a/B2a E6/LEX/B3 E5(a+b) */ +// {"CPYWMNSL", "CPYWMNDLSX", "IQX", "", "", ""}, /* GPS */ +// {"CPABX", "CPABX", "IQX", "", "", ""}, /* GLO */ +// {"CABXZ", "XIQ", "XIQ", "ABCXZ", "IQX", ""}, /* GAL */ +// {"CLSXZ", "LSX", "IQXDPZ", "LSXEZ", "", ""}, /* QZS */ +// {"C", "IQX", "", "", "", ""}, /* SBS */ +// {"IQXDPAN", "IQXDPZ", "DPX", "IQXA", "DPX", ""}, /* BDS */ +// {"ABCX", "ABCX", "", "", "", ""} /* IRN */ +//}; +//#endif +// +//static fatalfunc_t *fatalfunc = NULL; /* fatal callback function */ +// +///* crc tables generated by util/gencrc ---------------------------------------*/ +//// static const uint16_t tbl_CRC16[] = { +//// 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, +//// 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, +//// 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, +//// 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, +//// 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, +//// 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, +//// 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, +//// 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, +//// 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, +//// 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, +//// 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, +//// 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, +//// 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, +//// 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, +//// 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, +//// 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, +//// 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, +//// 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, +//// 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, +//// 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, +//// 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, +//// 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, +//// 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, +//// 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, +//// 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, +//// 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, +//// 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, +//// 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, +//// 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, +//// 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, +//// 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, +//// 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0}; +//static const uint32_t tbl_CRC24Q[] = { +// 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, +// 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, +// 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, +// 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, +// 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, +// 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, +// 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, +// 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, +// 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, +// 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, +// 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, +// 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, +// 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, +// 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, +// 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, +// 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, +// 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, +// 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, +// 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, +// 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, +// 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, +// 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, +// 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, +// 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, +// 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, +// 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, +// 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, +// 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, +// 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, +// 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, +// 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, +// 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538}; +///* function prototypes -------------------------------------------------------*/ +// +///* fatal error ---------------------------------------------------------------*/ +//static void fatalerr(const char *format, ...) +//{ +// char msg[1024]; +// va_list ap; +// va_start(ap, format); +// vsprintf(msg, format, ap); +// va_end(ap); +// if (fatalfunc) +// fatalfunc(msg); +// else +// fprintf(stderr, "%s", msg); +// while (1) +// ; +//} +///* add fatal callback function ------------------------------------------------- +// * add fatal callback function for mat(),zeros(),imat() +// * args : fatalfunc_t *func I callback function +// * return : none +// * notes : if malloc() failed in return : none +// *-----------------------------------------------------------------------------*/ +//extern void add_fatal(fatalfunc_t *func) +//{ +// fatalfunc = func; +//} +///* satellite system+prn/slot number to satellite number ------------------------ +// * convert satellite system+prn/slot number to satellite number +// * args : int sys I satellite system (SYS_GPS,SYS_GLO,...) +// * int prn I satellite prn/slot number +// * return : satellite number (0:error) +// *-----------------------------------------------------------------------------*/ +//extern int satno(int sys, int prn) +//{ +// if (prn <= 0) +// return 0; +// switch (sys) +// { +// case SYS_GPS: +// if (prn < MINPRNGPS || MAXPRNGPS < prn) +// return 0; +// return prn - MINPRNGPS + 1; +// case SYS_GLO: +// if (prn < MINPRNGLO || MAXPRNGLO < prn) +// return 0; +// return NSATGPS + prn - MINPRNGLO + 1; +// case SYS_GAL: +// if (prn < MINPRNGAL || MAXPRNGAL < prn) +// return 0; +// return NSATGPS + NSATGLO + prn - MINPRNGAL + 1; +// case SYS_QZS: +// if (prn < MINPRNQZS || MAXPRNQZS < prn) +// return 0; +// return NSATGPS + NSATGLO + NSATGAL + prn - MINPRNQZS + 1; +// case SYS_CMP: +// if (prn < MINPRNCMP || MAXPRNCMP < prn) +// return 0; +// return NSATGPS + NSATGLO + NSATGAL + NSATQZS + prn - MINPRNCMP + 1; +// case SYS_IRN: +// if (prn < MINPRNIRN || MAXPRNIRN < prn) +// return 0; +// return NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATCMP + prn - MINPRNIRN + 1; +// case SYS_LEO: +// if (prn < MINPRNLEO || MAXPRNLEO < prn) +// return 0; +// return NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATCMP + NSATIRN + +// prn - MINPRNLEO + 1; +// case SYS_SBS: +// if (prn < MINPRNSBS || MAXPRNSBS < prn) +// return 0; +// return NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATCMP + NSATIRN + NSATLEO + +// prn - MINPRNSBS + 1; +// } +// return 0; +//} +///* satellite number to satellite system ---------------------------------------- +// * convert satellite number to satellite system +// * args : int sat I satellite number (1-MAXSAT) +// * int *prn IO satellite prn/slot number (NULL: no output) +// * return : satellite system (SYS_GPS,SYS_GLO,...) +// *-----------------------------------------------------------------------------*/ +//extern int satsys(int sat, int *prn) +//{ +// int sys = SYS_NONE; +// if (sat <= 0 || MAXSAT < sat) +// sat = 0; +// else if (sat <= NSATGPS) +// { +// sys = SYS_GPS; +// sat += MINPRNGPS - 1; +// } +// else if ((sat -= NSATGPS) <= NSATGLO) +// { +// sys = SYS_GLO; +// sat += MINPRNGLO - 1; +// } +// else if ((sat -= NSATGLO) <= NSATGAL) +// { +// sys = SYS_GAL; +// sat += MINPRNGAL - 1; +// } +// else if ((sat -= NSATGAL) <= NSATQZS) +// { +// sys = SYS_QZS; +// sat += MINPRNQZS - 1; +// } +// else if ((sat -= NSATQZS) <= NSATCMP) +// { +// sys = SYS_CMP; +// sat += MINPRNCMP - 1; +// } +// else if ((sat -= NSATCMP) <= NSATIRN) +// { +// sys = SYS_IRN; +// sat += MINPRNIRN - 1; +// } +// else if ((sat -= NSATIRN) <= NSATLEO) +// { +// sys = SYS_LEO; +// sat += MINPRNLEO - 1; +// } +// else if ((sat -= NSATLEO) <= NSATSBS) +// { +// sys = SYS_SBS; +// sat += MINPRNSBS - 1; +// } +// else +// sat = 0; +// if (prn) +// *prn = sat; +// return sys; +//} +///* satellite id to satellite number -------------------------------------------- +// * convert satellite id to satellite number +// * args : char *id I satellite id (nn,Gnn,Rnn,Enn,Jnn,Cnn,Inn or Snn) +// * return : satellite number (0: error) +// * notes : 120-142 and 193-199 are also recognized as sbas and qzss +// *-----------------------------------------------------------------------------*/ +//extern int satid2no(const char *id) +//{ +// int sys, prn; +// char code; +// +// if (sscanf(id, "%d", &prn) == 1) +// { +// if (MINPRNGPS <= prn && prn <= MAXPRNGPS) +// sys = SYS_GPS; +// else if (MINPRNSBS <= prn && prn <= MAXPRNSBS) +// sys = SYS_SBS; +// else if (MINPRNQZS <= prn && prn <= MAXPRNQZS) +// sys = SYS_QZS; +// else +// return 0; +// return satno(sys, prn); +// } +// if (sscanf(id, "%c%d", &code, &prn) < 2) +// return 0; +// +// switch (code) +// { +// case 'G': +// sys = SYS_GPS; +// prn += MINPRNGPS - 1; +// break; +// case 'R': +// sys = SYS_GLO; +// prn += MINPRNGLO - 1; +// break; +// case 'E': +// sys = SYS_GAL; +// prn += MINPRNGAL - 1; +// break; +// case 'J': +// sys = SYS_QZS; +// prn += MINPRNQZS - 1; +// break; +// case 'C': +// sys = SYS_CMP; +// prn += MINPRNCMP - 1; +// break; +// case 'I': +// sys = SYS_IRN; +// prn += MINPRNIRN - 1; +// break; +// case 'L': +// sys = SYS_LEO; +// prn += MINPRNLEO - 1; +// break; +// case 'S': +// sys = SYS_SBS; +// prn += 100; +// break; +// default: +// return 0; +// } +// return satno(sys, prn); +//} +///* satellite number to satellite id -------------------------------------------- +// * convert satellite number to satellite id +// * args : int sat I satellite number +// * char *id O satellite id (Gnn,Rnn,Enn,Jnn,Cnn,Inn or nnn) +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void satno2id(int sat, char *id) +//{ +// int prn; +// switch (satsys(sat, &prn)) +// { +// case SYS_GPS: +// sprintf(id, "G%02d", prn - MINPRNGPS + 1); +// return; +// case SYS_GLO: +// sprintf(id, "R%02d", prn - MINPRNGLO + 1); +// return; +// case SYS_GAL: +// sprintf(id, "E%02d", prn - MINPRNGAL + 1); +// return; +// case SYS_QZS: +// sprintf(id, "J%02d", prn - MINPRNQZS + 1); +// return; +// case SYS_CMP: +// sprintf(id, "C%02d", prn - MINPRNCMP + 1); +// return; +// case SYS_IRN: +// sprintf(id, "I%02d", prn - MINPRNIRN + 1); +// return; +// case SYS_LEO: +// sprintf(id, "L%02d", prn - MINPRNLEO + 1); +// return; +// case SYS_SBS: +// sprintf(id, "%03d", prn); +// return; +// } +// strcpy(id, ""); +//} +///* test excluded satellite ----------------------------------------------------- +// * test excluded satellite +// * args : int sat I satellite number +// * double var I variance of ephemeris (m^2) +// * int svh I sv health flag +// * prcopt_t *opt I processing options (NULL: not used) +// * return : status (1:excluded,0:not excluded) +// *-----------------------------------------------------------------------------*/ +//extern int satexclude(int sat, double var, int svh, const prcopt_t *opt) +//{ +// int sys = satsys(sat, NULL); +// +// if (svh < 0) +// return 1; /* ephemeris unavailable */ +// +// if (opt) +// { +// if (opt->exsats[sat - 1] == 1) +// return 1; /* excluded satellite */ +// if (opt->exsats[sat - 1] == 2) +// return 0; /* included satellite */ +// if (!(sys & opt->navsys)) +// return 1; /* unselected sat sys */ +// } +// if (sys == SYS_QZS) +// svh &= 0xFE; /* mask QZSS LEX health */ +// if (svh) +// { +// trace(3, "unhealthy satellite: sat=%3d svh=%02X\n", sat, svh); +// return 1; +// } +// if (var > MAX_VAR_EPH) +// { +// trace(3, "invalid ura satellite: sat=%3d ura=%.2f\n", sat, sqrt(var)); +// return 1; +// } +// return 0; +//} +///* test SNR mask --------------------------------------------------------------- +// * test SNR mask +// * args : int base I rover or base-station (0:rover,1:base station) +// * int idx I frequency index (0:L1,1:L2,2:L3,...) +// * double el I elevation angle (rad) +// * double snr I C/N0 (dBHz) +// * snrmask_t *mask I SNR mask +// * return : status (1:masked,0:unmasked) +// *-----------------------------------------------------------------------------*/ +//extern int testsnr(int base, int idx, double el, double snr, +// const snrmask_t *mask) +//{ +// double minsnr, a; +// int i; +// +// if (!mask->ena[base] || idx < 0 || idx >= NFREQ) +// return 0; +// +// a = (el * R2D + 5.0) / 10.0; +// i = (int)floor(a); +// a -= i; +// if (i < 1) +// minsnr = mask->mask[idx][0]; +// else if (i > 8) +// minsnr = mask->mask[idx][8]; +// else +// minsnr = (1.0 - a) * mask->mask[idx][i - 1] + a * mask->mask[idx][i]; +// +// return snr < minsnr; +//} +///* obs type string to obs code ------------------------------------------------- +// * convert obs code type string to obs code +// * args : char *str I obs code string ("1C","1P","1Y",...) +// * return : obs code (CODE_???) +// * notes : obs codes are based on RINEX 3.04 +// *-----------------------------------------------------------------------------*/ +//extern uint8_t obs2code(const char *obs) +//{ +// int i; +// +// for (i = 1; *obscodes[i]; i++) +// { +// if (strcmp(obscodes[i], obs)) +// continue; +// return (uint8_t)i; +// } +// return CODE_NONE; +//} +///* obs code to obs code string ------------------------------------------------- +// * convert obs code to obs code string +// * args : uint8_t code I obs code (CODE_???) +// * return : obs code string ("1C","1P","1P",...) +// * notes : obs codes are based on RINEX 3.04 +// *-----------------------------------------------------------------------------*/ +//extern char *code2obs(uint8_t code) +//{ +// if (code <= CODE_NONE || MAXCODE < code) +// return ""; +// return obscodes[code]; +//} +///* GPS obs code to frequency -------------------------------------------------*/ +//static int code2freq_GPS(uint8_t code, double *freq) +//{ +// char *obs = code2obs(code); +// +// switch (obs[0]) +// { +// case '1': +// *freq = FREQL1; +// return 0; /* L1 */ +// case '2': +// *freq = FREQL2; +// return 1; /* L2 */ +// case '5': +// *freq = FREQL5; +// return 2; /* L5 */ +// } +// return -1; +//} +///* GLONASS obs code to frequency ---------------------------------------------*/ +//static int code2freq_GLO(uint8_t code, int fcn, double *freq) +//{ +// char *obs = code2obs(code); +// +// if (fcn < -7 || fcn > 6) +// return -1; +// +// switch (obs[0]) +// { +// case '1': +// *freq = FREQ1_GLO + DFRQ1_GLO * fcn; +// return 0; /* G1 */ +// case '2': +// *freq = FREQ2_GLO + DFRQ2_GLO * fcn; +// return 1; /* G2 */ +// case '3': +// *freq = FREQ3_GLO; +// return 2; /* G3 */ +// case '4': +// *freq = FREQ1a_GLO; +// return 0; /* G1a */ +// case '6': +// *freq = FREQ2a_GLO; +// return 1; /* G2a */ +// } +// return -1; +//} +///* Galileo obs code to frequency ---------------------------------------------*/ +//static int code2freq_GAL(uint8_t code, double *freq) +//{ +// char *obs = code2obs(code); +// +// switch (obs[0]) +// { +// case '1': +// *freq = FREQL1; +// return 0; /* E1 */ +// case '7': +// *freq = FREQE5b; +// return 1; /* E5b */ +// case '5': +// *freq = FREQL5; +// return 2; /* E5a */ +// case '6': +// *freq = FREQL6; +// return 3; /* E6 */ +// case '8': +// *freq = FREQE5ab; +// return 4; /* E5ab */ +// } +// return -1; +//} +// +///* QZSS obs code to frequency ------------------------------------------------*/ +//static int code2freq_QZS(uint8_t code, double *freq) +//{ +// char *obs = code2obs(code); +// +// switch (obs[0]) +// { +// case '1': +// *freq = FREQL1; +// return 0; /* L1 */ +// case '2': +// *freq = FREQL2; +// return 1; /* L2 */ +// case '5': +// *freq = FREQL5; +// return 2; /* L5 */ +// case '6': +// *freq = FREQL6; +// return 3; /* L6 */ +// } +// return -1; +//} +///* SBAS obs code to frequency ------------------------------------------------*/ +//static int code2freq_SBS(uint8_t code, double *freq) +//{ +// char *obs = code2obs(code); +// +// switch (obs[0]) +// { +// case '1': +// *freq = FREQL1; +// return 0; /* L1 */ +// case '5': +// *freq = FREQL5; +// return 1; /* L5 */ +// } +// return -1; +//} +///* BDS obs code to frequency -------------------------------------------------*/ +//static int code2freq_BDS(uint8_t code, double *freq) +//{ +// char *obs = code2obs(code); +// +// switch (obs[0]) +// { +// case '1': +// *freq = FREQL1; +// return 0; /* B1C ---->C1(BDS3)*/ +// case '2': +// *freq = FREQ1_CMP; +// return 0; /* B1I---->C2(BDS2,BDS3) */ +// case '7': +// *freq = FREQ2_CMP; +// return 1; /* B2I/B2b ---->C7(BDS2,BDS3)*/ +// case '5': +// *freq = FREQL5; +// return 2; /* B2a ----->C5(BDS3)*/ +// case '6': +// *freq = FREQ3_CMP; +// return 3; /* B3 ----C6(BDS2)*/ +// case '8': +// *freq = FREQE5ab; +// return 4; /* B2ab----C8,B2(BDS3) */ +// } +// return -1; +//} +// +///* NavIC obs code to frequency -----------------------------------------------*/ +//static int code2freq_IRN(uint8_t code, double *freq) +//{ +// char *obs = code2obs(code); +// +// switch (obs[0]) +// { +// case '5': +// *freq = FREQL5; +// return 0; /* L5 */ +// case '9': +// *freq = FREQs; +// return 1; /* S */ +// } +// return -1; +//} +///* system and obs code to frequency index -------------------------------------- +// * convert system and obs code to frequency index +// * args : int sys I satellite system (SYS_???) +// * uint8_t code I obs code (CODE_???) +// * return : frequency index (-1: error) +// * 0 1 2 3 4 +// * -------------------------------------- +// * GPS L1 L2 L5 - - +// * GLONASS G1 G2 G3 - - (G1=G1,G1a,G2=G2,G2a) +// * Galileo E1 E5b E5a E6 E5ab +// * QZSS L1 L2 L5 L6 - +// * SBAS L1 - L5 - - +// * BDS B1 B2 B2a B3 B2ab (B1=B1I,B1C,B2=B2I,B2b) +// * NavIC L5 S - - - +// *-----------------------------------------------------------------------------*/ +//extern int code2idx(int sys, uint8_t code) +//{ +// double freq; +// +// switch (sys) +// { +// case SYS_GPS: +// return code2freq_GPS(code, &freq); +// case SYS_GLO: +// return code2freq_GLO(code, 0, &freq); +// case SYS_GAL: +// return code2freq_GAL(code, &freq); +// case SYS_QZS: +// return code2freq_QZS(code, &freq); +// case SYS_SBS: +// return code2freq_SBS(code, &freq); +// case SYS_CMP: +// return code2freq_BDS(code, &freq); +// case SYS_IRN: +// return code2freq_IRN(code, &freq); +// } +// return -1; +//} +///* system and obs code to frequency -------------------------------------------- +// * convert system and obs code to carrier frequency +// * args : int sys I satellite system (SYS_???) +// * uint8_t code I obs code (CODE_???) +// * int fcn I frequency channel number for GLONASS +// * return : carrier frequency (Hz) (0.0: error) +// *-----------------------------------------------------------------------------*/ +//extern double code2freq(int sys, uint8_t code, int fcn) +//{ +// double freq = 0.0; +// +// switch (sys) +// { +// case SYS_GPS: +// (void)code2freq_GPS(code, &freq); +// break; +// case SYS_GLO: +// (void)code2freq_GLO(code, fcn, &freq); +// break; +// case SYS_GAL: +// (void)code2freq_GAL(code, &freq); +// break; +// case SYS_QZS: +// (void)code2freq_QZS(code, &freq); +// break; +// case SYS_SBS: +// (void)code2freq_SBS(code, &freq); +// break; +// case SYS_CMP: +// (void)code2freq_BDS(code, &freq); +// break; +// case SYS_IRN: +// (void)code2freq_IRN(code, &freq); +// break; +// } +// return freq; +//} +///* satellite and obs code to frequency ----------------------------------------- +// * convert satellite and obs code to carrier frequency +// * args : int sat I satellite number +// * uint8_t code I obs code (CODE_???) +// * nav_t *nav_t I navigation data for GLONASS (NULL: not used) +// * return : carrier frequency (Hz) (0.0: error) +// *-----------------------------------------------------------------------------*/ +//extern double sat2freq(int sat, uint8_t code, const nav_t *nav) +//{ +// int i, fcn = 0, sys, prn; +// +// sys = satsys(sat, &prn); +// +// if (sys == SYS_GLO) +// { +// if (!nav) +// return 0.0; +// for (i = 0; i < nav->ng; i++) +// { +// if (nav->geph[i].sat == sat) +// break; +// } +// if (i < nav->ng) +// { +// fcn = nav->geph[i].frq; +// } +// else if (nav->glo_fcn[prn - 1] > 0) +// { +// fcn = nav->glo_fcn[prn - 1] - 8; +// } +// else +// return 0.0; +// } +// return code2freq(sys, code, fcn); +//} +///* set code priority ----------------------------------------------------------- +// * set code priority for multiple codes in a frequency +// * args : int sys I system (or of SYS_???) +// * int idx I frequency index (0- ) +// * char *pri I priority of codes (series of code characters) +// * (higher priority precedes lower) +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void setcodepri(int sys, int idx, const char *pri) +//{ +// trace(3, "setcodepri:sys=%d idx=%d pri=%s\n", sys, idx, pri); +// +// if (idx < 0 || idx >= MAXFREQ) +// return; +// if (sys & SYS_GPS) +// strcpy(codepris[0][idx], pri); +// if (sys & SYS_GLO) +// strcpy(codepris[1][idx], pri); +// if (sys & SYS_GAL) +// strcpy(codepris[2][idx], pri); +// if (sys & SYS_QZS) +// strcpy(codepris[3][idx], pri); +// if (sys & SYS_SBS) +// strcpy(codepris[4][idx], pri); +// if (sys & SYS_CMP) +// strcpy(codepris[5][idx], pri); +// if (sys & SYS_IRN) +// strcpy(codepris[6][idx], pri); +//} +///* get code priority ----------------------------------------------------------- +// * get code priority for multiple codes in a frequency +// * args : int sys I system (SYS_???) +// * uint8_t code I obs code (CODE_???) +// * char *opt I code options (NULL:no option) +// * return : priority (15:highest-1:lowest,0:error) +// *-----------------------------------------------------------------------------*/ +//extern int getcodepri(int sys, uint8_t code, const char *opt) +//{ +// const char *p, *optstr; +// char *obs, str[8] = ""; +// int i, j; +// +// switch (sys) +// { +// case SYS_GPS: +// i = 0; +// optstr = "-GL%2s"; +// break; +// case SYS_GLO: +// i = 1; +// optstr = "-RL%2s"; +// break; +// case SYS_GAL: +// i = 2; +// optstr = "-EL%2s"; +// break; +// case SYS_QZS: +// i = 3; +// optstr = "-JL%2s"; +// break; +// case SYS_SBS: +// i = 4; +// optstr = "-SL%2s"; +// break; +// case SYS_CMP: +// i = 5; +// optstr = "-CL%2s"; +// break; +// case SYS_IRN: +// i = 6; +// optstr = "-IL%2s"; +// break; +// default: +// return 0; +// } +// if ((j = code2idx(sys, code)) < 0) +// return 0; +// obs = code2obs(code); +// +// /* parse code options */ +// for (p = opt; p && (p = strchr(p, '-')); p++) +// { +// if (sscanf(p, optstr, str) < 1 || str[0] != obs[0]) +// continue; +// return str[1] == obs[1] ? 15 : 0; +// } +// /* search code priority */ +// return (p = strchr(codepris[i][j], obs[1])) ? 14 - (int)(p - codepris[i][j]) : 0; +//} +///* extract unsigned/signed bits ------------------------------------------------ +// * extract unsigned/signed bits from byte data +// * args : uint8_t *buff I byte data +// * int pos I bit position from start of data (bits) +// * int len I bit length (bits) (len<=32) +// * return : extracted unsigned/signed bits +// *-----------------------------------------------------------------------------*/ +//extern uint32_t getbitu(const uint8_t *buff, int pos, int len) +//{ +// uint32_t bits = 0; +// int i; +// for (i = pos; i < pos + len; i++) +// bits = (bits << 1) + ((buff[i / 8] >> (7 - i % 8)) & 1u); +// return bits; +//} +//extern int32_t getbits(const uint8_t *buff, int pos, int len) +//{ +// uint32_t bits = getbitu(buff, pos, len); +// if (len <= 0 || 32 <= len || !(bits & (1u << (len - 1)))) +// return (int32_t)bits; +// return (int32_t)(bits | (~0u << len)); /* extend sign */ +//} +///* set unsigned/signed bits ---------------------------------------------------- +// * set unsigned/signed bits to byte data +// * args : uint8_t *buff IO byte data +// * int pos I bit position from start of data (bits) +// * int len I bit length (bits) (len<=32) +// * [u]int32_t data I unsigned/signed data +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void setbitu(uint8_t *buff, int pos, int len, uint32_t data) +//{ +// uint32_t mask = 1u << (len - 1); +// int i; +// if (len <= 0 || 32 < len) +// return; +// for (i = pos; i < pos + len; i++, mask >>= 1) +// { +// if (data & mask) +// buff[i / 8] |= 1u << (7 - i % 8); +// else +// buff[i / 8] &= ~(1u << (7 - i % 8)); +// } +//} +//extern void setbits(uint8_t *buff, int pos, int len, int32_t data) +//{ +// if (data < 0) +// data |= 1 << (len - 1); +// else +// data &= ~(1 << (len - 1)); /* set sign bit */ +// setbitu(buff, pos, len, (uint32_t)data); +//} +///* crc-32 parity --------------------------------------------------------------- +// * compute crc-32 parity for novatel raw +// * args : uint8_t *buff I data +// * int len I data length (bytes) +// * return : crc-32 parity +// * notes : see NovAtel OEMV firmware manual 1.7 32-bit CRC +// *-----------------------------------------------------------------------------*/ +//extern uint32_t rtk_crc32(const uint8_t *buff, int len) +//{ +// uint32_t crc = 0; +// int i, j; +// +// trace(4, "rtk_crc32: len=%d\n", len); +// +// for (i = 0; i < len; i++) +// { +// crc ^= buff[i]; +// for (j = 0; j < 8; j++) +// { +// if (crc & 1) +// crc = (crc >> 1) ^ POLYCRC32; +// else +// crc >>= 1; +// } +// } +// return crc; +//} +///* crc-24q parity -------------------------------------------------------------- +// * compute crc-24q parity for sbas, rtcm3 +// * args : uint8_t *buff I data +// * int len I data length (bytes) +// * return : crc-24Q parity +// * notes : see reference [2] A.4.3.3 Parity +// *-----------------------------------------------------------------------------*/ +//extern uint32_t rtk_crc24q(const uint8_t *buff, int len) +//{ +// uint32_t crc = 0; +// int i; +// +// trace(4, "rtk_crc24q: len=%d\n", len); +// +// for (i = 0; i < len; i++) +// crc = ((crc << 8) & 0xFFFFFF) ^ tbl_CRC24Q[(crc >> 16) ^ buff[i]]; +// return crc; +//} +///* crc-16 parity --------------------------------------------------------------- +// * compute crc-16 parity for binex, nvs +// * args : uint8_t *buff I data +// * int len I data length (bytes) +// * return : crc-16 parity +// * notes : see reference [10] A.3. +// *-----------------------------------------------------------------------------*/ +//// extern uint16_t rtk_crc16(const uint8_t *buff, int len) +//// { +//// uint16_t crc = 0; +//// int i; +// +//// trace(4, "rtk_crc16: len=%d\n", len); +// +//// for (i = 0; i < len; i++) +//// { +//// crc = (crc << 8) ^ tbl_CRC16[((crc >> 8) ^ buff[i]) & 0xFF]; +//// } +//// return crc; +//// } +///* decode navigation data word ------------------------------------------------- +// * check party and decode navigation data word +// * args : uint32_t word I navigation data word (2+30bit) +// * (previous word D29*-30* + current word D1-30) +// * uint8_t *data O decoded navigation data without parity +// * (8bitx3) +// * return : status (1:ok,0:parity error) +// * notes : see reference [1] 20.3.5.2 user parity algorithm +// *-----------------------------------------------------------------------------*/ +//extern int decode_word(uint32_t word, uint8_t *data) +//{ +// const uint32_t hamming[] = { +// 0xBB1F3480, 0x5D8F9A40, 0xAEC7CD00, 0x5763E680, 0x6BB1F340, 0x8B7A89C0}; +// uint32_t parity = 0, w; +// int i; +// +// trace(5, "decodeword: word=%08x\n", word); +// +// if (word & 0x40000000) +// word ^= 0x3FFFFFC0; +// +// for (i = 0; i < 6; i++) +// { +// parity <<= 1; +// for (w = (word & hamming[i]) >> 6; w; w >>= 1) +// parity ^= w & 1; +// } +// if (parity != (word & 0x3F)) +// return 0; +// +// for (i = 0; i < 3; i++) +// data[i] = (uint8_t)(word >> (22 - i * 8)); +// return 1; +//} +///* new matrix ------------------------------------------------------------------ +// * allocate memory of matrix +// * args : int n,m I number of rows and columns of matrix +// * return : matrix pointer (if n<=0 or m<=0, return NULL) +// *-----------------------------------------------------------------------------*/ +//extern double *mat(int n, int m) +//{ +// double *p; +// +// if (n <= 0 || m <= 0) +// return NULL; +// if (!(p = (double *)malloc(sizeof(double) * n * m))) +// { +// fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m); +// } +// return p; +//} +///* new integer matrix ---------------------------------------------------------- +// * allocate memory of integer matrix +// * args : int n,m I number of rows and columns of matrix +// * return : matrix pointer (if n<=0 or m<=0, return NULL) +// *-----------------------------------------------------------------------------*/ +//extern int *imat(int n, int m) +//{ +// int *p; +// +// if (n <= 0 || m <= 0) +// return NULL; +// if (!(p = (int *)malloc(sizeof(int) * n * m))) +// { +// fatalerr("integer matrix memory allocation error: n=%d,m=%d\n", n, m); +// } +// return p; +//} +///* zero matrix ----------------------------------------------------------------- +// * generate new zero matrix +// * args : int n,m I number of rows and columns of matrix +// * return : matrix pointer (if n<=0 or m<=0, return NULL) +// *-----------------------------------------------------------------------------*/ +//extern double *zeros(int n, int m) +//{ +// double *p; +// +//#if 1 +// if ((p = mat(n, m))) +// for (n = n * m - 1; n >= 0; n--) +// p[n] = 0.0; +//#else +// if (n <= 0 || m <= 0) +// return NULL; +// if (!(p = (double *)calloc(sizeof(double), n * m))) +// { +// fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m); +// } +//#endif +// return p; +//} +///* identity matrix ------------------------------------------------------------- +// * generate new identity matrix +// * args : int n I number of rows and columns of matrix +// * return : matrix pointer (if n<=0, return NULL) +// *-----------------------------------------------------------------------------*/ +//extern double *eye(int n) +//{ +// double *p; +// int i; +// +// if ((p = zeros(n, n))) +// for (i = 0; i < n; i++) +// p[i + i * n] = 1.0; +// return p; +//} +///* inner product --------------------------------------------------------------- +// * inner product of vectors +// * args : double *a,*b I vector a,b (n x 1) +// * int n I size of vector a,b +// * return : a'*b +// *-----------------------------------------------------------------------------*/ +//extern double dot(const double *a, const double *b, int n) +//{ +// double c = 0.0; +// +// while (--n >= 0) +// c += a[n] * b[n]; +// return c; +//} +///* euclid norm ----------------------------------------------------------------- +// * euclid norm of vector +// * args : double *a I vector a (n x 1) +// * int n I size of vector a +// * return : || a || +// *-----------------------------------------------------------------------------*/ +//extern double norm(const double *a, int n) +//{ +// return sqrt(dot(a, a, n)); +//} +///* outer product of 3d vectors ------------------------------------------------- +// * outer product of 3d vectors +// * args : double *a,*b I vector a,b (3 x 1) +// * double *c O outer product (a x b) (3 x 1) +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void cross3(const double *a, const double *b, double *c) +//{ +// c[0] = a[1] * b[2] - a[2] * b[1]; +// c[1] = a[2] * b[0] - a[0] * b[2]; +// c[2] = a[0] * b[1] - a[1] * b[0]; +//} +///* normalize 3d vector --------------------------------------------------------- +// * normalize 3d vector +// * args : double *a I vector a (3 x 1) +// * double *b O normlized vector (3 x 1) || b || = 1 +// * return : status (1:ok,0:error) +// *-----------------------------------------------------------------------------*/ +//extern int normv3(const double *a, double *b) +//{ +// double r; +// if ((r = norm(a, 3)) <= 0.0) +// return 0; +// b[0] = a[0] / r; +// b[1] = a[1] / r; +// b[2] = a[2] / r; +// return 1; +//} +///* copy matrix ----------------------------------------------------------------- +// * copy matrix +// * args : double *A O destination matrix A (n x m) +// * double *B I source matrix B (n x m) +// * int n,m I number of rows and columns of matrix +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void matcpy(double *A, const double *B, int n, int m) +//{ +// memcpy(A, B, sizeof(double) * n * m); +//} +// +///* multiply matrix ----------------------------------------------------------- +//�ڶ�RTKLIB���ж��ο���ʱһ�����õ�������˺���matmul�� +// +//extern void matmul(const char *tr, int n, int k, int m, double alpha, const double *A, const double *B, double beta, double *C) +// +//��������Ϊ��C=alpha*A*B+beta*C�� +// +//����trΪ�Ƿ�ת�õı�־�� +// +//n��k��m������������������������Ĵ�С������˵��������Ӧ�����ĸ���������������������ˣ���һ��������б�Ȼ���ڵڶ���������У��������������Ϳ��Ա�ʾ�� +// +//n�����һ��������У�k����ڶ���������У�m�����һ��������к͵ڶ���������С� +// +//����֮���԰�n��k����mǰ�棬������˳���߼�����ʵ�Ǿ������һ��Ҫѭ��n*k�Σ��ӱ�̵�˳�����ŵġ� +//*/ +//extern void matmul(const char *tr, int n, int k, int m, double alpha, +// const double *A, const double *B, double beta, double *C) +//{ +// double d; +// int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4); +// +// for (i = 0; i < n; i++) +// for (j = 0; j < k; j++) +// { +// d = 0.0; +// switch (f) +// { +// case 1: +// for (x = 0; x < m; x++) +// d += A[i + x * n] * B[x + j * m]; +// break; +// case 2: +// for (x = 0; x < m; x++) +// d += A[i + x * n] * B[j + x * k]; +// break; +// case 3: +// for (x = 0; x < m; x++) +// d += A[x + i * m] * B[x + j * m]; +// break; +// case 4: +// for (x = 0; x < m; x++) +// d += A[x + i * m] * B[j + x * k]; +// break; +// } +// if (beta == 0.0) +// C[i + j * n] = alpha * d; +// else +// C[i + j * n] = alpha * d + beta * C[i + j * n]; +// } +//} +///* LU decomposition ----------------------------------------------------------*/ +//static int ludcmp(double *A, int n, int *indx, double *d) +//{ +// double big, s, tmp, *vv = mat(n, 1); +// int i, imax = 0, j, k; +// +// *d = 1.0; +// for (i = 0; i < n; i++) +// { +// big = 0.0; +// for (j = 0; j < n; j++) +// if ((tmp = fabs(A[i + j * n])) > big) +// big = tmp; +// if (big > 0.0) +// vv[i] = 1.0 / big; +// else +// { +// free(vv); +// return -1; +// } +// } +// for (j = 0; j < n; j++) +// { +// for (i = 0; i < j; i++) +// { +// s = A[i + j * n]; +// for (k = 0; k < i; k++) +// s -= A[i + k * n] * A[k + j * n]; +// A[i + j * n] = s; +// } +// big = 0.0; +// for (i = j; i < n; i++) +// { +// s = A[i + j * n]; +// for (k = 0; k < j; k++) +// s -= A[i + k * n] * A[k + j * n]; +// A[i + j * n] = s; +// if ((tmp = vv[i] * fabs(s)) >= big) +// { +// big = tmp; +// imax = i; +// } +// } +// if (j != imax) +// { +// for (k = 0; k < n; k++) +// { +// tmp = A[imax + k * n]; +// A[imax + k * n] = A[j + k * n]; +// A[j + k * n] = tmp; +// } +// *d = -(*d); +// vv[imax] = vv[j]; +// } +// indx[j] = imax; +// if (A[j + j * n] == 0.0) +// { +// free(vv); +// return -1; +// } +// if (j != n - 1) +// { +// tmp = 1.0 / A[j + j * n]; +// for (i = j + 1; i < n; i++) +// A[i + j * n] *= tmp; +// } +// } +// free(vv); +// return 0; +//} +///* LU back-substitution ------------------------------------------------------*/ +//static void lubksb(const double *A, int n, const int *indx, double *b) +//{ +// double s; +// int i, ii = -1, ip, j; +// +// for (i = 0; i < n; i++) +// { +// ip = indx[i]; +// s = b[ip]; +// b[ip] = b[i]; +// if (ii >= 0) +// for (j = ii; j < i; j++) +// s -= A[i + j * n] * b[j]; +// else if (s) +// ii = i; +// b[i] = s; +// } +// for (i = n - 1; i >= 0; i--) +// { +// s = b[i]; +// for (j = i + 1; j < n; j++) +// s -= A[i + j * n] * b[j]; +// b[i] = s / A[i + i * n]; +// } +//} +///* inverse of matrix ---------------------------------------------------------*/ +//extern int matinv(double *A, int n) +//{ +// double d, *B; +// int i, j, *indx; +// +// indx = imat(n, 1); +// B = mat(n, n); +// matcpy(B, A, n, n); +// if (ludcmp(B, n, indx, &d)) +// { +// free(indx); +// free(B); +// return -1; +// } +// for (j = 0; j < n; j++) +// { +// for (i = 0; i < n; i++) +// A[i + j * n] = 0.0; +// A[j + j * n] = 1.0; +// lubksb(B, n, indx, A + j * n); +// } +// free(indx); +// free(B); +// return 0; +//} +///* solve linear equation -----------------------------------------------------*/ +//extern int solve(const char *tr, const double *A, const double *Y, int n, +// int m, double *X) +//{ +// double *B = mat(n, n); +// int info; +// +// matcpy(B, A, n, n); +// if (!(info = matinv(B, n))) +// matmul(tr[0] == 'N' ? "NN" : "TN", n, m, n, 1.0, B, Y, 0.0, X); +// free(B); +// return info; +//} +///* end of matrix routines ----------------------------------------------------*/ +// +///* least square estimation ----------------------------------------------------- +// * least square estimation by solving normal equation (x=(A*A')^-1*A*y) +// * args : double *A I transpose of (weighted) design matrix (n x m) +// * double *y I (weighted) measurements (m x 1) +// * int n,m I number of parameters and measurements (n<=m) +// * double *x O estmated parameters (n x 1) +// * double *Q O esimated parameters covariance matrix (n x n) +// * return : status (0:ok,0>:error) +// * notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2)) +// * matirix stored by column-major order (fortran convention) +// * ����õ����� $x=(AAT){-1}Ay$ ��ߵ�ֵ�͸�ֵ��Э������� $Q=(AAT){-1}$ +// *-----------------------------------------------------------------------------*/ +//extern int lsq(const double *A, const double *y, int n, int m, double *x, +// double *Q) +//{ +// double *Ay; +// int info; +// +// if (m < n) +// return -1; +// Ay = mat(n, 1); +// matmul("NN", n, 1, m, 1.0, A, y, 0.0, Ay); /* Ay=A*y 1�����ȼ����Ұ벿�� $A_y=Ay$��*/ +// matmul("NT", n, n, m, 1.0, A, A, 0.0, Q); /* Q=A*A' 2���ټ�����벿�����������ֵ $Q=AA^T$�� */ +// +// if (!(info = matinv(Q, n))) +// matmul("NN", n, 1, n, 1.0, Q, Ay, 0.0, x); /* x=Q^-1*Ay 3������ Q������� $Q^{-1}$�����Դ洢�� Q�У� +// ������ҳ� $A_y$���õ� x��ֵ*/ +// free(Ay); +// return info; +//} +///* kalman filter --------------------------------------------------------------- +//* kalman filter state update as follows: +//* +//* K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P +//* +//* args : double *x I states vector (n x 1) +//* double *P I covariance matrix of states (n x n) +//* double *H I transpose of design matrix (n x m) +//* double *v I innovation (measurement - model) (m x 1) +//* double *R I covariance matrix of measurement error (m x m) +//* int n,m I number of states and measurements +//* double *xp O states vector after update (n x 1) +//* double *Pp O covariance matrix of states after update (n x n) +//* return : status (0:ok,<0:error) +//* notes : matirix stored by column-major order (fortran convention) +//* if state x[i]==0.0, not updates state x[i]/P[i+i*n] +//* �������˲�״̬���£� K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P +//* ����������9���� +//double *x I ״̬���� (n x 1) +//double *P I ״̬���������Э������ (n x n) +//double *H I �۲�����ת�� (n x m) +//double *v I ʵ�ʹ۲�����Ԥ��۲����IJв� (measurement - model) (m x 1) +//double *R I ��������Э���� (m x m) +//int n I ״̬�������� +//int m I �۲�ֵ���� +//double *xp O ���º��״̬���� (n x 1) +//double *Pp O ���º��״̬���������Э������ (n x n) +//��������: +//int O (0:ok,<0:error) +//�����ǰ������ȵ�˳���� (fortran convention) +//*-----------------------------------------------------------------------------*/ +//static int filter_(const double *x, const double *P, const double *H, +// const double *v, const double *R, int n, int m, +// double *xp, double *Pp) +//{ +// //���þ������㺯�����չ�ʽ���о������� +// double *F = mat(n, m), *Q = mat(m, m), *K = mat(n, m), *I = eye(n); +// int info; +// +// matcpy(Q, R, m, m); +// matcpy(xp, x, n, 1); +// matmul("NN", n, m, n, 1.0, P, H, 0.0, F); /* Q=H'*P*H+R */ +// matmul("TN", m, m, n, 1.0, H, F, 1.0, Q); +// if (!(info = matinv(Q, m))) +// { +// matmul("NN", n, m, m, 1.0, F, Q, 0.0, K); /* K=P*H*Q^-1 */ +// matmul("NN", n, 1, m, 1.0, K, v, 1.0, xp); /* xp=x+K*v */ +// matmul("NT", n, n, m, -1.0, K, H, 1.0, I); /* Pp=(I-K*H')*P */ +// matmul("NN", n, n, n, 1.0, I, P, 0.0, Pp); +// } +// free(F); +// free(Q); +// free(K); +// free(I); +// return info; +//} +///*kalman�˲����� +//����������7���� +//double *x IO ״̬���� (n x 1) +//double *P IO ״̬���������Э������ (n x n) +//double *H I �۲�����ת�� (n x m) +//double *v I ʵ�ʹ۲�����Ԥ��۲����IJв� (measurement - model) (m x 1) +//double *R I ��������Э���� (m x m) +//int n I ״̬�������� +//int m I �۲�ֵ���� +//��������: +//int O (0:ok,<0:error) +//*/ +//extern int filter(double *x, double *P, const double *H, const double *v, +// const double *R, int n, int m) +//{ +// double *x_, *xp_, *P_, *Pp_, *H_; +// int i, j, k, info, *ix; +// +// /* create list of non-zero states +// 1��ѡ����Ҫ���µ�״̬ x �Ͷ�Ӧ�� P��H �� x_��p_��H_ ��*/ +// ix = imat(n, 1); +// for (i = k = 0; i < n; i++) +// if (x[i] != 0.0 && P[i + i * n] > 0.0) +// ix[k++] = i; +// x_ = mat(k, 1); +// xp_ = mat(k, 1); +// P_ = mat(k, k); +// Pp_ = mat(k, k); +// H_ = mat(k, m); +// /* compress array by removing zero elements to save computation time */ +// for (i = 0; i < k; i++) +// { +// x_[i] = x[ix[i]]; +// for (j = 0; j < k; j++) +// P_[i + j * k] = P[ix[i] + ix[j] * n]; +// for (j = 0; j < m; j++) +// H_[i + j * k] = H[ix[i] + j * n]; +// } +// /* do kalman filter state update on compressed arrays +// 2������ filter_ ����kalman�˲�����*/ +// info = filter_(x_, P_, H_, v, R, k, m, xp_, Pp_); +// /* copy values from compressed arrays back to full arrays +// 3��������ֵ�浽 x��P��*/ +// for (i = 0; i < k; i++) +// { +// x[ix[i]] = xp_[i]; +// for (j = 0; j < k; j++) +// P[ix[i] + ix[j] * n] = Pp_[i + j * k]; +// } +// free(ix); +// free(x_); +// free(xp_); +// free(P_); +// free(Pp_); +// free(H_); +// return info; +//} +///* smoother -------------------------------------------------------------------- +// * combine forward and backward filters by fixed-interval smoother as follows: +// * +// * xs=Qs*(Qf^-1*xf+Qb^-1*xb), Qs=(Qf^-1+Qb^-1)^-1) +// * +// * args : double *xf I forward solutions (n x 1) +// * args : double *Qf I forward solutions covariance matrix (n x n) +// * double *xb I backward solutions (n x 1) +// * double *Qb I backward solutions covariance matrix (n x n) +// * int n I number of solutions +// * double *xs O smoothed solutions (n x 1) +// * double *Qs O smoothed solutions covariance matrix (n x n) +// * return : status (0:ok,0>:error) +// * notes : see reference [4] 5.2 +// * matirix stored by column-major order (fortran convention) +// *-----------------------------------------------------------------------------*/ +//extern int smoother(const double *xf, const double *Qf, const double *xb, +// const double *Qb, int n, double *xs, double *Qs) +//{ +// double *invQf = mat(n, n), *invQb = mat(n, n), *xx = mat(n, 1); +// int i, info = -1; +// +// matcpy(invQf, Qf, n, n); +// matcpy(invQb, Qb, n, n); +// if (!matinv(invQf, n) && !matinv(invQb, n)) +// { +// for (i = 0; i < n * n; i++) +// Qs[i] = invQf[i] + invQb[i]; +// if (!(info = matinv(Qs, n))) +// { +// matmul("NN", n, 1, n, 1.0, invQf, xf, 0.0, xx); +// matmul("NN", n, 1, n, 1.0, invQb, xb, 1.0, xx); +// matmul("NN", n, 1, n, 1.0, Qs, xx, 0.0, xs); +// } +// } +// free(invQf); +// free(invQb); +// free(xx); +// return info; +//} +///* print matrix ---------------------------------------------------------------- +// * print matrix to stdout +// * args : double *A I matrix A (n x m) +// * int n,m I number of rows and columns of A +// * int p,q I total columns, columns under decimal point +// * (FILE *fp I output file pointer) +// * return : none +// * notes : matirix stored by column-major order (fortran convention) +// *-----------------------------------------------------------------------------*/ +//// extern void matfprint(const double A[], int n, int m, int p, int q, FILE *fp) +//// { +//// int i, j; +// +//// for (i = 0; i < n; i++) +//// { +//// for (j = 0; j < m; j++) +//// fprintf(fp, " %*.*f", p, q, A[i + j * n]); +//// fprintf(fp, "\n"); +//// } +//// } +//// extern void matprint(const double A[], int n, int m, int p, int q) +//// { +//// matfprint(A, n, m, p, q, stdout); +//// } +///* string to number ------------------------------------------------------------ +// * convert substring in string to number +// * args : char *s I string ("... nnn.nnn ...") +// * int i,n I substring position and width +// * return : converted number (0.0:error) +// *-----------------------------------------------------------------------------*/ +//extern double str2num(const char *s, int i, int n) +//{ +// double value; +// char str[256], *p = str; +// +// if (i < 0 || (int)strlen(s) < i || (int)sizeof(str) - 1 < n) +// return 0.0; +// for (s += i; *s && --n >= 0; s++) +// *p++ = *s == 'd' || *s == 'D' ? 'E' : *s; +// *p = '\0'; +// return sscanf(str, "%lf", &value) == 1 ? value : 0.0; +//} +///* string to time -------------------------------------------------------------- +// * convert substring in string to gtime_t struct +// * args : char *s I string ("... yyyy mm dd hh mm ss ...") +// * int i,n I substring position and width +// * gtime_t *t O gtime_t struct +// * return : status (0:ok,0>:error) +// *-----------------------------------------------------------------------------*/ +//extern int str2time(const char *s, int i, int n, gtime_t *t) +//{ +// double ep[6]; +// char str[256], *p = str; +// +// if (i < 0 || (int)strlen(s) < i || (int)sizeof(str) - 1 < i) +// return -1; +// for (s += i; *s && --n >= 0;) +// *p++ = *s++; +// *p = '\0'; +// if (sscanf(str, "%lf %lf %lf %lf %lf %lf", ep, ep + 1, ep + 2, ep + 3, ep + 4, ep + 5) < 6) +// return -1; +// if (ep[0] < 100.0) +// ep[0] += ep[0] < 80.0 ? 2000.0 : 1900.0; +// *t = epoch2time(ep); +// return 0; +//} +///* convert calendar day/time to time ------------------------------------------- +// * convert calendar day/time to gtime_t struct +// * args : double *ep I day/time {year,month,day,hour,min,sec} +// * return : gtime_t struct +// * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) +// *-----------------------------------------------------------------------------*/ +////extern gtime_t epoch2time(const double *ep) +////{ +//// const int doy[] = {1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}; +//// gtime_t time = {0}; +//// int days, sec, year = (int)ep[0], mon = (int)ep[1], day = (int)ep[2]; +//// +//// if (year < 1970 || 2099 < year || mon < 1 || 12 < mon) +//// return time; +//// +//// /* leap year if year%4==0 in 1901-2099 */ +//// days = (year - 1970) * 365 + (year - 1969) / 4 + doy[mon - 1] + day - 2 + (year % 4 == 0 && mon >= 3 ? 1 : 0); +//// sec = (int)floor(ep[5]); +//// time.time = (time_t)days * 86400 + (int)ep[3] * 3600 + (int)ep[4] * 60 + sec; +//// time.sec = ep[5] - sec; +//// return time; +////} +///* time to calendar day/time --------------------------------------------------- +// * convert gtime_t struct to calendar day/time +// * args : gtime_t t I gtime_t struct +// * double *ep O day/time {year,month,day,hour,min,sec} +// * return : none +// * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) +// *-----------------------------------------------------------------------------*/ +//extern void time2epoch(gtime_t t, double *ep) +//{ +// const int mday[] = {/* # of days in a month */ +// 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, +// 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; +// int days, sec, mon, day; +// +// /* leap year if year%4==0 in 1901-2099 */ +// days = (int)(t.time / 86400); +// sec = (int)(t.time - (time_t)days * 86400); +// for (day = days % 1461, mon = 0; mon < 48; mon++) +// { +// if (day >= mday[mon]) +// day -= mday[mon]; +// else +// break; +// } +// ep[0] = 1970 + days / 1461 * 4 + mon / 12; +// ep[1] = mon % 12 + 1; +// ep[2] = day + 1; +// ep[3] = sec / 3600; +// ep[4] = sec % 3600 / 60; +// ep[5] = sec % 60 + t.sec; +//} +///* same as above but output limited to n decimals for formatted output */ +//extern void time2epoch_n(gtime_t t, double *ep, int n) +//{ +// if (n < 0) +// n = 0; +// else if (n > 12) +// n = 12; +// if (1.0 - t.sec < 0.5 / pow(10.0, n)) +// { +// t.time++; +// t.sec = 0.0; +// }; +// time2epoch(t, ep); +//} +///* gps time to time ------------------------------------------------------------ +// * convert week and tow in gps time to gtime_t struct +// * args : int week I week number in gps time +// * double sec I time of week in gps time (s) +// * return : gtime_t struct +// *-----------------------------------------------------------------------------*/ +//extern gtime_t gpst2time(int week, double sec) +//{ +// gtime_t t = epoch2time(gpst0); +// +// if (sec < -1E9 || 1E9 < sec) +// sec = 0.0; +// t.time += (time_t)86400 * 7 * week + (int)sec; +// t.sec = sec - (int)sec; +// return t; +//} +///* time to gps time ------------------------------------------------------------ +// * convert gtime_t struct to week and tow in gps time +// * args : gtime_t t I gtime_t struct +// * int *week IO week number in gps time (NULL: no output) +// * return : time of week in gps time (s) +// *-----------------------------------------------------------------------------*/ +//extern double time2gpst(gtime_t t, int *week) +//{ +// gtime_t t0 = epoch2time(gpst0); +// time_t sec = t.time - t0.time; +// int w = (int)(sec / (86400 * 7)); +// +// if (week) +// *week = w; +// return (double)(sec - (double)w * 86400 * 7) + t.sec; +//} +///* galileo system time to time ------------------------------------------------- +// * convert week and tow in galileo system time (gst) to gtime_t struct +// * args : int week I week number in gst +// * double sec I time of week in gst (s) +// * return : gtime_t struct +// *-----------------------------------------------------------------------------*/ +//extern gtime_t gst2time(int week, double sec) +//{ +// gtime_t t = epoch2time(gst0); +// +// if (sec < -1E9 || 1E9 < sec) +// sec = 0.0; +// t.time += (time_t)86400 * 7 * week + (int)sec; +// t.sec = sec - (int)sec; +// return t; +//} +///* time to galileo system time ------------------------------------------------- +// * convert gtime_t struct to week and tow in galileo system time (gst) +// * args : gtime_t t I gtime_t struct +// * int *week IO week number in gst (NULL: no output) +// * return : time of week in gst (s) +// *-----------------------------------------------------------------------------*/ +//extern double time2gst(gtime_t t, int *week) +//{ +// gtime_t t0 = epoch2time(gst0); +// time_t sec = t.time - t0.time; +// int w = (int)(sec / (86400 * 7)); +// +// if (week) +// *week = w; +// return (double)(sec - (double)w * 86400 * 7) + t.sec; +//} +///* beidou time (bdt) to time --------------------------------------------------- +// * convert week and tow in beidou time (bdt) to gtime_t struct +// * args : int week I week number in bdt +// * double sec I time of week in bdt (s) +// * return : gtime_t struct +// *-----------------------------------------------------------------------------*/ +//extern gtime_t bdt2time(int week, double sec) +//{ +// gtime_t t = epoch2time(bdt0); +// +// if (sec < -1E9 || 1E9 < sec) +// sec = 0.0; +// t.time += (time_t)86400 * 7 * week + (int)sec; +// t.sec = sec - (int)sec; +// return t; +//} +///* time to beidouo time (bdt) -------------------------------------------------- +// * convert gtime_t struct to week and tow in beidou time (bdt) +// * args : gtime_t t I gtime_t struct +// * int *week IO week number in bdt (NULL: no output) +// * return : time of week in bdt (s) +// *-----------------------------------------------------------------------------*/ +//extern double time2bdt(gtime_t t, int *week) +//{ +// gtime_t t0 = epoch2time(bdt0); +// time_t sec = t.time - t0.time; +// int w = (int)(sec / (86400 * 7)); +// +// if (week) +// *week = w; +// return (double)(sec - (double)w * 86400 * 7) + t.sec; +//} +///* add time -------------------------------------------------------------------- +// * add time to gtime_t struct +// * args : gtime_t t I gtime_t struct +// * double sec I time to add (s) +// * return : gtime_t struct (t+sec) +// *-----------------------------------------------------------------------------*/ +extern gtime_t timeadd(gtime_t t, double sec) +{ + double tt; + + t.sec += sec; + tt = floor(t.sec); + t.time += (int)tt; + t.sec -= tt; + return t; +} +///* time difference ------------------------------------------------------------- +// * difference between gtime_t structs +// * args : gtime_t t1,t2 I gtime_t structs +// * return : time difference (t1-t2) (s) +// *-----------------------------------------------------------------------------*/ +extern double timediff(gtime_t t1, gtime_t t2) +{ + return difftime(t1.time, t2.time) + t1.sec - t2.sec; +} +///* get current time in utc ----------------------------------------------------- +// * get current time in utc +// * args : none +// * return : current time in utc +// *-----------------------------------------------------------------------------*/ +//static double timeoffset_ = 0.0; /* time offset (s) */ +// +//extern gtime_t timeget(void) +//{ +// gtime_t time; +// extern time_t system_time_utc; +// time.time = system_time_utc; +// time.sec = 0; +// return timeadd(time, timeoffset_); +//} +///* set current time in utc ----------------------------------------------------- +// * set current time in utc +// * args : gtime_t I current time in utc +// * return : none +// * notes : just set time offset between cpu time and current time +// * the time offset is reflected to only timeget() +// * not reentrant +// *-----------------------------------------------------------------------------*/ +//extern void timeset(gtime_t t) +//{ +// timeoffset_ += timediff(t, timeget()); +//} +///* reset current time ---------------------------------------------------------- +// * reset current time +// * args : none +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void timereset(void) +//{ +// timeoffset_ = 0.0; +//} +///* read leap seconds table by text -------------------------------------------*/ +//// static int read_leaps_text(FILE *fp) +//// { +//// char buff[256], *p; +//// int i, n = 0, ep[6], ls; +// +//// rewind(fp); +// +//// while (fgets(buff, sizeof(buff), fp) && n < MAXLEAPS) +//// { +//// if ((p = strchr(buff, '#'))) +//// *p = '\0'; +//// if (sscanf(buff, "%d %d %d %d %d %d %d", ep, ep + 1, ep + 2, ep + 3, ep + 4, ep + 5, +//// &ls) < 7) +//// continue; +//// for (i = 0; i < 6; i++) +//// leaps[n][i] = ep[i]; +//// leaps[n++][6] = ls; +//// } +//// return n; +//// } +///* read leap seconds table by usno -------------------------------------------*/ +//// static int read_leaps_usno(FILE *fp) +//// { +//// static const char *months[] = { +//// "JAN", "FEB", "MAR", "APR", "MAY", "JUN", "JUL", "AUG", "SEP", "OCT", "NOV", "DEC"}; +//// double jd, tai_utc; +//// char buff[256], month[32] = {'\0'}, ls[MAXLEAPS][7] = {{0}}; +//// int i, j, y, m, d, n = 0; +// +//// rewind(fp); +// +//// while (fgets(buff, sizeof(buff), fp) && n < MAXLEAPS) +//// { +//// if (sscanf(buff, "%d %s %d =JD %lf TAI-UTC= %lf", &y, month, &d, &jd, +//// &tai_utc) < 5) +//// continue; +//// if (y < 1980) +//// continue; +//// for (m = 1; m <= 12; m++) +//// if (!strcmp(months[m - 1], month)) +//// break; +//// if (m >= 13) +//// continue; +//// ls[n][0] = y; +//// ls[n][1] = m; +//// ls[n][2] = d; +//// ls[n++][6] = (char)(19.0 - tai_utc); +//// } +//// for (i = 0; i < n; i++) +//// for (j = 0; j < 7; j++) +//// { +//// leaps[i][j] = ls[n - i - 1][j]; +//// } +//// return n; +//// } +///* read leap seconds table ----------------------------------------------------- +// * read leap seconds table +// * args : char *file I leap seconds table file +// * return : status (1:ok,0:error) +// * notes : The leap second table should be as follows or leapsec.dat provided +// * by USNO. +// * (1) The records in the table file cosist of the following fields: +// * year month day hour min sec UTC-GPST(s) +// * (2) The date and time indicate the start UTC time for the UTC-GPST +// * (3) The date and time should be descending order. +// *-----------------------------------------------------------------------------*/ +//// extern int read_leaps(const char *file) +//// { +//// FILE *fp; +//// int i, n; +// +//// if (!(fp = fopen(file, "r"))) +//// return 0; +// +//// /* read leap seconds table by text or usno */ +//// if (!(n = read_leaps_text(fp)) && !(n = read_leaps_usno(fp))) +//// { +//// fclose(fp); +//// return 0; +//// } +//// for (i = 0; i < 7; i++) +//// leaps[n][i] = 0.0; +//// fclose(fp); +//// return 1; +//// } +///* gpstime to utc -------------------------------------------------------------- +// * convert gpstime to utc considering leap seconds +// * args : gtime_t t I time expressed in gpstime +// * return : time expressed in utc +// * notes : ignore slight time offset under 100 ns +// *-----------------------------------------------------------------------------*/ +extern gtime_t gpst2utc(gtime_t t) +{ + gtime_t tu; + int i; + + for (i = 0; leaps[i][0] > 0; i++) + { + tu = timeadd(t, leaps[i][6]); + if (timediff(tu, epoch2time(leaps[i])) >= 0.0) + return tu; + } + return t; +} +///* utc to gpstime -------------------------------------------------------------- +// * convert utc to gpstime considering leap seconds +// * args : gtime_t t I time expressed in utc +// * return : time expressed in gpstime +// * notes : ignore slight time offset under 100 ns +// *-----------------------------------------------------------------------------*/ +extern gtime_t utc2gpst(gtime_t t) +{ + int i; + + for (i = 0; leaps[i][0] > 0; i++) + { + if (timediff(t, epoch2time(leaps[i])) >= 0.0) + return timeadd(t, -leaps[i][6]); + } + return t; +} +/* gpstime to bdt -------------------------------------------------------------- + * convert gpstime to bdt (beidou navigation satellite system time) + * args : gtime_t t I time expressed in gpstime + * return : time expressed in bdt + * notes : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC + * no leap seconds in BDT + * ignore slight time offset under 100 ns + *-----------------------------------------------------------------------------*/ +//extern gtime_t gpst2bdt(gtime_t t) +//{ +// return timeadd(t, -14.0); +//} +///* bdt to gpstime -------------------------------------------------------------- +// * convert bdt (beidou navigation satellite system time) to gpstime +// * args : gtime_t t I time expressed in bdt +// * return : time expressed in gpstime +// * notes : see gpst2bdt() +// *-----------------------------------------------------------------------------*/ +//extern gtime_t bdt2gpst(gtime_t t) +//{ +// return timeadd(t, 14.0); +//} +///* time to day and sec -------------------------------------------------------*/ +//static double time2sec(gtime_t time, gtime_t *day) +//{ +// double ep[6], sec; +// time2epoch(time, ep); +// sec = ep[3] * 3600.0 + ep[4] * 60.0 + ep[5]; +// ep[3] = ep[4] = ep[5] = 0.0; +// *day = epoch2time(ep); +// return sec; +//} +///* utc to gmst ----------------------------------------------------------------- +// * convert utc to gmst (Greenwich mean sidereal time) +// * args : gtime_t t I time expressed in utc +// * double ut1_utc I UT1-UTC (s) +// * return : gmst (rad) +// *-----------------------------------------------------------------------------*/ +//extern double utc2gmst(gtime_t t, double ut1_utc) +//{ +// const double ep2000[] = {2000, 1, 1, 12, 0, 0}; +// gtime_t tut, tut0; +// double ut, t1, t2, t3, gmst0, gmst; +// +// tut = timeadd(t, ut1_utc); +// ut = time2sec(tut, &tut0); +// t1 = timediff(tut0, epoch2time(ep2000)) / 86400.0 / 36525.0; +// t2 = t1 * t1; +// t3 = t2 * t1; +// gmst0 = 24110.54841 + 8640184.812866 * t1 + 0.093104 * t2 - 6.2E-6 * t3; +// gmst = gmst0 + 1.002737909350795 * ut; +// +// return fmod(gmst, 86400.0) * PI / 43200.0; /* 0 <= gmst <= 2*PI */ +//} +///* time to string -------------------------------------------------------------- +// * convert gtime_t struct to string +// * args : gtime_t t I gtime_t struct +// * char *s O string ("yyyy/mm/dd hh:mm:ss.ssss") +// * int n I number of decimals +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void time2str(gtime_t t, char *s, int n) +//{ +// double ep[6]; +// +// if (n < 0) +// n = 0; +// else if (n > 12) +// n = 12; +// if (1.0 - t.sec < 0.5 / pow(10.0, n)) +// { +// t.time++; +// t.sec = 0.0; +// }; +// time2epoch(t, ep); +// sprintf(s, "%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%0*.*f", ep[0], ep[1], ep[2], +// ep[3], ep[4], n <= 0 ? 2 : n + 3, n <= 0 ? 0 : n, ep[5]); +//} +///* get time string ------------------------------------------------------------- +// * get time string +// * args : gtime_t t I gtime_t struct +// * int n I number of decimals +// * return : time string +// * notes : not reentrant, do not use multiple in a function +// *-----------------------------------------------------------------------------*/ +//extern char *time_str(gtime_t t, int n) +//{ +// static char buff[64]; +// time2str(t, buff, n); +// return buff; +//} +///* time to day of year --------------------------------------------------------- +// * convert time to day of year +// * args : gtime_t t I gtime_t struct +// * return : day of year (days) +// *-----------------------------------------------------------------------------*/ +//extern double time2doy(gtime_t t) +//{ +// double ep[6]; +// +// time2epoch(t, ep); +// ep[1] = ep[2] = 1.0; +// ep[3] = ep[4] = ep[5] = 0.0; +// return timediff(t, epoch2time(ep)) / 86400.0 + 1.0; +//} +///* adjust gps week number ------------------------------------------------------ +// * adjust gps week number using cpu time +// * args : int week I not-adjusted gps week number (0-1023) +// * return : adjusted gps week number +// *-----------------------------------------------------------------------------*/ +//extern int adjgpsweek(int week) +//{ +// int w; +// (void)time2gpst(utc2gpst(timeget()), &w); +// if (w < 1560) +// w = 1560; /* use 2009/12/1 if time is earlier than 2009/12/1 */ +// return week + (w - week + 1) / 1024 * 1024; +//} +///* get tick time --------------------------------------------------------------- +// * get current tick in ms +// * args : none +// * return : current tick in ms +// *-----------------------------------------------------------------------------*/ +////extern uint32_t tickget(void) +////{ +//// return Brd_TimerServerTick(); +////} +///* sleep ms -------------------------------------------------------------------- +// * sleep ms +// * args : int ms I miliseconds to sleep (<0:no sleep) +// * return : none +// *-----------------------------------------------------------------------------*/ +////extern void sleepms(int ms) +////{ +//// if (ms > 0) +//// SfSleep(ms); +//// else +//// return; +////} +///* convert degree to deg-min-sec ----------------------------------------------- +// * convert degree to degree-minute-second +// * args : double deg I degree +// * double *dms O degree-minute-second {deg,min,sec} +// * int ndec I number of decimals of second +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void deg2dms(double deg, double *dms, int ndec) +//{ +// double sign = deg < 0.0 ? -1.0 : 1.0, a = fabs(deg); +// double unit = pow(0.1, ndec); +// dms[0] = floor(a); +// a = (a - dms[0]) * 60.0; +// dms[1] = floor(a); +// a = (a - dms[1]) * 60.0; +// dms[2] = floor(a / unit + 0.5) * unit; +// if (dms[2] >= 60.0) +// { +// dms[2] = 0.0; +// dms[1] += 1.0; +// if (dms[1] >= 60.0) +// { +// dms[1] = 0.0; +// dms[0] += 1.0; +// } +// } +// dms[0] *= sign; +//} +///* convert deg-min-sec to degree ----------------------------------------------- +// * convert degree-minute-second to degree +// * args : double *dms I degree-minute-second {deg,min,sec} +// * return : degree +// *-----------------------------------------------------------------------------*/ +//extern double dms2deg(const double *dms) +//{ +// double sign = dms[0] < 0.0 ? -1.0 : 1.0; +// return sign * (fabs(dms[0]) + dms[1] / 60.0 + dms[2] / 3600.0); +//} +///* transform ecef to geodetic postion ------------------------------------------ +// * transform ecef position to geodetic position +// * args : double *r I ecef position {x,y,z} (m) +// * double *pos O geodetic position {lat,lon,h} (rad,m) +// * return : none +// * notes : WGS84, ellipsoidal height +// *-----------------------------------------------------------------------------*/ +//extern void ecef2pos(const double *r, double *pos) +//{ +// double e2 = FE_WGS84 * (2.0 - FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp; +// +// for (z = r[2], zk = 0.0; fabs(z - zk) >= 1E-4;) +// { +// zk = z; +// sinp = z / sqrt(r2 + z * z); +// v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp); +// z = r[2] + v * e2 * sinp; +// } +// pos[0] = r2 > 1E-12 ? atan(z / sqrt(r2)) : (r[2] > 0.0 ? PI / 2.0 : -PI / 2.0); +// pos[1] = r2 > 1E-12 ? atan2(r[1], r[0]) : 0.0; +// pos[2] = sqrt(r2 + z * z) - v; +//} +///* transform geodetic to ecef position ----------------------------------------- +// * transform geodetic position to ecef position +// * args : double *pos I geodetic position {lat,lon,h} (rad,m) +// * double *r O ecef position {x,y,z} (m) +// * return : none +// * notes : WGS84, ellipsoidal height +// *-----------------------------------------------------------------------------*/ +extern void pos2ecef(const double *pos, double *r) +{ + double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]); + double e2 = FE_WGS84 * (2.0 - FE_WGS84), v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp); + + r[0] = (v + pos[2]) * cosp * cosl; + r[1] = (v + pos[2]) * cosp * sinl; + r[2] = (v * (1.0 - e2) + pos[2]) * sinp; +} +///* ecef to local coordinate transfromation matrix ------------------------------ +// * compute ecef to local coordinate transfromation matrix +// * args : double *pos I geodetic position {lat,lon} (rad) +// * double *E O ecef to local coord transformation matrix (3x3) +// * return : none +// * notes : matirix stored by column-major order (fortran convention) +// *-----------------------------------------------------------------------------*/ +//extern void xyz2enu(const double *pos, double *E) +//{ +// double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]); +// +// E[0] = -sinl; +// E[1] = -sinp * cosl; +// E[2] = cosp * cosl; +// E[3] = cosl; +// E[4] = -sinp * sinl; +// E[5] = cosp * sinl; +// E[6] = 0.0; +// E[7] = cosp; +// E[8] = sinp; +//} +///* transform ecef vector to local tangental coordinate ------------------------- +// * transform ecef vector to local tangental coordinate +// * args : double *pos I geodetic position {lat,lon} (rad) +// * double *r I vector in ecef coordinate {x,y,z} +// * double *e O vector in local tangental coordinate {e,n,u} +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void ecef2enu(const double *pos, const double *r, double *e) +//{ +// double E[9]; +// +// xyz2enu(pos, E); +// matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e); +//} +///* transform local vector to ecef coordinate ----------------------------------- +// * transform local tangental coordinate vector to ecef +// * args : double *pos I geodetic position {lat,lon} (rad) +// * double *e I vector in local tangental coordinate {e,n,u} +// * double *r O vector in ecef coordinate {x,y,z} +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void enu2ecef(const double *pos, const double *e, double *r) +//{ +// double E[9]; +// +// xyz2enu(pos, E); +// matmul("TN", 3, 1, 3, 1.0, E, e, 0.0, r); +//} +///* transform covariance to local tangental coordinate -------------------------- +// * transform ecef covariance to local tangental coordinate +// * args : double *pos I geodetic position {lat,lon} (rad) +// * double *P I covariance in ecef coordinate +// * double *Q O covariance in local tangental coordinate +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void covenu(const double *pos, const double *P, double *Q) +//{ +// double E[9], EP[9]; +// +// xyz2enu(pos, E); +// matmul("NN", 3, 3, 3, 1.0, E, P, 0.0, EP); +// matmul("NT", 3, 3, 3, 1.0, EP, E, 0.0, Q); +//} +///* transform local enu coordinate covariance to xyz-ecef ----------------------- +// * transform local enu covariance to xyz-ecef coordinate +// * args : double *pos I geodetic position {lat,lon} (rad) +// * double *Q I covariance in local enu coordinate +// * double *P O covariance in xyz-ecef coordinate +// * return : none +// *-----------------------------------------------------------------------------*/ +//extern void covecef(const double *pos, const double *Q, double *P) +//{ +// double E[9], EQ[9]; +// +// xyz2enu(pos, E); +// matmul("TN", 3, 3, 3, 1.0, E, Q, 0.0, EQ); +// matmul("NN", 3, 3, 3, 1.0, EQ, E, 0.0, P); +//} +///* coordinate rotation matrix ------------------------------------------------*/ +//#define Rx(t, X) \ +// do \ +// { \ +// (X)[0] = 1.0; \ +// (X)[1] = (X)[2] = (X)[3] = (X)[6] = 0.0; \ +// (X)[4] = (X)[8] = cos(t); \ +// (X)[7] = sin(t); \ +// (X)[5] = -(X)[7]; \ +// } while (0) +// +//#define Ry(t, X) \ +// do \ +// { \ +// (X)[4] = 1.0; \ +// (X)[1] = (X)[3] = (X)[5] = (X)[7] = 0.0; \ +// (X)[0] = (X)[8] = cos(t); \ +// (X)[2] = sin(t); \ +// (X)[6] = -(X)[2]; \ +// } while (0) +// +//#define Rz(t, X) \ +// do \ +// { \ +// (X)[8] = 1.0; \ +// (X)[2] = (X)[5] = (X)[6] = (X)[7] = 0.0; \ +// (X)[0] = (X)[4] = cos(t); \ +// (X)[3] = sin(t); \ +// (X)[1] = -(X)[3]; \ +// } while (0) +// +///* astronomical arguments: f={l,l',F,D,OMG} (rad) ----------------------------*/ +////static void ast_args(double t, double *f) +////{ +//// static const double fc[][5] = {/* coefficients for iau 1980 nutation */ +//// {134.96340251, 1717915923.2178, 31.8792, 0.051635, -0.00024470}, +//// {357.52910918, 129596581.0481, -0.5532, 0.000136, -0.00001149}, +//// {93.27209062, 1739527262.8478, -12.7512, -0.001037, 0.00000417}, +//// {297.85019547, 1602961601.2090, -6.3706, 0.006593, -0.00003169}, +//// {125.04455501, -6962890.2665, 7.4722, 0.007702, -0.00005939}}; +//// double tt[4]; +//// int i, j; +//// +//// for (tt[0] = t, i = 1; i < 4; i++) +//// tt[i] = tt[i - 1] * t; +//// for (i = 0; i < 5; i++) +//// { +//// f[i] = fc[i][0] * 3600.0; +//// for (j = 0; j < 4; j++) +//// f[i] += fc[i][j + 1] * tt[j]; +//// f[i] = fmod(f[i] * AS2R, 2.0 * PI); +//// } +////} +///* iau 1980 nutation ---------------------------------------------------------*/ +//// static void nut_iau1980(double t, const double *f, double *dpsi, double *deps) +//// { +//// static const double nut[106][10] = { +//// {0, 0, 0, 0, 1, -6798.4, -171996, -174.2, 92025, 8.9}, +//// {0, 0, 2, -2, 2, 182.6, -13187, -1.6, 5736, -3.1}, +//// {0, 0, 2, 0, 2, 13.7, -2274, -0.2, 977, -0.5}, +//// {0, 0, 0, 0, 2, -3399.2, 2062, 0.2, -895, 0.5}, +//// {0, -1, 0, 0, 0, -365.3, -1426, 3.4, 54, -0.1}, +//// {1, 0, 0, 0, 0, 27.6, 712, 0.1, -7, 0.0}, +//// {0, 1, 2, -2, 2, 121.7, -517, 1.2, 224, -0.6}, +//// {0, 0, 2, 0, 1, 13.6, -386, -0.4, 200, 0.0}, +//// {1, 0, 2, 0, 2, 9.1, -301, 0.0, 129, -0.1}, +//// {0, -1, 2, -2, 2, 365.2, 217, -0.5, -95, 0.3}, +//// {-1, 0, 0, 2, 0, 31.8, 158, 0.0, -1, 0.0}, +//// {0, 0, 2, -2, 1, 177.8, 129, 0.1, -70, 0.0}, +//// {-1, 0, 2, 0, 2, 27.1, 123, 0.0, -53, 0.0}, +//// {1, 0, 0, 0, 1, 27.7, 63, 0.1, -33, 0.0}, +//// {0, 0, 0, 2, 0, 14.8, 63, 0.0, -2, 0.0}, +//// {-1, 0, 2, 2, 2, 9.6, -59, 0.0, 26, 0.0}, +//// {-1, 0, 0, 0, 1, -27.4, -58, -0.1, 32, 0.0}, +//// {1, 0, 2, 0, 1, 9.1, -51, 0.0, 27, 0.0}, +//// {-2, 0, 0, 2, 0, -205.9, -48, 0.0, 1, 0.0}, +//// {-2, 0, 2, 0, 1, 1305.5, 46, 0.0, -24, 0.0}, +//// {0, 0, 2, 2, 2, 7.1, -38, 0.0, 16, 0.0}, +//// {2, 0, 2, 0, 2, 6.9, -31, 0.0, 13, 0.0}, +//// {2, 0, 0, 0, 0, 13.8, 29, 0.0, -1, 0.0}, +//// {1, 0, 2, -2, 2, 23.9, 29, 0.0, -12, 0.0}, +//// {0, 0, 2, 0, 0, 13.6, 26, 0.0, -1, 0.0}, +//// {0, 0, 2, -2, 0, 173.3, -22, 0.0, 0, 0.0}, +//// {-1, 0, 2, 0, 1, 27.0, 21, 0.0, -10, 0.0}, +//// {0, 2, 0, 0, 0, 182.6, 17, -0.1, 0, 0.0}, +//// {0, 2, 2, -2, 2, 91.3, -16, 0.1, 7, 0.0}, +//// {-1, 0, 0, 2, 1, 32.0, 16, 0.0, -8, 0.0}, +//// {0, 1, 0, 0, 1, 386.0, -15, 0.0, 9, 0.0}, +//// {1, 0, 0, -2, 1, -31.7, -13, 0.0, 7, 0.0}, +//// {0, -1, 0, 0, 1, -346.6, -12, 0.0, 6, 0.0}, +//// {2, 0, -2, 0, 0, -1095.2, 11, 0.0, 0, 0.0}, +//// {-1, 0, 2, 2, 1, 9.5, -10, 0.0, 5, 0.0}, +//// {1, 0, 2, 2, 2, 5.6, -8, 0.0, 3, 0.0}, +//// {0, -1, 2, 0, 2, 14.2, -7, 0.0, 3, 0.0}, +//// {0, 0, 2, 2, 1, 7.1, -7, 0.0, 3, 0.0}, +//// {1, 1, 0, -2, 0, -34.8, -7, 0.0, 0, 0.0}, +//// {0, 1, 2, 0, 2, 13.2, 7, 0.0, -3, 0.0}, +//// {-2, 0, 0, 2, 1, -199.8, -6, 0.0, 3, 0.0}, +//// {0, 0, 0, 2, 1, 14.8, -6, 0.0, 3, 0.0}, +//// {2, 0, 2, -2, 2, 12.8, 6, 0.0, -3, 0.0}, +//// {1, 0, 0, 2, 0, 9.6, 6, 0.0, 0, 0.0}, +//// {1, 0, 2, -2, 1, 23.9, 6, 0.0, -3, 0.0}, +//// {0, 0, 0, -2, 1, -14.7, -5, 0.0, 3, 0.0}, +//// {0, -1, 2, -2, 1, 346.6, -5, 0.0, 3, 0.0}, +//// {2, 0, 2, 0, 1, 6.9, -5, 0.0, 3, 0.0}, +//// {1, -1, 0, 0, 0, 29.8, 5, 0.0, 0, 0.0}, +//// {1, 0, 0, -1, 0, 411.8, -4, 0.0, 0, 0.0}, +//// {0, 0, 0, 1, 0, 29.5, -4, 0.0, 0, 0.0}, +//// {0, 1, 0, -2, 0, -15.4, -4, 0.0, 0, 0.0}, +//// {1, 0, -2, 0, 0, -26.9, 4, 0.0, 0, 0.0}, +//// {2, 0, 0, -2, 1, 212.3, 4, 0.0, -2, 0.0}, +//// {0, 1, 2, -2, 1, 119.6, 4, 0.0, -2, 0.0}, +//// {1, 1, 0, 0, 0, 25.6, -3, 0.0, 0, 0.0}, +//// {1, -1, 0, -1, 0, -3232.9, -3, 0.0, 0, 0.0}, +//// {-1, -1, 2, 2, 2, 9.8, -3, 0.0, 1, 0.0}, +//// {0, -1, 2, 2, 2, 7.2, -3, 0.0, 1, 0.0}, +//// {1, -1, 2, 0, 2, 9.4, -3, 0.0, 1, 0.0}, +//// {3, 0, 2, 0, 2, 5.5, -3, 0.0, 1, 0.0}, +//// {-2, 0, 2, 0, 2, 1615.7, -3, 0.0, 1, 0.0}, +//// {1, 0, 2, 0, 0, 9.1, 3, 0.0, 0, 0.0}, +//// {-1, 0, 2, 4, 2, 5.8, -2, 0.0, 1, 0.0}, +//// {1, 0, 0, 0, 2, 27.8, -2, 0.0, 1, 0.0}, +//// {-1, 0, 2, -2, 1, -32.6, -2, 0.0, 1, 0.0}, +//// {0, -2, 2, -2, 1, 6786.3, -2, 0.0, 1, 0.0}, +//// {-2, 0, 0, 0, 1, -13.7, -2, 0.0, 1, 0.0}, +//// {2, 0, 0, 0, 1, 13.8, 2, 0.0, -1, 0.0}, +//// {3, 0, 0, 0, 0, 9.2, 2, 0.0, 0, 0.0}, +//// {1, 1, 2, 0, 2, 8.9, 2, 0.0, -1, 0.0}, +//// {0, 0, 2, 1, 2, 9.3, 2, 0.0, -1, 0.0}, +//// {1, 0, 0, 2, 1, 9.6, -1, 0.0, 0, 0.0}, +//// {1, 0, 2, 2, 1, 5.6, -1, 0.0, 1, 0.0}, +//// {1, 1, 0, -2, 1, -34.7, -1, 0.0, 0, 0.0}, +//// {0, 1, 0, 2, 0, 14.2, -1, 0.0, 0, 0.0}, +//// {0, 1, 2, -2, 0, 117.5, -1, 0.0, 0, 0.0}, +//// {0, 1, -2, 2, 0, -329.8, -1, 0.0, 0, 0.0}, +//// {1, 0, -2, 2, 0, 23.8, -1, 0.0, 0, 0.0}, +//// {1, 0, -2, -2, 0, -9.5, -1, 0.0, 0, 0.0}, +//// {1, 0, 2, -2, 0, 32.8, -1, 0.0, 0, 0.0}, +//// {1, 0, 0, -4, 0, -10.1, -1, 0.0, 0, 0.0}, +//// {2, 0, 0, -4, 0, -15.9, -1, 0.0, 0, 0.0}, +//// {0, 0, 2, 4, 2, 4.8, -1, 0.0, 0, 0.0}, +//// {0, 0, 2, -1, 2, 25.4, -1, 0.0, 0, 0.0}, +//// {-2, 0, 2, 4, 2, 7.3, -1, 0.0, 1, 0.0}, +//// {2, 0, 2, 2, 2, 4.7, -1, 0.0, 0, 0.0}, +//// {0, -1, 2, 0, 1, 14.2, -1, 0.0, 0, 0.0}, +//// {0, 0, -2, 0, 1, -13.6, -1, 0.0, 0, 0.0}, +//// {0, 0, 4, -2, 2, 12.7, 1, 0.0, 0, 0.0}, +//// {0, 1, 0, 0, 2, 409.2, 1, 0.0, 0, 0.0}, +//// {1, 1, 2, -2, 2, 22.5, 1, 0.0, -1, 0.0}, +//// {3, 0, 2, -2, 2, 8.7, 1, 0.0, 0, 0.0}, +//// {-2, 0, 2, 2, 2, 14.6, 1, 0.0, -1, 0.0}, +//// {-1, 0, 0, 0, 2, -27.3, 1, 0.0, -1, 0.0}, +//// {0, 0, -2, 2, 1, -169.0, 1, 0.0, 0, 0.0}, +//// {0, 1, 2, 0, 1, 13.1, 1, 0.0, 0, 0.0}, +//// {-1, 0, 4, 0, 2, 9.1, 1, 0.0, 0, 0.0}, +//// {2, 1, 0, -2, 0, 131.7, 1, 0.0, 0, 0.0}, +//// {2, 0, 0, 2, 0, 7.1, 1, 0.0, 0, 0.0}, +//// {2, 0, 2, -2, 1, 12.8, 1, 0.0, -1, 0.0}, +//// {2, 0, -2, 0, 1, -943.2, 1, 0.0, 0, 0.0}, +//// {1, -1, 0, -2, 0, -29.3, 1, 0.0, 0, 0.0}, +//// {-1, 0, 0, 1, 1, -388.3, 1, 0.0, 0, 0.0}, +//// {-1, -1, 0, 2, 1, 35.0, 1, 0.0, 0, 0.0}, +//// {0, 1, 0, 1, 0, 27.3, 1, 0.0, 0, 0.0}}; +//// double ang; +//// int i, j; +// +//// *dpsi = *deps = 0.0; +// +//// for (i = 0; i < 106; i++) +//// { +//// ang = 0.0; +//// for (j = 0; j < 5; j++) +//// ang += nut[i][j] * f[j]; +//// *dpsi += (nut[i][6] + nut[i][7] * t) * sin(ang); +//// *deps += (nut[i][8] + nut[i][9] * t) * cos(ang); +//// } +//// *dpsi *= 1E-4 * AS2R; /* 0.1 mas -> rad */ +//// *deps *= 1E-4 * AS2R; +//// } +///* eci to ecef transformation matrix ------------------------------------------- +// * compute eci to ecef transformation matrix +// * args : gtime_t tutc I time in utc +// * double *erpv I erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) +// * double *U O eci to ecef transformation matrix (3 x 3) +// * double *gmst IO greenwich mean sidereal time (rad) +// * (NULL: no output) +// * return : none +// * note : see ref [3] chap 5 +// * not thread-safe +// *-----------------------------------------------------------------------------*/ +//// extern void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst) +//// { +//// const double ep2000[] = {2000, 1, 1, 12, 0, 0}; +//// static gtime_t tutc_; +//// static double U_[9], gmst_; +//// gtime_t tgps; +//// double eps, ze, th, z, t, t2, t3, dpsi, deps, gast, f[5]; +//// double R1[9], R2[9], R3[9], R[9], W[9], N[9], P[9], NP[9]; +//// int i; +// +//// trace(4, "eci2ecef: tutc=%s\n", time_str(tutc, 3)); +// +//// if (fabs(timediff(tutc, tutc_)) < 0.01) +//// { /* read cache */ +//// for (i = 0; i < 9; i++) +//// U[i] = U_[i]; +//// if (gmst) +//// *gmst = gmst_; +//// return; +//// } +//// tutc_ = tutc; +// +//// /* terrestrial time */ +//// tgps = utc2gpst(tutc_); +//// t = (timediff(tgps, epoch2time(ep2000)) + 19.0 + 32.184) / 86400.0 / 36525.0; +//// t2 = t * t; +//// t3 = t2 * t; +// +//// /* astronomical arguments */ +//// ast_args(t, f); +// +//// /* iau 1976 precession */ +//// ze = (2306.2181 * t + 0.30188 * t2 + 0.017998 * t3) * AS2R; +//// th = (2004.3109 * t - 0.42665 * t2 - 0.041833 * t3) * AS2R; +//// z = (2306.2181 * t + 1.09468 * t2 + 0.018203 * t3) * AS2R; +//// eps = (84381.448 - 46.8150 * t - 0.00059 * t2 + 0.001813 * t3) * AS2R; +//// Rz(-z, R1); +//// Ry(th, R2); +//// Rz(-ze, R3); +//// matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, R); +//// matmul("NN", 3, 3, 3, 1.0, R, R3, 0.0, P); /* P=Rz(-z)*Ry(th)*Rz(-ze) */ +// +//// /* iau 1980 nutation */ +//// nut_iau1980(t, f, &dpsi, &deps); +//// Rx(-eps - deps, R1); +//// Rz(-dpsi, R2); +//// Rx(eps, R3); +//// matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, R); +//// matmul("NN", 3, 3, 3, 1.0, R, R3, 0.0, N); /* N=Rx(-eps)*Rz(-dspi)*Rx(eps) */ +// +//// /* greenwich aparent sidereal time (rad) */ +//// gmst_ = utc2gmst(tutc_, erpv[2]); +//// gast = gmst_ + dpsi * cos(eps); +//// gast += (0.00264 * sin(f[4]) + 0.000063 * sin(2.0 * f[4])) * AS2R; +// +//// /* eci to ecef transformation matrix */ +//// Ry(-erpv[0], R1); +//// Rx(-erpv[1], R2); +//// Rz(gast, R3); +//// matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, W); +//// matmul("NN", 3, 3, 3, 1.0, W, R3, 0.0, R); /* W=Ry(-xp)*Rx(-yp) */ +//// matmul("NN", 3, 3, 3, 1.0, N, P, 0.0, NP); +//// matmul("NN", 3, 3, 3, 1.0, R, NP, 0.0, U_); /* U=W*Rz(gast)*N*P */ +// +//// for (i = 0; i < 9; i++) +//// U[i] = U_[i]; +//// if (gmst) +//// *gmst = gmst_; +// +//// trace(5, "gmst=%.12f gast=%.12f\n", gmst_, gast); +//// trace(5, "P=\n"); +//// tracemat(5, P, 3, 3, 15, 12); +//// trace(5, "N=\n"); +//// tracemat(5, N, 3, 3, 15, 12); +//// trace(5, "W=\n"); +//// tracemat(5, W, 3, 3, 15, 12); +//// trace(5, "U=\n"); +//// tracemat(5, U, 3, 3, 15, 12); +//// } +///* decode antenna parameter field --------------------------------------------*/ +//// static int decodef(char *p, int n, double *v) +//// { +//// int i; +// +//// for (i = 0; i < n; i++) +//// v[i] = 0.0; +//// for (i = 0, p = strtok(p, " "); p && i < n; p = strtok(NULL, " ")) +//// { +//// v[i++] = atof(p) * 1E-3; +//// } +//// return i; +//// } +///* add antenna parameter -----------------------------------------------------*/ +//// static void addpcv(const pcv_t *pcv, pcvs_t *pcvs) +////{ +//// pcv_t *pcvs_pcv; +//// +//// if (pcvs->nmax<=pcvs->n) { +//// pcvs->nmax+=256; +//// if (!(pcvs_pcv=(pcv_t *)realloc(pcvs->pcv,sizeof(pcv_t)*pcvs->nmax))) { +//// trace(1,"addpcv: memory allocation error\n"); +//// free(pcvs->pcv); pcvs->pcv=NULL; pcvs->n=pcvs->nmax=0; +//// return; +//// } +//// pcvs->pcv=pcvs_pcv; +//// } +//// pcvs->pcv[pcvs->n++]=*pcv; +//// } +///* read ngs antenna parameter file -------------------------------------------*/ +//// static int readngspcv(const char *file, pcvs_t *pcvs) +////{ +//// FILE *fp; +//// static const pcv_t pcv0={0}; +//// pcv_t pcv; +//// double neu[3]; +//// int n=0; +//// char buff[256]; +//// +//// if (!(fp=fopen(file,"r"))) { +//// trace(2,"ngs pcv file open error: %s\n",file); +//// return 0; +//// } +//// while (fgets(buff,sizeof(buff),fp)) { +//// +//// if (strlen(buff)>=62&&buff[61]=='|') continue; +//// +//// if (buff[0]!=' ') n=0; /* start line */ +//// if (++n==1) { +//// pcv=pcv0; +//// strncpy(pcv.type,buff,61); pcv.type[61]='\0'; +//// } +//// else if (n==2) { +//// if (decodef(buff,3,neu)<3) continue; +//// pcv.off[0][0]=neu[1]; +//// pcv.off[0][1]=neu[0]; +//// pcv.off[0][2]=neu[2]; +//// } +//// else if (n==3) decodef(buff,10,pcv.var[0]); +//// else if (n==4) decodef(buff,9,pcv.var[0]+10); +//// else if (n==5) { +//// if (decodef(buff,3,neu)<3) continue;; +//// pcv.off[1][0]=neu[1]; +//// pcv.off[1][1]=neu[0]; +//// pcv.off[1][2]=neu[2]; +//// } +//// else if (n==6) decodef(buff,10,pcv.var[1]); +//// else if (n==7) { +//// decodef(buff,9,pcv.var[1]+10); +//// addpcv(&pcv,pcvs); +//// } +//// } +//// fclose(fp); +//// +//// return 1; +//// } +///* read antex file ----------------------------------------------------------*/ +//// static int readantex(const char *file, pcvs_t *pcvs) +////{ +//// FILE *fp; +//// static const pcv_t pcv0={0}; +//// pcv_t pcv; +//// double neu[3]; +//// int i,f,freq=0,state=0,freqs[]={1,2,5,0}; +//// char buff[256]; +//// +//// trace(3,"readantex: file=%s\n",file); +//// +//// if (!(fp=fopen(file,"r"))) { +//// trace(2,"antex pcv file open error: %s\n",file); +//// return 0; +//// } +//// while (fgets(buff,sizeof(buff),fp)) { +//// +//// if (strlen(buff)<60||strstr(buff+60,"COMMENT")) continue; +//// +//// if (strstr(buff+60,"START OF ANTENNA")) { +//// pcv=pcv0; +//// state=1; +//// } +//// if (strstr(buff+60,"END OF ANTENNA")) { +//// addpcv(&pcv,pcvs); +//// state=0; +//// } +//// if (!state) continue; +//// +//// if (strstr(buff+60,"TYPE / SERIAL NO")) { +//// strncpy(pcv.type,buff ,20); pcv.type[20]='\0'; +//// strncpy(pcv.code,buff+20,20); pcv.code[20]='\0'; +//// if (!strncmp(pcv.code+3," ",8)) { +//// pcv.sat=satid2no(pcv.code); +//// } +//// } +//// else if (strstr(buff+60,"VALID FROM")) { +//// if (!str2time(buff,0,43,&pcv.ts)) continue; +//// } +//// else if (strstr(buff+60,"VALID UNTIL")) { +//// if (!str2time(buff,0,43,&pcv.te)) continue; +//// } +//// else if (strstr(buff+60,"START OF FREQUENCY")) { +//// if (!pcv.sat&&buff[3]!='G') continue; /* only read rec ant for GPS */ +//// if (sscanf(buff+4,"%d",&f)<1) continue; +//// for (i=0;freqs[i];i++) if (freqs[i]==f) break; +//// if (freqs[i]) freq=i+1; +//// /* for Galileo E5b: save to E2, not E7 */ +//// if (satsys(pcv.sat,NULL)==SYS_GAL&&f==7) freq=2; +//// } +//// else if (strstr(buff+60,"END OF FREQUENCY")) { +//// freq=0; +//// } +//// else if (strstr(buff+60,"NORTH / EAST / UP")) { +//// if (freq<1||NFREQn;i++) { +//// pcv=pcvs->pcv+i; +//// trace(4,"sat=%2d type=%20s code=%s off=%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", +//// pcv->sat,pcv->type,pcv->code,pcv->off[0][0],pcv->off[0][1], +//// pcv->off[0][2],pcv->off[1][0],pcv->off[1][1],pcv->off[1][2]); +//// } +//// return stat; +//// } +///* search antenna parameter ---------------------------------------------------- +// * read satellite antenna phase center position +// * args : int sat I satellite number (0: receiver antenna) +// * char *type I antenna type for receiver antenna +// * gtime_t time I time to search parameters +// * pcvs_t *pcvs IO antenna parameters +// * return : antenna parameter (NULL: no antenna) +// *-----------------------------------------------------------------------------*/ +//// extern pcv_t *searchpcv(int sat, const char *type, gtime_t time, +//// const pcvs_t *pcvs) +////{ +//// pcv_t *pcv; +//// char buff[MAXANT],*types[2],*p; +//// int i,j,n=0; +//// +//// trace(4,"searchpcv: sat=%2d type=%s\n",sat,type); +//// +//// if (sat) { /* search satellite antenna */ +//// for (i=0;in;i++) { +//// pcv=pcvs->pcv+i; +//// if (pcv->sat!=sat) continue; +//// if (pcv->ts.time!=0&&timediff(pcv->ts,time)>0.0) continue; +//// if (pcv->te.time!=0&&timediff(pcv->te,time)<0.0) continue; +//// return pcv; +//// } +//// } +//// else { +//// strcpy(buff,type); +//// for (p=strtok(buff," ");p&&n<2;p=strtok(NULL," ")) types[n++]=p; +//// if (n<=0) return NULL; +//// +//// /* search receiver antenna with radome at first */ +//// for (i=0;in;i++) { +//// pcv=pcvs->pcv+i; +//// for (j=0;jtype,types[j])) break; +//// if (j>=n) return pcv; +//// } +//// /* search receiver antenna without radome */ +//// for (i=0;in;i++) { +//// pcv=pcvs->pcv+i; +//// if (strstr(pcv->type,types[0])!=pcv->type) continue; +//// +//// trace(2,"pcv without radome is used type=%s\n",type); +//// return pcv; +//// } +//// } +//// return NULL; +//// } +///* read station positions ------------------------------------------------------ +// * read positions from station position file +// * args : char *file I station position file containing +// * lat(deg) lon(deg) height(m) name in a line +// * char *rcvs I station name +// * double *pos O station position {lat,lon,h} (rad/m) +// * (all 0 if search error) +// * return : none +// *-----------------------------------------------------------------------------*/ +//// extern void readpos(const char *file, const char *rcv, double *pos) +////{ +//// static double poss[2048][3]; +//// static char stas[2048][16]; +//// FILE *fp; +//// int i, j, len, np = 0; +//// char buff[256], str[256]; +// +//// trace(3, "readpos: file=%s\n", file); +// +//// if (!(fp = fopen(file, "r"))) +//// { +//// fprintf(stderr, "reference position file open error : %s\n", file); +//// return; +//// } +//// while (np < 2048 && fgets(buff, sizeof(buff), fp)) +//// { +//// if (buff[0] == '%' || buff[0] == '#') +//// continue; +//// if (sscanf(buff, "%lf %lf %lf %s", &poss[np][0], &poss[np][1], &poss[np][2], +//// str) < 4) +//// continue; +//// sprintf(stas[np++], "%.15s", str); +//// } +//// fclose(fp); +//// len = (int)strlen(rcv); +//// for (i = 0; i < np; i++) +//// { +//// if (strncmp(stas[i], rcv, len)) +//// continue; +//// for (j = 0; j < 3; j++) +//// pos[j] = poss[i][j]; +//// pos[0] *= D2R; +//// pos[1] *= D2R; +//// return; +//// } +//// pos[0] = pos[1] = pos[2] = 0.0; +////} +///* read blq record -----------------------------------------------------------*/ +//// static int readblqrecord(FILE *fp, double *odisp) +//// { +//// double v[11]; +//// char buff[256]; +//// int i, n = 0; +// +//// while (fgets(buff, sizeof(buff), fp)) +//// { +//// if (!strncmp(buff, "$$", 2)) +//// continue; +//// if (sscanf(buff, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", +//// v, v + 1, v + 2, v + 3, v + 4, v + 5, v + 6, v + 7, v + 8, v + 9, v + 10) < 11) +//// continue; +//// for (i = 0; i < 11; i++) +//// odisp[n + i * 6] = v[i]; +//// if (++n == 6) +//// return 1; +//// } +//// return 0; +//// } +///* read blq ocean tide loading parameters -------------------------------------- +// * read blq ocean tide loading parameters +// * args : char *file I BLQ ocean tide loading parameter file +// * char *sta I station name +// * double *odisp O ocean tide loading parameters +// * return : status (1:ok,0:file open error) +// *-----------------------------------------------------------------------------*/ +//// extern int readblq(const char *file, const char *sta, double *odisp) +//// { +//// FILE *fp; +//// char buff[256], staname[32] = "", name[32], *p; +// +//// /* station name to upper case */ +//// (void)sscanf(sta, "%16s", staname); +//// for (p = staname; (*p = (char)toupper((int)(*p))); p++) +//// ; +// +//// if (!(fp = fopen(file, "r"))) +//// { +//// trace(2, "blq file open error: file=%s\n", file); +//// return 0; +//// } +//// while (fgets(buff, sizeof(buff), fp)) +//// { +//// if (!strncmp(buff, "$$", 2) || strlen(buff) < 2) +//// continue; +// +//// if (sscanf(buff + 2, "%16s", name) < 1) +//// continue; +//// for (p = name; (*p = (char)toupper((int)(*p))); p++) +//// ; +//// if (strcmp(name, staname)) +//// continue; +// +//// /* read blq record */ +//// if (readblqrecord(fp, odisp)) +//// { +//// fclose(fp); +//// return 1; +//// } +//// } +//// fclose(fp); +//// trace(2, "no otl parameters: sta=%s file=%s\n", sta, file); +//// return 0; +//// } +///* read earth rotation parameters ---------------------------------------------- +// * read earth rotation parameters +// * args : char *file I IGS ERP file (IGS ERP ver.2) +// * erp_t *erp O earth rotation parameters +// * return : status (1:ok,0:file open error) +// *-----------------------------------------------------------------------------*/ +//// extern int readerp(const char *file, erp_t *erp) +//// { +//// FILE *fp; +//// erpd_t *erp_data; +//// double v[14] = {0}; +//// char buff[256]; +// +//// trace(3, "readerp: file=%s\n", file); +// +//// if (!(fp = fopen(file, "r"))) +//// { +//// trace(2, "erp file open error: file=%s\n", file); +//// return 0; +//// } +//// while (fgets(buff, sizeof(buff), fp)) +//// { +//// if (sscanf(buff, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", +//// v, v + 1, v + 2, v + 3, v + 4, v + 5, v + 6, v + 7, v + 8, v + 9, v + 10, v + 11, v + 12, v + 13) < 5) +//// { +//// continue; +//// } +//// if (erp->n >= erp->nmax) +//// { +//// erp->nmax = erp->nmax <= 0 ? 128 : erp->nmax * 2; +//// erp_data = (erpd_t *)realloc(erp->data, sizeof(erpd_t) * erp->nmax); +//// if (!erp_data) +//// { +//// free(erp->data); +//// erp->data = NULL; +//// erp->n = erp->nmax = 0; +//// fclose(fp); +//// return 0; +//// } +//// erp->data = erp_data; +//// } +//// erp->data[erp->n].mjd = v[0]; +//// erp->data[erp->n].xp = v[1] * 1E-6 * AS2R; +//// erp->data[erp->n].yp = v[2] * 1E-6 * AS2R; +//// erp->data[erp->n].ut1_utc = v[3] * 1E-7; +//// erp->data[erp->n].lod = v[4] * 1E-7; +//// erp->data[erp->n].xpr = v[12] * 1E-6 * AS2R; +//// erp->data[erp->n++].ypr = v[13] * 1E-6 * AS2R; +//// } +//// fclose(fp); +//// return 1; +//// } +///* get earth rotation parameter values ----------------------------------------- +//* get earth rotation parameter values +//* args : erp_t *erp I earth rotation parameters +//* gtime_t time I time (gpst) +//* double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) +//* return : status (1:ok,0:error) +//* ��ȡERP��������ת������ֵ +//* ����������3���� +//erp_t *erp I earth rotation parameters +//gtime_t time I time (gpst) +//double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) +//��������: +//int O (1:ok,0:error) +//*-----------------------------------------------------------------------------*/ +//// extern int geterp(const erp_t *erp, gtime_t time, double *erpv) +//// { +//// const double ep[] = {2000, 1, 1, 12, 0, 0}; +//// double mjd, day, a; +//// int i, j, k; +// +//// trace(4, "geterp:\n"); +// +//// if (erp->n <= 0) +//// return 0; +//// // 1�����㵱ǰʱ����ERP������ʱ��IJ�ֵ +//// mjd = 51544.5 + (timediff(gpst2utc(time), epoch2time(ep))) / 86400.0; +// +//// // 2������ǰʱ������ERP�����������ʱ�䣬���������ʱ�������� +//// if (mjd <= erp->data[0].mjd) +//// { +//// day = mjd - erp->data[0].mjd; +//// erpv[0] = erp->data[0].xp + erp->data[0].xpr * day; +//// erpv[1] = erp->data[0].yp + erp->data[0].ypr * day; +//// erpv[2] = erp->data[0].ut1_utc - erp->data[0].lod * day; +//// erpv[3] = erp->data[0].lod; +//// return 1; +//// } +//// // 3������ǰʱ������ERP�����������ʱ�䣬���������ʱ�������� +//// if (mjd >= erp->data[erp->n - 1].mjd) +//// { +//// day = mjd - erp->data[erp->n - 1].mjd; +//// erpv[0] = erp->data[erp->n - 1].xp + erp->data[erp->n - 1].xpr * day; +//// erpv[1] = erp->data[erp->n - 1].yp + erp->data[erp->n - 1].ypr * day; +//// erpv[2] = erp->data[erp->n - 1].ut1_utc - erp->data[erp->n - 1].lod * day; +//// erpv[3] = erp->data[erp->n - 1].lod; +//// return 1; +//// } +//// // 4������ǰʱ����ERP�����������������ʱ��֮�䣬�����ҵ���ӽ�������ʱ�䣬Ȼ���ò�ֵ +//// for (j = 0, k = erp->n - 1; j < k - 1;) +//// { +//// i = (j + k) / 2; +//// if (mjd < erp->data[i].mjd) +//// k = i; +//// else +//// j = i; +//// } +//// if (erp->data[j].mjd == erp->data[j + 1].mjd) +//// { +//// a = 0.5; +//// } +//// else +//// { +//// a = (mjd - erp->data[j].mjd) / (erp->data[j + 1].mjd - erp->data[j].mjd); +//// } +//// erpv[0] = (1.0 - a) * erp->data[j].xp + a * erp->data[j + 1].xp; +//// erpv[1] = (1.0 - a) * erp->data[j].yp + a * erp->data[j + 1].yp; +//// erpv[2] = (1.0 - a) * erp->data[j].ut1_utc + a * erp->data[j + 1].ut1_utc; +//// erpv[3] = (1.0 - a) * erp->data[j].lod + a * erp->data[j + 1].lod; +//// return 1; +//// } +///* compare ephemeris ---------------------------------------------------------*/ +//static int cmpeph(const void *p1, const void *p2) +//{ +// eph_t *q1 = (eph_t *)p1, *q2 = (eph_t *)p2; +// return q1->ttr.time != q2->ttr.time ? (int)(q1->ttr.time - q2->ttr.time) : (q1->toe.time != q2->toe.time ? (int)(q1->toe.time - q2->toe.time) : q1->sat - q2->sat); +//} +///* sort and unique ephemeris -------------------------------------------------*/ +//static void uniqeph(nav_t *nav) +//{ +// eph_t *nav_eph; +// int i, j; +// +// trace(3, "uniqeph: n=%d\n", nav->n); +// +// if (nav->n <= 0) +// return; +// +// qsort(nav->eph, nav->n, sizeof(eph_t), cmpeph); +// +// for (i = 1, j = 0; i < nav->n; i++) +// { +// if (nav->eph[i].sat != nav->eph[j].sat || +// nav->eph[i].iode != nav->eph[j].iode) +// { +// nav->eph[++j] = nav->eph[i]; +// } +// } +// nav->n = j + 1; +// +// if (!(nav_eph = (eph_t *)realloc(nav->eph, sizeof(eph_t) * nav->n))) +// { +// trace(1, "uniqeph malloc error n=%d\n", nav->n); +// free(nav->eph); +// nav->eph = NULL; +// nav->n = nav->nmax = 0; +// return; +// } +// nav->eph = nav_eph; +// nav->nmax = nav->n; +// +// trace(4, "uniqeph: n=%d\n", nav->n); +//} +///* compare glonass ephemeris -------------------------------------------------*/ +//static int cmpgeph(const void *p1, const void *p2) +//{ +// geph_t *q1 = (geph_t *)p1, *q2 = (geph_t *)p2; +// return q1->tof.time != q2->tof.time ? (int)(q1->tof.time - q2->tof.time) : (q1->toe.time != q2->toe.time ? (int)(q1->toe.time - q2->toe.time) : q1->sat - q2->sat); +//} +///* sort and unique glonass ephemeris -----------------------------------------*/ +//static void uniqgeph(nav_t *nav) +//{ +// geph_t *nav_geph; +// int i, j; +// +// trace(3, "uniqgeph: ng=%d\n", nav->ng); +// +// if (nav->ng <= 0) +// return; +// +// qsort(nav->geph, nav->ng, sizeof(geph_t), cmpgeph); +// +// for (i = j = 0; i < nav->ng; i++) +// { +// if (nav->geph[i].sat != nav->geph[j].sat || +// nav->geph[i].toe.time != nav->geph[j].toe.time || +// nav->geph[i].svh != nav->geph[j].svh) +// { +// nav->geph[++j] = nav->geph[i]; +// } +// } +// nav->ng = j + 1; +// +// if (!(nav_geph = (geph_t *)realloc(nav->geph, sizeof(geph_t) * nav->ng))) +// { +// trace(1, "uniqgeph malloc error ng=%d\n", nav->ng); +// free(nav->geph); +// nav->geph = NULL; +// nav->ng = nav->ngmax = 0; +// return; +// } +// nav->geph = nav_geph; +// nav->ngmax = nav->ng; +// +// trace(4, "uniqgeph: ng=%d\n", nav->ng); +//} +///* compare sbas ephemeris ----------------------------------------------------*/ +//// static int cmpseph(const void *p1, const void *p2) +////{ +//// seph_t *q1=(seph_t *)p1,*q2=(seph_t *)p2; +//// return q1->tof.time!=q2->tof.time?(int)(q1->tof.time-q2->tof.time): +//// (q1->t0.time!=q2->t0.time?(int)(q1->t0.time-q2->t0.time): +//// q1->sat-q2->sat); +//// } +///* sort and unique sbas ephemeris --------------------------------------------*/ +//// static void uniqseph(nav_t *nav) +////{ +//// seph_t *nav_seph; +//// int i,j; +//// +//// trace(3,"uniqseph: ns=%d\n",nav->ns); +//// +//// if (nav->ns<=0) return; +//// +//// qsort(nav->seph,nav->ns,sizeof(seph_t),cmpseph); +//// +//// for (i=j=0;ins;i++) { +//// if (nav->seph[i].sat!=nav->seph[j].sat|| +//// nav->seph[i].t0.time!=nav->seph[j].t0.time) { +//// nav->seph[++j]=nav->seph[i]; +//// } +//// } +//// nav->ns=j+1; +//// +//// if (!(nav_seph=(seph_t *)realloc(nav->seph,sizeof(seph_t)*nav->ns))) { +//// trace(1,"uniqseph malloc error ns=%d\n",nav->ns); +//// free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0; +//// return; +//// } +//// nav->seph=nav_seph; +//// nav->nsmax=nav->ns; +//// +//// trace(4,"uniqseph: ns=%d\n",nav->ns); +//// } +///* unique ephemerides ---------------------------------------------------------- +// * unique ephemerides in navigation data and update carrier wave length +// * args : nav_t *nav IO navigation data +// * return : number of epochs +// *-----------------------------------------------------------------------------*/ +//extern void uniqnav(nav_t *nav) +//{ +// trace(3, "uniqnav: neph=%d ngeph=%d nseph=%d\n", nav->n, nav->ng, nav->ns); +// +// /* unique ephemeris */ +// uniqeph(nav); +// uniqgeph(nav); +// // uniqseph(nav); +//} +///* compare observation data -------------------------------------------------*/ +//static int cmpobs(const void *p1, const void *p2) +//{ +// obsd_t *q1 = (obsd_t *)p1, *q2 = (obsd_t *)p2; +// double tt = timediff(q1->time, q2->time); +// if (fabs(tt) > DTTOL) +// return tt < 0 ? -1 : 1; +// if (q1->rcv != q2->rcv) +// return (int)q1->rcv - (int)q2->rcv; +// return (int)q1->sat - (int)q2->sat; +//} +///* sort and unique observation data -------------------------------------------- +// * sort and unique observation data by time, rcv, sat +// * args : obs_t *obs IO observation data +// * return : number of epochs +// *-----------------------------------------------------------------------------*/ +//extern int sortobs(obs_t *obs) +//{ +// int i, j, n; +// +// trace(3, "sortobs: nobs=%d\n", obs->n); +// +// if (obs->n <= 0) +// return 0; +// +// qsort(obs->data, obs->n, sizeof(obsd_t), cmpobs); +// +// /* delete duplicated data */ +// for (i = j = 0; i < obs->n; i++) +// { +// if (obs->data[i].sat != obs->data[j].sat || +// obs->data[i].rcv != obs->data[j].rcv || +// timediff(obs->data[i].time, obs->data[j].time) != 0.0) +// { +// obs->data[++j] = obs->data[i]; +// } +// } +// obs->n = j + 1; +// +// for (i = n = 0; i < obs->n; i = j, n++) +// { +// for (j = i + 1; j < obs->n; j++) +// { +// if (timediff(obs->data[j].time, obs->data[i].time) > DTTOL) +// break; +// } +// } +// return n; +//} +///* screen by time -------------------------------------------------------------- +// * screening by time start, time end, and time interval +// * args : gtime_t time I time +// * gtime_t ts I time start (ts.time==0:no screening by ts) +// * gtime_t te I time end (te.time==0:no screening by te) +// * double tint I time interval (s) (0.0:no screen by tint) +// * return : 1:on condition, 0:not on condition +// *-----------------------------------------------------------------------------*/ +//extern int screent(gtime_t time, gtime_t ts, gtime_t te, double tint) +//{ +// return (tint <= 0.0 || fmod(time2gpst(time, NULL) + DTTOL, tint) <= DTTOL * 2.0) && +// (ts.time == 0 || timediff(time, ts) >= -DTTOL) && +// (te.time == 0 || timediff(time, te) < DTTOL); +//} +///* read/save navigation data --------------------------------------------------- +// * save or load navigation data +// * args : char file I file path +// * nav_t nav O/I navigation data +// * return : status (1:ok,0:no file) +// *-----------------------------------------------------------------------------*/ +//// extern int readnav(const char *file, nav_t *nav) +//// { +//// FILE *fp; +//// eph_t eph0 = {0}; +//// geph_t geph0 = {0}; +//// char buff[4096], *p; +//// long toe_time, tof_time, toc_time, ttr_time; +//// int i, sat, prn; +// +//// trace(3, "loadnav: file=%s\n", file); +// +//// if (!(fp = fopen(file, "r"))) +//// return 0; +// +//// while (fgets(buff, sizeof(buff), fp)) +//// { +//// if (!strncmp(buff, "IONUTC", 6)) +//// { +//// for (i = 0; i < 8; i++) +//// nav->ion_gps[i] = 0.0; +//// for (i = 0; i < 8; i++) +//// nav->utc_gps[i] = 0.0; +//// (void)sscanf(buff, "IONUTC,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf", +//// &nav->ion_gps[0], &nav->ion_gps[1], &nav->ion_gps[2], &nav->ion_gps[3], +//// &nav->ion_gps[4], &nav->ion_gps[5], &nav->ion_gps[6], &nav->ion_gps[7], +//// &nav->utc_gps[0], &nav->utc_gps[1], &nav->utc_gps[2], &nav->utc_gps[3], +//// &nav->utc_gps[4]); +//// continue; +//// } +//// if ((p = strchr(buff, ','))) +//// *p = '\0'; +//// else +//// continue; +//// if (!(sat = satid2no(buff))) +//// continue; +//// if (satsys(sat, &prn) == SYS_GLO) +//// { +//// nav->geph[prn - 1] = geph0; +//// nav->geph[prn - 1].sat = sat; +//// toe_time = tof_time = 0; +//// (void)sscanf(p + 1, "%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," +//// "%lf,%lf,%lf,%lf", +//// &nav->geph[prn - 1].iode, &nav->geph[prn - 1].frq, &nav->geph[prn - 1].svh, +//// &nav->geph[prn - 1].sva, &nav->geph[prn - 1].age, +//// &toe_time, &tof_time, +//// &nav->geph[prn - 1].pos[0], &nav->geph[prn - 1].pos[1], &nav->geph[prn - 1].pos[2], +//// &nav->geph[prn - 1].vel[0], &nav->geph[prn - 1].vel[1], &nav->geph[prn - 1].vel[2], +//// &nav->geph[prn - 1].acc[0], &nav->geph[prn - 1].acc[1], &nav->geph[prn - 1].acc[2], +//// &nav->geph[prn - 1].taun, &nav->geph[prn - 1].gamn, &nav->geph[prn - 1].dtaun); +//// nav->geph[prn - 1].toe.time = toe_time; +//// nav->geph[prn - 1].tof.time = tof_time; +//// } +//// else +//// { +//// nav->eph[sat - 1] = eph0; +//// nav->eph[sat - 1].sat = sat; +//// toe_time = toc_time = ttr_time = 0; +//// (void)sscanf(p + 1, "%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," +//// "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d", +//// &nav->eph[sat - 1].iode, &nav->eph[sat - 1].iodc, &nav->eph[sat - 1].sva, +//// &nav->eph[sat - 1].svh, +//// &toe_time, &toc_time, &ttr_time, +//// &nav->eph[sat - 1].A, &nav->eph[sat - 1].e, &nav->eph[sat - 1].i0, +//// &nav->eph[sat - 1].OMG0, &nav->eph[sat - 1].omg, &nav->eph[sat - 1].M0, +//// &nav->eph[sat - 1].deln, &nav->eph[sat - 1].OMGd, &nav->eph[sat - 1].idot, +//// &nav->eph[sat - 1].crc, &nav->eph[sat - 1].crs, &nav->eph[sat - 1].cuc, +//// &nav->eph[sat - 1].cus, &nav->eph[sat - 1].cic, &nav->eph[sat - 1].cis, +//// &nav->eph[sat - 1].toes, &nav->eph[sat - 1].fit, &nav->eph[sat - 1].f0, +//// &nav->eph[sat - 1].f1, &nav->eph[sat - 1].f2, &nav->eph[sat - 1].tgd[0], +//// &nav->eph[sat - 1].code, &nav->eph[sat - 1].flag); +//// nav->eph[sat - 1].toe.time = toe_time; +//// nav->eph[sat - 1].toc.time = toc_time; +//// nav->eph[sat - 1].ttr.time = ttr_time; +//// } +//// } +//// fclose(fp); +//// return 1; +//// } +//// extern int savenav(const char *file, const nav_t *nav) +//// { +//// FILE *fp; +//// int i; +//// char id[32]; +// +//// trace(3, "savenav: file=%s\n", file); +// +//// if (!(fp = fopen(file, "w"))) +//// return 0; +// +//// for (i = 0; i < MAXSAT; i++) +//// { +//// if (nav->eph[i].ttr.time == 0) +//// continue; +//// satno2id(nav->eph[i].sat, id); +//// fprintf(fp, "%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," +//// "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," +//// "%.14E,%.14E,%.14E,%.14E,%.14E,%d,%d\n", +//// id, nav->eph[i].iode, nav->eph[i].iodc, nav->eph[i].sva, +//// nav->eph[i].svh, (int)nav->eph[i].toe.time, +//// (int)nav->eph[i].toc.time, (int)nav->eph[i].ttr.time, +//// nav->eph[i].A, nav->eph[i].e, nav->eph[i].i0, nav->eph[i].OMG0, +//// nav->eph[i].omg, nav->eph[i].M0, nav->eph[i].deln, nav->eph[i].OMGd, +//// nav->eph[i].idot, nav->eph[i].crc, nav->eph[i].crs, nav->eph[i].cuc, +//// nav->eph[i].cus, nav->eph[i].cic, nav->eph[i].cis, nav->eph[i].toes, +//// nav->eph[i].fit, nav->eph[i].f0, nav->eph[i].f1, nav->eph[i].f2, +//// nav->eph[i].tgd[0], nav->eph[i].code, nav->eph[i].flag); +//// } +//// for (i = 0; i < MAXPRNGLO; i++) +//// { +//// if (nav->geph[i].tof.time == 0) +//// continue; +//// satno2id(nav->geph[i].sat, id); +//// fprintf(fp, "%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," +//// "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E\n", +//// id, nav->geph[i].iode, nav->geph[i].frq, nav->geph[i].svh, +//// nav->geph[i].sva, nav->geph[i].age, (int)nav->geph[i].toe.time, +//// (int)nav->geph[i].tof.time, +//// nav->geph[i].pos[0], nav->geph[i].pos[1], nav->geph[i].pos[2], +//// nav->geph[i].vel[0], nav->geph[i].vel[1], nav->geph[i].vel[2], +//// nav->geph[i].acc[0], nav->geph[i].acc[1], nav->geph[i].acc[2], +//// nav->geph[i].taun, nav->geph[i].gamn, nav->geph[i].dtaun); +//// } +//// /*fprintf(fp,"IONUTC,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," +//// "%.14E,%.14E,%.14E,%.0f", +//// nav->ion_gps[0],nav->ion_gps[1],nav->ion_gps[2],nav->ion_gps[3], +//// nav->ion_gps[4],nav->ion_gps[5],nav->ion_gps[6],nav->ion_gps[7], +//// nav->utc_gps[0],nav->utc_gps[1],nav->utc_gps[2],nav->utc_gps[3], +//// nav->utc_gps[4]);*/ +// +//// fclose(fp); +//// return 1; +//// } +///* free observation data ------------------------------------------------------- +// * free memory for observation data +// * args : obs_t *obs IO observation data +// * return : none +// *-----------------------------------------------------------------------------*/ +//// extern void freeobs(obs_t *obs) +//// { +//// free(obs->data); +//// obs->data = NULL; +//// obs->n = obs->nmax = 0; +//// } +///* free navigation data --------------------------------------------------------- +// * free memory for navigation data +// * args : nav_t *nav IO navigation data +// * int opt I option (or of followings) +// * (0x01: gps/qzs ephmeris, 0x02: glonass ephemeris, +// * 0x04: sbas ephemeris, 0x08: precise ephemeris, +// * 0x10: precise clock 0x20: almanac, +// * 0x40: tec data) +// * return : none +// *-----------------------------------------------------------------------------*/ +//// extern void freenav(nav_t *nav, int opt) +//// { +//// if (opt & 0x01) +//// { +//// free(nav->eph); +//// nav->eph = NULL; +//// nav->n = nav->nmax = 0; +//// } +//// if (opt & 0x02) +//// { +//// free(nav->geph); +//// nav->geph = NULL; +//// nav->ng = nav->ngmax = 0; +//// } +//// // if (opt&0x04) {free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0;} +//// // if (opt&0x08) {free(nav->peph); nav->peph=NULL; nav->ne=nav->nemax=0;} +//// // if (opt&0x10) {free(nav->pclk); nav->pclk=NULL; nav->nc=nav->ncmax=0;} +//// // if (opt&0x20) {free(nav->alm ); nav->alm =NULL; nav->na=nav->namax=0;} +//// // if (opt&0x40) {free(nav->tec ); nav->tec =NULL; nav->nt=nav->ntmax=0;} +//// } +// +///* trace */ +////static char *trace_buff = NULL; /* trace file */ +////static char level_trace = 0; /* level of trace */ +////static uint32_t tick_trace = 0; /* tick time at traceopen (ms) */ +////static gtime_t time_trace = {0}; /* time at traceopen */ +///* +//extern void traceopen(const char *file) +//{ +// gtime_t time = utc2gpst(timeget()); +// if (trace_buff == NULL) +// if ((trace_buff = malloc(sizeof(char) * 1024)) != NULL) +// { +// tick_trace = tickget(); +// time_trace = time; +// } +//} +//extern void traceclose(void) +//{ +// free(trace_buff); +// trace_buff = NULL; +//} +//extern void tracelevel(int level) +//{ +// level_trace = level; +//} +//extern void trace(int level, const char *format, ...) +//{ +// va_list ap; +// int nb = 0; +// if (!trace_buff || level > level_trace) +// return; +// nb += sprintf(trace_buff, "%d ", level); +// va_start(ap, format); +// nb += vsprintf(trace_buff + nb, format, ap); +// va_end(ap); +// if (nb > 0) +// dbg_show_trace(trace_buff, nb); +//} +//extern void tracet(int level, const char *format, ...) +//{ +// va_list ap; +// int nb = 0; +// if (!trace_buff || level > level_trace) +// return; +// nb += sprintf(trace_buff, "%d %9.3f: ", level, (tickget() - tick_trace) / 1000.0); +// va_start(ap, format); +// nb += vsprintf(trace_buff + nb, format, ap); +// va_end(ap); +// if (nb > 0) +// dbg_show_trace(trace_buff, nb); +//} +//extern void tracemat(int level, const double *A, int n, int m, int p, int q) +//{ +// if (!trace_buff || level > level_trace) +// return; +//} +//extern void traceobs(int level, const obsd_t *obs, int n) +//{ +// +//} +//extern void tracenav(int level, const nav_t *nav) {} +//extern void tracegnav(int level, const nav_t *nav) {} +//extern void tracehnav(int level, const nav_t *nav) {} +//extern void tracepeph(int level, const nav_t *nav) {} +//extern void tracepclk(int level, const nav_t *nav) {} +//extern void traceb(int level, const uint8_t *p, int n) {} +//*/ +///* select iono-free linear combination (L1/L2 or L1/L5) ----------------------*/ +//extern int seliflc(int optnf, int sys) +//{ +// return((optnf == 2 || sys == SYS_GLO || sys == SYS_CMP) ? 1 : 2); +//} +///* geometric distance ---------------------------------------------------------- +// * compute geometric distance and receiver-to-satellite unit vector +// * args : double *rs I satellilte position (ecef at transmission) (m) +// * double *rr I receiver position (ecef at reception) (m) +// * double *e O line-of-sight vector (ecef) +// * return : geometric distance (m) (0>:error/no satellite position) +// * notes : distance includes sagnac effect correction +// *-----------------------------------------------------------------------------*/ +//extern double geodist(const double *rs, const double *rr, double *e) +//{ +// double r; +// int i; +// +// if (norm(rs, 3) < RE_WGS84) +// return -1.0; +// for (i = 0; i < 3; i++) +// e[i] = rs[i] - rr[i]; +// r = norm(e, 3); +// for (i = 0; i < 3; i++) +// e[i] /= r; +// return r + OMGE * (rs[0] * rr[1] - rs[1] * rr[0]) / CLIGHT; +//} +///* satellite azimuth/elevation angle ------------------------------------------- +// * compute satellite azimuth/elevation angle +// * args : double *pos I geodetic position {lat,lon,h} (rad,m) +// * double *e I receiver-to-satellilte unit vevtor (ecef) +// * double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output) +// * (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2) +// * return : elevation angle (rad) +// *-----------------------------------------------------------------------------*/ +//extern double satazel(const double *pos, const double *e, double *azel) +//{ +// double az = 0.0, el = PI / 2.0, enu[3]; +// +// if (pos[2] > -RE_WGS84) +// { +// ecef2enu(pos, e, enu); +// az = dot(enu, enu, 2) < 1E-12 ? 0.0 : atan2(enu[0], enu[1]); +// if (az < 0.0) +// az += 2 * PI; +// el = asin(enu[2]); +// } +// if (azel) +// { +// azel[0] = az; +// azel[1] = el; +// } +// return el; +//} +///* compute dops ---------------------------------------------------------------- +// * compute DOP (dilution of precision) +// * args : int ns I number of satellites +// * double *azel I satellite azimuth/elevation angle (rad) +// * double elmin I elevation cutoff angle (rad) +// * double *dop O DOPs {GDOP,PDOP,HDOP,VDOP} +// * return : none +// * notes : dop[0]-[3] return 0 in case of dop computation error +// *-----------------------------------------------------------------------------*/ +//#define SQRT(x) ((x) < 0.0 || (x) != (x) ? 0.0 : sqrt(x)) +// +//extern void dops(int ns, const double *azel, double elmin, double *dop) +//{ +// double H[4 * MAXSAT], Q[16], cosel, sinel; +// int i, n; +// +// for (i = 0; i < 4; i++) +// dop[i] = 0.0; +// for (i = n = 0; i < ns && i < MAXSAT; i++) +// { +// if (azel[1 + i * 2] < elmin || azel[1 + i * 2] <= 0.0) +// continue; +// cosel = cos(azel[1 + i * 2]); +// sinel = sin(azel[1 + i * 2]); +// H[4 * n] = cosel * sin(azel[i * 2]); +// H[1 + 4 * n] = cosel * cos(azel[i * 2]); +// H[2 + 4 * n] = sinel; +// H[3 + 4 * n++] = 1.0; +// } +// if (n < 4) +// return; +// +// matmul("NT", 4, 4, n, 1.0, H, H, 0.0, Q); +// if (!matinv(Q, 4)) +// { +// dop[0] = SQRT(Q[0] + Q[5] + Q[10] + Q[15]); /* GDOP */ +// dop[1] = SQRT(Q[0] + Q[5] + Q[10]); /* PDOP */ +// dop[2] = SQRT(Q[0] + Q[5]); /* HDOP */ +// dop[3] = SQRT(Q[10]); /* VDOP */ +// } +//} +///* ionosphere model ------------------------------------------------------------ +// * compute ionospheric delay by broadcast ionosphere model (klobuchar model) +// * args : gtime_t t I time (gpst) +// * double *ion I iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} +// * double *pos I receiver position {lat,lon,h} (rad,m) +// * double *azel I azimuth/elevation angle {az,el} (rad) +// * return : ionospheric delay (L1) (m) +// *-----------------------------------------------------------------------------*/ +//extern double ionmodel(gtime_t t, const double *ion, const double *pos, +// const double *azel) +//{ +// const double ion_default[] = {/* 2004/1/1 */ +// 0.1118E-07, -0.7451E-08, -0.5961E-07, 0.1192E-06, +// 0.1167E+06, -0.2294E+06, -0.1311E+06, 0.1049E+07}; +// double tt, f, psi, phi, lam, amp, per, x; +// int week; +// +// if (pos[2] < -1E3 || azel[1] <= 0) +// return 0.0; +// if (norm(ion, 8) <= 0.0) +// ion = ion_default; +// +// /* earth centered angle (semi-circle) */ +// psi = 0.0137 / (azel[1] / PI + 0.11) - 0.022; +// +// /* subionospheric latitude/longitude (semi-circle) */ +// phi = pos[0] / PI + psi * cos(azel[0]); +// if (phi > 0.416) +// phi = 0.416; +// else if (phi < -0.416) +// phi = -0.416; +// lam = pos[1] / PI + psi * sin(azel[0]) / cos(phi * PI); +// +// /* geomagnetic latitude (semi-circle) */ +// phi += 0.064 * cos((lam - 1.617) * PI); +// +// /* local time (s) */ +// tt = 43200.0 * lam + time2gpst(t, &week); +// tt -= floor(tt / 86400.0) * 86400.0; /* 0<=tt<86400 */ +// +// /* slant factor */ +// f = 1.0 + 16.0 * pow(0.53 - azel[1] / PI, 3.0); +// +// /* ionospheric delay */ +// amp = ion[0] + phi * (ion[1] + phi * (ion[2] + phi * ion[3])); +// per = ion[4] + phi * (ion[5] + phi * (ion[6] + phi * ion[7])); +// amp = amp < 0.0 ? 0.0 : amp; +// per = per < 72000.0 ? 72000.0 : per; +// x = 2.0 * PI * (tt - 50400.0) / per; +// +// return CLIGHT * f * (fabs(x) < 1.57 ? 5E-9 + amp * (1.0 + x * x * (-0.5 + x * x / 24.0)) : 5E-9); +//} +///* ionosphere mapping function ------------------------------------------------- +// * compute ionospheric delay mapping function by single layer model +// * args : double *pos I receiver position {lat,lon,h} (rad,m) +// * double *azel I azimuth/elevation angle {az,el} (rad) +// * return : ionospheric mapping function +// *-----------------------------------------------------------------------------*/ +//extern double ionmapf(const double *pos, const double *azel) +//{ +// if (pos[2] >= HION) +// return 1.0; +// return 1.0 / cos(asin((RE_WGS84 + pos[2]) / (RE_WGS84 + HION) * sin(PI / 2.0 - azel[1]))); +//} +///* ionospheric pierce point position ------------------------------------------- +// * compute ionospheric pierce point (ipp) position and slant factor +// * args : double *pos I receiver position {lat,lon,h} (rad,m) +// * double *azel I azimuth/elevation angle {az,el} (rad) +// * double re I earth radius (km) +// * double hion I altitude of ionosphere (km) +// * double *posp O pierce point position {lat,lon,h} (rad,m) +// * return : slant factor +// * notes : see ref [2], only valid on the earth surface +// * fixing bug on ref [2] A.4.4.10.1 A-22,23 +// *-----------------------------------------------------------------------------*/ +//extern double ionppp(const double *pos, const double *azel, double re, +// double hion, double *posp) +//{ +// double cosaz, rp, ap, sinap, tanap; +// +// rp = re / (re + hion) * cos(azel[1]); +// ap = PI / 2.0 - azel[1] - asin(rp); +// sinap = sin(ap); +// tanap = tan(ap); +// cosaz = cos(azel[0]); +// posp[0] = asin(sin(pos[0]) * cos(ap) + cos(pos[0]) * sinap * cosaz); +// +// if ((pos[0] > 70.0 * D2R && tanap * cosaz > tan(PI / 2.0 - pos[0])) || +// (pos[0] < -70.0 * D2R && -tanap * cosaz > tan(PI / 2.0 + pos[0]))) +// { +// posp[1] = pos[1] + PI - asin(sinap * sin(azel[0]) / cos(posp[0])); +// } +// else +// { +// posp[1] = pos[1] + asin(sinap * sin(azel[0]) / cos(posp[0])); +// } +// return 1.0 / sqrt(1.0 - rp * rp); +//} +///* troposphere model ----------------------------------------------------------- +// * compute tropospheric delay by standard atmosphere and saastamoinen model +// * args : gtime_t time I time +// * double *pos I receiver position {lat,lon,h} (rad,m) +// * double *azel I azimuth/elevation angle {az,el} (rad) +// * double humi I relative humidity +// * return : tropospheric delay (m) +// *-----------------------------------------------------------------------------*/ +//extern double tropmodel(gtime_t time, const double *pos, const double *azel, +// double humi) +//{ +// const double temp0 = 15.0; /* temparature at sea level */ +// double hgt, pres, temp, e, z, trph, trpw; +// +// if (pos[2] < -100.0 || 1E4 < pos[2] || azel[1] <= 0) +// return 0.0; +// +// /* standard atmosphere */ +// hgt = pos[2] < 0.0 ? 0.0 : pos[2]; +// +// pres = 1013.25 * pow(1.0 - 2.2557E-5 * hgt, 5.2568); +// temp = temp0 - 6.5E-3 * hgt + 273.16; +// e = 6.108 * humi * exp((17.15 * temp - 4684.0) / (temp - 38.45)); +// +// /* saastamoninen model */ +// z = PI / 2.0 - azel[1]; +// trph = 0.0022768 * pres / (1.0 - 0.00266 * cos(2.0 * pos[0]) - 0.00028 * hgt / 1E3) / cos(z); +// trpw = 0.002277 * (1255.0 / temp + 0.05) * e / cos(z); +// return trph + trpw; +//} +//#ifndef IERS_MODEL +// +//static double interpc(const double coef[], double lat) +//{ +// int i = (int)(lat / 15.0); +// if (i < 1) +// return coef[0]; +// else if (i > 4) +// return coef[4]; +// return coef[i - 1] * (1.0 - lat / 15.0 + i) + coef[i] * (lat / 15.0 - i); +//} +//static double mapf(double el, double a, double b, double c) +//{ +// double sinel = sin(el); +// return (1.0 + a / (1.0 + b / (1.0 + c))) / (sinel + (a / (sinel + b / (sinel + c)))); +//} +//static double nmf(gtime_t time, const double pos[], const double azel[], +// double *mapfw) +//{ +// /* ref [5] table 3 */ +// /* hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75 */ +// const double coef[][5] = { +// {1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3}, +// {2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3}, +// {62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3}, +// +// {0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5}, +// {0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5}, +// {0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5}, +// +// {5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4}, +// {1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3}, +// {4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2}}; +// const double aht[] = {2.53E-5, 5.49E-3, 1.14E-3}; /* height correction */ +// +// double y, cosy, ah[3], aw[3], dm, el = azel[1], lat = pos[0] * R2D, hgt = pos[2]; +// int i; +// +// if (el <= 0.0) +// { +// if (mapfw) +// *mapfw = 0.0; +// return 0.0; +// } +// /* year from doy 28, added half a year for southern latitudes */ +// y = (time2doy(time) - 28.0) / 365.25 + (lat < 0.0 ? 0.5 : 0.0); +// +// cosy = cos(2.0 * PI * y); +// lat = fabs(lat); +// +// for (i = 0; i < 3; i++) +// { +// ah[i] = interpc(coef[i], lat) - interpc(coef[i + 3], lat) * cosy; +// aw[i] = interpc(coef[i + 6], lat); +// } +// /* ellipsoidal height is used instead of height above sea level */ +// dm = (1.0 / sin(el) - mapf(el, aht[0], aht[1], aht[2])) * hgt / 1E3; +// +// if (mapfw) +// *mapfw = mapf(el, aw[0], aw[1], aw[2]); +// +// return mapf(el, ah[0], ah[1], ah[2]) + dm; +//} +//#endif /* !IERS_MODEL */ +// +///* troposphere mapping function ------------------------------------------------ +// * compute tropospheric mapping function by NMF +// * args : gtime_t t I time +// * double *pos I receiver position {lat,lon,h} (rad,m) +// * double *azel I azimuth/elevation angle {az,el} (rad) +// * double *mapfw IO wet mapping function (NULL: not output) +// * return : dry mapping function +// * note : see ref [5] (NMF) and [9] (GMF) +// * original JGR paper of [5] has bugs in eq.(4) and (5). the corrected +// * paper is obtained from: +// * ftp://web.haystack.edu/pub/aen/nmf/NMF_JGR.pdf +// *-----------------------------------------------------------------------------*/ +//extern double tropmapf(gtime_t time, const double pos[], const double azel[], +// double *mapfw) +//{ +//#ifdef IERS_MODEL +// const double ep[] = {2000, 1, 1, 12, 0, 0}; +// double mjd, lat, lon, hgt, zd, gmfh, gmfw; +//#endif +// trace(4, "tropmapf: pos=%10.6f %11.6f %6.1f azel=%5.1f %4.1f\n", +// pos[0] * R2D, pos[1] * R2D, pos[2], azel[0] * R2D, azel[1] * R2D); +// +// if (pos[2] < -1000.0 || pos[2] > 20000.0) +// { +// if (mapfw) +// *mapfw = 0.0; +// return 0.0; +// } +//#ifdef IERS_MODEL +// mjd = 51544.5 + (timediff(time, epoch2time(ep))) / 86400.0; +// lat = pos[0]; +// lon = pos[1]; +// hgt = pos[2] - geoidh(pos); /* height in m (mean sea level) */ +// zd = PI / 2.0 - azel[1]; +// +// /* call GMF */ +// gmf_(&mjd, &lat, &lon, &hgt, &zd, &gmfh, &gmfw); +// +// if (mapfw) +// *mapfw = gmfw; +// return gmfh; +//#else +// return nmf(time, pos, azel, mapfw); /* NMF */ +//#endif +//} +///* interpolate antenna phase center variation --------------------------------*/ +////static double interpvar(double ang, const double *var) +////{ +//// double a = ang / 5.0; /* ang=0-90 */ +//// int i = (int)a; +//// if (i < 0) +//// return var[0]; +//// else if (i >= 18) +//// return var[18]; +//// return var[i] * (1.0 - a + i) + var[i + 1] * (a - i); +////} +///* receiver antenna model ------------------------------------------------------ +// * compute antenna offset by antenna phase center parameters +// * args : pcv_t *pcv I antenna phase center parameters +// * double *del I antenna delta {e,n,u} (m) +// * double *azel I azimuth/elevation for receiver {az,el} (rad) +// * int opt I option (0:only offset,1:offset+pcv) +// * double *dant O range offsets for each frequency (m) +// * return : none +// * notes : current version does not support azimuth dependent terms +// *-----------------------------------------------------------------------------*/ +//// extern void antmodel(const pcv_t *pcv, const double *del, const double *azel, +//// int opt, double *dant) +////{ +//// double e[3],off[3],cosel=cos(azel[1]); +//// int i,j; +//// +//// trace(4,"antmodel: azel=%6.1f %4.1f opt=%d\n",azel[0]*R2D,azel[1]*R2D,opt); +//// +//// e[0]=sin(azel[0])*cosel; +//// e[1]=cos(azel[0])*cosel; +//// e[2]=sin(azel[1]); +//// +//// for (i=0;ioff[i][j]+del[j]; +//// +//// dant[i]=-dot(off,e,3)+(opt?interpvar(90.0-azel[1]*R2D,pcv->var[i]):0.0); +//// } +//// trace(2,"antmodel: dant=%6.3f %6.3f\n",dant[0],dant[1]); +//// } +///* satellite antenna model ------------------------------------------------------ +// * compute satellite antenna phase center parameters +// * args : pcv_t *pcv I antenna phase center parameters +// * double nadir I nadir angle for satellite (rad) +// * double *dant O range offsets for each frequency (m) +// * return : none +// *-----------------------------------------------------------------------------*/ +//// extern void antmodel_s(const pcv_t *pcv, double nadir, double *dant) +////{ +//// int i; +//// +//// trace(4,"antmodel_s: nadir=%6.1f\n",nadir*R2D); +//// +//// for (i=0;ivar[i]); +//// } +//// trace(4,"antmodel_s: dant=%6.3f %6.3f\n",dant[0],dant[1]); +//// } +///* sun and moon position in eci (ref [4] 5.1.1, 5.2.1) -----------------------*/ +////static void sunmoonpos_eci(gtime_t tut, double *rsun, double *rmoon) +////{ +//// const double ep2000[] = {2000, 1, 1, 12, 0, 0}; +//// double t, f[5], eps, Ms, ls, rs, lm, pm, rm, sine, cose, sinp, cosp, sinl, cosl; +//// +//// trace(4, "sunmoonpos_eci: tut=%s\n", time_str(tut, 3)); +//// +//// t = timediff(tut, epoch2time(ep2000)) / 86400.0 / 36525.0; +//// +//// /* astronomical arguments */ +//// ast_args(t, f); +//// +//// /* obliquity of the ecliptic */ +//// eps = 23.439291 - 0.0130042 * t; +//// sine = sin(eps * D2R); +//// cose = cos(eps * D2R); +//// +//// /* sun position in eci */ +//// if (rsun) +//// { +//// Ms = 357.5277233 + 35999.05034 * t; +//// ls = 280.460 + 36000.770 * t + 1.914666471 * sin(Ms * D2R) + 0.019994643 * sin(2.0 * Ms * D2R); +//// rs = AU * (1.000140612 - 0.016708617 * cos(Ms * D2R) - 0.000139589 * cos(2.0 * Ms * D2R)); +//// sinl = sin(ls * D2R); +//// cosl = cos(ls * D2R); +//// rsun[0] = rs * cosl; +//// rsun[1] = rs * cose * sinl; +//// rsun[2] = rs * sine * sinl; +//// +//// trace(5, "rsun =%.3f %.3f %.3f\n", rsun[0], rsun[1], rsun[2]); +//// } +//// /* moon position in eci */ +//// if (rmoon) +//// { +//// lm = 218.32 + 481267.883 * t + 6.29 * sin(f[0]) - 1.27 * sin(f[0] - 2.0 * f[3]) + +//// 0.66 * sin(2.0 * f[3]) + 0.21 * sin(2.0 * f[0]) - 0.19 * sin(f[1]) - 0.11 * sin(2.0 * f[2]); +//// pm = 5.13 * sin(f[2]) + 0.28 * sin(f[0] + f[2]) - 0.28 * sin(f[2] - f[0]) - +//// 0.17 * sin(f[2] - 2.0 * f[3]); +//// rm = RE_WGS84 / sin((0.9508 + 0.0518 * cos(f[0]) + 0.0095 * cos(f[0] - 2.0 * f[3]) + +//// 0.0078 * cos(2.0 * f[3]) + 0.0028 * cos(2.0 * f[0])) * +//// D2R); +//// sinl = sin(lm * D2R); +//// cosl = cos(lm * D2R); +//// sinp = sin(pm * D2R); +//// cosp = cos(pm * D2R); +//// rmoon[0] = rm * cosp * cosl; +//// rmoon[1] = rm * (cose * cosp * sinl - sine * sinp); +//// rmoon[2] = rm * (sine * cosp * sinl + cose * sinp); +//// +//// trace(5, "rmoon=%.3f %.3f %.3f\n", rmoon[0], rmoon[1], rmoon[2]); +//// } +////} +///* sun and moon position ------------------------------------------------------- +// * get sun and moon position in ecef +// * args : gtime_t tut I time in ut1 +// * double *erpv I erp value {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) +// * double *rsun IO sun position in ecef (m) (NULL: not output) +// * double *rmoon IO moon position in ecef (m) (NULL: not output) +// * double *gmst O gmst (rad) +// * return : none +// *-----------------------------------------------------------------------------*/ +//// extern void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, +//// double *rmoon, double *gmst) +//// { +//// gtime_t tut; +//// double rs[3], rm[3], U[9], gmst_; +// +//// trace(4, "sunmoonpos: tutc=%s\n", time_str(tutc, 3)); +// +//// tut = timeadd(tutc, erpv[2]); /* utc -> ut1 */ +// +//// /* sun and moon position in eci */ +//// sunmoonpos_eci(tut, rsun ? rs : NULL, rmoon ? rm : NULL); +// +//// /* eci to ecef transformation matrix */ +//// eci2ecef(tutc, erpv, U, &gmst_); +// +//// /* sun and moon postion in ecef */ +//// if (rsun) +//// matmul("NN", 3, 1, 3, 1.0, U, rs, 0.0, rsun); +//// if (rmoon) +//// matmul("NN", 3, 1, 3, 1.0, U, rm, 0.0, rmoon); +//// if (gmst) +//// *gmst = gmst_; +//// } +///* dummy application functions for shared library ----------------------------*/ +//#ifdef WIN_DLL +//extern int showmsg(char *format, ...) +//{ +// return 0; +//} +//extern void settspan(gtime_t ts, gtime_t te) {} +//extern void settime(gtime_t time) {} +//#endif diff --git a/nmea/rtklib.h b/nmea/rtklib.h new file mode 100644 index 0000000..d75e472 --- /dev/null +++ b/nmea/rtklib.h @@ -0,0 +1,1746 @@ +#ifndef RTKLIB_H +#define RTKLIB_H +#include +#include +#include +#include +#include +#include +#include +#include + +/* 用户头文件 *********************/ +//#include "Gnss_uart.h" +//#include "alloc.h" + +/** 卫星系统 **********************/ +#define ENAGPS +#define ENACMP +// #define ENAGLO +// #define ENAGAL +// #define ENAQZS + +// #define L5_TO_L2 +// #define ONLY_2FREQ + +/** 定位选项 **********************/ +#define Using_NMAE_PNTPOS +#define PAR + +#define STATIC +#define EXPORT + + +/* Using personal mem management */ +//#define malloc(x) m_malloc(x) +//#define free(x) m_free(x) + +/* RTT_Steae */ +#define RTT_OBS 0 +#define RTT_SOL 2 +#define RTT_DBG 3 +#define RTT_SOLSTAT 4 + +/* constants -----------------------------------------------------------------*/ + +#define VER_RTKLIB "demo5" /* library version */ + +#define PATCH_LEVEL "b34e" /* patch level */ + +#define COPYRIGHT_RTKLIB \ + "Copyright (C) 2007-2020 T.Takasu\nAll rights reserved." +#define PI 3.1415926535897932 /* pi */ +#define D2R (PI / 180.0) /* deg to rad */ +#define R2D (180.0 / PI) /* rad to deg */ +#define CLIGHT 299792458.0 /* speed of light (m/s) */ +#define SC2RAD 3.1415926535898 /* semi-circle to radian (IS-GPS) */ +#define AU 149597870691.0 /* 1 AU (m) */ +#define AS2R (D2R / 3600.0) /* arc sec to radian */ + +#define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */ + +#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */ +#define FE_WGS84 (1.0 / 298.257223563) /* earth flattening (WGS84) */ + +#define HION 350000.0 /* ionosphere height (m) */ + +#define MAXFREQ 6 /* max NFREQ */ + +#define FREQL1 1.57542E9 /* L1/E1 frequency (Hz) */ +#define FREQL2 1.22760E9 /* L2 frequency (Hz) */ +#define FREQE5b 1.20714E9 /* E5b frequency (Hz) */ +#define FREQL5 1.17645E9 /* L5/E5a/B2a frequency (Hz) */ +#define FREQL6 1.27875E9 /* E6/L6 frequency (Hz) */ +#define FREQE5ab 1.191795E9 /* E5a+b frequency (Hz) */ +#define FREQs 2.492028E9 /* S frequency (Hz) */ +#define FREQ1_GLO 1.60200E9 /* GLONASS G1 base frequency (Hz) */ +#define DFRQ1_GLO 0.56250E6 /* GLONASS G1 bias frequency (Hz/n) */ +#define FREQ2_GLO 1.24600E9 /* GLONASS G2 base frequency (Hz) */ +#define DFRQ2_GLO 0.43750E6 /* GLONASS G2 bias frequency (Hz/n) */ +#define FREQ3_GLO 1.202025E9 /* GLONASS G3 frequency (Hz) */ +#define FREQ1a_GLO 1.600995E9 /* GLONASS G1a frequency (Hz) */ +#define FREQ2a_GLO 1.248060E9 /* GLONASS G2a frequency (Hz) */ +#define FREQ1_CMP 1.561098E9 /* BDS B1I frequency (Hz) */ +#define FREQ2_CMP 1.20714E9 /* BDS B2I/B2b frequency (Hz) */ +#define FREQ3_CMP 1.26852E9 /* BDS B3 frequency (Hz) */ + +#define EFACT_GPS 1.0 /* error factor: GPS */ +#define EFACT_GLO 1.5 /* error factor: GLONASS */ +#define EFACT_GAL 1.0 /* error factor: Galileo */ +#define EFACT_QZS 1.0 /* error factor: QZSS */ +#define EFACT_CMP 1.0 /* error factor: BeiDou */ +#define EFACT_IRN 1.5 /* error factor: IRNSS */ +#define EFACT_SBS 3.0 /* error factor: SBAS */ + +#define SYS_NONE 0x00 /* navigation system: none */ +#define SYS_GPS 0x01 /* navigation system: GPS */ +#define SYS_SBS 0x02 /* navigation system: SBAS */ +#define SYS_GLO 0x04 /* navigation system: GLONASS */ +#define SYS_GAL 0x08 /* navigation system: Galileo */ +#define SYS_QZS 0x10 /* navigation system: QZSS */ +#define SYS_CMP 0x20 /* navigation system: BeiDou */ +#define SYS_IRN 0x40 /* navigation system: IRNS */ +#define SYS_LEO 0x80 /* navigation system: LEO */ +#define SYS_ALL 0xFF /* navigation system: all */ + +#define TSYS_GPS 0 /* time system: GPS time */ +#define TSYS_UTC 1 /* time system: UTC */ +#define TSYS_GLO 2 /* time system: GLONASS time */ +#define TSYS_GAL 3 /* time system: Galileo time */ +#define TSYS_QZS 4 /* time system: QZSS time */ +#define TSYS_CMP 5 /* time system: BeiDou time */ +#define TSYS_IRN 6 /* time system: IRNSS time */ + +#ifndef NFREQ +#define NFREQ 1 /* number of carrier frequencies */ +#endif +#define NFREQGLO 2 /* number of carrier frequencies of GLONASS */ + +#ifndef NEXOBS +#define NEXOBS 0 /* number of extended obs codes */ +#endif + +#define SNR_UNIT 0.001 /* SNR unit (dBHz) */ + +#define MINPRNGPS 1 /* min satellite PRN number of GPS */ +#define MAXPRNGPS 32 /* max satellite PRN number of GPS */ +#define NSATGPS (MAXPRNGPS - MINPRNGPS + 1) /* number of GPS satellites */ +#define NSYSGPS 1 + +#ifdef ENAGLO +#define MINPRNGLO 1 /* min satellite slot number of GLONASS */ +#define MAXPRNGLO 27 /* max satellite slot number of GLONASS */ +#define NSATGLO (MAXPRNGLO - MINPRNGLO + 1) /* number of GLONASS satellites */ +#define NSYSGLO 1 +#else +#define MINPRNGLO 0 +#define MAXPRNGLO 0 +#define NSATGLO 0 +#define NSYSGLO 0 +#endif +#ifdef ENAGAL +#define MINPRNGAL 1 /* min satellite PRN number of Galileo */ +#define MAXPRNGAL 36 /* max satellite PRN number of Galileo */ +#define NSATGAL (MAXPRNGAL - MINPRNGAL + 1) /* number of Galileo satellites */ +#define NSYSGAL 1 +#else +#define MINPRNGAL 0 +#define MAXPRNGAL 0 +#define NSATGAL 0 +#define NSYSGAL 0 +#endif +#ifdef ENAQZS +#define MINPRNQZS 193 /* min satellite PRN number of QZSS */ +#define MAXPRNQZS 202 /* max satellite PRN number of QZSS */ +#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS L1S */ +#define MAXPRNQZS_S 191 /* max satellite PRN number of QZSS L1S */ +#define NSATQZS (MAXPRNQZS - MINPRNQZS + 1) /* number of QZSS satellites */ +#define NSYSQZS 1 +#else +#define MINPRNQZS 0 +#define MAXPRNQZS 0 +#define MINPRNQZS_S 0 +#define MAXPRNQZS_S 0 +#define NSATQZS 0 +#define NSYSQZS 0 +#endif +#ifdef ENACMP +#define MINPRNCMP 1 /* min satellite sat number of BeiDou */ +#define MAXPRNCMP 46 /* max satellite sat number of BeiDou */ +#define NSATCMP (MAXPRNCMP - MINPRNCMP + 1) /* number of BeiDou satellites */ +#define NSYSCMP 1 +#else +#define MINPRNCMP 0 +#define MAXPRNCMP 0 +#define NSATCMP 0 +#define NSYSCMP 0 +#endif +#ifdef ENAIRN +#define MINPRNIRN 1 /* min satellite sat number of IRNSS */ +#define MAXPRNIRN 14 /* max satellite sat number of IRNSS */ +#define NSATIRN (MAXPRNIRN - MINPRNIRN + 1) /* number of IRNSS satellites */ +#define NSYSIRN 1 +#else +#define MINPRNIRN 0 +#define MAXPRNIRN 0 +#define NSATIRN 0 +#define NSYSIRN 0 +#endif +#ifdef ENALEO +#define MINPRNLEO 1 /* min satellite sat number of LEO */ +#define MAXPRNLEO 10 /* max satellite sat number of LEO */ +#define NSATLEO (MAXPRNLEO - MINPRNLEO + 1) /* number of LEO satellites */ +#define NSYSLEO 1 +#else +#define MINPRNLEO 0 +#define MAXPRNLEO 0 +#define NSATLEO 0 +#define NSYSLEO 0 +#endif +#define NSYS (NSYSGPS + NSYSGLO + NSYSGAL + NSYSQZS + NSYSCMP + NSYSIRN + NSYSLEO) /* number of systems */ + +#define MINPRNSBS 120 /* min satellite PRN number of SBAS */ +#define MAXPRNSBS 158 /* max satellite PRN number of SBAS */ +//#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */ + +#define NSATSBS 0 +#define MAXSAT (NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATCMP + NSATIRN + NSATSBS + NSATLEO) +/* max satellite number (1 to MAXSAT) */ +#define MAXSTA 255 + +#ifndef MAXOBS +#define MAXOBS 40 /* max number of obs in an epoch 96*/ +#define MAXRTKOBS 20 /* max number of obs in an epoch 96*/ +#endif +#define MAXRCV 2 /* max receiver number (1 to MAXRCV) 64 */ +#define MAXOBSTYPE 64 /* max number of obs type in RINEX */ + +#define DTTOL 0.025 /* tolerance of time difference (s) */ +#define MAXDTOE 7200.0 /* max time difference to GPS Toe (s) */ +#define MAXDTOE_QZS 7200.0 /* max time difference to QZSS Toe (s) */ +#define MAXDTOE_GAL 14400.0 /* max time difference to Galileo Toe (s) */ +#define MAXDTOE_CMP 3600.0 /* max time difference to BeiDou Toe (s) */ +#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */ +#define MAXDTOE_IRN 7200.0 /* max time difference to IRNSS Toe (s) */ +#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */ +#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */ +#define MAXGDOP 300.0 /* max GDOP */ + +// #define INT_SWAP_TRAC 86400.0 /* swap interval of trace file (s) */ +// #define INT_SWAP_STAT 86400.0 /* swap interval of solution status file (s) */ + +#define MAXEXFILE 1024 /* max number of expanded files */ +#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ +#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ +#define MAXSBSURA 8 /* max URA of SBAS satellite */ +#define MAXBAND 10 /* max SBAS band of IGP */ +#define MAXNIGP 201 /* max number of IGP in SBAS band */ +#define MAXNGEO 4 /* max number of GEO satellites */ +#define MAXCOMMENT 100 /* max number of RINEX comments */ +#define MAXSTRPATH 1024 /* max length of stream path */ +#define MAXSTRMSG 1024 /* max length of stream message */ +#define MAXSTRRTK 8 /* max number of stream in RTK server */ +#define MAXSBSMSG 32 /* max number of SBAS msg in RTK server */ +#define MAXSOLMSG 255 /* max length of solution message 8191*/ +#define MAXRAWLEN 16384 /* max length of receiver raw message */ +#define MAXERRMSG 4096 /* max length of error/warning message */ +#define MAXANT 64 /* max length of station name/antenna type */ +#define MAXSOLBUF 1 /* max number of solution buffer 256*/ +#define MAXOBSBUF 1 /* max number of observation data buffer 128*/ +#define MAXNRPOS 16 /* max number of reference positions */ +#define MAXLEAPS 64 /* max number of leap seconds table */ +#define MAXGISLAYER 32 /* max number of GIS data layers */ +#define MAXRCVCMD 4096 /* max length of receiver commands */ + +#define RNX2VER 2.10 /* RINEX ver.2 default output version */ +#define RNX3VER 3.00 /* RINEX ver.3 default output version */ + +#define OBSTYPE_PR 0x01 /* observation type: pseudorange */ +#define OBSTYPE_CP 0x02 /* observation type: carrier-phase */ +#define OBSTYPE_DOP 0x04 /* observation type: doppler-freq */ +#define OBSTYPE_SNR 0x08 /* observation type: SNR */ +#define OBSTYPE_ALL 0xFF /* observation type: all */ + +#define FREQTYPE_L1 0x01 /* frequency type: L1/E1/B1 */ +#define FREQTYPE_L2 0x02 /* frequency type: L2/E5b/B2 */ +#define FREQTYPE_L3 0x04 /* frequency type: L5/E5a/L3 */ +#define FREQTYPE_L4 0x08 /* frequency type: L6/E6/B3 */ +#define FREQTYPE_L5 0x10 /* frequency type: E5ab */ +#define FREQTYPE_ALL 0xFF /* frequency type: all */ + +#define CODE_NONE 0 /* obs code: none or unknown */ +#define CODE_L1C 1 /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ +#define CODE_L1P 2 /* obs code: L1P,G1P,B1P (GPS,GLO,BDS) */ +#define CODE_L1W 3 /* obs code: L1 Z-track (GPS) */ +#define CODE_L1Y 4 /* obs code: L1Y (GPS) */ +#define CODE_L1M 5 /* obs code: L1M (GPS) */ +#define CODE_L1N 6 /* obs code: L1codeless,B1codeless (GPS,BDS) */ +#define CODE_L1S 7 /* obs code: L1C(D) (GPS,QZS) */ +#define CODE_L1L 8 /* obs code: L1C(P) (GPS,QZS) */ +#define CODE_L1E 9 /* (not used) */ +#define CODE_L1A 10 /* obs code: E1A,B1A (GAL,BDS) */ +#define CODE_L1B 11 /* obs code: E1B (GAL) */ +#define CODE_L1X 12 /* obs code: E1B+C,L1C(D+P),B1D+P (GAL,QZS,BDS) */ +#define CODE_L1Z 13 /* obs code: E1A+B+C,L1S (GAL,QZS) */ +#define CODE_L2C 14 /* obs code: L2C/A,G1C/A (GPS,GLO) */ +#define CODE_L2D 15 /* obs code: L2 L1C/A-(P2-P1) (GPS) */ +#define CODE_L2S 16 /* obs code: L2C(M) (GPS,QZS) */ +#define CODE_L2L 17 /* obs code: L2C(L) (GPS,QZS) */ +#define CODE_L2X 18 /* obs code: L2C(M+L),B1_2I+Q (GPS,QZS,BDS) */ +#define CODE_L2P 19 /* obs code: L2P,G2P (GPS,GLO) */ +#define CODE_L2W 20 /* obs code: L2 Z-track (GPS) */ +#define CODE_L2Y 21 /* obs code: L2Y (GPS) */ +#define CODE_L2M 22 /* obs code: L2M (GPS) */ +#define CODE_L2N 23 /* obs code: L2codeless (GPS) */ +#define CODE_L5I 24 /* obs code: L5I,E5aI (GPS,GAL,QZS,SBS) */ +#define CODE_L5Q 25 /* obs code: L5Q,E5aQ (GPS,GAL,QZS,SBS) */ +#define CODE_L5X 26 /* obs code: L5I+Q,E5aI+Q,L5B+C,B2aD+P (GPS,GAL,QZS,IRN,SBS,BDS) */ +#define CODE_L7I 27 /* obs code: E5bI,B2bI (GAL,BDS) */ +#define CODE_L7Q 28 /* obs code: E5bQ,B2bQ (GAL,BDS) */ +#define CODE_L7X 29 /* obs code: E5bI+Q,B2bI+Q (GAL,BDS) */ +#define CODE_L6A 30 /* obs code: E6A,B3A (GAL,BDS) */ +#define CODE_L6B 31 /* obs code: E6B (GAL) */ +#define CODE_L6C 32 /* obs code: E6C (GAL) */ +#define CODE_L6X 33 /* obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,BDS) */ +#define CODE_L6Z 34 /* obs code: E6A+B+C,L6D+E (GAL,QZS) */ +#define CODE_L6S 35 /* obs code: L6S (QZS) */ +#define CODE_L6L 36 /* obs code: L6L (QZS) */ +#define CODE_L8I 37 /* obs code: E5abI (GAL) */ +#define CODE_L8Q 38 /* obs code: E5abQ (GAL) */ +#define CODE_L8X 39 /* obs code: E5abI+Q,B2abD+P (GAL,BDS) */ +#define CODE_L2I 40 /* obs code: B1_2I (BDS) */ +#define CODE_L2Q 41 /* obs code: B1_2Q (BDS) */ +#define CODE_L6I 42 /* obs code: B3I (BDS) */ +#define CODE_L6Q 43 /* obs code: B3Q (BDS) */ +#define CODE_L3I 44 /* obs code: G3I (GLO) */ +#define CODE_L3Q 45 /* obs code: G3Q (GLO) */ +#define CODE_L3X 46 /* obs code: G3I+Q (GLO) */ +#define CODE_L1I 47 /* obs code: B1I (BDS) (obsolute) */ +#define CODE_L1Q 48 /* obs code: B1Q (BDS) (obsolute) */ +#define CODE_L5A 49 /* obs code: L5A SPS (IRN) */ +#define CODE_L5B 50 /* obs code: L5B RS(D) (IRN) */ +#define CODE_L5C 51 /* obs code: L5C RS(P) (IRN) */ +#define CODE_L9A 52 /* obs code: SA SPS (IRN) */ +#define CODE_L9B 53 /* obs code: SB RS(D) (IRN) */ +#define CODE_L9C 54 /* obs code: SC RS(P) (IRN) */ +#define CODE_L9X 55 /* obs code: SB+C (IRN) */ +#define CODE_L1D 56 /* obs code: B1D (BDS) */ +#define CODE_L5D 57 /* obs code: L5D(L5S),B2aD (QZS,BDS) */ +#define CODE_L5P 58 /* obs code: L5P(L5S),B2aP (QZS,BDS) */ +#define CODE_L5Z 59 /* obs code: L5D+P(L5S) (QZS) */ +#define CODE_L6E 60 /* obs code: L6E (QZS) */ +#define CODE_L7D 61 /* obs code: B2bD (BDS) */ +#define CODE_L7P 62 /* obs code: B2bP (BDS) */ +#define CODE_L7Z 63 /* obs code: B2bD+P (BDS) */ +#define CODE_L8D 64 /* obs code: B2abD (BDS) */ +#define CODE_L8P 65 /* obs code: B2abP (BDS) */ +#define CODE_L4A 66 /* obs code: G1aL1OCd (GLO) */ +#define CODE_L4B 67 /* obs code: G1aL1OCd (GLO) */ +#define CODE_L4X 68 /* obs code: G1al1OCd+p (GLO) */ +#define MAXCODE 68 /* max number of obs code */ + +#define PMODE_SINGLE 0 /* positioning mode: single */ +#define PMODE_DGPS 1 /* positioning mode: DGPS/DGNSS */ +#define PMODE_KINEMA 2 /* positioning mode: kinematic */ +#define PMODE_STATIC 3 /* positioning mode: static */ +#define PMODE_STATIC_START 4 /* positioning mode: static */ +#define PMODE_MOVEB 5 /* positioning mode: moving-base */ +#define PMODE_FIXED 6 /* positioning mode: fixed */ + +#define SOLF_LLH 0 /* solution format: lat/lon/height */ +#define SOLF_XYZ 1 /* solution format: x/y/z-ecef */ +#define SOLF_ENU 2 /* solution format: e/n/u-baseline */ +#define SOLF_NMEA 3 /* solution format: NMEA-183 */ +#define SOLF_STAT 4 /* solution format: solution status */ +#define SOLF_GSIF 5 /* solution format: GSI F1/F2 */ + +#define SOLQ_NONE 0 /* solution status: no solution */ +#define SOLQ_FIX 1 /* solution status: fix */ +#define SOLQ_FLOAT 2 /* solution status: float */ +#define SOLQ_SBAS 3 /* solution status: SBAS */ +#define SOLQ_DGPS 4 /* solution status: DGPS/DGNSS */ +#define SOLQ_SINGLE 5 /* solution status: single */ +#define SOLQ_PPP 6 /* solution status: PPP */ +#define SOLQ_DR 7 /* solution status: dead reconing */ +#define MAXSOLQ 7 /* max number of solution status */ + +#define TIMES_GPST 0 /* time system: gps time */ +#define TIMES_UTC 1 /* time system: utc */ +#define TIMES_JST 2 /* time system: jst */ + +#define IONOOPT_OFF 0 /* ionosphere option: correction off */ +#define IONOOPT_BRDC 1 /* ionosphere option: broadcast model */ +#define IONOOPT_SBAS 2 /* ionosphere option: SBAS model */ +#define IONOOPT_IFLC 3 /* ionosphere option: L1/L2 or L1/L5 iono-free LC */ +#define IONOOPT_EST 4 /* ionosphere option: estimation */ +#define IONOOPT_TEC 5 /* ionosphere option: IONEX TEC model */ +#define IONOOPT_QZS 6 /* ionosphere option: QZSS broadcast model */ +#define IONOOPT_STEC 8 /* ionosphere option: SLANT TEC model */ + +#define TROPOPT_OFF 0 /* troposphere option: correction off */ +#define TROPOPT_SAAS 1 /* troposphere option: Saastamoinen model */ +#define TROPOPT_SBAS 2 /* troposphere option: SBAS model */ +#define TROPOPT_EST 3 /* troposphere option: ZTD estimation */ +#define TROPOPT_ESTG 4 /* troposphere option: ZTD+grad estimation */ +#define TROPOPT_ZTD 5 /* troposphere option: ZTD correction */ + +#define EPHOPT_BRDC 0 /* ephemeris option: broadcast ephemeris */ +#define EPHOPT_PREC 1 /* ephemeris option: precise ephemeris */ +#define EPHOPT_SBAS 2 /* ephemeris option: broadcast + SBAS */ +#define EPHOPT_SSRAPC 3 /* ephemeris option: broadcast + SSR_APC */ +#define EPHOPT_SSRCOM 4 /* ephemeris option: broadcast + SSR_COM */ + +#define ARMODE_OFF 0 /* AR mode: off */ +#define ARMODE_CONT 1 /* AR mode: continuous */ +#define ARMODE_INST 2 /* AR mode: instantaneous */ +#define ARMODE_FIXHOLD 3 /* AR mode: fix and hold */ +#define ARMODE_WLNL 4 /* AR mode: wide lane/narrow lane */ +#define ARMODE_TCAR 5 /* AR mode: triple carrier ar */ + +#define GLO_ARMODE_OFF 0 /* GLO AR mode: off */ +#define GLO_ARMODE_ON 1 /* GLO AR mode: on */ +#define GLO_ARMODE_AUTOCAL 2 /* GLO AR mode: autocal */ +#define GLO_ARMODE_FIXHOLD 3 /* GLO AR mode: fix and hold */ + +#define SBSOPT_LCORR 1 /* SBAS option: long term correction */ +#define SBSOPT_FCORR 2 /* SBAS option: fast correction */ +#define SBSOPT_ICORR 4 /* SBAS option: ionosphere correction */ +#define SBSOPT_RANGE 8 /* SBAS option: ranging */ + +#define POSOPT_POS 0 /* pos option: LLH/XYZ */ +#define POSOPT_SINGLE 1 /* pos option: average of single pos */ +#define POSOPT_FILE 2 /* pos option: read from pos file */ +#define POSOPT_RINEX 3 /* pos option: rinex header pos */ +#define POSOPT_RTCM 4 /* pos option: rtcm/raw station pos */ + +// #define STR_NONE 0 /* stream type: none */ +// #define STR_SERIAL 1 /* stream type: serial */ +// #define STR_FILE 2 /* stream type: file */ +// #define STR_TCPSVR 3 /* stream type: TCP server */ +// #define STR_TCPCLI 4 /* stream type: TCP client */ +// #define STR_NTRIPSVR 5 /* stream type: NTRIP server */ +// #define STR_NTRIPCLI 6 /* stream type: NTRIP client */ +// #define STR_FTP 7 /* stream type: ftp */ +// #define STR_HTTP 8 /* stream type: http */ +// #define STR_NTRIPCAS 9 /* stream type: NTRIP caster */ +// #define STR_UDPSVR 10 /* stream type: UDP server */ +// #define STR_UDPCLI 11 /* stream type: UDP server */ +// #define STR_MEMBUF 12 /* stream type: memory buffer */ + +#define STRFMT_RTCM2 0 /* stream format: RTCM 2 */ +#define STRFMT_RTCM3 1 /* stream format: RTCM 3 */ +// #define STRFMT_OEM4 2 /* stream format: NovAtel OEMV/4 */ +// #define STRFMT_CNAV 3 /* stream format: ComNav */ +// #define STRFMT_UBX 4 /* stream format: u-blox LEA-*T */ +// #define STRFMT_SBP 5 /* stream format: Swift Navigation SBP */ +// #define STRFMT_CRES 6 /* stream format: Hemisphere */ +// #define STRFMT_STQ 7 /* stream format: SkyTraq S1315F */ +// #define STRFMT_JAVAD 8 /* stream format: JAVAD GRIL/GREIS */ +// #define STRFMT_NVS 9 /* stream format: NVS NVC08C */ +// #define STRFMT_BINEX 10 /* stream format: BINEX */ +// #define STRFMT_RT17 11 /* stream format: Trimble RT17 */ +// #define STRFMT_SEPT 12 /* stream format: Septentrio */ +// #define STRFMT_TERSUS 13 /* stream format: TERSUS */ +// #define STRFMT_RINEX 14 /* stream format: RINEX */ +// #define STRFMT_SP3 15 /* stream format: SP3 */ +// #define STRFMT_RNXCLK 16 /* stream format: RINEX CLK */ +// #define STRFMT_SBAS 17 /* stream format: SBAS messages */ +// #define STRFMT_NMEA 18 /* stream format: NMEA 0183 */ +// #define MAXRCVFMT 13 /* max number of receiver format */ + +// #define STR_MODE_R 0x1 /* stream mode: read */ +// #define STR_MODE_W 0x2 /* stream mode: write */ +// #define STR_MODE_RW 0x3 /* stream mode: read/write */ + +#define GEOID_EMBEDDED 0 /* geoid model: embedded geoid */ +// #define GEOID_EGM96_M150 1 /* geoid model: EGM96 15x15" */ +// #define GEOID_EGM2008_M25 2 /* geoid model: EGM2008 2.5x2.5" */ +// #define GEOID_EGM2008_M10 3 /* geoid model: EGM2008 1.0x1.0" */ +// #define GEOID_GSI2000_M15 4 /* geoid model: GSI geoid 2000 1.0x1.5" */ +// #define GEOID_RAF09 5 /* geoid model: IGN RAF09 for France 1.5"x2" */ + +#define COMMENTH "%" /* comment line indicator for solution */ +// #define MSG_DISCONN "$_DISCONNECT\r\n" /* disconnect message */ + +// #define DLOPT_FORCE 0x01 /* download option: force download existing */ +// #define DLOPT_KEEPCMP 0x02 /* download option: keep compressed file */ +// #define DLOPT_HOLDERR 0x04 /* download option: hold on error file */ +// #define DLOPT_HOLDLST 0x08 /* download option: hold on listing file */ + +#define LLI_SLIP 0x01 /* LLI: cycle-slip */ +#define LLI_HALFC 0x02 /* LLI: half-cycle not resovled */ +#define LLI_BOCTRK 0x04 /* LLI: boc tracking of mboc signal */ +#define LLI_HALFA 0x40 /* LLI: half-cycle added */ +#define LLI_HALFS 0x80 /* LLI: half-cycle subtracted */ + +#define LAMBDA_FAR 0 /* LAMBDA: FAR */ +#define LAMBDA_PAR 1 /* LAMBDA: PAR */ + +#define P2_5 0.03125 /* 2^-5 */ +#define P2_6 0.015625 /* 2^-6 */ +#define P2_11 4.882812500000000E-04 /* 2^-11 */ +#define P2_15 3.051757812500000E-05 /* 2^-15 */ +#define P2_17 7.629394531250000E-06 /* 2^-17 */ +#define P2_19 1.907348632812500E-06 /* 2^-19 */ +#define P2_20 9.536743164062500E-07 /* 2^-20 */ +#define P2_21 4.768371582031250E-07 /* 2^-21 */ +#define P2_23 1.192092895507810E-07 /* 2^-23 */ +#define P2_24 5.960464477539063E-08 /* 2^-24 */ +#define P2_27 7.450580596923828E-09 /* 2^-27 */ +#define P2_29 1.862645149230957E-09 /* 2^-29 */ +#define P2_30 9.313225746154785E-10 /* 2^-30 */ +#define P2_31 4.656612873077393E-10 /* 2^-31 */ +#define P2_32 2.328306436538696E-10 /* 2^-32 */ +#define P2_33 1.164153218269348E-10 /* 2^-33 */ +#define P2_35 2.910383045673370E-11 /* 2^-35 */ +#define P2_38 3.637978807091710E-12 /* 2^-38 */ +#define P2_39 1.818989403545856E-12 /* 2^-39 */ +#define P2_40 9.094947017729280E-13 /* 2^-40 */ +#define P2_43 1.136868377216160E-13 /* 2^-43 */ +#define P2_48 3.552713678800501E-15 /* 2^-48 */ +#define P2_50 8.881784197001252E-16 /* 2^-50 */ +#define P2_55 2.775557561562891E-17 /* 2^-55 */ + +//#define thread_t int +//#define lock_t TSfCriticalSection +//#define initlock(f) TSfCriticalSectionCreate(f) +//#define lock(f) TSfCriticalSectionEnter(f) +//#define unlock(f) TSfCriticalSectionLeave(f) +#define FILEPATHSEP '/' + +/* type definitions ----------------------------------------------------------*/ +typedef struct +{ /* time struct */ + time_t time; /* time (s) expressed by standard time_t */ + double sec; /* fraction of second under 1 s */ +} gtime_t; +typedef struct +{ + uint8_t ready; /*1:ready 0:not ready*/ + uint8_t lock; /*1:lock 0:unlock*/ + gtime_t time; /* time (GPST) */ + gtime_t utctime; +}IMU_mng; +typedef struct +{ /* observation data record */ + gtime_t time; /* receiver sampling time (GPST) */ + uint8_t sat, rcv; /* satellite/receiver number */ + uint16_t SNR[NFREQ + NEXOBS]; /* signal strength (0.001 dBHz) */ + uint8_t LLI[NFREQ + NEXOBS]; /* loss of lock indicator */ + uint8_t code[NFREQ + NEXOBS]; /* code indicator (CODE_???) */ + double L[NFREQ + NEXOBS]; /* observation data carrier-phase (cycle) */ + double P[NFREQ + NEXOBS]; /* observation data pseudorange (m) */ + float D[NFREQ + NEXOBS]; /* observation data doppler frequency (Hz) */ + int timevalid; /* time is valid (Valid GNSS fix) for time mark */ + // gtime_t eventime; /* time of event (GPST) */ + uint8_t Lstd[NFREQ + NEXOBS]; /* stdev of carrier phase (0.004 cycles) */ + uint8_t Pstd[NFREQ + NEXOBS]; /* stdev of pseudorange (0.01*2^(n+5) meters) */ + uint8_t freq; /* GLONASS frequency channel (0-13) */ +} obsd_t; +typedef struct +{ /* observation data */ + int n, nmax; /* number of obervation data/allocated */ + int flag; /* epoch flag (0:ok,1:power failure,>1:event flag) */ + int rcvcount; /* count of rcv event */ + int tmcount; /* time mark count */ + obsd_t *data; /* observation data records */ +} obs_t; +// typedef struct +// { /* earth rotation parameter data type */ +// double mjd; /* mjd (days) */ +// double xp, yp; /* pole offset (rad) */ +// double xpr, ypr; /* pole offset rate (rad/day) */ +// double ut1_utc; /* ut1-utc (s) */ +// double lod; /* length of day (s/day) */ +// } erpd_t; +// typedef struct +// { /* earth rotation parameter type */ +// int n, nmax; /* number and max number of data */ +// erpd_t *data; /* earth rotation parameter data */ +// } erp_t; +// typedef struct +// { /* antenna parameter type */ +// int sat; /* satellite number (0:receiver) */ +// char type[MAXANT]; /* antenna type */ +// char code[MAXANT]; /* serial number or satellite code */ +// gtime_t ts, te; /* valid time start and end */ +// double off[NFREQ][3]; /* phase center offset e/n/u or x/y/z (m) */ +// double var[NFREQ][19]; /* phase center variation (m) */ +// /* el=90,85,...,0 or nadir=0,1,2,3,... (deg) */ +// } pcv_t; +// typedef struct +// { /* antenna parameters type */ +// int n, nmax; /* number of data/allocated */ +// pcv_t *pcv; /* antenna parameters data */ +// } pcvs_t; +// typedef struct { /* almanac type */ +// int sat; /* satellite number */ +// int svh; /* sv health (0:ok) */ +// int svconf; /* as and sv config */ +// int week; /* GPS/QZS: gps week, GAL: galileo week */ +// gtime_t toa; /* Toa */ +// /* SV orbit parameters */ +// double A,e,i0,OMG0,omg,M0,OMGd; +// double toas; /* Toa (s) in week */ +// double f0,f1; /* SV clock parameters (af0,af1) */ +// } alm_t; +typedef struct +{ /* GPS/QZS/GAL broadcast ephemeris type */ + int sat; /* satellite number */ + int iode, iodc; /* IODE,IODC */ + int sva; /* SV accuracy (URA index) */ + int svh; /* SV health (0:ok) */ + int week; /* GPS/QZS: gps week, GAL: galileo week */ + int code; /* GPS/QZS: code on L2 */ + /* GAL: data source defined as rinex 3.03 */ + /* BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) */ + int flag; /* GPS/QZS: L2 P data flag */ + /* BDS: nav type (0:unknown,1:IGSO/MEO,2:GEO) */ + gtime_t toe, toc, ttr; /* Toe,Toc,T_trans */ + /* SV orbit parameters */ + /*toc????????????????????? time of clock + toe????????????????????? ??????????????? time of ephemeris*/ + double A, e, i0, OMG0, omg, M0, deln, OMGd, idot; + double crc, crs, cuc, cus, cic, cis; + double toes; /* Toe (s) in week */ + double fit; /* fit interval (h) */ + double f0, f1, f2; /* SV clock parameters (af0,af1,af2) */ + double tgd[6]; /* group delay parameters */ + /* GPS/QZS:tgd[0]=TGD */ + /* GAL:tgd[0]=BGD_E1E5a,tgd[1]=BGD_E1E5b */ + /* CMP:tgd[0]=TGD_B1I ,tgd[1]=TGD_B2I/B2b,tgd[2]=TGD_B1Cp */ + /* tgd[3]=TGD_B2ap,tgd[4]=ISC_B1Cd ,tgd[5]=ISC_B2ad */ + double Adot, ndot; /* Adot,ndot for CNAV */ +} eph_t; +typedef struct +{ /* GLONASS broadcast ephemeris type */ + int sat; /* satellite number */ + int iode; /* IODE (0-6 bit of tb field) */ + int frq; /* satellite frequency number */ + int svh, sva, age; /* satellite health, accuracy, age of operation */ + gtime_t toe; /* epoch of epherides (gpst) */ + gtime_t tof; /* message frame time (gpst) */ + double pos[3]; /* satellite position (ecef) (m) */ + double vel[3]; /* satellite velocity (ecef) (m/s) */ + double acc[3]; /* satellite acceleration (ecef) (m/s^2) */ + double taun, gamn; /* SV clock bias (s)/relative freq bias */ + double dtaun; /* delay between L1 and L2 (s) */ +} geph_t; +// typedef struct { /* precise ephemeris type */ +// gtime_t time; /* time (GPST) */ +// int index; /* ephemeris index for multiple files */ +// double pos[MAXSAT][4]; /* satellite position/clock (ecef) (m|s) */ +// float std[MAXSAT][4]; /* satellite position/clock std (m|s) */ +// double vel[MAXSAT][4]; /* satellite velocity/clk-rate (m/s|s/s) */ +// float vst[MAXSAT][4]; /* satellite velocity/clk-rate std (m/s|s/s) */ +// float cov[MAXSAT][3]; /* satellite position covariance (m^2) */ +// float vco[MAXSAT][3]; /* satellite velocity covariance (m^2) */ +// } peph_t; +// typedef struct { /* precise clock type */ +// gtime_t time; /* time (GPST) */ +// int index; /* clock index for multiple files */ +// double clk[MAXSAT][1]; /* satellite clock (s) */ +// float std[MAXSAT][1]; /* satellite clock std (s) */ +// } pclk_t; +// typedef struct { /* SBAS ephemeris type */ +// int sat; /* satellite number */ +// gtime_t t0; /* reference epoch time (GPST) */ +// gtime_t tof; /* time of message frame (GPST) */ +// int sva; /* SV accuracy (URA index) */ +// int svh; /* SV health (0:ok) */ +// double pos[3]; /* satellite position (m) (ecef) */ +// double vel[3]; /* satellite velocity (m/s) (ecef) */ +// double acc[3]; /* satellite acceleration (m/s^2) (ecef) */ +// double af0,af1; /* satellite clock-offset/drift (s,s/s) */ +// } seph_t; +// typedef struct { /* NORAL TLE data type */ +// char name [32]; /* common name */ +// char alias[32]; /* alias name */ +// char satno[16]; /* satellilte catalog number */ +// char satclass; /* classification */ +// char desig[16]; /* international designator */ +// gtime_t epoch; /* element set epoch (UTC) */ +// double ndot; /* 1st derivative of mean motion */ +// double nddot; /* 2st derivative of mean motion */ +// double bstar; /* B* drag term */ +// int etype; /* element set type */ +// int eleno; /* element number */ +// double inc; /* orbit inclination (deg) */ +// double OMG; /* right ascension of ascending node (deg) */ +// double ecc; /* eccentricity */ +// double omg; /* argument of perigee (deg) */ +// double M; /* mean anomaly (deg) */ +// double n; /* mean motion (rev/day) */ +// int rev; /* revolution number at epoch */ +// } tled_t; +// typedef struct { /* NORAD TLE (two line element) type */ +// int n,nmax; /* number/max number of two line element data */ +// tled_t *data; /* NORAD TLE data */ +// } tle_t; +// typedef struct { /* TEC grid type */ +// gtime_t time; /* epoch time (GPST) */ +// int ndata[3]; /* TEC grid data size {nlat,nlon,nhgt} */ +// double rb; /* earth radius (km) */ +// double lats[3]; /* latitude start/interval (deg) */ +// double lons[3]; /* longitude start/interval (deg) */ +// double hgts[3]; /* heights start/interval (km) */ +// double *data; /* TEC grid data (tecu) */ +// float *rms; /* RMS values (tecu) */ +// } tec_t; +// typedef struct { /* SBAS message type */ +// int week,tow; /* receiption time */ +// uint8_t prn,rcv; /* SBAS satellite PRN,receiver number */ +// uint8_t msg[29]; /* SBAS message (226bit) padded by 0 */ +// } sbsmsg_t; +// typedef struct { /* SBAS messages type */ +// int n,nmax; /* number of SBAS messages/allocated */ +// sbsmsg_t *msgs; /* SBAS messages */ +// } sbs_t; +// typedef struct { /* SBAS fast correction type */ +// gtime_t t0; /* time of applicability (TOF) */ +// double prc; /* pseudorange correction (PRC) (m) */ +// double rrc; /* range-rate correction (RRC) (m/s) */ +// double dt; /* range-rate correction delta-time (s) */ +// int iodf; /* IODF (issue of date fast corr) */ +// int16_t udre; /* UDRE+1 */ +// int16_t ai; /* degradation factor indicator */ +// } sbsfcorr_t; +// typedef struct { /* SBAS long term satellite error correction type */ +// gtime_t t0; /* correction time */ +// int iode; /* IODE (issue of date ephemeris) */ +// double dpos[3]; /* delta position (m) (ecef) */ +// double dvel[3]; /* delta velocity (m/s) (ecef) */ +// double daf0,daf1; /* delta clock-offset/drift (s,s/s) */ +// } sbslcorr_t; +// typedef struct { /* SBAS satellite correction type */ +// int sat; /* satellite number */ +// sbsfcorr_t fcorr; /* fast correction */ +// sbslcorr_t lcorr; /* long term correction */ +// } sbssatp_t; +// typedef struct { /* SBAS satellite corrections type */ +// int iodp; /* IODP (issue of date mask) */ +// int nsat; /* number of satellites */ +// int tlat; /* system latency (s) */ +// sbssatp_t sat[MAXSAT]; /* satellite correction */ +// } sbssat_t; +// typedef struct { /* SBAS ionospheric correction type */ +// gtime_t t0; /* correction time */ +// int16_t lat,lon; /* latitude/longitude (deg) */ +// int16_t give; /* GIVI+1 */ +// float delay; /* vertical delay estimate (m) */ +// } sbsigp_t; +// typedef struct { /* IGP band type */ +// int16_t x; /* longitude/latitude (deg) */ +// const int16_t *y; /* latitudes/longitudes (deg) */ +// uint8_t bits; /* IGP mask start bit */ +// uint8_t bite; /* IGP mask end bit */ +// } sbsigpband_t; +// typedef struct { /* SBAS ionospheric corrections type */ +// int iodi; /* IODI (issue of date ionos corr) */ +// int nigp; /* number of igps */ +// sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ +// } sbsion_t; +// typedef struct +// { /* DGPS/GNSS correction type */ +// gtime_t t0; /* correction time */ +// double prc; /* pseudorange correction (PRC) (m) */ +// double rrc; /* range rate correction (RRC) (m/s) */ +// int iod; /* issue of data (IOD) */ +// double udre; /* UDRE */ +// } dgps_t; +// typedef struct { /* SSR correction type */ +// gtime_t t0[6]; /* epoch time (GPST) {eph,clk,hrclk,ura,bias,pbias} */ +// double udi[6]; /* SSR update interval (s) */ +// int iod[6]; /* iod ssr {eph,clk,hrclk,ura,bias,pbias} */ +// int iode; /* issue of data */ +// int iodcrc; /* issue of data crc for beidou/sbas */ +// int ura; /* URA indicator */ +// int refd; /* sat ref datum (0:ITRF,1:regional) */ +// double deph [3]; /* delta orbit {radial,along,cross} (m) */ +// double ddeph[3]; /* dot delta orbit {radial,along,cross} (m/s) */ +// double dclk [3]; /* delta clock {c0,c1,c2} (m,m/s,m/s^2) */ +// double hrclk; /* high-rate clock corection (m) */ +// float cbias[MAXCODE]; /* code biases (m) */ +// double pbias[MAXCODE]; /* phase biases (m) */ +// float stdpb[MAXCODE]; /* std-dev of phase biases (m) */ +// double yaw_ang,yaw_rate; /* yaw angle and yaw rate (deg,deg/s) */ +// uint8_t update; /* update flag (0:no update,1:update) */ +// } ssr_t; +typedef struct +{ /* navigation data type */ + int n, nmax; /* number of broadcast ephemeris */ + int ng, ngmax; /* number of glonass ephemeris */ + int ns, nsmax; /* number of sbas ephemeris */ + int ne, nemax; /* number of precise ephemeris */ + int nc, ncmax; /* number of precise clock */ + int na, namax; /* number of almanac data */ + // int nt,ntmax; /* number of tec grid data */ + eph_t *eph; /* GPS/QZS/GAL/BDS/IRN ephemeris */ + geph_t *geph; /* GLONASS ephemeris */ + // seph_t *seph; /* SBAS ephemeris */ + // peph_t *peph; /* precise ephemeris */ + // pclk_t *pclk; /* precise clock */ + // alm_t *alm; /* almanac data */ + // tec_t *tec; /* tec grid data */ + // erp_t erp; /* earth rotation parameters */ + double utc_gps[8]; /* GPS delta-UTC parameters {A0,A1,Tot,WNt,dt_LS,WN_LSF,DN,dt_LSF} */ + // double utc_glo[8]; /* GLONASS UTC time parameters {tau_C,tau_GPS} */ + // double utc_gal[8]; /* Galileo UTC parameters */ + // double utc_qzs[8]; /* QZS UTC parameters */ + // double utc_cmp[8]; /* BeiDou UTC parameters */ + // double utc_irn[9]; /* IRNSS UTC parameters {A0,A1,Tot,...,dt_LSF,A2} */ + // double utc_sbs[4]; /* SBAS UTC parameters */ + double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + // double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */ + // double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + // double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + // double ion_irn[8]; /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + int glo_fcn[32]; /* GLONASS FCN + 8 */ + // double cbias[MAXSAT][3]; /* satellite DCB (0:P1-P2,1:P1-C1,2:P2-C2) (m) */ + // double rbias[MAXRCV][2][3]; /* receiver DCB (0:P1-P2,1:P1-C1,2:P2-C2) (m) */ + // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ + // sbssat_t sbssat; /* SBAS satellite corrections */ + // sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */ + // dgps_t dgps[MAXSAT]; /* DGPS corrections */ + // ssr_t ssr[MAXSAT]; /* SSR corrections */ +} nav_t; +typedef struct +{ /* station parameter type */ + char name[MAXANT]; /* marker name */ + char marker[MAXANT]; /* marker number */ + char antdes[MAXANT]; /* antenna descriptor */ + char antsno[MAXANT]; /* antenna serial number */ + // char rectype[MAXANT]; /* receiver type descriptor */ + // char recver[MAXANT]; /* receiver firmware version */ + // char recsno[MAXANT]; /* receiver serial number */ + int antsetup; /* antenna setup id */ + int itrf; /* ITRF realization year */ + int deltype; /* antenna delta type (0:enu,1:xyz) */ + double pos[3]; /* station position (ecef) (m) */ + double del[3]; /* antenna position delta (e/n/u or x/y/z) (m) */ + double hgt; /* antenna height (m) */ + int glo_cp_align; /* GLONASS code-phase alignment (0:no,1:yes) */ + double glo_cp_bias[4]; /* GLONASS code-phase biases {1C,1P,2C,2P} (m) */ +} sta_t; +typedef struct +{ /* solution type */ + gtime_t time; /* time (GPST) */ + gtime_t eventime; /* time of event (GPST) */ + gtime_t utctime; + double rr[6]; /* position/velocity (m|m/s) */ + /* {x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ + float qr[6]; /* position variance/covariance (m^2) */ + /* {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} or */ + /* {c_ee,c_nn,c_uu,c_en,c_nu,c_ue} */ + float qv[6]; /* velocity variance/covariance (m^2/s^2) */ + double dtr[6]; /* receiver clock bias to time systems (s) */ + uint8_t type; /* type (0:xyz-ecef,1:enu-baseline) */ + uint8_t stat; /* solution status (SOLQ_???) */ + uint8_t ns; /* number of valid satellites */ + float age; /* age of differential (s) */ + float ratio; /* AR ratio factor for valiation */ + float prev_ratio1; /* previous initial AR ratio factor for validation */ + float prev_ratio2; /* previous final AR ratio factor for validation */ + float thres; /* AR ratio threshold for valiation */ +} sol_t; +// typedef struct +// { /* solution buffer type */ +// int n, nmax; /* number of solution/max number of buffer */ +// int cyclic; /* cyclic buffer flag */ +// int start, end; /* start/end index */ +// gtime_t time; /* current solution time */ +// double rb[3]; /* reference position {x,y,z} (ecef) (m) */ +// uint8_t buff[MAXSOLMSG + 1]; /* message buffer */ +// int nb; /* number of byte in message buffer */ +// } solbuf_t; +// typedef struct +// { /* solution status type */ +// gtime_t time; /* time (GPST) */ +// uint8_t sat; /* satellite number */ +// uint8_t frq; /* frequency (1:L1,2:L2,...) */ +// float az, el; /* azimuth/elevation angle (rad) */ +// float resp; /* pseudorange residual (m) */ +// float resc; /* carrier-phase residual (m) */ +// uint8_t flag; /* flags: (vsat<<5)+(slip<<3)+fix */ +// uint16_t snr; /* signal strength (*SNR_UNIT dBHz) */ +// uint16_t lock; /* lock counter */ +// uint16_t outc; /* outage counter */ +// uint16_t slipc; /* slip counter */ +// uint16_t rejc; /* reject counter */ +// } solstat_t; +// typedef struct +// { /* solution status buffer type */ +// int n, nmax; /* number of solution/max number of buffer */ +// solstat_t *data; /* solution status data */ +// } solstatbuf_t; +typedef struct +{ /* RTCM control struct type */ + int staid; /* station id */ + int stah; /* station health */ + int seqno; /* sequence number for rtcm 2 or iods msm */ + // int outtype; /* output message type */ + gtime_t time; /* message time */ + gtime_t time_s; /* message start time */ + obs_t obs; /* observation data (uncorrected) */ + nav_t nav; /* satellite ephemerides */ + sta_t sta; /* station parameters */ + // dgps_t *dgps; /* output of dgps corrections */ + // ssr_t ssr[MAXSAT]; /* output of ssr corrections */ + // char msg[128]; /* special message */ + // char msgtype[256]; /* last message type */ + // char msmtype[7][128]; /* msm signal types */ + int obsflag; /* obs data complete flag (1:ok,0:not complete) */ + int ephsat; /* input ephemeris satellite number */ + int ephset; /* input ephemeris set (0-1) */ + double cp[MAXSAT][NFREQ + NEXOBS]; /* carrier-phase measurement */ + uint16_t lock[MAXSAT][NFREQ + NEXOBS]; /* lock time */ + uint16_t loss[MAXSAT][NFREQ + NEXOBS]; /* loss of lock count */ + // gtime_t lltime[MAXSAT][NFREQ + NEXOBS]; /* last lock time */ + int nbyte; /* number of bytes in message buffer */ + int nbit; /* number of bits in word buffer */ + int len; /* message length (bytes) */ + uint8_t buff[1200]; /* message buffer */ + // uint32_t word; /* word buffer for rtcm 2 */ + // uint32_t nmsg2[100]; /* message count of RTCM 2 (1-99:1-99,0:other) */ + // uint32_t nmsg3[400]; /* message count of RTCM 3 (1-299:1001-1299,300-329:4070-4099,0:ohter) */ + // char opt[256]; /* RTCM dependent options */ +} rtcm_t; +// typedef struct { /* RINEX control struct type */ +// gtime_t time; /* message time */ +// double ver; /* RINEX version */ +// char type; /* RINEX file type ('O','N',...) */ +// int sys; /* navigation system */ +// int tsys; /* time system */ +// char tobs[8][MAXOBSTYPE][4]; /* rinex obs types */ +// obs_t obs; /* observation data */ +// nav_t nav; /* navigation data */ +// sta_t sta; /* station info */ +// int ephsat; /* input ephemeris satellite number */ +// int ephset; /* input ephemeris set (0-1) */ +// char opt[256]; /* rinex dependent options */ +// } rnxctr_t; +// typedef struct { /* download URL type */ +// char type[32]; /* data type */ +// char path[1024]; /* URL path */ +// char dir [1024]; /* local directory */ +// double tint; /* time interval (s) */ +// } url_t; +typedef struct +{ /* option type */ + const char *name; /* option name */ + int format; /* option format (0:int,1:double,2:string,3:enum) */ + void *var; /* pointer to option variable */ + const char *comment; /* option comment/enum labels/unit */ +} opt_t; +typedef struct +{ /* SNR mask type */ + int ena[2]; /* enable flag {rover,base} */ + double mask[NFREQ][9]; /* mask (dBHz) at 5,10,...85 deg */ +} snrmask_t; +typedef struct +{ /* processing options type */ + int mode; /* positioning mode (PMODE_???) */ + int soltype; /* solution type (0:forward,1:backward,2:combined) */ + int nf; /* number of frequencies (1:L1,2:L1+L2,3:L1+L2+L5) */ + int navsys; /* navigation system */ + double elmin; /* elevation mask angle (rad) */ + snrmask_t snrmask; /* SNR mask */ + int sateph; /* satellite ephemeris/clock (EPHOPT_???) */ + int modear; /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ + int glomodear; /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */ + int gpsmodear; /* GPS AR mode, debug/learning only (0:off,1:on) */ + int bdsmodear; /* BeiDou AR mode (0:off,1:on) */ + int arfilter; /* AR filtering to reject bad sats (0:off,1:on) */ + int maxout; /* obs outage count to reset bias */ + int minlock; /* min lock count to fix ambiguity */ + int minfixsats; /* min sats to fix integer ambiguities */ + int minholdsats; /* min sats to hold integer ambiguities */ + int mindropsats; /* min sats to drop sats in AR */ + int minfix; /* min fix count to hold ambiguity */ + int armaxiter; /* max iteration to resolve ambiguity */ + int ionoopt; /* ionosphere option (IONOOPT_???) */ + int tropopt; /* troposphere option (TROPOPT_???) */ + int dynamics; /* dynamics model (0:none,1:velociy,2:accel) */ + int tidecorr; /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */ + int niter; /* number of filter iteration */ + int codesmooth; /* code smoothing window size (0:none) */ + int intpref; /* interpolate reference obs (for post mission) */ + int sbascorr; /* SBAS correction options */ + int sbassatsel; /* SBAS satellite selection (0:all) */ + int rovpos; /* rover position for fixed mode */ + int refpos; /* base position for relative mode */ + /* (0:pos in prcopt, 1:average of single pos, */ + /* 2:read from file, 3:rinex header, 4:rtcm pos) */ + double eratio[NFREQ]; /* code/phase error ratio */ + double err[8]; /* observation error terms */ + /* [reserved,constant,elevation,baseline,doppler,snr-max,snr, rcv_std] */ + double std[3]; /* initial-state std [0]bias,[1]iono [2]trop */ + double prn[6]; /* process-noise std [0]bias,[1]iono [2]trop [3]acch [4]accv [5] pos */ + double sclkstab; /* satellite clock stability (sec/sec) */ + double thresar[8]; /* AR validation threshold */ + double elmaskar; /* elevation mask of AR for rising satellite (deg) */ + double elmaskhold; /* elevation mask to hold ambiguity (deg) */ + double thresslip; /* slip threshold of geometry-free phase (m) */ + double thresdop; /* slip threshold of doppler (m) */ + double varholdamb; /* variance for fix-and-hold psuedo measurements (cycle^2) */ + double gainholdamb; /* gain used for GLO and SBAS sats to adjust ambiguity */ + double maxtdiff; /* max difference of time (sec) */ + double maxinno; /* reject threshold of innovation (m) */ + double maxgdop; /* reject threshold of gdop */ + double baseline[2]; /* baseline length constraint {const,sigma} (m) */ + double ru[3]; /* rover position for fixed mode {x,y,z} (ecef) (m) */ + double rb[3]; /* base position for relative mode {x,y,z} (ecef) (m) */ + // char anttype[2][MAXANT]; /* antenna types {rover,base} */ + double antdel[2][3]; /* antenna delta {{rov_e,rov_n,rov_u},{ref_e,ref_n,ref_u}} */ + // pcv_t pcvr[2]; /* receiver antenna parameters {rov,base} */ + uint8_t exsats[MAXSAT]; /* excluded satellites (1:excluded,2:included) */ + int maxaveep; /* max averaging epoches */ + int initrst; /* initialize by restart */ + int outsingle; /* output single by dgps/float/fix/ppp outage */ + // char rnxopt[2][256]; /* rinex options {rover,base} */ + int posopt[6]; /* positioning options */ + int syncsol; /* solution sync mode (0:off,1:on) */ + // double odisp[2][6 * 11]; /* ocean tide loading parameters {rov,base} */ + int freqopt; /* disable L2-AR */ + // char pppopt[256]; /* ppp option */ + int lambda_mode; /* lambda option */ +} prcopt_t; +typedef struct +{ /* solution options type */ + int posf; /* solution format (SOLF_???) */ + int times; /* time system (TIMES_???) */ + int timef; /* time format (0:sssss.s,1:yyyy/mm/dd hh:mm:ss.s) */ + int timeu; /* time digits under decimal point */ + int degf; /* latitude/longitude format (0:ddd.ddd,1:ddd mm ss) */ + int outhead; /* output header (0:no,1:yes) */ + int outopt; /* output processing options (0:no,1:yes) */ + int outvel; /* output velocity options (0:no,1:yes) */ + int datum; /* datum (0:WGS84,1:Tokyo) */ + int height; /* height (0:ellipsoidal,1:geodetic) */ + int geoid; /* geoid model (0:EGM96,1:JGD2000) */ + int solstatic; /* solution of static mode (0:all,1:single) */ + int sstat; /* solution statistics level (0:off,1:states,2:residuals) */ + int trace; /* debug trace level (0:off,1-5:debug) */ + double nmeaintv[2]; /* nmea output interval (s) (<0:no,0:all) */ + /* nmeaintv[0]:gprmc,gpgga,nmeaintv[1]:gpgsv */ + char sep[16]; /* field separator ???? */ + char prog[16]; /* program name ???? */ + double maxsolstd; /* max std-dev for solution output (m) (0:all) */ +} solopt_t; +// typedef struct { /* file options type */ +// char satantp[MAXSTRPATH]; /* satellite antenna parameters file */ +// char rcvantp[MAXSTRPATH]; /* receiver antenna parameters file */ +// char stapos [MAXSTRPATH]; /* station positions file */ +// char geoid [MAXSTRPATH]; /* external geoid data file */ +// char iono [MAXSTRPATH]; /* ionosphere data file */ +// char dcb [MAXSTRPATH]; /* dcb data file */ +// char eop [MAXSTRPATH]; /* eop data file */ +// char blq [MAXSTRPATH]; /* ocean tide loading blq file */ +// char tempdir[MAXSTRPATH]; /* ftp/http temporaly directory */ +// char geexe [MAXSTRPATH]; /* google earth exec file */ +// char solstat[MAXSTRPATH]; /* solution statistics file */ +// char trace [MAXSTRPATH]; /* debug trace file */ +// } filopt_t; +// typedef struct { /* RINEX options type */ +// gtime_t ts,te; /* time start/end */ +// double tint; /* time interval (s) */ +// double ttol; /* time tolerance (s) */ +// double tunit; /* time unit for multiple-session (s) */ +// int rnxver; /* RINEX version (x100) */ +// int navsys; /* navigation system */ +// int obstype; /* observation type */ +// int freqtype; /* frequency type */ +// char mask[7][64]; /* code mask {GPS,GLO,GAL,QZS,SBS,CMP,IRN} */ +// char staid [32]; /* station id for rinex file name */ +// char prog [32]; /* program */ +// char runby [32]; /* run-by */ +// char marker[64]; /* marker name */ +// char markerno[32]; /* marker number */ +// char markertype[32]; /* marker type (ver.3) */ +// char name[2][32]; /* observer/agency */ +// char rec [3][32]; /* receiver #/type/vers */ +// char ant [3][32]; /* antenna #/type */ +// double apppos[3]; /* approx position x/y/z */ +// double antdel[3]; /* antenna delta h/e/n */ +// double glo_cp_bias[4]; /* GLONASS code-phase biases (m) */ +// char comment[MAXCOMMENT][64]; /* comments */ +// char rcvopt[256]; /* receiver dependent options */ +// uint8_t exsats[MAXSAT]; /* excluded satellites */ +// int glofcn[32]; /* glonass fcn+8 */ +// int outiono; /* output iono correction */ +// int outtime; /* output time system correction */ +// int outleaps; /* output leap seconds */ +// int autopos; /* auto approx position */ +// int phshift; /* phase shift correction */ +// int halfcyc; /* half cycle correction */ +// int sep_nav; /* separated nav files */ +// gtime_t tstart; /* first obs time */ +// gtime_t tend; /* last obs time */ +// gtime_t trtcm; /* approx log start time for rtcm */ +// char tobs[7][MAXOBSTYPE][4]; /* obs types {GPS,GLO,GAL,QZS,SBS,CMP,IRN} */ +// double shift[7][MAXOBSTYPE]; /* phase shift (cyc) {GPS,GLO,GAL,QZS,SBS,CMP,IRN} */ +// int nobs[7]; /* number of obs types {GPS,GLO,GAL,QZS,SBS,CMP,IRN} */ +// } rnxopt_t; +typedef struct +{ /* satellite status type */ + uint8_t sys; /* navigation system */ + uint8_t vs; /* valid satellite flag single */ + double azel[2]; /* azimuth/elevation angles {az,el} (rad) */ + double resp[NFREQ]; /* residuals of pseudorange (m) */ + double resc[NFREQ]; /* residuals of carrier-phase (m) */ + double icbias[NFREQ]; /* glonass IC bias (cycles) */ + uint8_t vsat[NFREQ]; /* valid satellite flag */ + uint16_t snr_rover[NFREQ]; /* rover signal strength (0.25 dBHz) */ + uint16_t snr_base[NFREQ]; /* base signal strength (0.25 dBHz) */ + uint8_t fix[NFREQ]; /* ambiguity fix flag (1:fix,2:float,3:hold) */ + uint8_t slip[NFREQ]; /* cycle-slip flag */ + uint8_t half[NFREQ]; /* half-cycle valid flag */ + int lock[NFREQ]; /* lock counter of phase */ + uint32_t outc[NFREQ]; /* obs outage counter of phase */ + uint32_t slipc[NFREQ]; /* cycle-slip counter */ + uint32_t rejc[NFREQ]; /* reject counter */ + + // double gf[NFREQ - 1]; /* geometry-free phase (m) */ + // double mw[NFREQ - 1]; /* MW-LC (m) */ + + double phw; /* phase windup (cycle) */ + gtime_t pt[2][NFREQ]; /* previous carrier-phase time */ + double ph[2][NFREQ]; /* previous carrier-phase observable (cycle) */ +} ssat_t; +typedef struct +{ /* ambiguity control type */ + gtime_t epoch[4]; /* last epoch */ + int n[4]; /* number of epochs */ + double LC[4]; /* linear combination average */ + double LCv[4]; /* linear combination variance */ + int fixcnt; /* fix count */ + char flags[MAXSAT]; /* fix flags */ +} ambc_t; +// RTK??????? +typedef struct +{ /* RTK control/result type */ + sol_t sol; /* RTK solution */ + double rb[6]; /* base position/velocity (ecef) (m|m/s) */ + int nx, na; /* number of float states/fixed states */ + double tt; /* time difference between current and previous (s) */ + double *x, *P; /* float states and their covariance */ + double *xa, *Pa; /* fixed states and their covariance */ + int nfix; /* number of continuous fixes of ambiguity */ + int excsat; /* index of next satellite to be excluded for partial ambiguity resolution */ + int nb_ar; /* number of ambiguities used for AR last epoch */ + double com_bias; /* phase bias common between all sats (used to be distributed to all sats */ + char holdamb; /* set if fix-and-hold has occurred at least once */ + ambc_t ambc[MAXSAT]; /* ambiguity control */ + ssat_t ssat[MAXSAT]; /* satellite status */ + int neb; /* bytes in error message buffer */ + // char errbuf[MAXERRMSG]; /* error message buffer */ + prcopt_t opt; /* processing options */ + int initial_mode; /* initial positioning mode */ +} rtk_t; +// typedef struct { /* receiver raw data control type */ +// gtime_t time; /* message time */ +// gtime_t tobs[MAXSAT][NFREQ+NEXOBS]; /* observation data time */ +// obs_t obs; /* observation data */ +// obs_t obuf; /* observation data buffer */ +// nav_t nav; /* satellite ephemerides */ +// sta_t sta; /* station parameters */ +// int ephsat; /* update satelle of ephemeris (0:no satellite) */ +// int ephset; /* update set of ephemeris (0-1) */ +// // sbsmsg_t sbsmsg; /* SBAS message */ +// char msgtype[256]; /* last message type */ +// uint8_t subfrm[MAXSAT][380]; /* subframe buffer */ +// double lockt[MAXSAT][NFREQ+NEXOBS]; /* lock time (s) */ +// unsigned char lockflag[MAXSAT][NFREQ+NEXOBS]; /* used for carrying forward cycle slip */ +// double icpp[MAXSAT],off[MAXSAT],icpc; /* carrier params for ss2 */ +// double prCA[MAXSAT],dpCA[MAXSAT]; /* L1/CA pseudrange/doppler for javad */ +// uint8_t halfc[MAXSAT][NFREQ+NEXOBS]; /* half-cycle add flag */ +// char freqn[MAXOBS]; /* frequency number for javad */ +// int nbyte; /* number of bytes in message buffer */ +// int len; /* message length (bytes) */ +// int iod; /* issue of data */ +// int tod; /* time of day (ms) */ +// int tbase; /* time base (0:gpst,1:utc(usno),2:glonass,3:utc(su) */ +// int flag; /* general purpose flag */ +// int outtype; /* output message type */ +// uint8_t buff[MAXRAWLEN]; /* message buffer */ +// char opt[256]; /* receiver dependent options */ +// int format; /* receiver stream format */ +// void *rcv_data; /* receiver dependent data */ +// } raw_t; +// typedef struct +// { /* stream type */ +// int type; /* type (STR_???) */ +// int mode; /* mode (STR_MODE_?) */ +// int state; /* state (-1:error,0:close,1:open) */ +// uint32_t inb, inr; /* input bytes/rate */ +// uint32_t outb, outr; /* output bytes/rate */ +// uint32_t tick_i; /* input tick tick */ +// uint32_t tick_o; /* output tick */ +// uint32_t tact; /* active tick */ +// uint32_t inbt, outbt; /* input/output bytes at tick */ +// lock_t lock; /* lock flag */ +// void *port; /* type dependent port control struct */ +// char path[MAXSTRPATH]; /* stream path */ +// char msg[MAXSTRMSG]; /* stream message */ +// } stream_t; +// typedef struct { /* stream converter type */ +// int itype,otype; /* input and output stream type */ +// int nmsg; /* number of output messages */ +// int msgs[32]; /* output message types */ +// double tint[32]; /* output message intervals (s) */ +// uint32_t tick[32]; /* cycle tick of output message */ +// int ephsat[32]; /* satellites of output ephemeris */ +// int stasel; /* station info selection (0:remote,1:local) */ +// rtcm_t rtcm; /* rtcm input data buffer */ +// raw_t raw; /* raw input data buffer */ +// rtcm_t out; /* rtcm output data buffer */ +// } strconv_t; +// typedef struct { /* stream server type */ +// int state; /* server state (0:stop,1:running) */ +// int cycle; /* server cycle (ms) */ +// int buffsize; /* input/monitor buffer size (bytes) */ +// int nmeacycle; /* NMEA request cycle (ms) (0:no) */ +// int relayback; /* relay back of output streams (0:no) */ +// int nstr; /* number of streams (1 input + (nstr-1) outputs */ +// int npb; /* data length in peek buffer (bytes) */ +// char cmds_periodic[16][MAXRCVCMD]; /* periodic commands */ +// double nmeapos[3]; /* NMEA request position (ecef) (m) */ +// uint8_t *buff; /* input buffers */ +// uint8_t *pbuf; /* peek buffer */ +// uint32_t tick; /* start tick */ +// stream_t stream[16]; /* input/output streams */ +// stream_t strlog[16]; /* return log streams */ +// strconv_t *conv[16]; /* stream converter */ +// thread_t thread; /* server thread */ +// lock_t lock; /* lock flag */ +// } strsvr_t; +typedef struct +{ /* RTK server type */ + int state; /* server state (0:stop,1:running) */ + int cycle; /* processing cycle (ms) */ + int nmeacycle; /* NMEA request cycle (ms) (0:no req) */ + int nmeareq; /* NMEA request (0:no,1:nmeapos,2:single sol) */ + double nmeapos[3]; /* NMEA request position (ecef) (m) */ + int buffsize; /* input buffer size (bytes) */ + int format[2]; /* input format {rov,base} */ + solopt_t solopt; /* output solution options {sol1,sol2} */ + int navsel; /* ephemeris select (0:all,1:rover,2:base,3) */ + int nsbs; /* number of sbas message */ + int nsol; /* number of solution buffer */ + rtk_t rtk; /* RTK control/result struct */ + int nb[2]; /* bytes in input buffers {rov,base} */ + // int nsb[2]; /* bytes in soulution buffers */ + // int npb[2]; /* bytes in input peek buffers */ + uint8_t *buff[2]; /* input buffers {rov,base} */ + // uint8_t *sbuf[2]; /* output buffers {sol1,sol2} */ + // uint8_t *pbuf[2]; /* peek buffers {rov,base} */ + sol_t solbuf[MAXSOLBUF]; /* solution buffer */ + uint32_t nmsg[2][10]; /* input message counts */ + // raw_t raw [2]; /* receiver raw control {rov,base} */ + rtcm_t rtcm[2]; /* RTCM control {rov,base} */ + // gtime_t ftime[2]; /* download time {rov,base} */ + // char files[2][MAXSTRPATH]; /* download paths {rov,base} */ + obs_t obs[2][MAXOBSBUF]; /* observation data {rov,base} */ + nav_t nav; /* navigation data */ + // sbsmsg_t sbsmsg[MAXSBSMSG]; /* SBAS message buffer */ + // stream_t stream[8]; /* streams {rov,base,corr,sol1,sol2,logr,logb,logc} */ + // stream_t *moni; /* monitor stream */ + uint32_t tick; /* start tick */ +// thread_t thread; /* server thread */ + int cputime; /* CPU time (ms) for a processing cycle */ + int prcout; /* missing observation data count */ + int nave; /* number of averaging base pos */ + double rb_ave[3]; /* averaging base pos */ + // char cmds_periodic[2][MAXRCVCMD]; /* periodic commands */ + // char cmd_reset[MAXRCVCMD]; /* reset command */ + double bl_reset; /* baseline length to reset (km) */ +// lock_t lock; /* lock flag */ +} rtksvr_t; +// typedef struct { /* GIS data point type */ +// double pos[3]; /* point data {lat,lon,height} (rad,m) */ +// } gis_pnt_t; +// typedef struct { /* GIS data polyline type */ +// int npnt; /* number of points */ +// double bound[4]; /* boundary {lat0,lat1,lon0,lon1} */ +// double *pos; /* position data (3 x npnt) */ +// } gis_poly_t; +// typedef struct { /* GIS data polygon type */ +// int npnt; /* number of points */ +// double bound[4]; /* boundary {lat0,lat1,lon0,lon1} */ +// double *pos; /* position data (3 x npnt) */ +// } gis_polygon_t; +// typedef struct gisd_tag { /* GIS data list type */ +// int type; /* data type (1:point,2:polyline,3:polygon) */ +// void *data; /* data body */ +// struct gisd_tag *next; /* pointer to next */ +// } gisd_t; +// typedef struct { /* GIS type */ +// char name[MAXGISLAYER][256]; /* name */ +// int flag[MAXGISLAYER]; /* flag */ +// gisd_t *data[MAXGISLAYER]; /* gis data list */ +// double bound[4]; /* boundary {lat0,lat1,lon0,lon1} */ +// } gis_t; +typedef struct +{ + sol_t sol; + int nbyte; /* number of bytes in message buffer */ + int len; /* message length (bytes) */ + uint8_t buff[256]; /* message buffer */ +} nmea_t; +typedef void fatalfunc_t(const char *); /* fatal callback function type */ +/* global variables ----------------------------------------------------------*/ +extern const double chisqr[]; /* chi-sqr(n) table (alpha=0.001) */ +extern const prcopt_t prcopt_default; /* default positioning options */ +extern const solopt_t solopt_default; /* default solution output options */ +extern const char *formatstrs[]; /* stream format strings */ +extern opt_t sysopts[]; /* system options table */ +/* satellites, systems, codes functions --------------------------------------*/ +EXPORT int satno(int sys, int prn); +EXPORT int satsys(int sat, int *prn); +EXPORT int satid2no(const char *id); +EXPORT void satno2id(int sat, char *id); +EXPORT uint8_t obs2code(const char *obs); +EXPORT char *code2obs(uint8_t code); +EXPORT double code2freq(int sys, uint8_t code, int fcn); +EXPORT double sat2freq(int sat, uint8_t code, const nav_t *nav); +EXPORT int code2idx(int sys, uint8_t code); +EXPORT int satexclude(int sat, double var, int svh, const prcopt_t *opt); +EXPORT int testsnr(int base, int freq, double el, double snr, + const snrmask_t *mask); +EXPORT void setcodepri(int sys, int idx, const char *pri); +EXPORT int getcodepri(int sys, uint8_t code, const char *opt); + +/* matrix and vector functions -----------------------------------------------*/ +EXPORT double *mat(int n, int m); +EXPORT int *imat(int n, int m); +EXPORT double *zeros(int n, int m); +EXPORT double *eye(int n); +EXPORT double dot(const double *a, const double *b, int n); +EXPORT double norm(const double *a, int n); +EXPORT void cross3(const double *a, const double *b, double *c); +EXPORT int normv3(const double *a, double *b); +EXPORT void matcpy(double *A, const double *B, int n, int m); +EXPORT void matmul(const char *tr, int n, int k, int m, double alpha, + const double *A, const double *B, double beta, double *C); +EXPORT int matinv(double *A, int n); +EXPORT int solve(const char *tr, const double *A, const double *Y, int n, + int m, double *X); +EXPORT int lsq(const double *A, const double *y, int n, int m, double *x, + double *Q); +EXPORT int filter(double *x, double *P, const double *H, const double *v, + const double *R, int n, int m); +EXPORT int smoother(const double *xf, const double *Qf, const double *xb, + const double *Qb, int n, double *xs, double *Qs); +// EXPORT void matprint(const double *A, int n, int m, int p, int q); +// EXPORT void matfprint(const double *A, int n, int m, int p, int q, FILE *fp); +EXPORT void add_fatal(fatalfunc_t *func); + +/* time and string functions -------------------------------------------------*/ +EXPORT double str2num(const char *s, int i, int n); +EXPORT int str2time(const char *s, int i, int n, gtime_t *t); +EXPORT void time2str(gtime_t t, char *str, int n); +EXPORT gtime_t epoch2time(const double *ep); +EXPORT void time2epoch(gtime_t t, double *ep); +EXPORT void time2epoch_n(gtime_t t, double *ep, int n); +EXPORT gtime_t gpst2time(int week, double sec); +EXPORT double time2gpst(gtime_t t, int *week); +EXPORT gtime_t gst2time(int week, double sec); +EXPORT double time2gst(gtime_t t, int *week); +EXPORT gtime_t bdt2time(int week, double sec); +EXPORT double time2bdt(gtime_t t, int *week); +EXPORT char *time_str(gtime_t t, int n); +EXPORT gtime_t timeadd(gtime_t t, double sec); +EXPORT double timediff(gtime_t t1, gtime_t t2); +EXPORT gtime_t gpst2utc(gtime_t t); +EXPORT gtime_t utc2gpst(gtime_t t); +EXPORT gtime_t gpst2bdt(gtime_t t); +EXPORT gtime_t bdt2gpst(gtime_t t); +EXPORT gtime_t timeget(void); +EXPORT void timeset(gtime_t t); +EXPORT void timereset(void); +EXPORT double time2doy(gtime_t t); +EXPORT double utc2gmst(gtime_t t, double ut1_utc); +// EXPORT int read_leaps(const char *file); +EXPORT int adjgpsweek(int week); +EXPORT uint32_t tickget(void); +EXPORT void sleepms(int ms); +// EXPORT int reppath(const char *path, char *rpath, gtime_t time, const char *rov, +// const char *base); +// EXPORT int reppaths(const char *path, char *rpaths[], int nmax, gtime_t ts, gtime_t te, +// const char *rov, const char *base); + +/* coordinates transformation ------------------------------------------------*/ +EXPORT void ecef2pos(const double *r, double *pos); +EXPORT void pos2ecef(const double *pos, double *r); +EXPORT void ecef2enu(const double *pos, const double *r, double *e); +EXPORT void enu2ecef(const double *pos, const double *e, double *r); +EXPORT void covenu(const double *pos, const double *P, double *Q); +EXPORT void covecef(const double *pos, const double *Q, double *P); +EXPORT void xyz2enu(const double *pos, double *E); +// EXPORT void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst); +EXPORT void deg2dms(double deg, double *dms, int ndec); +EXPORT double dms2deg(const double *dms); + +/* input and output functions ------------------------------------------------*/ +// EXPORT void readpos(const char *file, const char *rcv, double *pos); +EXPORT int sortobs(obs_t *obs); +EXPORT void uniqnav(nav_t *nav); +EXPORT int screent(gtime_t time, gtime_t ts, gtime_t te, double tint); +// EXPORT void freeobs(obs_t *obs); +// EXPORT void freenav(nav_t *nav, int opt); +// EXPORT int geterp(const erp_t *erp, gtime_t time, double *val); +// EXPORT int readnav(const char *file, nav_t *nav); +// EXPORT int savenav(const char *file, const nav_t *nav); +// EXPORT int readblq(const char *file, const char *sta, double *odisp); +// EXPORT int readerp(const char *file, erp_t *erp); +/* debug trace functions -----------------------------------------------------*/ +EXPORT void traceopen(const char *file); +EXPORT void traceclose(void); +EXPORT void tracelevel(int level); +EXPORT void trace(int level, const char *format, ...); +EXPORT void tracet(int level, const char *format, ...); +EXPORT void tracemat(int level, const double *A, int n, int m, int p, int q); +EXPORT void traceobs(int level, const obsd_t *obs, int n); +EXPORT void tracenav(int level, const nav_t *nav); +EXPORT void tracegnav(int level, const nav_t *nav); +EXPORT void tracehnav(int level, const nav_t *nav); +EXPORT void tracepeph(int level, const nav_t *nav); +EXPORT void tracepclk(int level, const nav_t *nav); +EXPORT void traceb(int level, const uint8_t *p, int n); +EXPORT int gettracelevel(void); + +/* platform dependent functions ----------------------------------------------*/ +// EXPORT int execcmd(const char *cmd); +// EXPORT int expath(const char *path, char *paths[], int nmax); +// EXPORT void createdir(const char *path); + +/* positioning models --------------------------------------------------------*/ +EXPORT double satazel(const double *pos, const double *e, double *azel); +EXPORT double geodist(const double *rs, const double *rr, double *e); +EXPORT void dops(int ns, const double *azel, double elmin, double *dop); +EXPORT int seliflc(int optnf, int sys); +/* atmosphere models ---------------------------------------------------------*/ +EXPORT double ionmodel(gtime_t t, const double *ion, const double *pos, + const double *azel); +EXPORT double ionmapf(const double *pos, const double *azel); +EXPORT double ionppp(const double *pos, const double *azel, double re, + double hion, double *pppos); +EXPORT double tropmodel(gtime_t time, const double *pos, const double *azel, + double humi); +EXPORT double tropmapf(gtime_t time, const double *pos, const double *azel, + double *mapfw); +EXPORT int iontec(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, int opt, double *delay, double *var); +// EXPORT void readtec(const char *file, nav_t *nav, int opt); +EXPORT int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, + const double *azel, int ionoopt, double *ion, double *var); +EXPORT int tropcorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, int tropopt, double *trp, double *var); + +/* antenna models ------------------------------------------------------------*/ +// EXPORT int readpcv(const char *file, pcvs_t *pcvs); +// EXPORT pcv_t *searchpcv(int sat, const char *type, gtime_t time, +// const pcvs_t *pcvs); +// EXPORT void antmodel(const pcv_t *pcv, const double *del, const double *azel, +// int opt, double *dant); +// EXPORT void antmodel_s(const pcv_t *pcv, double nadir, double *dant); +/* earth tide models ---------------------------------------------------------*/ +// EXPORT void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, +// double *rmoon, double *gmst); +// EXPORT void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, +// const double *odisp, double *dr); + +/* geiod models --------------------------------------------------------------*/ +// EXPORT int opengeoid(int model, const char *file); +// EXPORT void closegeoid(void); +EXPORT double geoidh(const double *pos); + +/* datum transformation ------------------------------------------------------*/ +// EXPORT int loaddatump(const char *file); +// EXPORT int tokyo2jgd(double *pos); +// EXPORT int jgd2tokyo(double *pos); + +/* ephemeris and clock functions ---------------------------------------------*/ +EXPORT double eph2clk(gtime_t time, const eph_t *eph); +EXPORT double geph2clk(gtime_t time, const geph_t *geph); +// EXPORT double seph2clk(gtime_t time, const seph_t *seph); +EXPORT void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, + double *var); +EXPORT void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts, + double *var); +// EXPORT void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts, +// double *var); +// EXPORT int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, +// double *rs, double *dts, double *var); +// EXPORT void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav, +// double *dant); +EXPORT int satpos(gtime_t time, gtime_t teph, int sat, int ephopt, + const nav_t *nav, double *rs, double *dts, double *var, + int *svh); +EXPORT void satposs(gtime_t time, const obsd_t *obs, int n, const nav_t *nav, + int sateph, double *rs, double *dts, double *var, int *svh); +EXPORT void setseleph(int sys, int sel); +EXPORT int getseleph(int sys); +// EXPORT void readsp3(const char *file, nav_t *nav, int opt); +// EXPORT int readsap(const char *file, gtime_t time, nav_t *nav); +// EXPORT int readdcb(const char *file, nav_t *nav, const sta_t *sta); +// EXPORT int readfcb(const char *file, nav_t *nav); +// EXPORT void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts); +// EXPORT int tle_read(const char *file, tle_t *tle); +// EXPORT int tle_name_read(const char *file, tle_t *tle); +// EXPORT int tle_pos(gtime_t time, const char *name, const char *satno, +// const char *desig, const tle_t *tle, const erp_t *erp, +// double *rs); +/* receiver raw data functions -----------------------------------------------*/ +EXPORT uint32_t getbitu(const uint8_t *buff, int pos, int len); +EXPORT int32_t getbits(const uint8_t *buff, int pos, int len); +EXPORT void setbitu(uint8_t *buff, int pos, int len, uint32_t data); +EXPORT void setbits(uint8_t *buff, int pos, int len, int32_t data); +EXPORT uint32_t rtk_crc32(const uint8_t *buff, int len); +EXPORT uint32_t rtk_crc24q(const uint8_t *buff, int len); +// EXPORT uint16_t rtk_crc16(const uint8_t *buff, int len); +// EXPORT int decode_word (uint32_t word, uint8_t *data); +// EXPORT int decode_frame(const uint8_t *buff, eph_t *eph, alm_t *alm, +// double *ion, double *utc); +// EXPORT int test_glostr(const uint8_t *buff); +// EXPORT int decode_glostr(const uint8_t *buff, geph_t *geph, double *utc); +// EXPORT int decode_bds_d1(const uint8_t *buff, eph_t *eph, double *ion, +// double *utc); +// EXPORT int decode_bds_d2(const uint8_t *buff, eph_t *eph, double *utc); +// EXPORT int decode_gal_inav(const uint8_t *buff, eph_t *eph, double *ion, +// double *utc); +// EXPORT int decode_gal_fnav(const uint8_t *buff, eph_t *eph, double *ion, +// double *utc); +// EXPORT int decode_irn_nav(const uint8_t *buff, eph_t *eph, double *ion, +// double *utc); +// EXPORT int init_raw (raw_t *raw, int format); +// EXPORT void free_raw (raw_t *raw); +// EXPORT int input_raw (raw_t *raw, int format, uint8_t data); +// EXPORT int input_rawf (raw_t *raw, int format, FILE *fp); +// EXPORT int update_cmr (raw_t *raw, rtksvr_t *svr, obs_t *obs); +// EXPORT int input_ubx (raw_t *raw, uint8_t data); +// EXPORT int input_ubxf (raw_t *raw, FILE *fp); +// EXPORT int input_oem4 (raw_t *raw, uint8_t data); +// EXPORT int input_cnav (raw_t *raw, uint8_t data); +// EXPORT int input_sbp (raw_t *raw, uint8_t data); +// EXPORT int input_cres (raw_t *raw, uint8_t data); +// EXPORT int input_stq (raw_t *raw, uint8_t data); +// EXPORT int input_javad (raw_t *raw, uint8_t data); +// EXPORT int input_nvs (raw_t *raw, uint8_t data); +// EXPORT int input_bnx (raw_t *raw, uint8_t data); +// EXPORT int input_rt17 (raw_t *raw, uint8_t data); +// EXPORT int input_sbf (raw_t *raw, uint8_t data); +// EXPORT int input_tersus(raw_t *raw, uint8_t data); +// EXPORT int input_oem4f (raw_t *raw, FILE *fp); +// EXPORT int input_cnavf (raw_t *raw, FILE *fp); +// EXPORT int input_sbpf (raw_t *raw, FILE *fp); +// EXPORT int input_cresf (raw_t *raw, FILE *fp); +// EXPORT int input_stqf (raw_t *raw, FILE *fp); +// EXPORT int input_javadf(raw_t *raw, FILE *fp); +// EXPORT int input_nvsf (raw_t *raw, FILE *fp); +// EXPORT int input_bnxf (raw_t *raw, FILE *fp); +// EXPORT int input_rt17f (raw_t *raw, FILE *fp); +// EXPORT int input_sbff (raw_t *raw, FILE *fp); +// EXPORT int input_tersusf(raw_t *raw, FILE *fp); +// EXPORT int gen_ubx (const char *msg, uint8_t *buff); +// EXPORT int gen_stq (const char *msg, uint8_t *buff); +// EXPORT int gen_nvs (const char *msg, uint8_t *buff); +/* rtcm functions ------------------------------------------------------------*/ +EXPORT int init_rtcm(rtcm_t *rtcm); +EXPORT int init_rtcm_static(rtcm_t *rtcm, char index); +EXPORT void free_rtcm(rtcm_t *rtcm); +EXPORT int input_rtcm3(rtcm_t *rtcm, uint8_t data); +EXPORT int input_nmea(nmea_t* nmea, const uint8_t data); +// EXPORT int input_rtcm3f(rtcm_t *rtcm, FILE *fp); +// EXPORT int input_rtcm2 (rtcm_t *rtcm, uint8_t data); +// EXPORT int input_rtcm2f(rtcm_t *rtcm, FILE *fp); +// EXPORT int gen_rtcm2 (rtcm_t *rtcm, int type, int sync); +// EXPORT int gen_rtcm3 (rtcm_t *rtcm, int type, int subtype, int sync); +/* solution functions --------------------------------------------------------*/ +// EXPORT void initsolbuf(solbuf_t *solbuf, int cyclic, int nmax); +// EXPORT void freesolbuf(solbuf_t *solbuf); +// EXPORT void freesolstatbuf(solstatbuf_t *solstatbuf); +// EXPORT sol_t *getsol(solbuf_t *solbuf, int index); +// EXPORT int addsol(solbuf_t *solbuf, const sol_t *sol); +// EXPORT int readsol (char *files[], int nfile, solbuf_t *sol); +// EXPORT int readsolt(char *files[], int nfile, gtime_t ts, gtime_t te, +// double tint, int qflag, solbuf_t *sol); +// EXPORT int readsolstat(char *files[], int nfile, solstatbuf_t *statbuf); +// EXPORT int readsolstatt(char *files[], int nfile, gtime_t ts, gtime_t te, +// double tint, solstatbuf_t *statbuf); +// EXPORT int inputsol(uint8_t data, gtime_t ts, gtime_t te, double tint, +// int qflag, const solopt_t *opt, solbuf_t *solbuf); + +/* ?????????? */ +// EXPORT int outprcopts(uint8_t *buff, const prcopt_t *opt); +// EXPORT int outsolheads(uint8_t *buff, const solopt_t *opt); +EXPORT int outsols(uint8_t *buff, const sol_t *sol, const double *rb, const solopt_t *opt); +// EXPORT int outsolexs(uint8_t *buff, const sol_t *sol, const ssat_t *ssat, const solopt_t *opt); +// EXPORT void outprcopt(FILE *fp, const prcopt_t *opt); +// EXPORT void outsolhead(FILE *fp, const solopt_t *opt); +// EXPORT void outsol(FILE *fp, const sol_t *sol, const double *rb, const solopt_t *opt); +// EXPORT void outsolex(FILE *fp, const sol_t *sol, const ssat_t *ssat, const solopt_t *opt); +// EXPORT int outnmea_rmc(uint8_t *buff, const sol_t *sol); +// EXPORT int outnmea_gga(uint8_t *buff, const sol_t *sol); +// EXPORT int outnmea_gsa(uint8_t *buff, const sol_t *sol, const ssat_t *ssat); +// EXPORT int outnmea_gsv(uint8_t *buff, const sol_t *sol, const ssat_t *ssat); + +// /* google earth kml converter ------------------------------------------------*/ +// EXPORT int convkml(const char *infile, const char *outfile, gtime_t ts, +// gtime_t te, double tint, int qflg, double *offset, +// int tcolor, int pcolor, int outalt, int outtime); +// /* gpx converter -------------------------------------------------------------*/ +// EXPORT int convgpx(const char *infile, const char *outfile, gtime_t ts, +// gtime_t te, double tint, int qflg, double *offset, +// int outtrk, int outpnt, int outalt, int outtime); +/* sbas functions ------------------------------------------------------------*/ +// EXPORT int sbsreadmsg (const char *file, int sel, sbs_t *sbs); +// EXPORT int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te, +// sbs_t *sbs); +// EXPORT void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg); +// EXPORT int sbsdecodemsg(gtime_t time, int prn, const uint32_t *words, +// sbsmsg_t *sbsmsg); +// EXPORT int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav); +// EXPORT int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs, +// double *dts, double *var); +// EXPORT int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos, +// const double *azel, double *delay, double *var); +// EXPORT double sbstropcorr(gtime_t time, const double *pos, const double *azel, +// double *var); +/* options functions ---------------------------------------------------------*/ +// EXPORT opt_t *searchopt(const char *name, const opt_t *opts); +// EXPORT int str2opt(opt_t *opt, const char *str); +// EXPORT int opt2str(const opt_t *opt, char *str); +// EXPORT int opt2buf(const opt_t *opt, char *buff); +// EXPORT int loadopts(const char *file, opt_t *opts); +// EXPORT int saveopts(const char *file, const char *mode, const char *comment, +// const opt_t *opts); +EXPORT void resetsysopts(void); +// EXPORT void getsysopts(prcopt_t *popt, solopt_t *sopt, filopt_t *fopt); +// EXPORT void setsysopts(const prcopt_t *popt, const solopt_t *sopt, +// const filopt_t *fopt); +// EXPORT void getsysopts(prcopt_t *popt, solopt_t *sopt); +// EXPORT void setsysopts(const prcopt_t *popt, const solopt_t *sopt); +/* stream data input and output functions ------------------------------------*/ +// EXPORT void strinitcom(void); +// EXPORT void strinit(stream_t *stream); +// EXPORT void strlock(stream_t *stream); +// EXPORT void strunlock(stream_t *stream); +// EXPORT int stropen(stream_t *stream, int type, int mode, const char *path); +// EXPORT void strclose(stream_t *stream); +// EXPORT int strread(stream_t *stream, uint8_t *buff, int n); +// EXPORT int strwrite(stream_t *stream, uint8_t *buff, int n); +// EXPORT void strsync(stream_t *stream1, stream_t *stream2); +// EXPORT int strstat(stream_t *stream, char *msg); +// EXPORT int strstatx(stream_t *stream, char *msg); +// EXPORT void strsum(stream_t *stream, int *inb, int *inr, int *outb, int *outr); +// EXPORT void strsetopt(const int *opt); +// EXPORT gtime_t strgettime(stream_t *stream); +// EXPORT void strsendnmea(stream_t *stream, const sol_t *sol); +// EXPORT void strsendcmd(stream_t *stream, const char *cmd); +// EXPORT void strsettimeout(stream_t *stream, int toinact, int tirecon); +// EXPORT void strsetdir(const char *dir); +// EXPORT void strsetproxy(const char *addr); +/* integer ambiguity resolution ----------------------------------------------*/ +EXPORT int lambda(rtk_t *rtk, int n, int m, const double *a, const double *Q, double *F, + double *s); +EXPORT int lambda_reduction(int n, const double *Q, double *Z); +EXPORT int lambda_search(int n, int m, const double *a, const double *Q, + double *F, double *s); +EXPORT int parlambda(rtk_t *rtk, int n, int m, const double *a, const double *Q, double *F, double *s); +/* standard positioning ------------------------------------------------------*/ +EXPORT int pntpos(const obsd_t *obs, int n, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, + ssat_t *ssat, char *msg); +/* precise positioning -------------------------------------------------------*/ +EXPORT void rtkinit(rtk_t *rtk, const prcopt_t *opt); +EXPORT void rtkfree(rtk_t *rtk); +EXPORT int rtkpos(rtk_t *rtk, obsd_t *obs, int nobs, const nav_t *nav); +extern int rtkpos_static(rtk_t *rtk, obsd_t *obs[], int n, const nav_t *nav, char nr1, char nu1); +// EXPORT int rtkopenstat(const char *file, int level); +// EXPORT void rtkclosestat(void); +EXPORT int rtkoutstat(rtk_t *rtk, char *buff); +// /* precise point positioning -------------------------------------------------*/ +// EXPORT void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); +// EXPORT int pppnx(const prcopt_t *opt); +// EXPORT int pppoutstat(rtk_t *rtk, char *buff); +// EXPORT int ppp_ar(rtk_t *rtk, const obsd_t *obs, int n, int *exc, +// const nav_t *nav, const double *azel, double *x, double *P); +/* post-processing positioning -----------------------------------------------*/ +// EXPORT int postpos(gtime_t ts, gtime_t te, double ti, double tu, +// const prcopt_t *popt, const solopt_t *sopt, +// const filopt_t *fopt, char **infile, int n, char *outfile, +// const char *rov, const char *base); +// EXPORT int postpos(gtime_t ts, gtime_t te, double ti, double tu, +// const prcopt_t *popt, const solopt_t *sopt, +// char **infile, int n, char *outfile, +// const char *rov, const char *base); +/* stream server functions ---------------------------------------------------*/ +// EXPORT void strsvrinit (strsvr_t *svr, int nout); +// EXPORT int strsvrstart(strsvr_t *svr, int *opts, int *strs, char **paths, +// char **logs, strconv_t **conv, char **cmds, +// char **cmds_priodic, const double *nmeapos); +// EXPORT void strsvrstop (strsvr_t *svr, char **cmds); +// EXPORT void strsvrstat (strsvr_t *svr, int *stat, int *log_stat, int *byte, +// int *bps, char *msg); +// EXPORT strconv_t *strconvnew(int itype, int otype, const char *msgs, int staid, +// int stasel, const char *opt); +// EXPORT void strconvfree(strconv_t *conv); +/* rtk server functions ------------------------------------------------------*/ +EXPORT int rtksvrinit(rtksvr_t *svr); +EXPORT void rtksvrfree(rtksvr_t *svr); +// EXPORT int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs, +// char **paths, int *formats, int navsel, char **cmds, +// char **cmds_periodic, char **rcvopts, int nmeacycle, +// int nmeareq, const double *nmeapos, prcopt_t *prcopt, +// solopt_t *solopt, stream_t *moni, char *errmsg); +EXPORT void rtksvrstop(rtksvr_t *svr, char **cmds); +// EXPORT int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path, +// const solopt_t *solopt); +// EXPORT void rtksvrclosestr(rtksvr_t *svr, int index); +EXPORT void rtksvrlock(rtksvr_t *svr); +EXPORT void rtksvrunlock(rtksvr_t *svr); +// EXPORT int rtksvrostat(rtksvr_t *svr, int type, gtime_t *time, int *sat, +// double *az, double *el, int **snr, int *vsat); +// EXPORT void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg); +// EXPORT int rtksvrmark(rtksvr_t *svr, const char *name, const char *comment); +// /* downloader functions ------------------------------------------------------*/ +// EXPORT int dl_readurls(const char *file, char **types, int ntype, url_t *urls, +// int nmax); +// EXPORT int dl_readstas(const char *file, char **stas, int nmax); +// EXPORT int dl_exec(gtime_t ts, gtime_t te, double ti, int seqnos, int seqnoe, +// const url_t *urls, int nurl, char **stas, int nsta, +// const char *dir, const char *usr, const char *pwd, +// const char *proxy, int opts, char *msg, FILE *fp); +// EXPORT void dl_test(gtime_t ts, gtime_t te, double ti, const url_t *urls, +// int nurl, char **stas, int nsta, const char *dir, +// int ncol, int datefmt, FILE *fp); +/* GIS data functions --------------------------------------------------------*/ +// EXPORT int gis_read(const char *file, gis_t *gis, int layer); +// EXPORT void gis_free(gis_t *gis); +/* application defined functions ---------------------------------------------*/ +extern int showmsg(const char *format, ...); +extern void settspan(gtime_t ts, gtime_t te); +extern void settime(gtime_t time); + +/* global variables ----------------------------------------------------------*/ +extern rtksvr_t svr; /* rtk server struct */ +extern prcopt_t prcopt_; +extern solopt_t solopt_; + +extern obsd_t RTCM_obsd[2][MAXOBS]; +extern eph_t RTCM_eph_r[MAXSAT]; +// extern eph_t svr_eph[MAXSAT]; +extern nmea_t nmea_pnt; +extern void rtksvrRun(void *arg); +extern int rtksvrstart_mini(rtksvr_t *svr, int cycle, int buffsize, int *formats, + int nmeacycle, int nmeareq, const double *nmeapos, + prcopt_t *prcopt, solopt_t *solopt); + +/* rtkmiddleware.c */ +extern int strread(int index, uint8_t *buff, int n); +extern int dbg_show_obs(const obsd_t *obs, int nobs, char mode, char *buffer, int index); +extern int dbg_show_sol(char *buff, int n); +extern int dbg_show_trace(char *buff, int n); +extern int dbg_show_solstat(char *buff, int n); +#endif /* RTKLIB_H */