同步测试代码
This commit is contained in:
parent
17854a4b1a
commit
817774b043
33
.config
33
.config
@ -205,7 +205,23 @@ CONFIG_RT_SDIO_DEBUG=y
|
||||
# Using USB
|
||||
#
|
||||
# CONFIG_RT_USING_USB_HOST is not set
|
||||
# CONFIG_RT_USING_USB_DEVICE is not set
|
||||
CONFIG_RT_USING_USB_DEVICE=y
|
||||
CONFIG_RT_USBD_THREAD_STACK_SZ=4096
|
||||
CONFIG_USB_VENDOR_ID=0x0FFE
|
||||
CONFIG_USB_PRODUCT_ID=0x0001
|
||||
CONFIG_RT_USB_DEVICE_COMPOSITE=y
|
||||
CONFIG_RT_USB_DEVICE_CDC=y
|
||||
CONFIG_RT_USB_DEVICE_NONE=y
|
||||
# CONFIG_RT_USB_DEVICE_MSTORAGE is not set
|
||||
# CONFIG_RT_USB_DEVICE_HID is not set
|
||||
# CONFIG_RT_USB_DEVICE_WINUSB is not set
|
||||
# CONFIG_RT_USB_DEVICE_AUDIO is not set
|
||||
CONFIG_RT_VCOM_TASK_STK_SIZE=512
|
||||
CONFIG_RT_CDC_RX_BUFSIZE=128
|
||||
# CONFIG_RT_VCOM_TX_USE_DMA is not set
|
||||
CONFIG_RT_VCOM_SERNO="32021919830108"
|
||||
CONFIG_RT_VCOM_SER_LEN=14
|
||||
CONFIG_RT_VCOM_TX_TIMEOUT=1000
|
||||
# end of Using USB
|
||||
# end of Device Drivers
|
||||
|
||||
@ -503,15 +519,7 @@ CONFIG_ULOG_BACKEND_USING_CONSOLE=y
|
||||
# CONFIG_PKG_USING_SEGGER_RTT is not set
|
||||
# CONFIG_PKG_USING_RDB is not set
|
||||
# CONFIG_PKG_USING_ULOG_EASYFLASH is not set
|
||||
CONFIG_PKG_USING_ULOG_FILE=y
|
||||
CONFIG_PKG_ULOG_FILE_PATH="/packages/tools/ulog_file"
|
||||
CONFIG_ULOG_FILE_ROOT_PATH="/logs"
|
||||
CONFIG_ULOG_FILE_NAME_BASE="ulog.log"
|
||||
CONFIG_ULOG_FILE_MAX_NUM=5
|
||||
CONFIG_ULOG_FILE_MAX_SIZE=524288
|
||||
CONFIG_PKG_USING_ULOG_FILE_V100=y
|
||||
# CONFIG_PKG_USING_ULOG_FILE_LATEST_VERSION is not set
|
||||
CONFIG_PKG_ULOG_FILE_VER="v1.0.0"
|
||||
# CONFIG_PKG_USING_ULOG_FILE is not set
|
||||
# CONFIG_PKG_USING_LOGMGR is not set
|
||||
# CONFIG_PKG_USING_ADBD is not set
|
||||
# CONFIG_PKG_USING_COREMARK is not set
|
||||
@ -546,6 +554,7 @@ CONFIG_PKG_ULOG_FILE_VER="v1.0.0"
|
||||
# CONFIG_PKG_USING_SNOWFLAKE is not set
|
||||
# CONFIG_PKG_USING_HASH_MATCH is not set
|
||||
# CONFIG_PKG_USING_FIRE_PID_CURVE is not set
|
||||
# CONFIG_PKG_USING_ARMV7M_DWT_TOOL is not set
|
||||
# end of tools packages
|
||||
|
||||
#
|
||||
@ -841,7 +850,9 @@ CONFIG_BSP_UART3_RX_USING_DMA=y
|
||||
# CONFIG_BSP_UART3_TX_USING_DMA is not set
|
||||
# CONFIG_BSP_USING_UART4 is not set
|
||||
# CONFIG_BSP_USING_UART5 is not set
|
||||
# CONFIG_BSP_USING_UART6 is not set
|
||||
CONFIG_BSP_USING_UART6=y
|
||||
CONFIG_BSP_UART6_RX_USING_DMA=y
|
||||
# CONFIG_BSP_UART6_TX_USING_DMA is not set
|
||||
# CONFIG_BSP_USING_TIM is not set
|
||||
# CONFIG_BSP_USING_PWM is not set
|
||||
# CONFIG_BSP_USING_ON_CHIP_FLASH is not set
|
||||
|
||||
Binary file not shown.
92
.settings/RTK_base.JLink.Debug.rttlaunch
Normal file
92
.settings/RTK_base.JLink.Debug.rttlaunch
Normal file
@ -0,0 +1,92 @@
|
||||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<launchConfiguration type="ilg.gnumcueclipse.debug.gdbjtag.jlink.launchConfigurationType">
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.adapterName" value="J-Link"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.binFileStartAddress" value="0x08000000"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doConnectToRunning" value="false"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doContinue" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doDebugInRam" value="false"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doFirstReset" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doGdbServerAllocateConsole" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doGdbServerAllocateSemihostingConsole" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doGdbServerInitRegs" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doGdbServerLocalOnly" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doGdbServerSilent" value="false"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doGdbServerVerifyDownload" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doSecondReset" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.doStartGdbServer" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.enableFlashBreakpoints" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.enableSemihosting" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.enableSemihostingIoclientGdbClient" value="false"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.enableSemihostingIoclientTelnet" value="true"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.enableSwo" value="true"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.eraseEndAddress" value=""/>
|
||||
<intAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.eraseMode" value="0"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.eraseStartAddress" value=""/>
|
||||
<intAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.firstResetSpeed" value="1500"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.firstResetType" value=""/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.flashDeviceName" value="STM32F407ZG"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.flashDownloadHex" value="false"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.forceQuitGdbServer" value="false"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbClientOtherCommands" value="set mem inaccessible-by-default off"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbClientOtherOptions" value=""/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerConnection" value="usb"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerConnectionAddress" value=""/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerDebugInterface" value="swd"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerDeviceEndianness" value="little"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerDeviceName" value="STM32F407ZG"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerDeviceSpeed" value="1000"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerExecutable" value="${debugger_install_path}/${jlink_debugger_relative_path}\JLinkGDBServerCL.exe"/>
|
||||
<intAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerGdbPortNumber" value="2331"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerLog" value=""/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerOther" value="-singlerun"/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerRunAfterStopDebug" value="true"/>
|
||||
<intAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerSwoPortNumber" value="2332"/>
|
||||
<intAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.gdbServerTelnetPortNumber" value="2333"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.interfaceSpeed" value="auto"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.jlinkExecutable" value="${debugger_install_path}/${jlink_debugger_relative_path}\JLink.exe"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.otherInitCommands" value=""/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.otherRunCommands" value=""/>
|
||||
<booleanAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.runAfterDownload" value="true"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.secondResetType" value=""/>
|
||||
<intAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.swoEnableTargetCpuFreq" value="0"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.swoEnableTargetPortMask" value="0x1"/>
|
||||
<intAttribute key="ilg.gnumcueclipse.debug.gdbjtag.jlink.swoEnableTargetSwoFreq" value="0"/>
|
||||
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.svdPath" value="${studio_install_path}\repo\Extract\Chip_Support_Packages\RealThread\STM32F4\0.2.0\debug\svd\STM32F40x.svd"/>
|
||||
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.imageFileName" value=""/>
|
||||
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.imageOffset" value=""/>
|
||||
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.ipAddress" value="localhost"/>
|
||||
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.jtagDevice" value="GNU MCU J-Link"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.loadImage" value="true"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.loadSymbols" value="true"/>
|
||||
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.pcRegister" value=""/>
|
||||
<intAttribute key="org.eclipse.cdt.debug.gdbjtag.core.portNumber" value="2331"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setPcRegister" value="false"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setResume" value="false"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setStopAt" value="true"/>
|
||||
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="main"/>
|
||||
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.symbolsFileName" value=""/>
|
||||
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.symbolsOffset" value=""/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.useFileForImage" value="false"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.useFileForSymbols" value="false"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.useProjBinaryForImage" value="true"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.useProjBinaryForSymbols" value="true"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.useRemoteTarget" value="true"/>
|
||||
<stringAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_NAME" value="${rtt_gnu_gcc}/arm-none-eabi-gdb.exe"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.dsf.gdb.UPDATE_THREADLIST_ON_SUSPEND" value="false"/>
|
||||
<intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="0"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="Debug/rtthread.elf"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="RTK_base"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="false"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value=""/>
|
||||
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_PATHS">
|
||||
<listEntry value="/RTK_base"/>
|
||||
</listAttribute>
|
||||
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_TYPES">
|
||||
<listEntry value="4"/>
|
||||
</listAttribute>
|
||||
<stringAttribute key="org.eclipse.debug.core.source_locator_id" value="org.eclipse.cdt.debug.core.sourceLocator"/>
|
||||
<stringAttribute key="org.eclipse.debug.core.source_locator_memento" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?> <sourceLookupDirector> <sourceContainers duplicates="false"> <container memento="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;default/&gt;&#13;&#10;" typeId="org.eclipse.debug.core.containerType.default"/> </sourceContainers> </sourceLookupDirector> "/>
|
||||
<stringAttribute key="org.eclipse.debug.ui.ATTR_CONSOLE_ENCODING" value="GBK"/>
|
||||
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?> <memoryBlockExpressionList context="Context string"> <memoryBlockExpression address="1744848496" label="0x68004670"/> </memoryBlockExpressionList> "/>
|
||||
<stringAttribute key="process_factory_id" value="org.eclipse.cdt.dsf.gdb.GdbProcessFactory"/>
|
||||
</launchConfiguration>
|
||||
@ -5,7 +5,7 @@
|
||||
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
|
||||
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
|
||||
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
|
||||
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="39115038496290056" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD "${INPUTS}"" prefer-non-shared="true">
|
||||
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="38926886275555880" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD "${INPUTS}"" prefer-non-shared="true">
|
||||
<language-scope id="org.eclipse.cdt.core.gcc"/>
|
||||
<language-scope id="org.eclipse.cdt.core.g++"/>
|
||||
</provider>
|
||||
@ -16,7 +16,7 @@
|
||||
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
|
||||
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
|
||||
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
|
||||
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="103830345138989147" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD "${INPUTS}"" prefer-non-shared="true">
|
||||
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="11935337743060155" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD "${INPUTS}"" prefer-non-shared="true">
|
||||
<language-scope id="org.eclipse.cdt.core.gcc"/>
|
||||
<language-scope id="org.eclipse.cdt.core.g++"/>
|
||||
</provider>
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
#RT-Thread Studio Project Configuration
|
||||
#Thu Jun 09 18:46:30 CST 2022
|
||||
#Tue Jun 21 21:06:22 CST 2022
|
||||
cfg_version=v3.0
|
||||
board_name=STM32F407-ATK-EXPLORER
|
||||
example_name=
|
||||
@ -7,13 +7,13 @@ hardware_adapter=J-Link
|
||||
board_base_nano_proj=False
|
||||
project_type=rt-thread
|
||||
chip_name=STM32F407ZG
|
||||
bsp_version=0.1.2
|
||||
selected_rtt_version=4.0.5
|
||||
bsp_version=0.1.2
|
||||
os_branch=full
|
||||
project_base_bsp=true
|
||||
is_use_scons_build=True
|
||||
is_base_example_project=False
|
||||
output_project_path=D\:\\Dev\\GitHome\\rt_thread_workspace\\RTK_base
|
||||
is_base_example_project=False
|
||||
is_use_scons_build=True
|
||||
project_base_bsp=true
|
||||
project_name=RTK_base
|
||||
bsp_path=repo/Extract/Board_Support_Packages/RealThread/STM32F407-ATK-EXPLORER/0.1.2
|
||||
os_version=4.0.5
|
||||
bsp_path=repo/Extract/Board_Support_Packages/RealThread/STM32F407-ATK-EXPLORER/0.1.2
|
||||
|
||||
1227
RTK/ST_rtksvr.c
Normal file
1227
RTK/ST_rtksvr.c
Normal file
File diff suppressed because it is too large
Load Diff
1071
RTK/ephemeris.c
Normal file
1071
RTK/ephemeris.c
Normal file
File diff suppressed because it is too large
Load Diff
7489
RTK/geoid.c
Normal file
7489
RTK/geoid.c
Normal file
File diff suppressed because it is too large
Load Diff
359
RTK/lambda.c
Normal file
359
RTK/lambda.c
Normal file
@ -0,0 +1,359 @@
|
||||
/*------------------------------------------------------------------------------
|
||||
* lambda.c : integer ambiguity resolution
|
||||
*
|
||||
* Copyright (C) 2007-2008 by T.TAKASU, All rights reserved.
|
||||
*
|
||||
* reference :
|
||||
* [1] P.J.G.Teunissen, The least-square ambiguity decorrelation adjustment:
|
||||
* a method for fast GPS ambiguity estimation, J.Geodesy, Vol.70, 65-82,
|
||||
* 1995
|
||||
* [2] X.-W.Chang, X.Yang, T.Zhou, MLAMBDA: A modified LAMBDA method for
|
||||
* integer least-squares estimation, J.Geodesy, Vol.79, 552-565, 2005
|
||||
*
|
||||
* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
|
||||
* history : 2007/01/13 1.0 new
|
||||
* 2015/05/31 1.1 add api lambda_reduction(), lambda_search()
|
||||
*-----------------------------------------------------------------------------*/
|
||||
#include "rtklib.h"
|
||||
|
||||
/* constants/macros ----------------------------------------------------------*/
|
||||
|
||||
#define LOOPMAX 10000 /* maximum count of search loop */
|
||||
|
||||
#define SGN(x) ((x) <= 0.0 ? -1.0 : 1.0)
|
||||
#define ROUND(x) (floor((x) + 0.5))
|
||||
#define SWAP(x, y) \
|
||||
do \
|
||||
{ \
|
||||
double tmp_; \
|
||||
tmp_ = x; \
|
||||
x = y; \
|
||||
y = tmp_; \
|
||||
} while (0)
|
||||
|
||||
/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
|
||||
static int LD(int n, const double *Q, double *L, double *D)
|
||||
{
|
||||
int i, j, k, info = 0;
|
||||
double a, *A = mat(n, n);
|
||||
|
||||
memcpy(A, Q, sizeof(double) * n * n);
|
||||
for (i = n - 1; i >= 0; i--)
|
||||
{
|
||||
if ((D[i] = A[i + i * n]) <= 0.0)
|
||||
{
|
||||
info = -1;
|
||||
break;
|
||||
}
|
||||
a = sqrt(D[i]);
|
||||
for (j = 0; j <= i; j++)
|
||||
L[i + j * n] = A[i + j * n] / a;
|
||||
for (j = 0; j <= i - 1; j++)
|
||||
for (k = 0; k <= j; k++)
|
||||
A[j + k * n] -= L[i + k * n] * L[i + j * n];
|
||||
for (j = 0; j <= i; j++)
|
||||
L[i + j * n] /= L[i + i * n];
|
||||
}
|
||||
free(A);
|
||||
if (info)
|
||||
fprintf(stderr, "%s : LD factorization error\n", __FILE__);
|
||||
return info;
|
||||
}
|
||||
/* integer gauss transformation ----------------------------------------------*/
|
||||
static void gauss(int n, double *L, double *Z, int i, int j)
|
||||
{
|
||||
int k, mu;
|
||||
|
||||
if ((mu = (int)ROUND(L[i + j * n])) != 0)
|
||||
{
|
||||
for (k = i; k < n; k++)
|
||||
L[k + n * j] -= (double)mu * L[k + i * n];
|
||||
for (k = 0; k < n; k++)
|
||||
Z[k + n * j] -= (double)mu * Z[k + i * n];
|
||||
}
|
||||
}
|
||||
/* permutations --------------------------------------------------------------*/
|
||||
static void perm(int n, double *L, double *D, int j, double del, double *Z)
|
||||
{
|
||||
int k;
|
||||
double eta, lam, a0, a1;
|
||||
|
||||
eta = D[j] / del;
|
||||
lam = D[j + 1] * L[j + 1 + j * n] / del;
|
||||
D[j] = eta * D[j + 1];
|
||||
D[j + 1] = del;
|
||||
for (k = 0; k <= j - 1; k++)
|
||||
{
|
||||
a0 = L[j + k * n];
|
||||
a1 = L[j + 1 + k * n];
|
||||
L[j + k * n] = -L[j + 1 + j * n] * a0 + a1;
|
||||
L[j + 1 + k * n] = eta * a0 + lam * a1;
|
||||
}
|
||||
L[j + 1 + j * n] = lam;
|
||||
for (k = j + 2; k < n; k++)
|
||||
SWAP(L[k + j * n], L[k + (j + 1) * n]);
|
||||
for (k = 0; k < n; k++)
|
||||
SWAP(Z[k + j * n], Z[k + (j + 1) * n]);
|
||||
}
|
||||
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
|
||||
static void reduction(int n, double *L, double *D, double *Z)
|
||||
{
|
||||
int i, j, k;
|
||||
double del;
|
||||
|
||||
j = n - 2;
|
||||
k = n - 2;
|
||||
//这里的调序变换类似插入排序的思路？
|
||||
while (j >= 0)
|
||||
{
|
||||
//由于第k+1，k+2，...，n-2列都进行过降相关并且没有被上一次调序变换影响，
|
||||
//因此只需对第0,1，...，k-1，k列进行降相关
|
||||
if (j <= k)
|
||||
for (i = j + 1; i < n; i++)
|
||||
gauss(n, L, Z, i, j); //从最后一列开始，各列非对角线元素从上往下依次降相关
|
||||
del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1];
|
||||
if (del + 1E-6 < D[j + 1])
|
||||
{ /* 检验条件，若不满足检验条件则开始进行调序变换compared considering numerical error */
|
||||
perm(n, L, D, j, del, Z); //调序变换
|
||||
k = j;
|
||||
j = n - 2; //完成调序变换后重新从最后一列开始进行降相关及排序，k记录最后一次进行过调序变换的列序号
|
||||
}
|
||||
else
|
||||
j--;
|
||||
}
|
||||
}
|
||||
/* modified lambda (mlambda) search (ref. [2]) -------------------------------
|
||||
* args : n I number of float parameters
|
||||
* m I number of fixed solution
|
||||
L,D I transformed covariance matrix
|
||||
zs I transformed double-diff phase biases
|
||||
zn O fixed solutions
|
||||
s O sum of residuals for fixed solutions */
|
||||
static int search(int n, int m, const double *L, const double *D,
|
||||
const double *zs, double *zn, double *s)
|
||||
{
|
||||
int i, j, k, c, nn = 0, imax = 0;
|
||||
double newdist, maxdist = 1E99, y; // maxdist，当前超椭圆半径
|
||||
double *S = zeros(n, n), *dist = mat(n, 1), *zb = mat(n, 1), *z = mat(n, 1), *step = mat(n, 1);
|
||||
|
||||
k = n - 1;
|
||||
dist[k] = 0.0; // k表示当前层，从最后一层（n-1）开始计算
|
||||
zb[k] = zs[k]; //即zn
|
||||
z[k] = ROUND(zb[k]); //四舍五入取整；取整后的数与未取整的数作差；step记录z[k]是四舍还是五入
|
||||
y = zb[k] - z[k];
|
||||
step[k] = SGN(y); /* step towards closest integer */
|
||||
for (c = 0; c < LOOPMAX; c++)
|
||||
{
|
||||
newdist = dist[k] + y * y / D[k]; /* newdist=sum(((z(j)-zb(j))^2/d(j))) */
|
||||
if (newdist < maxdist)
|
||||
{ //如果当前累积目标函数计算值小于当前超椭圆半径
|
||||
/* Case 1: move down 情况1：若还未计算至第一层，继续计算累积目标函数值 */
|
||||
if (k != 0)
|
||||
{
|
||||
dist[--k] = newdist; //记录下当前层的累积目标函数值，dist[k]表示了第k,k+1,...,n-1层的目标函数计算和
|
||||
for (i = 0; i <= k; i++)
|
||||
S[k + i * n] = S[k + 1 + i * n] + (z[k + 1] - zb[k + 1]) * L[k + 1 + i * n];
|
||||
zb[k] = zs[k] + S[k + k * n]; //计算Zk，即第k个整数模糊度参数的备选组的中心
|
||||
z[k] = ROUND(zb[k]); /* next valid integer 四舍五入取整；取整后的数与未取整的数作差；记录是四舍还是五入*/
|
||||
y = zb[k] - z[k];
|
||||
step[k] = SGN(y);
|
||||
}
|
||||
/* Case 2: store the found candidate and try next valid integer 情况2：若已经计算至第一层，意味着所有层的累积目标函数值计算完毕 */
|
||||
else
|
||||
{
|
||||
// nn为当前候选解数，m为我们需要的固定解数，这里为2，表示需要一个最优解及一个次优解
|
||||
// s记录候选解的目标函数值，imax记录之前候选解中的最大目标函数值的坐标
|
||||
if (nn < m)
|
||||
{ //若候选解数还没满
|
||||
/* store the first m initial points */
|
||||
if (nn == 0 || newdist > s[imax])
|
||||
imax = nn; //若当前解的目标函数值比之前最大的目标函数值都大，那么更新imax使s[imax]指向当前解中具有的最大目标函数值
|
||||
for (i = 0; i < n; i++)
|
||||
zn[i + nn * n] = z[i]; // zn存放所有候选解
|
||||
s[nn++] = newdist; // s记录当前目标函数值newdist，并加加当前候选解数nn
|
||||
}
|
||||
else
|
||||
{ //若候选解数已满（即当前zn中已经存了2个候选解）
|
||||
if (newdist < s[imax])
|
||||
{ //若 当前解的目标函数值 比 s中的最大目标函数值 小
|
||||
for (i = 0; i < n; i++)
|
||||
zn[i + imax * n] = z[i]; //用当前解替换zn中具有较大目标函数值的解
|
||||
s[imax] = newdist; //用当前解的目标函数值替换s中的最大目标函数值
|
||||
for (i = imax = 0; i < m; i++)
|
||||
if (s[imax] < s[i])
|
||||
imax = i; //更新imax保证imax始终指向s中的最大目标函数值
|
||||
}
|
||||
maxdist = s[imax]; //用当前最大的目标函数值更新超椭圆半径
|
||||
}
|
||||
z[0] += step[0]; /* next valid integer在第一层，取下一个有效的整数模糊度参数进行计算（若zb为5.3，则z取值顺序为5,6,4,7，...） */
|
||||
y = zb[0] - z[0];
|
||||
step[0] = -step[0] - SGN(step[0]);
|
||||
}
|
||||
}
|
||||
/* Case 3: exit or move up情况3：如果当前累积目标函数计算值大于当前超椭圆半径 */
|
||||
else
|
||||
{
|
||||
if (k == n - 1)
|
||||
break; //如果当前层为第n-1层，意味着后续目标函数各项的计算都会超出超椭圆半径，因此终止搜索
|
||||
else
|
||||
{ //若当前层不是第n-1层
|
||||
k++; /* move up 退后一层，即从第k层退到第k+1层*/
|
||||
z[k] += step[k]; /* next valid integer 计算退后一层后，当前层的下一个有效备选解*/
|
||||
y = zb[k] - z[k];
|
||||
step[k] = -step[k] - SGN(step[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
// 对s中的目标函数值及zn中的候选解进行排序（以s中目标函数值为排序标准，进行升序排序）
|
||||
// RTKLIB中最终可以得到一个最优解一个次优解，存在zn中，两解对应的目标函数值，存在s中
|
||||
for (i = 0; i < m - 1; i++)
|
||||
{ /* sort by s */
|
||||
for (j = i + 1; j < m; j++)
|
||||
{
|
||||
if (s[i] < s[j])
|
||||
continue;
|
||||
SWAP(s[i], s[j]);
|
||||
for (k = 0; k < n; k++)
|
||||
SWAP(zn[k + i * n], zn[k + j * n]);
|
||||
}
|
||||
}
|
||||
free(S);
|
||||
free(dist);
|
||||
free(zb);
|
||||
free(z);
|
||||
free(step);
|
||||
|
||||
if (c >= LOOPMAX)
|
||||
{
|
||||
fprintf(stderr, "%s : search loop count overflow\n", __FILE__);
|
||||
return -2;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/* lambda/mlambda integer least-square estimation ------------------------------
|
||||
* integer least-square estimation. reduction is performed by lambda (ref.[1]),
|
||||
* and search by mlambda (ref.[2]).
|
||||
* args :
|
||||
* rtk_t* rtk I
|
||||
* int n I number of float parameters
|
||||
* int m I number of fixed solutions
|
||||
* double *a I float parameters (n x 1) (double-diff phase biases)
|
||||
* double *Q I covariance matrix of float parameters (n x n)
|
||||
* double *F O fixed solutions (n x m)
|
||||
* double *s O sum of squared residulas of fixed solutions (1 x m)
|
||||
* return : status (0:ok,other:error)
|
||||
* notes : matrix stored by column-major order (fortran convension)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int lambda(rtk_t* rtk, int n, int m, const double *a, const double *Q, double *F,
|
||||
double *s)
|
||||
{
|
||||
int info;
|
||||
double *L, *D, *Z, *z, *E;
|
||||
if (n <= 0 || m <= 0)
|
||||
return -1;
|
||||
L = zeros(n, n);
|
||||
D = mat(n, 1);
|
||||
Z = eye(n);
|
||||
z = mat(n, 1);
|
||||
E = mat(n, m);
|
||||
|
||||
/* LD (lower diaganol) factorization (Q=L'*diag(D)*L) */
|
||||
if (!(info = LD(n, Q, L, D)))
|
||||
{
|
||||
|
||||
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) */
|
||||
reduction(n, L, D, Z);
|
||||
matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z); /* z=Z'*a */
|
||||
/* mlambda search
|
||||
z = transformed double-diff phase biases
|
||||
L,D = transformed covariance matrix */
|
||||
if (!(info = search(n, m, L, D, z, E, s)))
|
||||
{ /* returns 0 if no error */
|
||||
|
||||
/* 将在新空间中固定的模糊度逆变换回双差模糊度空间中 */
|
||||
info = solve("T", Z, E, n, m, F); /* F=Z'\E */
|
||||
}
|
||||
}
|
||||
free(L);
|
||||
free(D);
|
||||
free(Z);
|
||||
free(z);
|
||||
free(E);
|
||||
return info;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* lambda reduction ------------------------------------------------------------
|
||||
* reduction by lambda (ref [1]) for integer least square
|
||||
* args : int n I number of float parameters
|
||||
* double *Q I covariance matrix of float parameters (n x n)
|
||||
* double *Z O lambda reduction matrix (n x n)
|
||||
* return : status (0:ok,other:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int lambda_reduction(int n, const double *Q, double *Z)
|
||||
{
|
||||
double *L, *D;
|
||||
int i, j, info;
|
||||
|
||||
if (n <= 0)
|
||||
return -1;
|
||||
|
||||
L = zeros(n, n);
|
||||
D = mat(n, 1);
|
||||
|
||||
for (i = 0; i < n; i++)
|
||||
for (j = 0; j < n; j++)
|
||||
{
|
||||
Z[i + j * n] = i == j ? 1.0 : 0.0;
|
||||
}
|
||||
/* LD factorization */
|
||||
if ((info = LD(n, Q, L, D)))
|
||||
{
|
||||
free(L);
|
||||
free(D);
|
||||
return info;
|
||||
}
|
||||
/* lambda reduction */
|
||||
reduction(n, L, D, Z);
|
||||
|
||||
free(L);
|
||||
free(D);
|
||||
return 0;
|
||||
}
|
||||
/* mlambda search --------------------------------------------------------------
|
||||
* search by mlambda (ref [2]) for integer least square
|
||||
* args : int n I number of float parameters
|
||||
* int m I number of fixed solutions
|
||||
* double *a I float parameters (n x 1)
|
||||
* double *Q I covariance matrix of float parameters (n x n)
|
||||
* double *F O fixed solutions (n x m)
|
||||
* double *s O sum of squared residulas of fixed solutions (1 x m)
|
||||
* return : status (0:ok,other:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int lambda_search(int n, int m, const double *a, const double *Q,
|
||||
double *F, double *s)
|
||||
{
|
||||
double *L, *D;
|
||||
int info;
|
||||
|
||||
if (n <= 0 || m <= 0)
|
||||
return -1;
|
||||
|
||||
L = zeros(n, n);
|
||||
D = mat(n, 1);
|
||||
|
||||
/* LD factorization */
|
||||
if ((info = LD(n, Q, L, D)))
|
||||
{
|
||||
free(L);
|
||||
free(D);
|
||||
return info;
|
||||
}
|
||||
/* mlambda search */
|
||||
info = search(n, m, L, D, a, F, s);
|
||||
|
||||
free(L);
|
||||
free(D);
|
||||
return info;
|
||||
}
|
||||
464
RTK/lambda_par.c
Normal file
464
RTK/lambda_par.c
Normal file
@ -0,0 +1,464 @@
|
||||
#include "rtklib.h"
|
||||
|
||||
#define LAMBDA_P0 0.80
|
||||
#define MIN_SAT 6
|
||||
|
||||
#define LOOPMAX 10000 /* maximum count of search loop */
|
||||
#define SGN(x) ((x) <= 0.0 ? -1.0 : 1.0)
|
||||
#define ROUND(x) (floor((x) + 0.5))
|
||||
#define SWAP(x, y) \
|
||||
do \
|
||||
{ \
|
||||
double tmp_; \
|
||||
tmp_ = x; \
|
||||
x = y; \
|
||||
y = tmp_; \
|
||||
} while (0)
|
||||
|
||||
static int parsearch(rtk_t* rtk, int n, const double* zhat, const double* Qzhat, const double* Z,
|
||||
const double* L, const double* D, int m, double* F, double* s);
|
||||
|
||||
/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
|
||||
static int LD(int n, const double* Q, double* L, double* D)
|
||||
{
|
||||
int i, j, k, info = 0;
|
||||
double a, * A = mat(n, n);
|
||||
|
||||
memcpy(A, Q, sizeof(double) * n * n);
|
||||
for (i = n - 1; i >= 0; i--)
|
||||
{
|
||||
if ((D[i] = A[i + i * n]) <= 0.0)
|
||||
{
|
||||
info = -1;
|
||||
break;
|
||||
}
|
||||
a = sqrt(D[i]);
|
||||
for (j = 0; j <= i; j++)
|
||||
L[i + j * n] = A[i + j * n] / a;
|
||||
for (j = 0; j <= i - 1; j++)
|
||||
for (k = 0; k <= j; k++)
|
||||
A[j + k * n] -= L[i + k * n] * L[i + j * n];
|
||||
for (j = 0; j <= i; j++)
|
||||
L[i + j * n] /= L[i + i * n];
|
||||
}
|
||||
free(A);
|
||||
if (info)
|
||||
fprintf(stderr, "%s : LD factorization error\n", __FILE__);
|
||||
return info;
|
||||
}
|
||||
/* integer gauss transformation ----------------------------------------------*/
|
||||
static void gauss(int n, double* L, double* Z, int i, int j)
|
||||
{
|
||||
int k, mu;
|
||||
|
||||
if ((mu = (int)ROUND(L[i + j * n])) != 0)
|
||||
{
|
||||
for (k = i; k < n; k++)
|
||||
L[k + n * j] -= (double)mu * L[k + i * n];
|
||||
for (k = 0; k < n; k++)
|
||||
Z[k + n * j] -= (double)mu * Z[k + i * n];
|
||||
}
|
||||
}
|
||||
/* permutations --------------------------------------------------------------*/
|
||||
static void perm(int n, double* L, double* D, int j, double del, double* Z)
|
||||
{
|
||||
int k;
|
||||
double eta, lam, a0, a1;
|
||||
|
||||
eta = D[j] / del;
|
||||
lam = D[j + 1] * L[j + 1 + j * n] / del;
|
||||
D[j] = eta * D[j + 1];
|
||||
D[j + 1] = del;
|
||||
for (k = 0; k <= j - 1; k++)
|
||||
{
|
||||
a0 = L[j + k * n];
|
||||
a1 = L[j + 1 + k * n];
|
||||
L[j + k * n] = -L[j + 1 + j * n] * a0 + a1;
|
||||
L[j + 1 + k * n] = eta * a0 + lam * a1;
|
||||
}
|
||||
L[j + 1 + j * n] = lam;
|
||||
for (k = j + 2; k < n; k++)
|
||||
SWAP(L[k + j * n], L[k + (j + 1) * n]);
|
||||
for (k = 0; k < n; k++)
|
||||
SWAP(Z[k + j * n], Z[k + (j + 1) * n]);
|
||||
}
|
||||
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
|
||||
static void reduction(int n, double* L, double* D, double* Z)
|
||||
{
|
||||
int i, j, k;
|
||||
double del;
|
||||
|
||||
j = n - 2;
|
||||
k = n - 2;
|
||||
//这里的调序变换类似插入排序的思路？
|
||||
while (j >= 0)
|
||||
{
|
||||
//由于第k+1，k+2，...，n-2列都进行过降相关并且没有被上一次调序变换影响，
|
||||
//因此只需对第0,1，...，k-1，k列进行降相关
|
||||
if (j <= k)
|
||||
for (i = j + 1; i < n; i++)
|
||||
gauss(n, L, Z, i, j); //从最后一列开始，各列非对角线元素从上往下依次降相关
|
||||
del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1];
|
||||
if (del + 1E-6 < D[j + 1])
|
||||
{ /* 检验条件，若不满足检验条件则开始进行调序变换compared considering numerical error */
|
||||
perm(n, L, D, j, del, Z); //调序变换
|
||||
k = j;
|
||||
j = n - 2; //完成调序变换后重新从最后一列开始进行降相关及排序，k记录最后一次进行过调序变换的列序号
|
||||
}
|
||||
else
|
||||
j--;
|
||||
}
|
||||
}
|
||||
/* modified lambda (mlambda) search (ref. [2]) -------------------------------
|
||||
* args : n I number of float parameters
|
||||
* m I number of fixed solution
|
||||
L,D I transformed covariance matrix
|
||||
zs I transformed double-diff phase biases
|
||||
zn O fixed solutions
|
||||
s O sum of residuals for fixed solutions */
|
||||
static int search(int n, int m, const double* L, const double* D,
|
||||
const double* zs, double* zn, double* s)
|
||||
{
|
||||
int i, j, k, c, nn = 0, imax = 0;
|
||||
double newdist, maxdist = 1E99, y; // maxdist，当前超椭圆半径
|
||||
double* S = zeros(n, n), * dist = mat(n, 1), * zb = mat(n, 1), * z = mat(n, 1), * step = mat(n, 1);
|
||||
|
||||
k = n - 1;
|
||||
dist[k] = 0.0; // k表示当前层，从最后一层（n-1）开始计算
|
||||
zb[k] = zs[k]; //即zn
|
||||
z[k] = ROUND(zb[k]); //四舍五入取整；取整后的数与未取整的数作差；step记录z[k]是四舍还是五入
|
||||
y = zb[k] - z[k];
|
||||
step[k] = SGN(y); /* step towards closest integer */
|
||||
for (c = 0; c < LOOPMAX; c++)
|
||||
{
|
||||
newdist = dist[k] + y * y / D[k]; /* newdist=sum(((z(j)-zb(j))^2/d(j))) */
|
||||
if (newdist < maxdist)
|
||||
{ //如果当前累积目标函数计算值小于当前超椭圆半径
|
||||
/* Case 1: move down 情况1：若还未计算至第一层，继续计算累积目标函数值 */
|
||||
if (k != 0)
|
||||
{
|
||||
dist[--k] = newdist; //记录下当前层的累积目标函数值，dist[k]表示了第k,k+1,...,n-1层的目标函数计算和
|
||||
for (i = 0; i <= k; i++)
|
||||
S[k + i * n] = S[k + 1 + i * n] + (z[k + 1] - zb[k + 1]) * L[k + 1 + i * n];
|
||||
zb[k] = zs[k] + S[k + k * n]; //计算Zk，即第k个整数模糊度参数的备选组的中心
|
||||
z[k] = ROUND(zb[k]); /* next valid integer 四舍五入取整；取整后的数与未取整的数作差；记录是四舍还是五入*/
|
||||
y = zb[k] - z[k];
|
||||
step[k] = SGN(y);
|
||||
}
|
||||
/* Case 2: store the found candidate and try next valid integer 情况2：若已经计算至第一层，意味着所有层的累积目标函数值计算完毕 */
|
||||
else
|
||||
{
|
||||
// nn为当前候选解数，m为我们需要的固定解数，这里为2，表示需要一个最优解及一个次优解
|
||||
// s记录候选解的目标函数值，imax记录之前候选解中的最大目标函数值的坐标
|
||||
if (nn < m)
|
||||
{ //若候选解数还没满
|
||||
/* store the first m initial points */
|
||||
if (nn == 0 || newdist > s[imax])
|
||||
imax = nn; //若当前解的目标函数值比之前最大的目标函数值都大，那么更新imax使s[imax]指向当前解中具有的最大目标函数值
|
||||
for (i = 0; i < n; i++)
|
||||
zn[i + nn * n] = z[i]; // zn存放所有候选解
|
||||
s[nn++] = newdist; // s记录当前目标函数值newdist，并加加当前候选解数nn
|
||||
}
|
||||
else
|
||||
{ //若候选解数已满（即当前zn中已经存了2个候选解）
|
||||
if (newdist < s[imax])
|
||||
{ //若 当前解的目标函数值 比 s中的最大目标函数值 小
|
||||
for (i = 0; i < n; i++)
|
||||
zn[i + imax * n] = z[i]; //用当前解替换zn中具有较大目标函数值的解
|
||||
s[imax] = newdist; //用当前解的目标函数值替换s中的最大目标函数值
|
||||
for (i = imax = 0; i < m; i++)
|
||||
if (s[imax] < s[i])
|
||||
imax = i; //更新imax保证imax始终指向s中的最大目标函数值
|
||||
}
|
||||
maxdist = s[imax]; //用当前最大的目标函数值更新超椭圆半径
|
||||
}
|
||||
z[0] += step[0]; /* next valid integer在第一层，取下一个有效的整数模糊度参数进行计算（若zb为5.3，则z取值顺序为5,6,4,7，...） */
|
||||
y = zb[0] - z[0];
|
||||
step[0] = -step[0] - SGN(step[0]);
|
||||
}
|
||||
}
|
||||
/* Case 3: exit or move up情况3：如果当前累积目标函数计算值大于当前超椭圆半径 */
|
||||
else
|
||||
{
|
||||
if (k == n - 1)
|
||||
break; //如果当前层为第n-1层，意味着后续目标函数各项的计算都会超出超椭圆半径，因此终止搜索
|
||||
else
|
||||
{ //若当前层不是第n-1层
|
||||
k++; /* move up 退后一层，即从第k层退到第k+1层*/
|
||||
z[k] += step[k]; /* next valid integer 计算退后一层后，当前层的下一个有效备选解*/
|
||||
y = zb[k] - z[k];
|
||||
step[k] = -step[k] - SGN(step[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
// 对s中的目标函数值及zn中的候选解进行排序（以s中目标函数值为排序标准，进行升序排序）
|
||||
// RTKLIB中最终可以得到一个最优解一个次优解，存在zn中，两解对应的目标函数值，存在s中
|
||||
for (i = 0; i < m - 1; i++)
|
||||
{ /* sort by s */
|
||||
for (j = i + 1; j < m; j++)
|
||||
{
|
||||
if (s[i] < s[j])
|
||||
continue;
|
||||
SWAP(s[i], s[j]);
|
||||
for (k = 0; k < n; k++)
|
||||
SWAP(zn[k + i * n], zn[k + j * n]);
|
||||
}
|
||||
}
|
||||
free(S);
|
||||
free(dist);
|
||||
free(zb);
|
||||
free(z);
|
||||
free(step);
|
||||
|
||||
if (c >= LOOPMAX)
|
||||
{
|
||||
fprintf(stderr, "%s : search loop count overflow\n", __FILE__);
|
||||
return -2;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
static float my_normcdf(float a) // 获取正态分布CDF
|
||||
{
|
||||
float h, l, r;
|
||||
const float SQRT_HALF_HI = 0x1.6a09e6p-01f; // 7.07106769e-1
|
||||
const float SQRT_HALF_LO = 0x1.9fcef4p-27f; // 1.21016175e-8
|
||||
/* clamp input as normcdf(x) is either 0 or 1 asymptotically */
|
||||
if (fabsf(a) > 14.171875f)
|
||||
a = (a < 0.0f) ? -14.171875f : 14.171875f;
|
||||
h = fmaf(-SQRT_HALF_HI, a, -SQRT_HALF_LO * a);
|
||||
l = fmaf(SQRT_HALF_LO, a, fmaf(SQRT_HALF_HI, a, h));
|
||||
r = erfcf(h);
|
||||
if (h > 0.0f)
|
||||
r = fmaf(2.0f * h * l, r, r);
|
||||
return 0.5f * r;
|
||||
}
|
||||
|
||||
static double* nmatgetkmat_RD(const double* nmat, int n1, int n2, int m1, int m2)
|
||||
{
|
||||
double* mmat;
|
||||
int i, j;
|
||||
mmat = mat(m1, m2);
|
||||
//从大矩阵里取小矩阵
|
||||
for (i = 0; i < m1; i++)
|
||||
for (j = 0; j < m2; j++)
|
||||
{
|
||||
*(mmat + i * m2 + j) = *(nmat + (n1 - m1) * n2 + (n2 - m2) + i * n2 + j);
|
||||
}
|
||||
|
||||
return mmat;
|
||||
}
|
||||
|
||||
static double* nmatgetkmat_RU(const double* nmat, int n1, int n2, int m1, int m2)
|
||||
{
|
||||
double* mmat;
|
||||
int i, j;
|
||||
mmat = mat(m1, m2);
|
||||
//从大矩阵里取小矩阵
|
||||
for (i = 0; i < m1; i++)
|
||||
for (j = 0; j < m2; j++)
|
||||
{
|
||||
*(mmat + i * m2 + j) = *(nmat + (n2 - m2) + i * n2 + j);
|
||||
}
|
||||
|
||||
return mmat;
|
||||
}
|
||||
/* parlambda ------------------------------
|
||||
* integer least-square estimation. reduction is performed by lambda (ref.[1]),
|
||||
* and search by mlambda (ref.[2]).
|
||||
* args :
|
||||
* rtk_t* rtk I
|
||||
* int n I number of float parameters
|
||||
* int m I number of fixed solutions
|
||||
* double *a I float parameters (n x 1) (double-diff phase biases)
|
||||
* double *Q I covariance matrix of float parameters (n x n)
|
||||
* double *F O fixed solutions (n x m)
|
||||
* double *s O sum of squared residulas of fixed solutions (1 x m)
|
||||
* return : status (0:ok,other:error)
|
||||
* notes : matrix stored by column-major order (fortran convension)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int parlambda(rtk_t* rtk, int n, int m, const double* a, const double* Q, double* F,
|
||||
double* s)
|
||||
{
|
||||
int info;
|
||||
double* L, * D, * Z, * z, * E, * Qt, * Qzhat;
|
||||
if (n <= 0 || m <= 0)
|
||||
return -1;
|
||||
L = zeros(n, n);
|
||||
D = mat(n, 1);
|
||||
Z = eye(n);
|
||||
z = mat(n, 1);
|
||||
E = mat(n, m);
|
||||
|
||||
/* LD (lower diaganol) factorization (Q=L'*diag(D)*L) */
|
||||
if (!(info = LD(n, Q, L, D)))
|
||||
{
|
||||
|
||||
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) */
|
||||
reduction(n, L, D, Z);
|
||||
matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z); /* z=Z'*a */
|
||||
|
||||
/*par search*/
|
||||
// Qz = Z'*Q*Z
|
||||
Qt = mat(n, n);
|
||||
Qzhat = mat(n, n);
|
||||
matmul("TN", n, n, n, 1.0, Z, Q, 0.0, Qt); // Qt = Z'*Q
|
||||
matmul("NN", n, n, n, 1.0, Qt, Z, 0.0, Qzhat); // Qz =QtZ
|
||||
if (!(info = parsearch(rtk, n, z, Qzhat, Z, L, D, 2, F, s)))
|
||||
{
|
||||
};
|
||||
}
|
||||
free(L);
|
||||
free(D);
|
||||
free(Z);
|
||||
free(z);
|
||||
free(E);
|
||||
return info;
|
||||
}
|
||||
|
||||
/*parsearch
|
||||
*by LongRui Peng&ZiWen Qu from ZJUT
|
||||
args:
|
||||
rtk_t *rtk I
|
||||
int n I number of float parameter
|
||||
const double *zhat I decorrelated float ambiguities
|
||||
const double *Qzhat I variance-covariance matrix of decorrelated float ambiguities
|
||||
const double *Z I Z-matrix from decorrel
|
||||
L,D I lower-triangular and diagonal matrix from LtDL-decomposition of Qzhat
|
||||
m I Number of requested integer candidate vectors [DEFAULT=2]
|
||||
double *F O fixed solutions (n x m)
|
||||
double *s O sum of squared residulas of fixed solutions (1 x m)
|
||||
|
||||
*/
|
||||
static int parsearch(rtk_t* rtk, int n, const double* zhat, const double* Qzhat, const double* Z,
|
||||
const double* L, const double* D, int m, double* F, double* s)
|
||||
|
||||
{
|
||||
int info=0, p, k, i, j;
|
||||
float Ps = 1; /* Ps ：cumulative success rate P0:Minimum required sucess rate [DEFAULT=0.8] */
|
||||
|
||||
double* zk_n, * Lk_n, * Dk_n, * z1_fix, * Q11, * Q12, * Qp, * z2_fix, * z_t1, * z_t2, * z_fix;
|
||||
for (k = n; k > 2; k--)
|
||||
{
|
||||
if (Ps > LAMBDA_P0)
|
||||
{
|
||||
Ps = Ps * (2 * my_normcdf((float)(1 / (2 * sqrt(20 * D[k - 1])))) - 1);
|
||||
}
|
||||
else
|
||||
break;
|
||||
}
|
||||
// Lk_n: new matrix formed by [k:n,k:n] of L
|
||||
// Dk_n: new diag formed by [k:n] row of D
|
||||
// zk_n: new column formed by [k:n] element of zhat
|
||||
// Lk_n: new matrix formed by [k:n,k:n] of L
|
||||
// Dk_n: new diag formed by [k:n] row of D
|
||||
// zk_n: new column formed by [k:n] element of zhat
|
||||
p = n - k + 1;
|
||||
|
||||
while (p >= MIN_SAT)
|
||||
{
|
||||
Dk_n = mat(p, 1);
|
||||
zk_n = mat(p, 1);
|
||||
Lk_n = mat(p, p);
|
||||
z1_fix = mat(p, m);
|
||||
j = k - 1;
|
||||
for (i = 0; i < p; i++)
|
||||
{
|
||||
Dk_n[i] = D[j];
|
||||
zk_n[i] = zhat[j];
|
||||
j++;
|
||||
}
|
||||
Lk_n = nmatgetkmat_RD(L, n, n, p, p);
|
||||
|
||||
if (!(info = search(p, m, Lk_n, Dk_n, zk_n, z1_fix, s))) /* returns 0 if no error */
|
||||
{
|
||||
if (s[0] <= 0.0 || s[1] / s[0] >= rtk->sol.thres) // Ratio test
|
||||
{
|
||||
Q12 = mat(k - 1, p);
|
||||
Q11 = mat(p, p);
|
||||
Qp = mat(k - 1, p);
|
||||
z2_fix = mat(k - 1, m);
|
||||
z_t1 = mat(p, m);
|
||||
z_t2 = mat(k - 1, m);
|
||||
z_fix = mat(n, m);
|
||||
// first k - 1 ambiguities are adjusted based on correlation with the fixed ambiguities
|
||||
Q11 = nmatgetkmat_RD(Qzhat, n, n, p, p);
|
||||
Q12 = nmatgetkmat_RU(Qzhat, n, n, k - 1, p);
|
||||
matinv(Q11, p);
|
||||
matmul("NN", k - 1, p, p, 1.0, Q12, Q11, 0.0, Qp); // Qp = Q12 / Q11;
|
||||
for (i = 0; i < 2 * p; i++)
|
||||
{
|
||||
if (i < p)
|
||||
z_t1[i] = zk_n[i] - z1_fix[i];
|
||||
else
|
||||
z_t1[i] = zk_n[i - p] - z1_fix[i];
|
||||
}
|
||||
|
||||
matmul("NN", k - 1, m, p, 1.0, Qp, z_t1, 0.0, z_t2); // zt_2=Qp(zk_n - z1_fix)
|
||||
/*z2_fix = z2_float - Qp(zk_n - z1_fix);*/
|
||||
for (i = 0; i < 2 * k - 2; i++)
|
||||
{
|
||||
if (i < k - 1)
|
||||
z2_fix[i] = zhat[i] - z_t2[i];
|
||||
else
|
||||
z2_fix[i] = zhat[i - k + 1] - z_t2[i];
|
||||
}
|
||||
//for (i = 0; i < 2 * k - 2; i++)
|
||||
//{
|
||||
// z2_fix[i] = ROUND(z2_fix[i]);
|
||||
//}
|
||||
/* z_fix = [z2_fix, z1_fix]; */
|
||||
for (i = 0; i < 2 * n; i++)
|
||||
{
|
||||
if (i < k - 1)
|
||||
z_fix[i] = z2_fix[i];
|
||||
else if (i < n)
|
||||
z_fix[i] = z1_fix[i - k + 1];
|
||||
else if (i < k - 1 + n)
|
||||
z_fix[i] = z2_fix[i - n + k - 1];
|
||||
else
|
||||
z_fix[i] = z1_fix[i - 2 * k + 2];
|
||||
}
|
||||
|
||||
/* 将在新空间中固定的模糊度逆变换回双差模糊度空间中 */
|
||||
info = solve("T", Z, z_fix, n, m, F); /* F=Z'\E */
|
||||
free(Dk_n);
|
||||
free(zk_n);
|
||||
free(Lk_n);
|
||||
free(z1_fix);
|
||||
free(Q12);
|
||||
free(Q11);
|
||||
free(Qp);
|
||||
free(z2_fix);
|
||||
free(z_t1);
|
||||
free(z_t2);
|
||||
free(z_fix);
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
trace(3, "PAR ratio test failed ! ,remove the ambiguity with the largest variance \n");
|
||||
p--;
|
||||
k++;
|
||||
free(Dk_n);
|
||||
free(zk_n);
|
||||
free(Lk_n);
|
||||
free(z1_fix);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (p < MIN_SAT) // set the minimum number of part ambiguities 6
|
||||
{
|
||||
trace(3, "PAR lack sat! Now:%d output float solution... \n", p);
|
||||
z_fix = mat(n, m);
|
||||
if (!(info = search(n, m, L, D, zhat, z_fix, s)))
|
||||
{ /* returns 0 if no error */
|
||||
|
||||
/* 将在新空间中固定的模糊度逆变换回双差模糊度空间中 */
|
||||
info = solve("T", Z, z_fix, n, m, F); /* F=Z'\E */
|
||||
}
|
||||
free(z_fix);
|
||||
info = 0;
|
||||
}
|
||||
|
||||
return info;
|
||||
}
|
||||
793
RTK/options.c
Normal file
793
RTK/options.c
Normal file
@ -0,0 +1,793 @@
|
||||
#include "rtklib.h"
|
||||
|
||||
/* system options buffer -----------------------------------------------------*/
|
||||
prcopt_t prcopt_;
|
||||
solopt_t solopt_;
|
||||
// static filopt_t filopt_;
|
||||
static double elmask_, elmaskar_, elmaskhold_;
|
||||
static int antpostype_[2]; /* 数据流天线位置格式 */
|
||||
static double antpos_[2][3];
|
||||
|
||||
#ifdef USING_FILE_OPT
|
||||
static char exsats_[1024];
|
||||
static char snrmask_[NFREQ][1024];
|
||||
|
||||
/* system options table ------------------------------------------------------*/
|
||||
#define SWTOPT "0:off,1:on"
|
||||
#define MODOPT "0:single,1:dgps,2:kinematic,3:static,4:static-start,5:movingbase,6:fixed,7:ppp-kine,8:ppp-static,9:ppp-fixed"
|
||||
#define FRQOPT "1:l1,2:l1+l2,3:l1+l2+l5,4:l1+l2+l5+l6"
|
||||
#define TYPOPT "0:forward,1:backward,2:combined,3:combined-nophasereset"
|
||||
#define IONOPT "0:off,1:brdc,2:sbas,3:dual-freq,4:est-stec,5:ionex-tec,6:qzs-brdc"
|
||||
#define TRPOPT "0:off,1:saas,2:sbas,3:est-ztd,4:est-ztdgrad"
|
||||
#define EPHOPT "0:brdc,1:precise,2:brdc+sbas,3:brdc+ssrapc,4:brdc+ssrcom"
|
||||
#define NAVOPT "1:gps+2:sbas+4:glo+8:gal+16:qzs+32:bds+64:navic"
|
||||
#define GAROPT "0:off,1:on,2:autocal,3:fix-and-hold"
|
||||
#define WEIGHTOPT "0:elevation,1:snr"
|
||||
#define SOLOPT "0:llh,1:xyz,2:enu,3:nmea"
|
||||
#define TSYOPT "0:gpst,1:utc,2:jst"
|
||||
#define TFTOPT "0:tow,1:hms"
|
||||
#define DFTOPT "0:deg,1:dms"
|
||||
#define HGTOPT "0:ellipsoidal,1:geodetic"
|
||||
#define GEOOPT "0:internal,1:egm96,2:egm08_2.5,3:egm08_1,4:gsi2000"
|
||||
#define STAOPT "0:all,1:single"
|
||||
#define STSOPT "0:off,1:state,2:residual"
|
||||
#define ARMOPT "0:off,1:continuous,2:instantaneous,3:fix-and-hold"
|
||||
#define POSOPT "0:llh,1:xyz,2:single,3:posfile,4:rinexhead,5:rtcm,6:raw"
|
||||
#define TIDEOPT "0:off,1:on,2:otl"
|
||||
#define PHWOPT "0:off,1:on,2:precise"
|
||||
|
||||
extern opt_t sysopts[] = {
|
||||
{"pos1-posmode", 3, (void *)&prcopt_.mode, MODOPT},
|
||||
{"pos1-frequency", 3, (void *)&prcopt_.nf, FRQOPT},
|
||||
{"pos1-soltype", 3, (void *)&prcopt_.soltype, TYPOPT},
|
||||
{"pos1-elmask", 1, (void *)&elmask_, "deg"},
|
||||
{"pos1-snrmask_r", 3, (void *)&prcopt_.snrmask.ena[0], SWTOPT},
|
||||
{"pos1-snrmask_b", 3, (void *)&prcopt_.snrmask.ena[1], SWTOPT},
|
||||
{"pos1-snrmask_L1", 2, (void *)snrmask_[0], ""},
|
||||
{"pos1-snrmask_L2", 2, (void *)snrmask_[1], ""},
|
||||
{"pos1-snrmask_L5", 2, (void *)snrmask_[2], ""},
|
||||
{"pos1-dynamics", 3, (void *)&prcopt_.dynamics, SWTOPT},
|
||||
{"pos1-tidecorr", 3, (void *)&prcopt_.tidecorr, TIDEOPT},
|
||||
{"pos1-ionoopt", 3, (void *)&prcopt_.ionoopt, IONOPT},
|
||||
{"pos1-tropopt", 3, (void *)&prcopt_.tropopt, TRPOPT},
|
||||
{"pos1-sateph", 3, (void *)&prcopt_.sateph, EPHOPT},
|
||||
{"pos1-posopt1", 3, (void *)&prcopt_.posopt[0], SWTOPT},
|
||||
{"pos1-posopt2", 3, (void *)&prcopt_.posopt[1], SWTOPT},
|
||||
{"pos1-posopt3", 3, (void *)&prcopt_.posopt[2], PHWOPT},
|
||||
{"pos1-posopt4", 3, (void *)&prcopt_.posopt[3], SWTOPT},
|
||||
{"pos1-posopt5", 3, (void *)&prcopt_.posopt[4], SWTOPT},
|
||||
{"pos1-posopt6", 3, (void *)&prcopt_.posopt[5], SWTOPT},
|
||||
{"pos1-exclsats", 2, (void *)exsats_, "prn ..."},
|
||||
{"pos1-navsys", 0, (void *)&prcopt_.navsys, NAVOPT},
|
||||
|
||||
{"pos2-armode", 3, (void *)&prcopt_.modear, ARMOPT},
|
||||
{"pos2-gloarmode", 3, (void *)&prcopt_.glomodear, GAROPT},
|
||||
{"pos2-bdsarmode", 3, (void *)&prcopt_.bdsmodear, SWTOPT},
|
||||
{"pos2-arfilter", 3, (void *)&prcopt_.arfilter, SWTOPT},
|
||||
{"pos2-arthres", 1, (void *)&prcopt_.thresar[0], ""},
|
||||
{"pos2-arthresmin", 1, (void *)&prcopt_.thresar[5], ""},
|
||||
{"pos2-arthresmax", 1, (void *)&prcopt_.thresar[6], ""},
|
||||
{"pos2-arthres1", 1, (void *)&prcopt_.thresar[1], ""},
|
||||
{"pos2-arthres2", 1, (void *)&prcopt_.thresar[2], ""},
|
||||
{"pos2-arthres3", 1, (void *)&prcopt_.thresar[3], ""},
|
||||
{"pos2-arthres4", 1, (void *)&prcopt_.thresar[4], ""},
|
||||
{"pos2-varholdamb", 1, (void *)&prcopt_.varholdamb, "cyc^2"},
|
||||
{"pos2-gainholdamb", 1, (void *)&prcopt_.gainholdamb, ""},
|
||||
{"pos2-arlockcnt", 0, (void *)&prcopt_.minlock, ""},
|
||||
{"pos2-minfixsats", 0, (void *)&prcopt_.minfixsats, ""},
|
||||
{"pos2-minholdsats", 0, (void *)&prcopt_.minholdsats, ""},
|
||||
{"pos2-mindropsats", 0, (void *)&prcopt_.mindropsats, ""},
|
||||
{"pos2-arelmask", 1, (void *)&elmaskar_, "deg"},
|
||||
{"pos2-arminfix", 0, (void *)&prcopt_.minfix, ""},
|
||||
{"pos2-armaxiter", 0, (void *)&prcopt_.armaxiter, ""},
|
||||
{"pos2-elmaskhold", 1, (void *)&elmaskhold_, "deg"},
|
||||
{"pos2-aroutcnt", 0, (void *)&prcopt_.maxout, ""},
|
||||
{"pos2-maxage", 1, (void *)&prcopt_.maxtdiff, "s"},
|
||||
{"pos2-syncsol", 3, (void *)&prcopt_.syncsol, SWTOPT},
|
||||
{"pos2-slipthres", 1, (void *)&prcopt_.thresslip, "m"},
|
||||
{"pos2-dopthres", 1, (void *)&prcopt_.thresdop, "m"},
|
||||
{"pos2-rejionno", 1, (void *)&prcopt_.maxinno, "m"},
|
||||
{"pos2-rejgdop", 1, (void *)&prcopt_.maxgdop, ""},
|
||||
{"pos2-niter", 0, (void *)&prcopt_.niter, ""},
|
||||
{"pos2-baselen", 1, (void *)&prcopt_.baseline[0], "m"},
|
||||
{"pos2-basesig", 1, (void *)&prcopt_.baseline[1], "m"},
|
||||
|
||||
{"out-solformat", 3, (void *)&solopt_.posf, SOLOPT},
|
||||
{"out-outhead", 3, (void *)&solopt_.outhead, SWTOPT},
|
||||
{"out-outopt", 3, (void *)&solopt_.outopt, SWTOPT},
|
||||
{"out-outvel", 3, (void *)&solopt_.outvel, SWTOPT},
|
||||
{"out-timesys", 3, (void *)&solopt_.times, TSYOPT},
|
||||
{"out-timeform", 3, (void *)&solopt_.timef, TFTOPT},
|
||||
{"out-timendec", 0, (void *)&solopt_.timeu, ""},
|
||||
{"out-degform", 3, (void *)&solopt_.degf, DFTOPT},
|
||||
{"out-fieldsep", 2, (void *)solopt_.sep, ""},
|
||||
{"out-outsingle", 3, (void *)&prcopt_.outsingle, SWTOPT},
|
||||
{"out-maxsolstd", 1, (void *)&solopt_.maxsolstd, "m"},
|
||||
{"out-height", 3, (void *)&solopt_.height, HGTOPT},
|
||||
{"out-geoid", 3, (void *)&solopt_.geoid, GEOOPT},
|
||||
{"out-solstatic", 3, (void *)&solopt_.solstatic, STAOPT},
|
||||
{"out-nmeaintv1", 1, (void *)&solopt_.nmeaintv[0], "s"},
|
||||
{"out-nmeaintv2", 1, (void *)&solopt_.nmeaintv[1], "s"},
|
||||
{"out-outstat", 3, (void *)&solopt_.sstat, STSOPT},
|
||||
{"stats-eratio1", 1, (void *)&prcopt_.eratio[0], ""},
|
||||
{"stats-eratio2", 1, (void *)&prcopt_.eratio[1], ""},
|
||||
{"stats-eratio5", 1, (void *)&prcopt_.eratio[2], ""},
|
||||
{"stats-errphase", 1, (void *)&prcopt_.err[1], "m"},
|
||||
{"stats-errphaseel", 1, (void *)&prcopt_.err[2], "m"},
|
||||
{"stats-errphasebl", 1, (void *)&prcopt_.err[3], "m/10km"},
|
||||
{"stats-errdoppler", 1, (void *)&prcopt_.err[4], "Hz"},
|
||||
{"stats-snrmax", 1, (void *)&prcopt_.err[5], "dB.Hz"},
|
||||
{"stats-errsnr", 1, (void *)&prcopt_.err[6], "m"},
|
||||
{"stats-errrcv", 1, (void *)&prcopt_.err[7], " "},
|
||||
{"stats-stdbias", 1, (void *)&prcopt_.std[0], "m"},
|
||||
{"stats-stdiono", 1, (void *)&prcopt_.std[1], "m"},
|
||||
{"stats-stdtrop", 1, (void *)&prcopt_.std[2], "m"},
|
||||
{"stats-prnaccelh", 1, (void *)&prcopt_.prn[3], "m/s^2"},
|
||||
{"stats-prnaccelv", 1, (void *)&prcopt_.prn[4], "m/s^2"},
|
||||
{"stats-prnbias", 1, (void *)&prcopt_.prn[0], "m"},
|
||||
{"stats-prniono", 1, (void *)&prcopt_.prn[1], "m"},
|
||||
{"stats-prntrop", 1, (void *)&prcopt_.prn[2], "m"},
|
||||
{"stats-prnpos", 1, (void *)&prcopt_.prn[5], "m"},
|
||||
{"stats-clkstab", 1, (void *)&prcopt_.sclkstab, "s/s"},
|
||||
|
||||
{"ant1-postype", 3, (void *)&antpostype_[0], POSOPT},
|
||||
{"ant1-pos1", 1, (void *)&antpos_[0][0], "deg|m"},
|
||||
{"ant1-pos2", 1, (void *)&antpos_[0][1], "deg|m"},
|
||||
{"ant1-pos3", 1, (void *)&antpos_[0][2], "m|m"},
|
||||
{"ant1-anttype", 2, (void *)prcopt_.anttype[0], ""},
|
||||
{"ant1-antdele", 1, (void *)&prcopt_.antdel[0][0], "m"},
|
||||
{"ant1-antdeln", 1, (void *)&prcopt_.antdel[0][1], "m"},
|
||||
{"ant1-antdelu", 1, (void *)&prcopt_.antdel[0][2], "m"},
|
||||
|
||||
{"ant2-postype", 3, (void *)&antpostype_[1], POSOPT},
|
||||
{"ant2-pos1", 1, (void *)&antpos_[1][0], "deg|m"},
|
||||
{"ant2-pos2", 1, (void *)&antpos_[1][1], "deg|m"},
|
||||
{"ant2-pos3", 1, (void *)&antpos_[1][2], "m|m"},
|
||||
{"ant2-anttype", 2, (void *)prcopt_.anttype[1], ""},
|
||||
{"ant2-antdele", 1, (void *)&prcopt_.antdel[1][0], "m"},
|
||||
{"ant2-antdeln", 1, (void *)&prcopt_.antdel[1][1], "m"},
|
||||
{"ant2-antdelu", 1, (void *)&prcopt_.antdel[1][2], "m"},
|
||||
{"ant2-maxaveep", 0, (void *)&prcopt_.maxaveep, ""},
|
||||
{"ant2-initrst", 3, (void *)&prcopt_.initrst, SWTOPT},
|
||||
|
||||
{"misc-timeinterp", 3, (void *)&prcopt_.intpref, SWTOPT},
|
||||
{"misc-sbasatsel", 0, (void *)&prcopt_.sbassatsel, "0:all"},
|
||||
{"misc-rnxopt1", 2, (void *)prcopt_.rnxopt[0], ""},
|
||||
{"misc-rnxopt2", 2, (void *)prcopt_.rnxopt[1], ""},
|
||||
{"misc-pppopt", 2, (void *)prcopt_.pppopt, ""},
|
||||
|
||||
// {"file-satantfile", 2, (void *)&filopt_.satantp, "" },
|
||||
// {"file-rcvantfile", 2, (void *)&filopt_.rcvantp, "" },
|
||||
// {"file-staposfile", 2, (void *)&filopt_.stapos, "" },
|
||||
// {"file-geoidfile", 2, (void *)&filopt_.geoid, "" },
|
||||
// {"file-ionofile", 2, (void *)&filopt_.iono, "" },
|
||||
// {"file-dcbfile", 2, (void *)&filopt_.dcb, "" },
|
||||
// {"file-eopfile", 2, (void *)&filopt_.eop, "" },
|
||||
// {"file-blqfile", 2, (void *)&filopt_.blq, "" },
|
||||
// {"file-tempdir", 2, (void *)&filopt_.tempdir, "" },
|
||||
// {"file-geexefile", 2, (void *)&filopt_.geexe, "" },
|
||||
// {"file-solstatfile",2, (void *)&filopt_.solstat, "" },
|
||||
// {"file-tracefile", 2, (void *)&filopt_.trace, "" },
|
||||
|
||||
{"", 0, NULL, ""} /* terminator */
|
||||
};
|
||||
#endif
|
||||
const prcopt_t prcopt_default = {
|
||||
/* defaults processing options */
|
||||
PMODE_KINEMA, /* positioning mode (PMODE_???) */
|
||||
0, /* solution type (0:forward,1:backward,2:combined) */
|
||||
1, /* number of frequencies (1:L1,2:L1+L2,3:L1+L2+L5) */
|
||||
SYS_GPS | SYS_CMP | SYS_QZS, /* navigation system */
|
||||
15.0 * D2R, /* elevation mask angle (rad) */
|
||||
{{1, 1},
|
||||
{{37,37,37,35,35,35,35,35,35},
|
||||
{37,37,37,35,35,35,35,35,35},
|
||||
{37,37,37,35,35,35,35,35,35}}}, /* SNR mask */
|
||||
0, /* satellite ephemeris/clock (EPHOPT_???) */
|
||||
1, /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */
|
||||
1, /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */
|
||||
1, /* GPS AR mode, debug/learning only (0:off,1:on) */
|
||||
1, /* BeiDou AR mode (0:off,1:on) */
|
||||
1, /* AR filtering to reject bad sats (0:off,1:on) */
|
||||
5, /* obs outage count to reset bias */
|
||||
10, /* min lock count to fix ambiguity */
|
||||
7, /* min sats to fix integer ambiguities */
|
||||
7, /* min sats to hold integer ambiguities */
|
||||
10, /* min sats to drop sats in AR */
|
||||
10, /* min fix count to hold ambiguity */
|
||||
1, /* max iteration to resolve ambiguity */
|
||||
0, /* ionosphere option (IONOOPT_???) */
|
||||
0, /* troposphere option (TROPOPT_???) */
|
||||
1, /* dynamics model (0:none,1:velociy,2:accel) */
|
||||
0, /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */
|
||||
1, /* number of filter iteration */
|
||||
0, /* code smoothing window size (0:none) */
|
||||
0, /* interpolate reference obs (for post mission) */
|
||||
0, /* SBAS correction options */
|
||||
0, /* SBAS satellite selection (0:all) */
|
||||
0, /* rover position for fixed mode */
|
||||
4, /* (0:pos in prcopt, 1:average of single pos, */
|
||||
/* 2:read from file, 3:rinex header, 4:rtcm pos) */
|
||||
{100.0, 100.0, 300.0}, /* base position for relative mode *//* eratio[] */
|
||||
{100.0, 0.015, 0.015, 0.0, 1.0, 52.0, 0.0, 0.0}, /* err[-,base,el,bl,dop,snr_max,snr,rcverr] */
|
||||
{30.0, 0.03, 0.3}, /* initial-state std [0]bias,[1]iono [2]trop */
|
||||
{1E-4, 1E-3, 1E-4, 1.0, 1.0, 0.0}, /* process-noise std [0]bias,[1]iono [2]trop [3]acch [4]accv [5] pos */
|
||||
5E-12, /* satellite clock stability (sec/sec) */
|
||||
{3.0, 1, 0.25, 1E-9, 1E-5, 3.0, 3.0, 0.0}, /* AR validation threshold */
|
||||
15.0, /* elevation mask of AR for rising satellite (deg) */
|
||||
15.0, /* elevation mask to hold ambiguity (deg) */
|
||||
0.05, /* slip threshold of geometry-free phase (m) */
|
||||
0, /* slip threshold of doppler (m) */
|
||||
0.1, /* variance for fix-and-hold psuedo measurements (cycle^2) */
|
||||
0.01, /* gain used for GLO and SBAS sats to adjust ambiguity */
|
||||
30.0, /* max difference of time (sec) */
|
||||
30.0, /* reject threshold of innovation (m) */
|
||||
30.0, /* reject threshold of gdop */
|
||||
{0}, /* baseline length constraint {const,sigma} (m) */
|
||||
{0}, /* rover position for fixed mode {x,y,z} (ecef) (m) */
|
||||
{0}, /* base position for relative mode {x,y,z} (ecef) (m) */
|
||||
{"", ""}, /* antenna types {rover,base} */
|
||||
{{0}}, /* antenna delta {{rov_e,rov_n,rov_u},{ref_e,ref_n,ref_u}} */
|
||||
{{0}}, /* receiver antenna parameters {rov,base} */
|
||||
{0}, /* excluded satellites (1:excluded,2:included) */
|
||||
0, /* max averaging epoches */
|
||||
0 /* initialize by restart */
|
||||
};
|
||||
// const solopt_t solopt_default = {
|
||||
// /* defaults solution output options */
|
||||
// SOLF_NMEA, /* solution format (SOLF_???) */
|
||||
// TIMES_UTC, /* time system (TIMES_???)*/
|
||||
// 1, /* time format (0:sssss.s,1:yyyy/mm/dd hh:mm:ss.s) */
|
||||
// 3, /* time digits under decimal point */
|
||||
// 0, /* latitude/longitude format (0:ddd.ddd,1:ddd mm ss) */
|
||||
// 1, /* output header (0:no,1:yes) */
|
||||
// 1, /* output processing options (0:no,1:yes) */
|
||||
// 0, /* output velocity options (0:no,1:yes) */
|
||||
// 0, /* datum (0:WGS84,1:Tokyo) */
|
||||
// 0, /* height (0:ellipsoidal,1:geodetic) */
|
||||
// 0, /* geoid model (0:EGM96,1:JGD2000) */
|
||||
// 0, /* solution of static mode (0:all,1:single) */
|
||||
// 0, /* solution statistics level (0:off,1:states,2:residuals) */
|
||||
// 0, /* debug trace level (0:off,1-5:debug) */
|
||||
// {0.0, 0.0}, /* nmea output interval (s) (<0:no,0:all) */
|
||||
// /* nmeaintv[0]:gprmc,gpgga,nmeaintv[1]:gpgsv */
|
||||
// " ",/* field separator */
|
||||
// "" /* program name */
|
||||
// /* solution options type */
|
||||
// };
|
||||
//const prcopt_t prcopt_default = {
|
||||
// /* defaults processing options */
|
||||
// PMODE_KINEMA,
|
||||
// 0,
|
||||
// 2,
|
||||
// SYS_GPS | SYS_GLO | SYS_GAL, /* mode,soltype,nf,navsys */
|
||||
// 15.0 * D2R,
|
||||
// {{0, 0}}, /* elmin,snrmask */
|
||||
// 0,
|
||||
// 3,
|
||||
// 3,
|
||||
// 1,
|
||||
// 0,
|
||||
// 1, /* sateph,modear,glomodear,gpsmodear,bdsmodear,arfilter */
|
||||
// 20,
|
||||
// 0,
|
||||
// 4,
|
||||
// 5,
|
||||
// 10,
|
||||
// 20, /* maxout,minlock,minfixsats,minholdsats,mindropsats,minfix */
|
||||
// 1,
|
||||
// 1,
|
||||
// 1,
|
||||
// 1,
|
||||
// 0, /* armaxiter,estion,esttrop,dynamics,tidecorr */
|
||||
// 1,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0, /* niter,codesmooth,intpref,sbascorr,sbassatsel */
|
||||
// 0,
|
||||
// 4, /* rovpos,refpos */
|
||||
// {300.0, 300.0, 300.0}, /* eratio[] */
|
||||
// {100.0, 0.003, 0.003, 0.0, 1.0, 52.0, 0.0, 0.0}, /* err[-,base,el,bl,dop,snr_max,snr,rcverr] */
|
||||
// {30.0, 0.03, 0.3}, /* std[] */
|
||||
// {1E-4, 1E-3, 1E-4, 1E-1, 1E-2, 0.0}, /* prn[] */
|
||||
// 5E-12, /* sclkstab */
|
||||
// {3.0, 0.25, 0.0, 1E-9, 1E-5, 3.0, 3.0, 0.0}, /* thresar */
|
||||
// 0.0,
|
||||
// 0.0,
|
||||
// 0.05,
|
||||
// 0, /* elmaskar,elmaskhold,thresslip,thresdop, */
|
||||
// 0.1,
|
||||
// 0.01,
|
||||
// 30.0,
|
||||
// 5.0,
|
||||
// 30.0, /* varholdamb,gainholdamb,maxtdif,maxinno,maxgdop */
|
||||
// {0},
|
||||
// {0},
|
||||
// {0}, /* baseline,ru,rb */
|
||||
// {"", ""}, /* anttype */
|
||||
// {{0}},
|
||||
// {{0}},
|
||||
// {0}, /* antdel,pcv,exsats */
|
||||
// 1,
|
||||
// 1 /* maxaveep,initrst */
|
||||
//};
|
||||
const solopt_t solopt_default = {
|
||||
/* defaults solution output options */
|
||||
SOLF_LLH,
|
||||
TIMES_GPST,
|
||||
1,
|
||||
3, /* posf,times,timef,timeu */
|
||||
0,
|
||||
1,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0, /* degf,outhead,outopt,outvel,datum,height,geoid */
|
||||
0,
|
||||
0,
|
||||
0, /* solstatic,sstat,trace */
|
||||
{0.0, 0.0}, /* nmeaintv */
|
||||
" ",
|
||||
"" /* separator/program name */
|
||||
};
|
||||
#ifdef USING_FILE_OPT
|
||||
/* discard space characters at tail ------------------------------------------*/
|
||||
static void chop(char *str)
|
||||
{
|
||||
char *p;
|
||||
if ((p = strchr(str, '#')))
|
||||
*p = '\0'; /* comment */
|
||||
for (p = str + strlen(str) - 1; p >= str && !isgraph((int)*p); p--)
|
||||
*p = '\0';
|
||||
}
|
||||
/* enum to string ------------------------------------------------------------*/
|
||||
static int enum2str(char *s, const char *comment, int val)
|
||||
{
|
||||
char str[32], *p, *q;
|
||||
int n;
|
||||
|
||||
n = sprintf(str, "%d:", val);
|
||||
if (!(p = strstr(comment, str)))
|
||||
{
|
||||
return sprintf(s, "%d", val);
|
||||
}
|
||||
if (!(q = strchr(p + n, ',')) && !(q = strchr(p + n, ')')))
|
||||
{
|
||||
strcpy(s, p + n);
|
||||
return (int)strlen(p + n);
|
||||
}
|
||||
strncpy(s, p + n, q - p - n);
|
||||
s[q - p - n] = '\0';
|
||||
return (int)(q - p - n);
|
||||
}
|
||||
/* string to enum ------------------------------------------------------------*/
|
||||
static int str2enum(const char *str, const char *comment, int *val)
|
||||
{
|
||||
const char *p;
|
||||
char s[32];
|
||||
|
||||
for (p = comment;; p++)
|
||||
{
|
||||
if (!(p = strstr(p, str)))
|
||||
break;
|
||||
if (*(p - 1) != ':')
|
||||
continue;
|
||||
for (p -= 2; '0' <= *p && *p <= '9'; p--)
|
||||
;
|
||||
return sscanf(p + 1, "%d", val) == 1;
|
||||
}
|
||||
sprintf(s, "%.30s:", str);
|
||||
if ((p = strstr(comment, s)))
|
||||
{ /* number */
|
||||
return sscanf(p, "%d", val) == 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/* search option ---------------------------------------------------------------
|
||||
* search option record
|
||||
* args : char *name I option name
|
||||
* opt_t *opts I options table
|
||||
* (terminated with table[i].name="")
|
||||
* return : option record (NULL: not found)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern opt_t *searchopt(const char *name, const opt_t *opts)
|
||||
{
|
||||
int i;
|
||||
|
||||
trace(4, "searchopt: name=%s\n", name);
|
||||
|
||||
for (i = 0; *opts[i].name; i++)
|
||||
{
|
||||
if (strstr(opts[i].name, name))
|
||||
return (opt_t *)(opts + i);
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
/* string to option value ------------------------------------------------------
|
||||
* convert string to option value
|
||||
* args : opt_t *opt O option
|
||||
* char *str I option value string
|
||||
* return : status (1:ok,0:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int str2opt(opt_t *opt, const char *str)
|
||||
{
|
||||
switch (opt->format)
|
||||
{
|
||||
case 0:
|
||||
*(int *)opt->var = atoi(str);
|
||||
break;
|
||||
case 1:
|
||||
*(double *)opt->var = atof(str);
|
||||
break;
|
||||
case 2:
|
||||
strcpy((char *)opt->var, str);
|
||||
break;
|
||||
case 3:
|
||||
return str2enum(str, opt->comment, (int *)opt->var);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/* option value to string ------------------------------------------------------
|
||||
* convert option value to string
|
||||
* args : opt_t *opt I option
|
||||
* char *str O option value string
|
||||
* return : length of output string
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int opt2str(const opt_t *opt, char *str)
|
||||
{
|
||||
char *p = str;
|
||||
|
||||
trace(3, "opt2str : name=%s\n", opt->name);
|
||||
|
||||
switch (opt->format)
|
||||
{
|
||||
case 0:
|
||||
p += sprintf(p, "%d", *(int *)opt->var);
|
||||
break;
|
||||
case 1:
|
||||
p += sprintf(p, "%.15g", *(double *)opt->var);
|
||||
break;
|
||||
case 2:
|
||||
p += sprintf(p, "%s", (char *)opt->var);
|
||||
break;
|
||||
case 3:
|
||||
p += enum2str(p, opt->comment, *(int *)opt->var);
|
||||
break;
|
||||
}
|
||||
return (int)(p - str);
|
||||
}
|
||||
/* option to string -------------------------------------------------------------
|
||||
* convert option to string (keyword=value # comment)
|
||||
* args : opt_t *opt I option
|
||||
* char *buff O option string
|
||||
* return : length of output string
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int opt2buf(const opt_t *opt, char *buff)
|
||||
{
|
||||
char *p = buff;
|
||||
int n;
|
||||
|
||||
trace(3, "opt2buf : name=%s\n", opt->name);
|
||||
|
||||
p += sprintf(p, "%-18s =", opt->name);
|
||||
p += opt2str(opt, p);
|
||||
if (*opt->comment)
|
||||
{
|
||||
if ((n = (int)(buff + 30 - p)) > 0)
|
||||
p += sprintf(p, "%*s", n, "");
|
||||
p += sprintf(p, " # (%s)", opt->comment);
|
||||
}
|
||||
return (int)(p - buff);
|
||||
}
|
||||
/* load options ----------------------------------------------------------------
|
||||
* load options from file
|
||||
* args : char *file I options file
|
||||
* opt_t *opts IO options table
|
||||
* (terminated with table[i].name="")
|
||||
* return : status (1:ok,0:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int loadopts(const char *file, opt_t *opts)
|
||||
{
|
||||
FILE *fp;
|
||||
opt_t *opt;
|
||||
char buff[2048], *p;
|
||||
int n = 0;
|
||||
|
||||
trace(4, "loadopts: file=%s\n", file);
|
||||
|
||||
if (!(fp = fopen(file, "r")))
|
||||
{
|
||||
trace(1, "loadopts: options file open error (%s)\n", file);
|
||||
return 0;
|
||||
}
|
||||
while (fgets(buff, sizeof(buff), fp))
|
||||
{
|
||||
n++;
|
||||
chop(buff);
|
||||
|
||||
if (buff[0] == '\0')
|
||||
continue;
|
||||
|
||||
if (!(p = strstr(buff, "=")))
|
||||
{
|
||||
fprintf(stderr, "invalid option %s (%s:%d)\n", buff, file, n);
|
||||
continue;
|
||||
}
|
||||
*p++ = '\0';
|
||||
chop(buff);
|
||||
if (!(opt = searchopt(buff, opts)))
|
||||
continue;
|
||||
|
||||
if (!str2opt(opt, p))
|
||||
{
|
||||
fprintf(stderr, "invalid option value %s (%s:%d)\n", buff, file, n);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
fclose(fp);
|
||||
|
||||
return 1;
|
||||
}
|
||||
/* save options to file --------------------------------------------------------
|
||||
* save options to file
|
||||
* args : char *file I options file
|
||||
* char *mode I write mode ("w":overwrite,"a":append);
|
||||
* char *comment I header comment (NULL: no comment)
|
||||
* opt_t *opts I options table
|
||||
* (terminated with table[i].name="")
|
||||
* return : status (1:ok,0:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern int saveopts(const char *file, const char *mode, const char *comment,
|
||||
const opt_t *opts)
|
||||
{
|
||||
FILE *fp;
|
||||
char buff[2048];
|
||||
int i;
|
||||
|
||||
trace(3, "saveopts: file=%s mode=%s\n", file, mode);
|
||||
|
||||
if (!(fp = fopen(file, mode)))
|
||||
{
|
||||
trace(1, "saveopts: options file open error (%s)\n", file);
|
||||
return 0;
|
||||
}
|
||||
if (comment)
|
||||
fprintf(fp, "# %s\n\n", comment);
|
||||
|
||||
for (i = 0; *opts[i].name; i++)
|
||||
{
|
||||
opt2buf(opts + i, buff);
|
||||
fprintf(fp, "%s\n", buff);
|
||||
}
|
||||
fclose(fp);
|
||||
return 1;
|
||||
}
|
||||
/* options to system options buffer ------------------------------------------*/
|
||||
static void sysopts2buff(void)
|
||||
{
|
||||
double pos[3], *rr;
|
||||
char id[32], *p;
|
||||
int i, j, sat, *ps;
|
||||
|
||||
elmask_ = prcopt_.elmin * R2D;
|
||||
elmaskar_ = prcopt_.elmaskar * R2D;
|
||||
elmaskhold_ = prcopt_.elmaskhold * R2D;
|
||||
|
||||
for (i = 0; i < 2; i++)
|
||||
{
|
||||
ps = i == 0 ? &prcopt_.rovpos : &prcopt_.refpos;
|
||||
rr = i == 0 ? prcopt_.ru : prcopt_.rb;
|
||||
|
||||
if (*ps == 0)
|
||||
{
|
||||
antpostype_[i] = 0;
|
||||
ecef2pos(rr, pos);
|
||||
antpos_[i][0] = pos[0] * R2D;
|
||||
antpos_[i][1] = pos[1] * R2D;
|
||||
antpos_[i][2] = pos[2];
|
||||
}
|
||||
else
|
||||
antpostype_[i] = *ps + 1;
|
||||
}
|
||||
/* excluded satellites */
|
||||
exsats_[0] = '\0';
|
||||
for (sat = 1, p = exsats_; sat <= MAXSAT && p - exsats_ < (int)sizeof(exsats_) - 32; sat++)
|
||||
{
|
||||
if (prcopt_.exsats[sat - 1])
|
||||
{
|
||||
satno2id(sat, id);
|
||||
p += sprintf(p, "%s%s%s", p == exsats_ ? "" : " ",
|
||||
prcopt_.exsats[sat - 1] == 2 ? "+" : "", id);
|
||||
}
|
||||
}
|
||||
/* snrmask */
|
||||
for (i = 0; i < NFREQ; i++)
|
||||
{
|
||||
snrmask_[i][0] = '\0';
|
||||
p = snrmask_[i];
|
||||
for (j = 0; j < 9; j++)
|
||||
{
|
||||
p += sprintf(p, "%s%.0f", j > 0 ? "," : "", prcopt_.snrmask.mask[i][j]);
|
||||
}
|
||||
}
|
||||
/* number of frequency (4:L1+L5) TODO ???? */
|
||||
/*if (prcopt_.nf==3&&prcopt_.freqopt==1) {
|
||||
prcopt_.nf=4;
|
||||
prcopt_.freqopt=0;
|
||||
}*/
|
||||
}
|
||||
#endif
|
||||
/* system options buffer to options ------------------------------------------*/
|
||||
static void buff2sysopts(void)
|
||||
{
|
||||
double pos[3], *rr;
|
||||
char buff[1024], *p, *id;
|
||||
int i, j, sat, *ps;
|
||||
|
||||
prcopt_.elmin = elmask_ * D2R;
|
||||
prcopt_.elmaskar = elmaskar_ * D2R;
|
||||
prcopt_.elmaskhold = elmaskhold_ * D2R;
|
||||
for (i = 0; i < MAXSAT; i++)
|
||||
prcopt_.exsats[i] = 0;
|
||||
|
||||
for (i = 0; i < 2; i++)
|
||||
{
|
||||
ps = i == 0 ? &prcopt_.rovpos : &prcopt_.refpos;
|
||||
rr = i == 0 ? prcopt_.ru : prcopt_.rb;
|
||||
|
||||
if (antpostype_[i] == 0)
|
||||
{ /* lat/lon/hgt */
|
||||
*ps = 0;
|
||||
pos[0] = antpos_[i][0] * D2R;
|
||||
pos[1] = antpos_[i][1] * D2R;
|
||||
pos[2] = antpos_[i][2];
|
||||
pos2ecef(pos, rr);
|
||||
}
|
||||
else if (antpostype_[i] == 1)
|
||||
{ /* xyz-ecef */
|
||||
*ps = 0;
|
||||
rr[0] = antpos_[i][0];
|
||||
rr[1] = antpos_[i][1];
|
||||
rr[2] = antpos_[i][2];
|
||||
}
|
||||
else
|
||||
*ps = antpostype_[i] - 1;
|
||||
}
|
||||
#ifdef USING_FILE_OPT
|
||||
/* excluded satellites */
|
||||
if (exsats_[0] != '\0')
|
||||
{
|
||||
strcpy(buff, exsats_);
|
||||
for (p = strtok(buff, " "); p; p = strtok(NULL, " "))
|
||||
{
|
||||
if (*p == '+')
|
||||
id = p + 1;
|
||||
else
|
||||
id = p;
|
||||
if (!(sat = satid2no(id)))
|
||||
continue;
|
||||
prcopt_.exsats[sat - 1] = *p == '+' ? 2 : 1;
|
||||
}
|
||||
}
|
||||
/* snrmask */
|
||||
for (i = 0; i < NFREQ; i++)
|
||||
{
|
||||
for (j = 0; j < 9; j++)
|
||||
prcopt_.snrmask.mask[i][j] = 0.0;
|
||||
strcpy(buff, snrmask_[i]);
|
||||
for (p = strtok(buff, ","), j = 0; p && j < 9; p = strtok(NULL, ","))
|
||||
{
|
||||
prcopt_.snrmask.mask[i][j++] = atof(p);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
/* number of frequency (4:L1+L5) TODO ????*/
|
||||
/*if (prcopt_.nf==4) {
|
||||
prcopt_.nf=3;
|
||||
prcopt_.freqopt=1;
|
||||
}*/
|
||||
}
|
||||
|
||||
/* reset system options to default ---------------------------------------------
|
||||
* reset system options to default
|
||||
* args : none
|
||||
* return : none
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern void resetsysopts(void)
|
||||
{
|
||||
int i, j;
|
||||
|
||||
trace(3, "resetsysopts:\n");
|
||||
|
||||
prcopt_ = prcopt_default;
|
||||
solopt_ = solopt_default;
|
||||
|
||||
// filopt_.satantp[0]='\0';
|
||||
// filopt_.rcvantp[0]='\0';
|
||||
// filopt_.stapos [0]='\0';
|
||||
// filopt_.geoid [0]='\0';
|
||||
// filopt_.dcb [0]='\0';
|
||||
// filopt_.blq [0]='\0';
|
||||
// filopt_.solstat[0]='\0';
|
||||
// filopt_.trace [0]='\0';
|
||||
|
||||
elmask_ = 15.0;
|
||||
elmaskar_ = 0.0;
|
||||
elmaskhold_ = 0.0;
|
||||
for (i = 0; i < 2; i++)
|
||||
antpostype_[i] = 0;
|
||||
for (i = 0; i < 2; i++)
|
||||
for (j = 0; j < 3; j++)
|
||||
{
|
||||
antpos_[i][j] = 0.0;
|
||||
}
|
||||
#ifdef USING_FILE_OPT
|
||||
exsats_[0] = '\0';
|
||||
#endif
|
||||
antpostype_[0] = 0;
|
||||
antpostype_[1] = 5;
|
||||
}
|
||||
/* opt_init ---------------------------------------------
|
||||
* 对规则做自定义, 相当于是导入文件的写死版本
|
||||
* args : none
|
||||
* return : none
|
||||
*-----------------------------------------------------------------------------*/
|
||||
static void opt_init(void)
|
||||
{
|
||||
// prcopt_.nf = 1;
|
||||
}
|
||||
|
||||
/* get system options ----------------------------------------------------------
|
||||
* get system options
|
||||
* args : prcopt_t *popt IO processing options (NULL: no output)
|
||||
* solopt_t *sopt IO solution options (NULL: no output)
|
||||
* folopt_t *fopt IO file options (NULL: no output)
|
||||
* return : none
|
||||
* notes : to load system options, use loadopts() before calling the function
|
||||
*-----------------------------------------------------------------------------*/
|
||||
// extern void getsysopts(prcopt_t *popt, solopt_t *sopt, filopt_t *fopt)
|
||||
extern void getsysopts(prcopt_t *popt, solopt_t *sopt)
|
||||
{
|
||||
trace(3, "getsysopts:\n");
|
||||
|
||||
opt_init();
|
||||
buff2sysopts();
|
||||
|
||||
/* confirm overwrite */
|
||||
if (prcopt_.refpos == 4) /* 4:rtcm pos */
|
||||
{ /* rtcm */
|
||||
for (char i = 0; i < 3; i++)
|
||||
prcopt_.rb[i] = 0.0;
|
||||
}
|
||||
if (popt)
|
||||
*popt = prcopt_;
|
||||
if (sopt)
|
||||
*sopt = solopt_;
|
||||
// if (fopt) *fopt=filopt_;
|
||||
}
|
||||
/* set system options ----------------------------------------------------------
|
||||
* set system options
|
||||
* args : prcopt_t *prcopt I processing options (NULL: default)
|
||||
* solopt_t *solopt I solution options (NULL: default)
|
||||
* filopt_t *filopt I file options (NULL: default)
|
||||
* return : none
|
||||
* notes : to save system options, use saveopts() after calling the function
|
||||
*-----------------------------------------------------------------------------*/
|
||||
// extern void setsysopts(const prcopt_t *prcopt, const solopt_t *solopt,
|
||||
// const filopt_t *filopt)
|
||||
// extern void setsysopts(const prcopt_t *prcopt, const solopt_t *solopt)
|
||||
// {
|
||||
// trace(3, "setsysopts:\n");
|
||||
|
||||
// resetsysopts();
|
||||
// if (prcopt)
|
||||
// prcopt_ = *prcopt;
|
||||
// if (solopt)
|
||||
// solopt_ = *solopt;
|
||||
// // if (filopt) filopt_=*filopt;
|
||||
// sysopts2buff();
|
||||
// }
|
||||
1220
RTK/pntpos.c
Normal file
1220
RTK/pntpos.c
Normal file
File diff suppressed because it is too large
Load Diff
22
RTK/rtklib.h
22
RTK/rtklib.h
@ -31,14 +31,14 @@
|
||||
#define ENAGPS
|
||||
// #define ENAGLO
|
||||
// #define ENAGAL
|
||||
#define ENACMP
|
||||
// #define ENACMP
|
||||
// #define ENAQZS
|
||||
// #define TRACE
|
||||
// #define L5_TO_L2
|
||||
// #define ONLY_2FREQ
|
||||
#define PAR
|
||||
|
||||
#define USING_FILE_OPT
|
||||
//#define USING_FILE_OPT
|
||||
//#define STATIC
|
||||
|
||||
#define STM32
|
||||
@ -56,6 +56,7 @@
|
||||
|
||||
#include <rtthread.h>
|
||||
#include <rtdevice.h>
|
||||
#include <rtk_task.h>
|
||||
|
||||
/* constants -----------------------------------------------------------------*/
|
||||
|
||||
@ -1242,15 +1243,15 @@ typedef struct
|
||||
int format[2]; /* input format {rov,base} */
|
||||
solopt_t solopt[2]; /* output solution options {sol1,sol2} */
|
||||
int navsel; /* ephemeris select (0:all,1:rover,2:base,3) */
|
||||
int nsbs; /* number of sbas message */
|
||||
// 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 */
|
||||
// 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} */
|
||||
// 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} */
|
||||
@ -1260,7 +1261,7 @@ typedef struct
|
||||
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 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 */
|
||||
@ -1699,8 +1700,8 @@ extern int rtkpos_static(rtk_t *rtk, obsd_t *obs[], int n, const nav_t *nav, cha
|
||||
int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
|
||||
const solopt_t *solopt);
|
||||
void rtksvrclosestr(rtksvr_t *svr, int index);
|
||||
void rtksvrlock(rtksvr_t *svr);
|
||||
void rtksvrunlock(rtksvr_t *svr);
|
||||
// void rtksvrlock(rtksvr_t *svr);
|
||||
// void rtksvrunlock(rtksvr_t *svr);
|
||||
int rtksvrostat(rtksvr_t *svr, int type, gtime_t *time, int *sat,
|
||||
double *az, double *el, int **snr, int *vsat);
|
||||
void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg);
|
||||
@ -1724,4 +1725,5 @@ extern int showmsg(const char *format, ...);
|
||||
extern void settspan(gtime_t ts, gtime_t te);
|
||||
extern void settime(gtime_t time);
|
||||
|
||||
int dbg_show_obs(const obsd_t *obs, int nobs, char mode, char *buffer);
|
||||
#endif /* RTKLIB_H */
|
||||
|
||||
2981
RTK/rtkpos.c
Normal file
2981
RTK/rtkpos.c
Normal file
File diff suppressed because it is too large
Load Diff
2111
RTK/solution.c
Normal file
2111
RTK/solution.c
Normal file
File diff suppressed because it is too large
Load Diff
@ -1,36 +0,0 @@
|
||||
#include "rtklib.h"
|
||||
|
||||
#ifdef STATIC
|
||||
/* number of parameters (pos,ionos,tropos,hw-bias,phase-bias,real,estimated) */
|
||||
#define NX 87
|
||||
#define NR 9
|
||||
/* global variables ----------------------------------------------------------*/
|
||||
rtksvr_t svr; /* rtk server struct */
|
||||
prcopt_t prcopt; /* processing options */
|
||||
solopt_t solopt[2] = {{0}}; /* solution options */
|
||||
|
||||
/* rtkpos.c */
|
||||
double rtk_x[NX] = {0.0}; /* NX*8 = B */
|
||||
double rtk_P[NX * NX] = {0.0}; /* NX*NX*8 = B */
|
||||
double rtk_xa[NR] = {0.0}; /* NR*8 = B */
|
||||
double rtk_Pa[NR * NR] = {0.0}; /* NR*NR*8 = B */
|
||||
|
||||
/* rtksvr.c */
|
||||
eph_t svr_eph[MAXSAT];
|
||||
#ifdef ENAGLO
|
||||
geph_t svr_geph[NSATGLO];
|
||||
#endif
|
||||
#ifdef ENASBS
|
||||
seph_t svr_seph[NSATSBS];
|
||||
#endif
|
||||
obsd_t svr_obsd[2][MAXOBS];
|
||||
|
||||
/* rtcm.c */
|
||||
obsd_t RTCM_obsd_r[MAXOBS];
|
||||
obsd_t RTCM_obsd_b[MAXOBS];
|
||||
eph_t RTCM_eph_r[MAXSAT];
|
||||
#ifdef ENAGLO
|
||||
geph_t RTCM_geph[MAXPRNGLO];
|
||||
#endif
|
||||
|
||||
#endif
|
||||
310
RTK/tides.c
Normal file
310
RTK/tides.c
Normal file
@ -0,0 +1,310 @@
|
||||
/*------------------------------------------------------------------------------
|
||||
* tides.c : tidal displacement corrections
|
||||
*
|
||||
* Copyright (C) 2015-2017 by T.TAKASU, All rights reserved.
|
||||
*
|
||||
* options : -DIERS_MODEL use IERS tide model
|
||||
*
|
||||
* references :
|
||||
* [1] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996
|
||||
* [2] D.D.McCarthy and G.Petit, IERS Technical Note 32, IERS Conventions
|
||||
* 2003, November 2003
|
||||
* [3] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
|
||||
* Space Technology Library, 2004
|
||||
* [4] J.Kouba, A Guide to using International GNSS Service (IGS) products,
|
||||
* May 2009
|
||||
* [5] G.Petit and B.Luzum (eds), IERS Technical Note No. 36, IERS
|
||||
* Conventions (2010), 2010
|
||||
*
|
||||
* version : $Revision:$ $Date:$
|
||||
* history : 2015/05/10 1.0 separated from ppp.c
|
||||
* 2015/06/11 1.1 fix bug on computing days in tide_oload() (#128)
|
||||
* 2017/04/11 1.2 fix bug on calling geterp() in timdedisp()
|
||||
*-----------------------------------------------------------------------------*/
|
||||
#include "rtklib.h"
|
||||
|
||||
#define SQR(x) ((x)*(x))
|
||||
|
||||
//#define AS2R (D2R/3600.0) /* arc sec to radian */
|
||||
#define GME 3.986004415E+14 /* earth gravitational constant */
|
||||
#define GMS 1.327124E+20 /* sun gravitational constant */
|
||||
#define GMM 4.902801E+12 /* moon gravitational constant */
|
||||
|
||||
/* function prototypes -------------------------------------------------------*/
|
||||
#ifdef IERS_MODEL
|
||||
extern int dehanttideinel_(double *xsta, int *year, int *mon, int *day,
|
||||
double *fhr, double *xsun, double *xmon,
|
||||
double *dxtide);
|
||||
#endif
|
||||
|
||||
/* solar/lunar tides (ref [2] 7) ---------------------------------------------*/
|
||||
#ifndef IERS_MODEL
|
||||
static void tide_pl(const double *eu, const double *rp, double GMp,
|
||||
const double *pos, double *dr)
|
||||
{
|
||||
const double H3=0.292,L3=0.015;
|
||||
double r,ep[3],latp,lonp,p,K2,K3,a,H2,L2,dp,du,cosp,sinl,cosl;
|
||||
int i;
|
||||
|
||||
trace(4,"tide_pl : pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
|
||||
|
||||
if ((r=norm(rp,3))<=0.0) return;
|
||||
|
||||
for (i=0;i<3;i++) ep[i]=rp[i]/r;
|
||||
|
||||
K2=GMp/GME*SQR(RE_WGS84)*SQR(RE_WGS84)/(r*r*r);
|
||||
K3=K2*RE_WGS84/r;
|
||||
latp=asin(ep[2]); lonp=atan2(ep[1],ep[0]);
|
||||
cosp=cos(latp); sinl=sin(pos[0]); cosl=cos(pos[0]);
|
||||
|
||||
/* step1 in phase (degree 2) */
|
||||
p=(3.0*sinl*sinl-1.0)/2.0;
|
||||
H2=0.6078-0.0006*p;
|
||||
L2=0.0847+0.0002*p;
|
||||
a=dot(ep,eu,3);
|
||||
dp=K2*3.0*L2*a;
|
||||
du=K2*(H2*(1.5*a*a-0.5)-3.0*L2*a*a);
|
||||
|
||||
/* step1 in phase (degree 3) */
|
||||
dp+=K3*L3*(7.5*a*a-1.5);
|
||||
du+=K3*(H3*(2.5*a*a*a-1.5*a)-L3*(7.5*a*a-1.5)*a);
|
||||
|
||||
/* step1 out-of-phase (only radial) */
|
||||
du+=3.0/4.0*0.0025*K2*sin(2.0*latp)*sin(2.0*pos[0])*sin(pos[1]-lonp);
|
||||
du+=3.0/4.0*0.0022*K2*cosp*cosp*cosl*cosl*sin(2.0*(pos[1]-lonp));
|
||||
|
||||
dr[0]=dp*ep[0]+du*eu[0];
|
||||
dr[1]=dp*ep[1]+du*eu[1];
|
||||
dr[2]=dp*ep[2]+du*eu[2];
|
||||
|
||||
trace(5,"tide_pl : dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
|
||||
}
|
||||
/* displacement by solid earth tide (ref [2] 7) ------------------------------*/
|
||||
static void tide_solid(const double *rsun, const double *rmoon,
|
||||
const double *pos, const double *E, double gmst, int opt,
|
||||
double *dr)
|
||||
{
|
||||
double dr1[3],dr2[3],eu[3],du,dn,sinl,sin2l;
|
||||
|
||||
trace(3,"tide_solid: pos=%.3f %.3f opt=%d\n",pos[0]*R2D,pos[1]*R2D,opt);
|
||||
|
||||
/* step1: time domain */
|
||||
eu[0]=E[2]; eu[1]=E[5]; eu[2]=E[8];
|
||||
tide_pl(eu,rsun, GMS,pos,dr1);
|
||||
tide_pl(eu,rmoon,GMM,pos,dr2);
|
||||
|
||||
/* step2: frequency domain, only K1 radial */
|
||||
sin2l=sin(2.0*pos[0]);
|
||||
du=-0.012*sin2l*sin(gmst+pos[1]);
|
||||
|
||||
dr[0]=dr1[0]+dr2[0]+du*E[2];
|
||||
dr[1]=dr1[1]+dr2[1]+du*E[5];
|
||||
dr[2]=dr1[2]+dr2[2]+du*E[8];
|
||||
|
||||
/* eliminate permanent deformation */
|
||||
if (opt&8) {
|
||||
sinl=sin(pos[0]);
|
||||
du=0.1196*(1.5*sinl*sinl-0.5);
|
||||
dn=0.0247*sin2l;
|
||||
dr[0]+=du*E[2]+dn*E[1];
|
||||
dr[1]+=du*E[5]+dn*E[4];
|
||||
dr[2]+=du*E[8]+dn*E[7];
|
||||
}
|
||||
trace(5,"tide_solid: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
|
||||
}
|
||||
#endif /* !IERS_MODEL */
|
||||
|
||||
/* displacement by ocean tide loading (ref [2] 7) ----------------------------*/
|
||||
static void tide_oload(gtime_t tut, const double *odisp, double *denu)
|
||||
{
|
||||
const double args[][5]={
|
||||
{1.40519E-4, 2.0,-2.0, 0.0, 0.00}, /* M2 */
|
||||
{1.45444E-4, 0.0, 0.0, 0.0, 0.00}, /* S2 */
|
||||
{1.37880E-4, 2.0,-3.0, 1.0, 0.00}, /* N2 */
|
||||
{1.45842E-4, 2.0, 0.0, 0.0, 0.00}, /* K2 */
|
||||
{0.72921E-4, 1.0, 0.0, 0.0, 0.25}, /* K1 */
|
||||
{0.67598E-4, 1.0,-2.0, 0.0,-0.25}, /* O1 */
|
||||
{0.72523E-4,-1.0, 0.0, 0.0,-0.25}, /* P1 */
|
||||
{0.64959E-4, 1.0,-3.0, 1.0,-0.25}, /* Q1 */
|
||||
{0.53234E-5, 0.0, 2.0, 0.0, 0.00}, /* Mf */
|
||||
{0.26392E-5, 0.0, 1.0,-1.0, 0.00}, /* Mm */
|
||||
{0.03982E-5, 2.0, 0.0, 0.0, 0.00} /* Ssa */
|
||||
};
|
||||
const double ep1975[]={1975,1,1,0,0,0};
|
||||
double ep[6],fday,days,t,t2,t3,a[5],ang,dp[3]={0};
|
||||
int i,j;
|
||||
|
||||
trace(3,"tide_oload:\n");
|
||||
|
||||
/* angular argument: see subroutine arg.f for reference [1] */
|
||||
time2epoch(tut,ep);
|
||||
fday=ep[3]*3600.0+ep[4]*60.0+ep[5];
|
||||
ep[3]=ep[4]=ep[5]=0.0;
|
||||
days=timediff(epoch2time(ep),epoch2time(ep1975))/86400.0+1.0;
|
||||
t=(27392.500528+1.000000035*days)/36525.0;
|
||||
t2=t*t; t3=t2*t;
|
||||
|
||||
a[0]=fday;
|
||||
a[1]=(279.69668+36000.768930485*t+3.03E-4*t2)*D2R; /* H0 */
|
||||
a[2]=(270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R; /* S0 */
|
||||
a[3]=(334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R; /* P0 */
|
||||
a[4]=2.0*PI;
|
||||
|
||||
/* displacements by 11 constituents */
|
||||
for (i=0;i<11;i++) {
|
||||
ang=0.0;
|
||||
for (j=0;j<5;j++) ang+=a[j]*args[i][j];
|
||||
for (j=0;j<3;j++) dp[j]+=odisp[j+i*6]*cos(ang-odisp[j+3+i*6]*D2R);
|
||||
}
|
||||
denu[0]=-dp[1];
|
||||
denu[1]=-dp[2];
|
||||
denu[2]= dp[0];
|
||||
|
||||
trace(5,"tide_oload: denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]);
|
||||
}
|
||||
/* iers mean pole (ref [7] eq.7.25) ------------------------------------------*/
|
||||
static void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar)
|
||||
{
|
||||
const double ep2000[]={2000,1,1,0,0,0};
|
||||
double y,y2,y3;
|
||||
|
||||
y=timediff(tut,epoch2time(ep2000))/86400.0/365.25;
|
||||
|
||||
if (y<3653.0/365.25) { /* until 2010.0 */
|
||||
y2=y*y; y3=y2*y;
|
||||
*xp_bar= 55.974+1.8243*y+0.18413*y2+0.007024*y3; /* (mas) */
|
||||
*yp_bar=346.346+1.7896*y-0.10729*y2-0.000908*y3;
|
||||
}
|
||||
else { /* after 2010.0 */
|
||||
*xp_bar= 23.513+7.6141*y; /* (mas) */
|
||||
*yp_bar=358.891-0.6287*y;
|
||||
}
|
||||
}
|
||||
/* displacement by pole tide (ref [7] eq.7.26) --------------------------------*/
|
||||
static void tide_pole(gtime_t tut, const double *pos, const double *erpv,
|
||||
double *denu)
|
||||
{
|
||||
double xp_bar,yp_bar,m1,m2,cosl,sinl;
|
||||
|
||||
trace(3,"tide_pole: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
|
||||
|
||||
/* iers mean pole (mas) */
|
||||
iers_mean_pole(tut,&xp_bar,&yp_bar);
|
||||
|
||||
/* ref [7] eq.7.24 */
|
||||
m1= erpv[0]/AS2R-xp_bar*1E-3; /* (as) */
|
||||
m2=-erpv[1]/AS2R+yp_bar*1E-3;
|
||||
|
||||
/* sin(2*theta) = sin(2*phi), cos(2*theta)=-cos(2*phi) */
|
||||
cosl=cos(pos[1]);
|
||||
sinl=sin(pos[1]);
|
||||
denu[0]= 9E-3*sin(pos[0]) *(m1*sinl-m2*cosl); /* de= Slambda (m) */
|
||||
denu[1]= -9E-3*cos(2.0*pos[0])*(m1*cosl+m2*sinl); /* dn=-Stheta (m) */
|
||||
denu[2]=-33E-3*sin(2.0*pos[0])*(m1*cosl+m2*sinl); /* du= Sr (m) */
|
||||
|
||||
trace(5,"tide_pole : denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]);
|
||||
}
|
||||
/* tidal displacement ----------------------------------------------------------
|
||||
* displacements by earth tides
|
||||
* args : gtime_t tutc I time in utc
|
||||
* double *rr I site position (ecef) (m)
|
||||
* int opt I options (or of the followings)
|
||||
* 1: solid earth tide
|
||||
* 2: ocean tide loading
|
||||
* 4: pole tide
|
||||
* 8: elimate permanent deformation
|
||||
* double *erp I earth rotation parameters (NULL: not used)
|
||||
* double *odisp I ocean loading parameters (NULL: not used)
|
||||
* odisp[0+i*6]: consituent i amplitude radial(m)
|
||||
* odisp[1+i*6]: consituent i amplitude west (m)
|
||||
* odisp[2+i*6]: consituent i amplitude south (m)
|
||||
* odisp[3+i*6]: consituent i phase radial (deg)
|
||||
* odisp[4+i*6]: consituent i phase west (deg)
|
||||
* odisp[5+i*6]: consituent i phase south (deg)
|
||||
* (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1,
|
||||
* 8:Mf,9:Mm,10:Ssa)
|
||||
* double *dr O displacement by earth tides (ecef) (m)
|
||||
* return : none
|
||||
* notes : see ref [1], [2] chap 7
|
||||
* see ref [4] 5.2.1, 5.2.2, 5.2.3
|
||||
* ver.2.4.0 does not use ocean loading and pole tide corrections
|
||||
* 计算因地球潮汐而引起的站点位移校正值 dr
|
||||
* 函数参数，6个：
|
||||
gtime_t tutc I time in utc
|
||||
double *rr I 站点位置 (ecef) (m)
|
||||
int opt I 选项（指定包含哪些潮的影响）
|
||||
1: solid earth tide
|
||||
2: ocean tide loading
|
||||
4: pole tide
|
||||
8: elimate permanent deformation
|
||||
double *erp I 地球自转参数
|
||||
double *odisp I 海潮负荷参数
|
||||
odisp[0+i*6]: consituent i amplitude radial(m)
|
||||
odisp[1+i*6]: consituent i amplitude west (m)
|
||||
odisp[2+i*6]: consituent i amplitude south (m)
|
||||
odisp[3+i*6]: consituent i phase radial (deg)
|
||||
odisp[4+i*6]: consituent i phase west (deg)
|
||||
odisp[5+i*6]: consituent i phase south (deg)
|
||||
(i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1,8:Mf,9:Mm,10:Ssa)
|
||||
double *dr O 因地球潮汐而引起的站点位移校正值 (ecef) (m)
|
||||
返回类型: 无
|
||||
*-----------------------------------------------------------------------------*/
|
||||
extern void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
|
||||
const double *odisp, double *dr)
|
||||
{
|
||||
gtime_t tut;
|
||||
double pos[2],E[9],drt[3],denu[3],rs[3],rm[3],gmst,erpv[5]={0};
|
||||
int i;
|
||||
#ifdef IERS_MODEL
|
||||
double ep[6],fhr;
|
||||
int year,mon,day;
|
||||
#endif
|
||||
|
||||
trace(3,"tidedisp: tutc=%s\n",time_str(tutc,0));
|
||||
|
||||
if (erp) {
|
||||
//1、若有erp，调用 geterp 获取erp参数。
|
||||
geterp(erp,utc2gpst(tutc),erpv);
|
||||
}
|
||||
tut=timeadd(tutc,erpv[2]);
|
||||
|
||||
dr[0]=dr[1]=dr[2]=0.0;
|
||||
|
||||
if (norm(rr,3)<=0.0) return;
|
||||
|
||||
pos[0]=asin(rr[2]/norm(rr,3));
|
||||
pos[1]=atan2(rr[1],rr[0]);
|
||||
xyz2enu(pos,E);
|
||||
|
||||
if (opt&1) { /* solid earth tides */
|
||||
|
||||
/* sun and moon position in ecef */
|
||||
sunmoonpos(tutc,erpv,rs,rm,&gmst);
|
||||
|
||||
#ifdef IERS_MODEL
|
||||
time2epoch(tutc,ep);
|
||||
year=(int)ep[0];
|
||||
mon =(int)ep[1];
|
||||
day =(int)ep[2];
|
||||
fhr =ep[3]+ep[4]/60.0+ep[5]/3600.0;
|
||||
|
||||
/* call DEHANTTIDEINEL */
|
||||
dehanttideinel_((double *)rr,&year,&mon,&day,&fhr,rs,rm,drt);
|
||||
#else
|
||||
//2、若有选项，调用 tide_solid 计算固体潮
|
||||
tide_solid(rs,rm,pos,E,gmst,opt,drt);
|
||||
#endif
|
||||
for (i=0;i<3;i++) dr[i]+=drt[i];
|
||||
}
|
||||
if ((opt&2)&&odisp) { /* ocean tide loading 3、若有选项，调用 tide_oload 计算海潮负荷*/
|
||||
tide_oload(tut,odisp,denu);
|
||||
matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
|
||||
for (i=0;i<3;i++) dr[i]+=drt[i];
|
||||
}
|
||||
if ((opt&4)&&erp) { /* pole tide 4、若有选项，调用 tide_pole 计算极潮*/
|
||||
tide_pole(tut,pos,erpv,denu);
|
||||
matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
|
||||
for (i=0;i<3;i++) dr[i]+=drt[i];
|
||||
}
|
||||
trace(5,"tidedisp: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
|
||||
}
|
||||
@ -16,6 +16,7 @@
|
||||
#include <dfs_posix.h>
|
||||
#include <dfs.h>
|
||||
#include "user_uart.h"
|
||||
#include <rtklib.h>
|
||||
|
||||
|
||||
/* defined the LED0 pin: PF9 */
|
||||
@ -26,6 +27,7 @@ int main(void)
|
||||
{
|
||||
|
||||
uart3_init();
|
||||
uart6_init();
|
||||
task_init();
|
||||
|
||||
rt_pin_mode(LED0_PIN, PIN_MODE_OUTPUT);
|
||||
@ -38,10 +40,24 @@ int main(void)
|
||||
|
||||
}
|
||||
}
|
||||
struct test{
|
||||
int a;
|
||||
int b;
|
||||
int *a_p;
|
||||
int *b_p;
|
||||
}test,* test_p;
|
||||
void test_entry()
|
||||
{
|
||||
const char testbuff[] = {0xD3,0x00,0x13,0x3E,0xD7,0xD3,0x02,0x02,0x98,0x0E,0xDE,0xEF,0x34,0xB4,0xBD,0x62,0xAC,0x09,0x41,0x98,0x6F,0x33,0x36,0x0B,0x98};
|
||||
rt_device_write(serial3,0,testbuff,sizeof(testbuff));
|
||||
// const char testbuff[] = {0xD3,0x00,0x13,0x3E,0xD7,0xD3,0x02,0x02,0x98,0x0E,0xDE,0xEF,0x34,0xB4,0xBD,0x62,0xAC,0x09,0x41,0x98,0x6F,0x33,0x36,0x0B,0x98};
|
||||
// rt_device_write(serial3,0,testbuff,sizeof(testbuff));
|
||||
// LOG_I("rtksvr_t size= %d",sizeof(rtksvr_t));
|
||||
test.a=1;
|
||||
test.b=2;
|
||||
test.a_p=&test.a;
|
||||
test.b_p=&test.b;
|
||||
test_p = &test;
|
||||
LOG_I("%d",&test_p->a);
|
||||
|
||||
}
|
||||
MSH_CMD_EXPORT(test_entry,test_entry);
|
||||
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -1,11 +1,12 @@
|
||||
#MicroXplorer Configuration settings - do not modify
|
||||
ETH.IPParameters=MediaInterface
|
||||
ETH.MediaInterface=ETH_MEDIA_INTERFACE_RMII
|
||||
ETH.MediaInterface=HAL_ETH_RMII_MODE
|
||||
FSMC.IPParameters=WriteOperation1
|
||||
FSMC.WriteOperation1=FSMC_WRITE_OPERATION_ENABLE
|
||||
File.Version=6
|
||||
GPIO.groupedBy=Group By Peripherals
|
||||
KeepUserPlacement=false
|
||||
Mcu.CPN=STM32F407ZGT6
|
||||
Mcu.Family=STM32F4
|
||||
Mcu.IP0=DAC
|
||||
Mcu.IP1=ETH
|
||||
@ -17,7 +18,8 @@ Mcu.IP14=TIM13
|
||||
Mcu.IP15=TIM14
|
||||
Mcu.IP16=USART1
|
||||
Mcu.IP17=USART3
|
||||
Mcu.IP18=USB_OTG_FS
|
||||
Mcu.IP18=USART6
|
||||
Mcu.IP19=USB_OTG_FS
|
||||
Mcu.IP2=FSMC
|
||||
Mcu.IP3=IWDG
|
||||
Mcu.IP4=NVIC
|
||||
@ -26,7 +28,7 @@ Mcu.IP6=RTC
|
||||
Mcu.IP7=SDIO
|
||||
Mcu.IP8=SPI1
|
||||
Mcu.IP9=SPI2
|
||||
Mcu.IPNb=19
|
||||
Mcu.IPNb=20
|
||||
Mcu.Name=STM32F407Z(E-G)Tx
|
||||
Mcu.Package=LQFP144
|
||||
Mcu.Pin0=PC14-OSC32_IN
|
||||
@ -76,70 +78,68 @@ Mcu.Pin48=PG3
|
||||
Mcu.Pin49=PG4
|
||||
Mcu.Pin5=PF3
|
||||
Mcu.Pin50=PG5
|
||||
Mcu.Pin51=PC8
|
||||
Mcu.Pin52=PC9
|
||||
Mcu.Pin53=PA9
|
||||
Mcu.Pin54=PA10
|
||||
Mcu.Pin55=PA11
|
||||
Mcu.Pin56=PA12
|
||||
Mcu.Pin57=PA13
|
||||
Mcu.Pin58=PA14
|
||||
Mcu.Pin59=PC10
|
||||
Mcu.Pin51=PC6
|
||||
Mcu.Pin52=PC7
|
||||
Mcu.Pin53=PC8
|
||||
Mcu.Pin54=PC9
|
||||
Mcu.Pin55=PA9
|
||||
Mcu.Pin56=PA10
|
||||
Mcu.Pin57=PA11
|
||||
Mcu.Pin58=PA12
|
||||
Mcu.Pin59=PA13
|
||||
Mcu.Pin6=PF4
|
||||
Mcu.Pin60=PC11
|
||||
Mcu.Pin61=PC12
|
||||
Mcu.Pin62=PD0
|
||||
Mcu.Pin63=PD1
|
||||
Mcu.Pin64=PD2
|
||||
Mcu.Pin65=PD4
|
||||
Mcu.Pin66=PD5
|
||||
Mcu.Pin67=PG10
|
||||
Mcu.Pin68=PG11
|
||||
Mcu.Pin69=PG13
|
||||
Mcu.Pin60=PA14
|
||||
Mcu.Pin61=PC10
|
||||
Mcu.Pin62=PC11
|
||||
Mcu.Pin63=PC12
|
||||
Mcu.Pin64=PD0
|
||||
Mcu.Pin65=PD1
|
||||
Mcu.Pin66=PD2
|
||||
Mcu.Pin67=PD4
|
||||
Mcu.Pin68=PD5
|
||||
Mcu.Pin69=PG10
|
||||
Mcu.Pin7=PF5
|
||||
Mcu.Pin70=PG14
|
||||
Mcu.Pin71=PB3
|
||||
Mcu.Pin72=PB4
|
||||
Mcu.Pin73=PB5
|
||||
Mcu.Pin74=PB6
|
||||
Mcu.Pin75=PB7
|
||||
Mcu.Pin76=PE0
|
||||
Mcu.Pin77=PE1
|
||||
Mcu.Pin78=VP_IWDG_VS_IWDG
|
||||
Mcu.Pin79=VP_RTC_VS_RTC_Activate
|
||||
Mcu.Pin70=PG11
|
||||
Mcu.Pin71=PG13
|
||||
Mcu.Pin72=PG14
|
||||
Mcu.Pin73=PB3
|
||||
Mcu.Pin74=PB4
|
||||
Mcu.Pin75=PB5
|
||||
Mcu.Pin76=PB6
|
||||
Mcu.Pin77=PB7
|
||||
Mcu.Pin78=PE0
|
||||
Mcu.Pin79=PE1
|
||||
Mcu.Pin8=PH0-OSC_IN
|
||||
Mcu.Pin80=VP_SYS_VS_Systick
|
||||
Mcu.Pin81=VP_TIM2_VS_ClockSourceINT
|
||||
Mcu.Pin82=VP_TIM11_VS_ClockSourceINT
|
||||
Mcu.Pin83=VP_TIM13_VS_ClockSourceINT
|
||||
Mcu.Pin84=VP_TIM14_VS_ClockSourceINT
|
||||
Mcu.Pin80=VP_IWDG_VS_IWDG
|
||||
Mcu.Pin81=VP_RTC_VS_RTC_Activate
|
||||
Mcu.Pin82=VP_SYS_VS_Systick
|
||||
Mcu.Pin83=VP_TIM2_VS_ClockSourceINT
|
||||
Mcu.Pin84=VP_TIM11_VS_ClockSourceINT
|
||||
Mcu.Pin85=VP_TIM13_VS_ClockSourceINT
|
||||
Mcu.Pin86=VP_TIM14_VS_ClockSourceINT
|
||||
Mcu.Pin9=PH1-OSC_OUT
|
||||
Mcu.PinsNb=85
|
||||
Mcu.PinsNb=87
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
Mcu.UserName=STM32F407ZGTx
|
||||
MxCube.Version=6.0.1
|
||||
MxDb.Version=DB.6.0.0
|
||||
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false
|
||||
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false
|
||||
MxCube.Version=6.5.0
|
||||
MxDb.Version=DB.6.0.50
|
||||
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
|
||||
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
|
||||
NVIC.ForceEnableDMAVector=true
|
||||
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false
|
||||
NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false
|
||||
NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false
|
||||
NVIC.OTG_FS_IRQn=true\:0\:0\:false\:false\:true\:true\:true
|
||||
NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false
|
||||
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
|
||||
NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
|
||||
NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
|
||||
NVIC.OTG_FS_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
|
||||
NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
|
||||
NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
|
||||
NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:true\:true
|
||||
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false
|
||||
NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true
|
||||
NVIC.USART1_IRQn=true\:0\:0\:false\:false\:true\:true\:true
|
||||
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false
|
||||
NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
|
||||
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
|
||||
NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
|
||||
NVIC.USART1_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
|
||||
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
|
||||
PA1.Mode=RMII
|
||||
PA1.Signal=ETH_REF_CLK
|
||||
PA9.GPIOParameters=GPIO_PuPd
|
||||
PA9.GPIO_PuPd=GPIO_PULLUP
|
||||
PA9.Mode=Asynchronous
|
||||
PA9.Signal=USART1_TX
|
||||
PA10.GPIOParameters=GPIO_PuPd
|
||||
PA10.GPIO_PuPd=GPIO_PULLUP
|
||||
PA10.Mode=Asynchronous
|
||||
@ -160,6 +160,10 @@ PA5.Locked=true
|
||||
PA5.Signal=COMP_DAC2_group
|
||||
PA7.Mode=RMII
|
||||
PA7.Signal=ETH_CRS_DV
|
||||
PA9.GPIOParameters=GPIO_PuPd
|
||||
PA9.GPIO_PuPd=GPIO_PULLUP
|
||||
PA9.Mode=Asynchronous
|
||||
PA9.Signal=USART1_TX
|
||||
PB10.GPIOParameters=GPIO_PuPd
|
||||
PB10.GPIO_PuPd=GPIO_PULLUP
|
||||
PB10.Mode=Asynchronous
|
||||
@ -203,6 +207,10 @@ PC4.Mode=RMII
|
||||
PC4.Signal=ETH_RXD0
|
||||
PC5.Mode=RMII
|
||||
PC5.Signal=ETH_RXD1
|
||||
PC6.Mode=Asynchronous
|
||||
PC6.Signal=USART6_TX
|
||||
PC7.Mode=Asynchronous
|
||||
PC7.Signal=USART6_RX
|
||||
PC8.Mode=SD_4_bits_Wide_bus
|
||||
PC8.Signal=SDIO_D0
|
||||
PC9.Mode=SD_4_bits_Wide_bus
|
||||
@ -273,7 +281,7 @@ ProjectManager.CustomerFirmwarePackage=
|
||||
ProjectManager.DefaultFWLocation=true
|
||||
ProjectManager.DeletePrevious=true
|
||||
ProjectManager.DeviceId=STM32F407ZGTx
|
||||
ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.25.2
|
||||
ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.27.0
|
||||
ProjectManager.FreePins=false
|
||||
ProjectManager.HalAssertFull=false
|
||||
ProjectManager.HeapSize=0x200
|
||||
@ -291,7 +299,7 @@ ProjectManager.StackSize=0x400
|
||||
ProjectManager.TargetToolchain=MDK-ARM V5
|
||||
ProjectManager.ToolChainLocation=
|
||||
ProjectManager.UnderRoot=false
|
||||
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_USART1_UART_Init-USART1-false-HAL-true,4-MX_SPI1_Init-SPI1-false-HAL-true,5-MX_ETH_Init-ETH-false-HAL-true,6-MX_USART3_UART_Init-USART3-false-HAL-true,7-MX_ADC1_Init-ADC1-false-HAL-true,8-MX_RTC_Init-RTC-false-HAL-true,9-MX_IWDG_Init-IWDG-false-HAL-true,10-MX_TIM14_Init-TIM14-false-HAL-true,11-MX_TIM13_Init-TIM13-false-HAL-true,12-MX_TIM11_Init-TIM11-false-HAL-true,13-MX_SDIO_SD_Init-SDIO-false-HAL-true,14-MX_TIM2_Init-TIM2-false-HAL-true,15-MX_SPI2_Init-SPI2-false-HAL-true,16-MX_TIM4_Init-TIM4-false-HAL-true,17-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,18-MX_FSMC_Init-FSMC-false-HAL-true
|
||||
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_USART1_UART_Init-USART1-false-HAL-true,4-MX_SPI1_Init-SPI1-false-HAL-true,5-MX_ETH_Init-ETH-false-HAL-true,6-MX_USART3_UART_Init-USART3-false-HAL-true,7-MX_RTC_Init-RTC-false-HAL-true,8-MX_IWDG_Init-IWDG-false-HAL-true,9-MX_TIM14_Init-TIM14-false-HAL-true,10-MX_TIM13_Init-TIM13-false-HAL-true,11-MX_TIM11_Init-TIM11-false-HAL-true,12-MX_SDIO_SD_Init-SDIO-false-HAL-true,13-MX_TIM2_Init-TIM2-false-HAL-true,14-MX_SPI2_Init-SPI2-false-HAL-true,15-MX_TIM4_Init-TIM4-false-HAL-true,16-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,17-MX_FSMC_Init-FSMC-false-HAL-true,18-MX_DAC_Init-DAC-false-HAL-true
|
||||
RCC.48MHZClocksFreq_Value=48000000
|
||||
RCC.AHBFreq_Value=168000000
|
||||
RCC.APB1CLKDivider=RCC_HCLK_DIV4
|
||||
@ -430,6 +438,8 @@ USART1.IPParameters=VirtualMode
|
||||
USART1.VirtualMode=VM_ASYNC
|
||||
USART3.IPParameters=VirtualMode
|
||||
USART3.VirtualMode=VM_ASYNC
|
||||
USART6.IPParameters=VirtualMode
|
||||
USART6.VirtualMode=VM_ASYNC
|
||||
USB_OTG_FS.IPParameters=VirtualMode
|
||||
USB_OTG_FS.VirtualMode=Device_Only
|
||||
VP_IWDG_VS_IWDG.Mode=IWDG_Activate
|
||||
|
||||
@ -73,7 +73,8 @@ extern "C" {
|
||||
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
|
||||
|
||||
/* Exported functions prototypes ---------------------------------------------*/
|
||||
void Error_Handler(void);
|
||||
//void Error_Handler(void);
|
||||
#define Error_Handler() _Error_Handler()
|
||||
|
||||
/* USER CODE BEGIN EFP */
|
||||
|
||||
@ -89,5 +90,3 @@ void Error_Handler(void);
|
||||
#endif
|
||||
|
||||
#endif /* __MAIN_H */
|
||||
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
||||
|
||||
@ -1,3 +1,4 @@
|
||||
/* USER CODE BEGIN Header */
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file stm32f4xx_hal_conf_template.h
|
||||
@ -8,16 +9,16 @@
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* <h2><center>© Copyright (c) 2017 STMicroelectronics.
|
||||
* All rights reserved.</center></h2>
|
||||
* 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
|
||||
* 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 __STM32F4xx_HAL_CONF_H
|
||||
@ -76,6 +77,7 @@
|
||||
/* #define HAL_QSPI_MODULE_ENABLED */
|
||||
/* #define HAL_CEC_MODULE_ENABLED */
|
||||
/* #define HAL_FMPI2C_MODULE_ENABLED */
|
||||
/* #define HAL_FMPSMBUS_MODULE_ENABLED */
|
||||
/* #define HAL_SPDIFRX_MODULE_ENABLED */
|
||||
/* #define HAL_DFSDM_MODULE_ENABLED */
|
||||
/* #define HAL_LPTIM_MODULE_ENABLED */
|
||||
@ -94,11 +96,11 @@
|
||||
* (when HSE is used as system clock source, directly or through the PLL).
|
||||
*/
|
||||
#if !defined (HSE_VALUE)
|
||||
#define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */
|
||||
#define HSE_VALUE 8000000U /*!< Value of the External oscillator in Hz */
|
||||
#endif /* HSE_VALUE */
|
||||
|
||||
#if !defined (HSE_STARTUP_TIMEOUT)
|
||||
#define HSE_STARTUP_TIMEOUT ((uint32_t)100U) /*!< Time out for HSE start up, in ms */
|
||||
#define HSE_STARTUP_TIMEOUT 100U /*!< Time out for HSE start up, in ms */
|
||||
#endif /* HSE_STARTUP_TIMEOUT */
|
||||
|
||||
/**
|
||||
@ -114,7 +116,7 @@
|
||||
* @brief Internal Low Speed oscillator (LSI) value.
|
||||
*/
|
||||
#if !defined (LSI_VALUE)
|
||||
#define LSI_VALUE ((uint32_t)32000U) /*!< LSI Typical Value in Hz*/
|
||||
#define LSI_VALUE 32000U /*!< 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.*/
|
||||
@ -122,11 +124,11 @@
|
||||
* @brief External Low Speed oscillator (LSE) value.
|
||||
*/
|
||||
#if !defined (LSE_VALUE)
|
||||
#define LSE_VALUE ((uint32_t)32768U) /*!< Value of the External Low Speed oscillator in Hz */
|
||||
#define LSE_VALUE 32768U /*!< Value of the External Low Speed oscillator in Hz */
|
||||
#endif /* LSE_VALUE */
|
||||
|
||||
#if !defined (LSE_STARTUP_TIMEOUT)
|
||||
#define LSE_STARTUP_TIMEOUT ((uint32_t)5000U) /*!< Time out for LSE start up, in ms */
|
||||
#define LSE_STARTUP_TIMEOUT 5000U /*!< Time out for LSE start up, in ms */
|
||||
#endif /* LSE_STARTUP_TIMEOUT */
|
||||
|
||||
/**
|
||||
@ -135,7 +137,7 @@
|
||||
* frequency, this source is inserted directly through I2S_CKIN pad.
|
||||
*/
|
||||
#if !defined (EXTERNAL_CLOCK_VALUE)
|
||||
#define EXTERNAL_CLOCK_VALUE ((uint32_t)12288000U) /*!< Value of the External audio frequency in Hz*/
|
||||
#define EXTERNAL_CLOCK_VALUE 12288000U /*!< Value of the External audio frequency in Hz*/
|
||||
#endif /* EXTERNAL_CLOCK_VALUE */
|
||||
|
||||
/* Tip: To avoid modifying this file each time you need to use different HSE,
|
||||
@ -145,8 +147,8 @@
|
||||
/**
|
||||
* @brief This is the HAL system configuration section
|
||||
*/
|
||||
#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */
|
||||
#define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */
|
||||
#define VDD_VALUE 3300U /*!< Value of VDD in mv */
|
||||
#define TICK_INT_PRIORITY 0U /*!< tick interrupt priority */
|
||||
#define USE_RTOS 0U
|
||||
#define PREFETCH_ENABLE 1U
|
||||
#define INSTRUCTION_CACHE_ENABLE 1U
|
||||
@ -166,6 +168,7 @@
|
||||
#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_FMPI2C_REGISTER_CALLBACKS 0U /* FMPI2C register callback disabled */
|
||||
#define USE_HAL_FMPSMBUS_REGISTER_CALLBACKS 0U /* FMPSMBUS register callback disabled */
|
||||
#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */
|
||||
#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */
|
||||
#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U /* LPTIM register callback disabled */
|
||||
@ -213,20 +216,20 @@
|
||||
/* 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 ((uint32_t)4U) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
|
||||
#define ETH_TXBUFNB ((uint32_t)4U) /* 4 Tx buffers of size ETH_TX_BUF_SIZE */
|
||||
#define ETH_RXBUFNB 4U /* 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 */
|
||||
|
||||
/* LAN8742A_PHY_ADDRESS Address*/
|
||||
#define LAN8742A_PHY_ADDRESS 1U
|
||||
/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
|
||||
#define PHY_RESET_DELAY ((uint32_t)0x000000FFU)
|
||||
#define PHY_RESET_DELAY 0x000000FFU
|
||||
/* PHY Configuration delay */
|
||||
#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFFU)
|
||||
#define PHY_CONFIG_DELAY 0x00000FFFU
|
||||
|
||||
#define PHY_READ_TO ((uint32_t)0x0000FFFFU)
|
||||
#define PHY_WRITE_TO ((uint32_t)0x0000FFFFU)
|
||||
#define PHY_READ_TO 0x0000FFFFU
|
||||
#define PHY_WRITE_TO 0x0000FFFFU
|
||||
|
||||
/* Section 3: Common PHY Registers */
|
||||
|
||||
@ -249,10 +252,10 @@
|
||||
#define PHY_JABBER_DETECTION ((uint16_t)0x0002U) /*!< Jabber condition detected */
|
||||
|
||||
/* Section 4: Extended PHY Registers */
|
||||
#define PHY_SR ((uint16_t)0x1FU) /*!< PHY status register Offset */
|
||||
#define PHY_SR ((uint16_t)0x10U) /*!< PHY status register Offset */
|
||||
|
||||
#define PHY_SPEED_STATUS ((uint16_t)0x0004U) /*!< PHY Speed mask */
|
||||
#define PHY_DUPLEX_STATUS ((uint16_t)0x0010U) /*!< PHY Duplex mask */
|
||||
#define PHY_SPEED_STATUS ((uint16_t)0x0002U) /*!< PHY Speed mask */
|
||||
#define PHY_DUPLEX_STATUS ((uint16_t)0x0004U) /*!< PHY Duplex mask */
|
||||
|
||||
#define PHY_ISFR ((uint16_t)0x001DU) /*!< PHY Interrupt Source Flag register Offset */
|
||||
#define PHY_ISFR_INT4 ((uint16_t)0x000BU) /*!< PHY Link down inturrupt */
|
||||
@ -447,6 +450,10 @@
|
||||
#include "stm32f4xx_hal_fmpi2c.h"
|
||||
#endif /* HAL_FMPI2C_MODULE_ENABLED */
|
||||
|
||||
#ifdef HAL_FMPSMBUS_MODULE_ENABLED
|
||||
#include "stm32f4xx_hal_fmpsmbus.h"
|
||||
#endif /* HAL_FMPSMBUS_MODULE_ENABLED */
|
||||
|
||||
#ifdef HAL_SPDIFRX_MODULE_ENABLED
|
||||
#include "stm32f4xx_hal_spdifrx.h"
|
||||
#endif /* HAL_SPDIFRX_MODULE_ENABLED */
|
||||
@ -485,5 +492,3 @@
|
||||
#endif
|
||||
|
||||
#endif /* __STM32F4xx_HAL_CONF_H */
|
||||
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
||||
|
||||
@ -83,5 +83,3 @@ void OTG_FS_IRQHandler(void);
|
||||
#endif
|
||||
|
||||
#endif /* __STM32F4xx_IT_H */
|
||||
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
/* USER CODE END Header */
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "main.h"
|
||||
#include "string.h"
|
||||
|
||||
/* Private includes ----------------------------------------------------------*/
|
||||
/* USER CODE BEGIN Includes */
|
||||
@ -61,6 +62,11 @@
|
||||
/* USER CODE END PM */
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
|
||||
ETH_TxPacketConfig TxConfig;
|
||||
ETH_DMADescTypeDef DMARxDscrTab[ETH_RX_DESC_CNT]; /* Ethernet Rx DMA Descriptors */
|
||||
ETH_DMADescTypeDef DMATxDscrTab[ETH_TX_DESC_CNT]; /* Ethernet Tx DMA Descriptors */
|
||||
|
||||
DAC_HandleTypeDef hdac;
|
||||
|
||||
ETH_HandleTypeDef heth;
|
||||
@ -82,6 +88,7 @@ TIM_HandleTypeDef htim14;
|
||||
|
||||
UART_HandleTypeDef huart1;
|
||||
UART_HandleTypeDef huart3;
|
||||
UART_HandleTypeDef huart6;
|
||||
|
||||
PCD_HandleTypeDef hpcd_USB_OTG_FS;
|
||||
|
||||
@ -111,6 +118,7 @@ static void MX_TIM4_Init(void);
|
||||
static void MX_USB_OTG_FS_PCD_Init(void);
|
||||
static void MX_FSMC_Init(void);
|
||||
static void MX_DAC_Init(void);
|
||||
static void MX_USART6_UART_Init(void);
|
||||
/* USER CODE BEGIN PFP */
|
||||
/* Private function prototypes -----------------------------------------------*/
|
||||
|
||||
@ -165,6 +173,7 @@ int main(void)
|
||||
MX_USB_OTG_FS_PCD_Init();
|
||||
MX_FSMC_Init();
|
||||
MX_DAC_Init();
|
||||
MX_USART6_UART_Init();
|
||||
/* USER CODE BEGIN 2 */
|
||||
|
||||
/* USER CODE END 2 */
|
||||
@ -190,12 +199,12 @@ void SystemClock_Config(void)
|
||||
{
|
||||
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
|
||||
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
|
||||
RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
|
||||
|
||||
/** Configure the main internal regulator output voltage
|
||||
*/
|
||||
__HAL_RCC_PWR_CLK_ENABLE();
|
||||
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
|
||||
|
||||
/** Initializes the RCC Oscillators according to the specified parameters
|
||||
* in the RCC_OscInitTypeDef structure.
|
||||
*/
|
||||
@ -214,6 +223,7 @@ void SystemClock_Config(void)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
/** Initializes the CPU, AHB and APB buses clocks
|
||||
*/
|
||||
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|
||||
@ -227,12 +237,6 @@ void SystemClock_Config(void)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_RTC;
|
||||
PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSE;
|
||||
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -252,6 +256,7 @@ static void MX_DAC_Init(void)
|
||||
/* USER CODE BEGIN DAC_Init 1 */
|
||||
|
||||
/* USER CODE END DAC_Init 1 */
|
||||
|
||||
/** DAC Initialization
|
||||
*/
|
||||
hdac.Instance = DAC;
|
||||
@ -259,6 +264,7 @@ static void MX_DAC_Init(void)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
/** DAC channel OUT1 config
|
||||
*/
|
||||
sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
|
||||
@ -267,6 +273,7 @@ static void MX_DAC_Init(void)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
/** DAC channel OUT2 config
|
||||
*/
|
||||
if (HAL_DAC_ConfigChannel(&hdac, &sConfig, DAC_CHANNEL_2) != HAL_OK)
|
||||
@ -291,21 +298,23 @@ static void MX_ETH_Init(void)
|
||||
|
||||
/* USER CODE END ETH_Init 0 */
|
||||
|
||||
static uint8_t MACAddr[6];
|
||||
|
||||
/* USER CODE BEGIN ETH_Init 1 */
|
||||
|
||||
/* USER CODE END ETH_Init 1 */
|
||||
heth.Instance = ETH;
|
||||
heth.Init.AutoNegotiation = ETH_AUTONEGOTIATION_ENABLE;
|
||||
heth.Init.PhyAddress = LAN8742A_PHY_ADDRESS;
|
||||
heth.Init.MACAddr[0] = 0x00;
|
||||
heth.Init.MACAddr[1] = 0x80;
|
||||
heth.Init.MACAddr[2] = 0xE1;
|
||||
heth.Init.MACAddr[3] = 0x00;
|
||||
heth.Init.MACAddr[4] = 0x00;
|
||||
heth.Init.MACAddr[5] = 0x00;
|
||||
heth.Init.RxMode = ETH_RXPOLLING_MODE;
|
||||
heth.Init.ChecksumMode = ETH_CHECKSUM_BY_HARDWARE;
|
||||
heth.Init.MediaInterface = ETH_MEDIA_INTERFACE_RMII;
|
||||
MACAddr[0] = 0x00;
|
||||
MACAddr[1] = 0x80;
|
||||
MACAddr[2] = 0xE1;
|
||||
MACAddr[3] = 0x00;
|
||||
MACAddr[4] = 0x00;
|
||||
MACAddr[5] = 0x00;
|
||||
heth.Init.MACAddr = &MACAddr[0];
|
||||
heth.Init.MediaInterface = HAL_ETH_RMII_MODE;
|
||||
heth.Init.TxDesc = DMATxDscrTab;
|
||||
heth.Init.RxDesc = DMARxDscrTab;
|
||||
heth.Init.RxBuffLen = 1524;
|
||||
|
||||
/* USER CODE BEGIN MACADDRESS */
|
||||
|
||||
@ -315,6 +324,11 @@ static void MX_ETH_Init(void)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
memset(&TxConfig, 0 , sizeof(ETH_TxPacketConfig));
|
||||
TxConfig.Attributes = ETH_TX_PACKETS_FEATURES_CSUM | ETH_TX_PACKETS_FEATURES_CRCPAD;
|
||||
TxConfig.ChecksumCtrl = ETH_CHECKSUM_IPHDR_PAYLOAD_INSERT_PHDR_CALC;
|
||||
TxConfig.CRCPadCtrl = ETH_CRC_PAD_INSERT;
|
||||
/* USER CODE BEGIN ETH_Init 2 */
|
||||
|
||||
/* USER CODE END ETH_Init 2 */
|
||||
@ -364,6 +378,7 @@ static void MX_RTC_Init(void)
|
||||
/* USER CODE BEGIN RTC_Init 1 */
|
||||
|
||||
/* USER CODE END RTC_Init 1 */
|
||||
|
||||
/** Initialize RTC Only
|
||||
*/
|
||||
hrtc.Instance = RTC;
|
||||
@ -762,6 +777,39 @@ static void MX_USART3_UART_Init(void)
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief USART6 Initialization Function
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
static void MX_USART6_UART_Init(void)
|
||||
{
|
||||
|
||||
/* USER CODE BEGIN USART6_Init 0 */
|
||||
|
||||
/* USER CODE END USART6_Init 0 */
|
||||
|
||||
/* USER CODE BEGIN USART6_Init 1 */
|
||||
|
||||
/* USER CODE END USART6_Init 1 */
|
||||
huart6.Instance = USART6;
|
||||
huart6.Init.BaudRate = 115200;
|
||||
huart6.Init.WordLength = UART_WORDLENGTH_8B;
|
||||
huart6.Init.StopBits = UART_STOPBITS_1;
|
||||
huart6.Init.Parity = UART_PARITY_NONE;
|
||||
huart6.Init.Mode = UART_MODE_TX_RX;
|
||||
huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE;
|
||||
huart6.Init.OverSampling = UART_OVERSAMPLING_16;
|
||||
if (HAL_UART_Init(&huart6) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
/* USER CODE BEGIN USART6_Init 2 */
|
||||
|
||||
/* USER CODE END USART6_Init 2 */
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief USB_OTG_FS Initialization Function
|
||||
* @param None
|
||||
@ -904,5 +952,3 @@ void assert_failed(uint8_t *file, uint32_t line)
|
||||
/* USER CODE END 6 */
|
||||
}
|
||||
#endif /* USE_FULL_ASSERT */
|
||||
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
||||
|
||||
@ -269,11 +269,22 @@ void HAL_ETH_MspDeInit(ETH_HandleTypeDef* heth)
|
||||
*/
|
||||
void HAL_RTC_MspInit(RTC_HandleTypeDef* hrtc)
|
||||
{
|
||||
RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
|
||||
if(hrtc->Instance==RTC)
|
||||
{
|
||||
/* USER CODE BEGIN RTC_MspInit 0 */
|
||||
|
||||
/* USER CODE END RTC_MspInit 0 */
|
||||
|
||||
/** Initializes the peripherals clock
|
||||
*/
|
||||
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_RTC;
|
||||
PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSE;
|
||||
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
/* Peripheral clock enable */
|
||||
__HAL_RCC_RTC_ENABLE();
|
||||
/* USER CODE BEGIN RTC_MspInit 1 */
|
||||
@ -777,6 +788,30 @@ void HAL_UART_MspInit(UART_HandleTypeDef* huart)
|
||||
|
||||
/* USER CODE END USART3_MspInit 1 */
|
||||
}
|
||||
else if(huart->Instance==USART6)
|
||||
{
|
||||
/* USER CODE BEGIN USART6_MspInit 0 */
|
||||
|
||||
/* USER CODE END USART6_MspInit 0 */
|
||||
/* Peripheral clock enable */
|
||||
__HAL_RCC_USART6_CLK_ENABLE();
|
||||
|
||||
__HAL_RCC_GPIOC_CLK_ENABLE();
|
||||
/**USART6 GPIO Configuration
|
||||
PC6 ------> USART6_TX
|
||||
PC7 ------> USART6_RX
|
||||
*/
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
GPIO_InitStruct.Alternate = GPIO_AF8_USART6;
|
||||
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
|
||||
|
||||
/* USER CODE BEGIN USART6_MspInit 1 */
|
||||
|
||||
/* USER CODE END USART6_MspInit 1 */
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -826,6 +861,24 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* huart)
|
||||
|
||||
/* USER CODE END USART3_MspDeInit 1 */
|
||||
}
|
||||
else if(huart->Instance==USART6)
|
||||
{
|
||||
/* USER CODE BEGIN USART6_MspDeInit 0 */
|
||||
|
||||
/* USER CODE END USART6_MspDeInit 0 */
|
||||
/* Peripheral clock disable */
|
||||
__HAL_RCC_USART6_CLK_DISABLE();
|
||||
|
||||
/**USART6 GPIO Configuration
|
||||
PC6 ------> USART6_TX
|
||||
PC7 ------> USART6_RX
|
||||
*/
|
||||
HAL_GPIO_DeInit(GPIOC, GPIO_PIN_6|GPIO_PIN_7);
|
||||
|
||||
/* USER CODE BEGIN USART6_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END USART6_MspDeInit 1 */
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -1094,5 +1147,3 @@ void HAL_SRAM_MspDeInit(SRAM_HandleTypeDef* hsram){
|
||||
/* USER CODE BEGIN 1 */
|
||||
|
||||
/* USER CODE END 1 */
|
||||
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
||||
|
||||
@ -259,4 +259,3 @@ void OTG_FS_IRQHandler(void)
|
||||
/* USER CODE BEGIN 1 */
|
||||
|
||||
/* USER CODE END 1 */
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
||||
|
||||
20
rtconfig.h
20
rtconfig.h
@ -129,6 +129,18 @@
|
||||
|
||||
/* Using USB */
|
||||
|
||||
#define RT_USING_USB_DEVICE
|
||||
#define RT_USBD_THREAD_STACK_SZ 4096
|
||||
#define USB_VENDOR_ID 0x0FFE
|
||||
#define USB_PRODUCT_ID 0x0001
|
||||
#define RT_USB_DEVICE_COMPOSITE
|
||||
#define RT_USB_DEVICE_CDC
|
||||
#define RT_USB_DEVICE_NONE
|
||||
#define RT_VCOM_TASK_STK_SIZE 512
|
||||
#define RT_CDC_RX_BUFSIZE 128
|
||||
#define RT_VCOM_SERNO "32021919830108"
|
||||
#define RT_VCOM_SER_LEN 14
|
||||
#define RT_VCOM_TX_TIMEOUT 1000
|
||||
/* end of Using USB */
|
||||
/* end of Device Drivers */
|
||||
|
||||
@ -241,12 +253,6 @@
|
||||
|
||||
/* tools packages */
|
||||
|
||||
#define PKG_USING_ULOG_FILE
|
||||
#define ULOG_FILE_ROOT_PATH "/logs"
|
||||
#define ULOG_FILE_NAME_BASE "ulog.log"
|
||||
#define ULOG_FILE_MAX_NUM 5
|
||||
#define ULOG_FILE_MAX_SIZE 524288
|
||||
#define PKG_USING_ULOG_FILE_V100
|
||||
/* end of tools packages */
|
||||
|
||||
/* system packages */
|
||||
@ -322,6 +328,8 @@
|
||||
#define BSP_USING_UART1
|
||||
#define BSP_USING_UART3
|
||||
#define BSP_UART3_RX_USING_DMA
|
||||
#define BSP_USING_UART6
|
||||
#define BSP_UART6_RX_USING_DMA
|
||||
#define BSP_USING_ONCHIP_RTC
|
||||
#define BSP_RTC_USING_LSE
|
||||
#define BSP_USING_SDIO
|
||||
|
||||
242
task/rtk_task.c
242
task/rtk_task.c
@ -11,61 +11,221 @@
|
||||
#include <rtk_task.h>
|
||||
|
||||
#define LOG_TAG "USER_TASK"
|
||||
#define EVENT_FLAG(...) (1<<__VA_ARGS__)
|
||||
|
||||
#define THREAD_get_rtcm_data_priority 10
|
||||
#define THREAD_rtcm_data_proces_priority 9
|
||||
#define EVENT_REF_RTCM_DATA_FLAG EVENT_FLAG(0)
|
||||
#define EVENT_UAER_RTCM_DATA_FLAG EVENT_FLAG(1)
|
||||
|
||||
#define get_rtcm_data_stack_size DEFAULT_USER_THREAD_STACK_SIZE
|
||||
#define rtcm_data_proces_stack_size DEFAULT_USER_THREAD_STACK_SIZE
|
||||
#define THREAD_get_user_rtcm_data_priority 10
|
||||
#define THREAD_get_ref_rtcm_data_priority 10
|
||||
#define thread_rtk_proces_priority 9
|
||||
|
||||
rtcm_t rtcm_data;
|
||||
struct rt_completion get_rtcm_data_completion;
|
||||
struct rt_completion rtcm_data_process_completion;
|
||||
#define get_rtcm_user_data_stack_size DEFAULT_USER_THREAD_STACK_SIZE
|
||||
#define get_rtcm_ref_data_stack_size DEFAULT_USER_THREAD_STACK_SIZE * 4
|
||||
#define rtk_proces_stack_size DEFAULT_USER_THREAD_STACK_SIZE
|
||||
|
||||
static struct rt_thread get_rtcm_data;
|
||||
static char thread_get_rtcm_data_stack[get_rtcm_data_stack_size];
|
||||
static struct rt_thread rtcm_data_proces;
|
||||
static char thread_rtcm_data_proces_stack[DEFAULT_USER_THREAD_STACK_SIZE];
|
||||
//rtcm_t rtcm_data;
|
||||
struct rt_event uart_event;
|
||||
struct rt_completion rtcm_ref_data_process_completion;
|
||||
struct rt_completion rtcm_user_data_process_completion;
|
||||
|
||||
static struct rt_thread get_user_rtcm_data;
|
||||
static char thread_user_rtcm_data_stack[get_rtcm_user_data_stack_size];
|
||||
static struct rt_thread get_ref_rtcm_data;
|
||||
static char thread_ref_rtcm_data_stack[get_rtcm_ref_data_stack_size];
|
||||
static struct rt_thread rtk_proces;
|
||||
static char thread_rtk_proces_stack[rtk_proces_stack_size];
|
||||
|
||||
|
||||
static char rtcm_buff[2048];
|
||||
struct rt_mutex userf_svrlock;
|
||||
rtksvr_t *svr; /* rtk server struct */
|
||||
//serial3 -->user serial_rtcm_buff[0][]
|
||||
//serial6 -->ref serial_rtcm_buff[1][]
|
||||
|
||||
static void thread_get_rtcm_data(void *parameter)
|
||||
static void thread_get_ref_rtcm_data(void *parameter)
|
||||
{
|
||||
|
||||
init_rtcm(&rtcm_data);
|
||||
rt_completion_init(&get_rtcm_data_completion);
|
||||
while (1)
|
||||
{
|
||||
rt_sem_take(&uart3_simpack.rx_sem, RT_WAITING_FOREVER);
|
||||
rt_completion_wait(&rtcm_ref_data_process_completion, RT_WAITING_FOREVER);
|
||||
//to make sure there is only one sem exist at uart,keep data from being damaged by dma transmit.
|
||||
//rt thread could not get uart status when it receiving data.
|
||||
//it may cross frame lost if the system runs slow.
|
||||
if (rt_completion_wait(&rtcm_data_process_completion, RT_WAITING_FOREVER)==RT_EOK) //need a completion sem connect to rtcm data processing thread.
|
||||
if (rt_sem_take(&uart3_simpack.rx_sem, RT_WAITING_FOREVER) == RT_EOK) //need a completion sem connect to rtcm data processing thread.
|
||||
{
|
||||
get_rtcm_data_completion.flag = RT_UNCOMPLETED;
|
||||
length = uart3_simpack.rx_num;
|
||||
READ_SERIAL3(rtcm_buff);
|
||||
rt_completion_done(&get_rtcm_data_completion);
|
||||
READ_SERIAL6(svr->buff[1]);
|
||||
svr->nb[1] = uart6_simpack.rx_num;
|
||||
rt_event_send(&uart_event, EVENT_REF_RTCM_DATA_FLAG);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void thread_rtcm_data_proces(void *parameter)
|
||||
static void thread_get_user_rtcm_data(void *parameter)
|
||||
{
|
||||
rt_completion_init(&rtcm_data_process_completion);
|
||||
rt_completion_done(&rtcm_data_process_completion);
|
||||
|
||||
while (1)
|
||||
{
|
||||
rt_completion_wait(&get_rtcm_data_completion, RT_WAITING_FOREVER);
|
||||
rtcm_data_process_completion.flag = RT_UNCOMPLETED;
|
||||
for (int var = 0; var < uart3_simpack.rx_num; var++)
|
||||
rt_completion_wait(&rtcm_user_data_process_completion, RT_WAITING_FOREVER);
|
||||
|
||||
//to make sure there is only one sem exist at uart,keep data from being damaged by dma transmit.
|
||||
//rt thread could not get uart status when it receiving data.
|
||||
//it may cross frame lost if the system runs slow.
|
||||
if (rt_sem_take(&uart3_simpack.rx_sem, RT_WAITING_FOREVER) == RT_EOK) //need a completion sem connect to rtcm data processing thread.
|
||||
{
|
||||
input_rtcm3(&rtcm_data, rtcm_buff[var]);
|
||||
READ_SERIAL3(svr->buff[0]);
|
||||
// char test[2048];
|
||||
// READ_SERIAL3(test);
|
||||
svr->nb[0] = uart3_simpack.rx_num;
|
||||
rt_event_send(&uart_event, EVENT_UAER_RTCM_DATA_FLAG);
|
||||
}
|
||||
}
|
||||
rt_completion_done(&rtcm_data_process_completion);
|
||||
}
|
||||
|
||||
/* global variables ----------------------------------------------------------*/
|
||||
|
||||
static int svrcycle = 50; /* server cycle (ms) */
|
||||
static int buffsize = 4096; /* input buffer size (bytes) */
|
||||
static int strfmt[] = { STRFMT_RTCM3, STRFMT_RTCM3 }; /* stream formats */
|
||||
static int nmeacycle = 5000; /* nmea request cycle (ms) */
|
||||
static int nmeareq = 0; /* nmea request type (0:off,1:lat/lon,2:single) */
|
||||
extern prcopt_t prcopt_;
|
||||
extern solopt_t solopt_;
|
||||
static void thread_rtk_proces(void *parameter)
|
||||
{
|
||||
double pos[3] = { 0.0, 0.0, 0.0 }, npos[3] = { 0.0, 0.0, 0.0 };
|
||||
static double nmeapos[] = { 0, 0, 0 };
|
||||
|
||||
svr = (rtksvr_t *) rt_malloc(sizeof(rtksvr_t));
|
||||
if (svr == RT_NULL)
|
||||
{
|
||||
LOG_E("mem not enough for svr&prcopt&solopt");
|
||||
}
|
||||
|
||||
/* STEP-1: 加载RTK配置信息 */
|
||||
resetsysopts(); /* 清空配置信息 */
|
||||
getsysopts(NULL, NULL); /* 指针指向新的配置信息 */
|
||||
|
||||
/* STEP-2: 初始化RTK服务结构体 */
|
||||
rtksvrinit(svr);
|
||||
|
||||
/* STEP-3: 初始化发送的NMEA信息 */
|
||||
/* 设置发送GGA的位置信息, 也可以从单点定位中获取 */
|
||||
pos[0] = nmeapos[0] * D2R;
|
||||
pos[1] = nmeapos[1] * D2R;
|
||||
pos[2] = nmeapos[2];
|
||||
pos2ecef(pos, npos);
|
||||
/* STEP-5: 开始RTK子线程 */
|
||||
rtksvrstart_mini(svr, svrcycle, buffsize, strfmt, nmeacycle, nmeareq, nmeapos, &prcopt_, &solopt_);
|
||||
|
||||
obs_t obs; /* 28 B */
|
||||
// obsd_t data[MAXOBS * 2]; /* 7616 B */
|
||||
sol_t sol = { { 0 } }; /* 199 B */
|
||||
double tt;
|
||||
uint32_t tick, ticknmea, tick1hz, tickreset;
|
||||
uint8_t *p, *q;
|
||||
int i, j, n = 0, fobs[2] = { 0 }, cycle, cputime=0;
|
||||
uint8_t nr, nu;
|
||||
|
||||
svr->state = 1;
|
||||
obs.data = (obsd_t *)rt_malloc(sizeof(obsd_t)*MAXOBS*2);
|
||||
svr->tick = tickget();
|
||||
ticknmea = tick1hz = svr->tick - 1000;
|
||||
tickreset = svr->tick - 30000;
|
||||
|
||||
rt_completion_done(&rtcm_user_data_process_completion);
|
||||
rt_completion_done(&rtcm_ref_data_process_completion);
|
||||
|
||||
while (1)
|
||||
{
|
||||
if (!svr->state)
|
||||
{
|
||||
rt_thread_yield();
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
tick = tickget();
|
||||
uint32_t recevd_event = 0;
|
||||
rt_event_recv(&uart_event, (EVENT_UAER_RTCM_DATA_FLAG | EVENT_REF_RTCM_DATA_FLAG),
|
||||
(RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR), RT_WAITING_FOREVER, &recevd_event);
|
||||
if (recevd_event & EVENT_UAER_RTCM_DATA_FLAG)
|
||||
{
|
||||
rtcm_user_data_process_completion.flag = RT_UNCOMPLETED;
|
||||
fobs[0] = decoderaw(svr, 0);
|
||||
|
||||
}
|
||||
else if (recevd_event & EVENT_REF_RTCM_DATA_FLAG)
|
||||
{
|
||||
rtcm_ref_data_process_completion.flag = RT_UNCOMPLETED;
|
||||
fobs[1] = decoderaw(svr, 1);
|
||||
|
||||
}
|
||||
else {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* 将基准站和移动站的观测数据合二为一 这里重新申请了一个MAXOBS*2 的数组, 我觉得是不是有点问题? */
|
||||
for (i = 0; i < fobs[0]; i++)
|
||||
{
|
||||
obs.n = 0;
|
||||
nr = nu = 0;
|
||||
for (j = 0; j < svr->obs[0][i].n && obs.n < MAXOBS * 2; j++)
|
||||
{
|
||||
obs.data[obs.n++] = svr->obs[0][i].data[j];
|
||||
nu++;
|
||||
}
|
||||
|
||||
for (j = 0; j < svr->obs[1][0].n && obs.n < MAXOBS * 2; j++)
|
||||
{
|
||||
obs.data[obs.n++] = svr->obs[1][0].data[j];
|
||||
nr++;
|
||||
}
|
||||
/* rtk positioning */
|
||||
// rtksvrlock(svr);
|
||||
//// rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav);
|
||||
// rtksvrunlock(svr);
|
||||
|
||||
/* 如果解算结果有效, 设置解算时间点 */
|
||||
if (svr->rtk.sol.stat != SOLQ_NONE)
|
||||
{
|
||||
/* adjust current time */
|
||||
tt = (int) (tickget() - tick) / 1000.0 + DTTOL;
|
||||
timeset(gpst2utc(timeadd(svr->rtk.sol.time, tt)));
|
||||
|
||||
/* write solution */
|
||||
// writesol_mini(svr, i);
|
||||
}
|
||||
/* 如果设定的运算时间到了发现还没有计算完, 则记录没有计算完的观测次数 */
|
||||
if ((int) (tickget() - tick) >= svr->cycle)
|
||||
{
|
||||
svr->prcout += fobs[0] - i - 1;
|
||||
}
|
||||
}
|
||||
/* send null solution if no solution (1hz) */
|
||||
if (svr->rtk.sol.stat == SOLQ_NONE && (int) (tick - tick1hz) >= 1000)
|
||||
{
|
||||
// writesol_mini(svr, 0);
|
||||
tick1hz = tick;
|
||||
}
|
||||
/* send nmea request to base/nrtk input stream */
|
||||
if (svr->nmeacycle > 0 && (int) (tick - ticknmea) >= svr->nmeacycle)
|
||||
{
|
||||
// TODO: 完成send_nmea 到到串口的移植
|
||||
// send_nmea(svr, &tickreset);
|
||||
ticknmea = tick;
|
||||
}
|
||||
cputime = (int) (tickget() - tick);
|
||||
if (cputime > 0)
|
||||
svr->cputime = cputime;
|
||||
|
||||
/* sleep until next cycle */
|
||||
sleepms(15);
|
||||
}
|
||||
}
|
||||
|
||||
while (1)
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
static void user_init_task(rt_thread_t thread, const char *name, void (*entry)(void *parameter), void *stack_start,
|
||||
@ -87,10 +247,26 @@ static void user_init_task(rt_thread_t thread, const char *name, void (*entry)(v
|
||||
}
|
||||
|
||||
/*===========================================================================================================*/
|
||||
|
||||
void task_init(void)
|
||||
{
|
||||
user_init_task(&get_rtcm_data, "get_rtcm_data", thread_get_rtcm_data, thread_get_rtcm_data_stack,
|
||||
get_rtcm_data_stack_size, THREAD_get_rtcm_data_priority, DEFAULT_USER_THREA_TICK, RT_TRUE);
|
||||
user_init_task(&rtcm_data_proces, "rtcm_data_proces", thread_rtcm_data_proces, thread_rtcm_data_proces_stack, rtcm_data_proces_stack_size, THREAD_rtcm_data_proces_priority, DEFAULT_USER_THREA_TICK, RT_TRUE);
|
||||
|
||||
rt_event_init(&uart_event, "uart_event", RT_IPC_FLAG_PRIO);
|
||||
rt_mutex_init(&userf_svrlock,"svrlock",RT_IPC_FLAG_PRIO);
|
||||
rt_completion_init(&rtcm_user_data_process_completion);
|
||||
rt_completion_init(&rtcm_ref_data_process_completion);
|
||||
|
||||
rt_thread_t trd=rt_thread_create("rtk_proces", thread_rtk_proces, RT_NULL, 3000, thread_rtk_proces_priority, DEFAULT_USER_THREA_TICK);
|
||||
rt_thread_startup(trd);
|
||||
user_init_task(&get_user_rtcm_data, "thread_get_user_rtcm_data", thread_get_user_rtcm_data,
|
||||
thread_user_rtcm_data_stack,
|
||||
get_rtcm_user_data_stack_size, THREAD_get_user_rtcm_data_priority, DEFAULT_USER_THREA_TICK, RT_TRUE);
|
||||
user_init_task(&get_ref_rtcm_data, "thread_get_ref_rtcm_data", thread_get_ref_rtcm_data,
|
||||
thread_ref_rtcm_data_stack,
|
||||
get_rtcm_ref_data_stack_size, THREAD_get_ref_rtcm_data_priority, DEFAULT_USER_THREA_TICK, RT_TRUE);
|
||||
// user_init_task(&rtk_proces, "rtk_proces", thread_rtk_proces, thread_rtk_proces_stack,
|
||||
// rtk_proces_stack_size, thread_rtk_proces_priority, DEFAULT_USER_THREA_TICK, RT_TRUE);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
*
|
||||
* Change Logs:
|
||||
* Date Author Notes
|
||||
* 2022-06-15 ¸µÐ¡¶þ the first version
|
||||
* 2022-06-15 Fize the first version
|
||||
*/
|
||||
#ifndef TASK_RTK_TASK_H_
|
||||
#define TASK_RTK_TASK_H_
|
||||
@ -19,10 +19,16 @@
|
||||
#define RT_COMPLETED 1
|
||||
#define RT_UNCOMPLETED 0
|
||||
|
||||
#define DEFAULT_USER_THREAD_STACK_SIZE 1024
|
||||
#define DEFAULT_USER_THREA_TICK 5
|
||||
#define DEFAULT_USER_THREAD_STACK_SIZE 4096
|
||||
#define DEFAULT_USER_THREA_TICK 2
|
||||
|
||||
|
||||
extern struct rt_mutex userf_svrlock;
|
||||
struct rt_mutex userf_svrlock;
|
||||
#define rtksvrlock(svr) rt_mutex_take(&userf_svrlock,RT_WAITING_FOREVER)
|
||||
#define rtksvrunlock(svr) rt_mutex_release(&userf_svrlock)
|
||||
|
||||
extern uart_simpack uart3_simpack; //from user_uart.h
|
||||
extern rtcm_t rtcm_data; //from rtklib.h
|
||||
extern uart_simpack uart6_simpack; //from user_uart.h
|
||||
void task_init(void);
|
||||
#endif /* TASK_RTK_TASK_H_ */
|
||||
|
||||
@ -9,57 +9,79 @@
|
||||
*/
|
||||
#include "user_uart.h"
|
||||
|
||||
|
||||
|
||||
static rt_err_t uart3_tx_complete(rt_device_t dev, void *buffer)
|
||||
{
|
||||
//todo: uart3_tx_complete sem: do nothing for now.
|
||||
//since dma for tx not enabled,so this callback will not be called at any time.
|
||||
LOG_I("massage sent %s",strlen(buffer));
|
||||
return RT_EOK;
|
||||
}
|
||||
|
||||
//dev 设备句柄（回调函数参数）
|
||||
//size 缓冲区数据大小（回调函数参数）
|
||||
//still in ISR func,so can only use ulog at asynchronous mode.
|
||||
static rt_err_t uart3_rx_complete(rt_device_t dev, rt_size_t size)
|
||||
{
|
||||
|
||||
uart3_simpack.rx_num = size;
|
||||
rt_sem_release(&uart3_simpack.rx_sem);
|
||||
return RT_EOK;
|
||||
}
|
||||
static rt_err_t uart6_rx_complete(rt_device_t dev, rt_size_t size)
|
||||
{
|
||||
uart6_simpack.rx_num = size;
|
||||
rt_sem_release(&uart6_simpack.rx_sem);
|
||||
return RT_EOK;
|
||||
}
|
||||
|
||||
rt_device_t uart3_init(void)
|
||||
{
|
||||
struct serial_configure config;
|
||||
|
||||
serial3 = rt_device_find(USER_UART_NAME);
|
||||
serial3 = rt_device_find(USER_UART3_NAME);
|
||||
if (serial3 == RT_NULL)
|
||||
{
|
||||
LOG_E("could not find device: %s", USER_UART_NAME);
|
||||
LOG_E("could not find device: %s", USER_UART3_NAME);
|
||||
return RT_NULL;
|
||||
}
|
||||
config.baud_rate = BAUD_RATE_115200; //修改波特率为 115200
|
||||
config.baud_rate = BAUD_RATE_460800; //修改波特率为 460800
|
||||
config.data_bits = DATA_BITS_8; //数据位 8
|
||||
config.stop_bits = STOP_BITS_1; //停止位 1
|
||||
config.bufsz = 2048; //修改缓冲区 buff size 为 2048
|
||||
config.bufsz = 4096; //修改缓冲区 buff size 为 2048
|
||||
config.parity = PARITY_NONE; //无奇偶校验位
|
||||
rt_device_control(serial3, RT_DEVICE_CTRL_CONFIG, &config);
|
||||
if ((rt_device_set_tx_complete(serial3, uart3_tx_complete) || rt_device_set_rx_indicate(serial3, uart3_rx_complete))
|
||||
!= RT_EOK)
|
||||
if (rt_device_set_rx_indicate(serial3, uart3_rx_complete) != RT_EOK)
|
||||
{
|
||||
LOG_E("could not set %s 's rx|tx callback func", USER_UART_NAME);
|
||||
LOG_E("could not set %s 's rx callback func", USER_UART3_NAME);
|
||||
return RT_NULL;
|
||||
}
|
||||
//do not ust dma for rx and tx at same time
|
||||
if (rt_device_open(serial3, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_DMA_RX | RT_DEVICE_OFLAG_RDWR))
|
||||
{
|
||||
LOG_E("could not open device: %s", USER_UART_NAME);
|
||||
LOG_E("could not open device: %s", USER_UART3_NAME);
|
||||
return RT_NULL;
|
||||
}
|
||||
rt_sem_init(&uart3_simpack.rx_sem, "rx3_sem", 0, RT_IPC_FLAG_PRIO);
|
||||
rt_sem_init(&uart3_simpack.tx_sem, "tx3_sem", 0, RT_IPC_FLAG_PRIO);
|
||||
return serial3;
|
||||
}
|
||||
rt_device_t uart6_init(void)
|
||||
{
|
||||
struct serial_configure config;
|
||||
serial6 = rt_device_find(USER_UART6_NAME);
|
||||
if (serial6 == RT_NULL)
|
||||
{
|
||||
LOG_E("could not find device: %s", USER_UART6_NAME);
|
||||
return RT_NULL;
|
||||
}
|
||||
config.baud_rate = BAUD_RATE_460800; //修改波特率为 460800
|
||||
config.data_bits = DATA_BITS_8; //数据位 8
|
||||
config.stop_bits = STOP_BITS_1; //停止位 1
|
||||
config.bufsz = 4096; //修改缓冲区 buff size 为 2048
|
||||
config.parity = PARITY_NONE; //无奇偶校验位
|
||||
rt_device_control(serial6, RT_DEVICE_CTRL_CONFIG, &config);
|
||||
if (rt_device_set_rx_indicate(serial6, uart6_rx_complete) != RT_EOK)
|
||||
{
|
||||
LOG_E("could not set %s 's rx callback func", USER_UART6_NAME);
|
||||
return RT_NULL;
|
||||
}
|
||||
//do not ust dma for rx and tx at same time
|
||||
if (rt_device_open(serial6, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_DMA_RX | RT_DEVICE_OFLAG_RDWR))
|
||||
{
|
||||
LOG_E("could not open device: %s", USER_UART6_NAME);
|
||||
return RT_NULL;
|
||||
}
|
||||
rt_sem_init(&uart6_simpack.rx_sem, "rx6_sem", 0, RT_IPC_FLAG_PRIO);
|
||||
return serial6;
|
||||
}
|
||||
|
||||
|
||||
@ -15,12 +15,14 @@
|
||||
#include <ulog.h>
|
||||
#include <rtdef.h>
|
||||
|
||||
#define USER_UART_NAME "uart3"
|
||||
#define USER_UART3_NAME "uart3"
|
||||
#define USER_UART6_NAME "uart6"
|
||||
|
||||
#define READ_SERIAL3(...) rt_device_read(serial3,0,__VA_ARGS__,uart3_simpack.rx_num)
|
||||
#define READ_SERIAL6(...) rt_device_read(serial6,0,__VA_ARGS__,uart3_simpack.rx_num)
|
||||
|
||||
rt_device_t serial3;
|
||||
|
||||
rt_device_t serial6;
|
||||
struct uart_simpack
|
||||
{
|
||||
rt_size_t rx_num;
|
||||
@ -32,7 +34,9 @@ typedef struct uart_simpack uart_simpack;
|
||||
|
||||
|
||||
uart_simpack uart3_simpack;
|
||||
uart_simpack uart6_simpack;
|
||||
|
||||
rt_device_t uart3_init(void);
|
||||
rt_device_t uart6_init(void);
|
||||
|
||||
#endif /* APPLICATIONS_USER_UART_USER_UART_H_ */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user