超声状态机及后处理1.0

超声状态机及后处理

整车系统

超声波雷达 12
摄像头12
激光雷达3
毫米波雷达5
硬件连接

API

td {white-space:pre-wrap;border:1px solid #dee0e3;} SPI_Transfer_uss2(uint8_t *SPITxBuffer, uint8_t *SPIRxBuffer, uint8_t bytes) SPI USS2收发函数 SPI_Transfer_uss1(uint8_t *SPITxBuffer, uint8_t *SPIRxBuffer, uint8_t bytes) SPI USS1收发函数 void Uss_Master1Dcr1bIsr(void); USS1Dcr1b（USS1DSI3通道1数据准备好）中断服务函数 void Uss_Master1Dcr2bIsr(void); USS1Dcr2b（USS1DSI3通道2数据准备好）中断服务函数 void Uss_Master2Dcr1bIsr(void); USS2Dcr1b（USS2DSI3通道1数据准备好）中断服务函数 void Uss_Master2Dcr2bIsr(void); USS2Dcr2b（USS2DSI3通道2数据准备好）中断服务函数 void Uss_Master1RfcIsr(void); USS1RFC（E521.42可以进行 SPI/DSI3通信标志）中断服务函数 void Uss_Master2RfcIsr(void); USS2RFC（E521.42可以进行 SPI/DSI3通信标志）中断服务函数 USS1_Master1_PwmOutput() 启动USS1PWM输出 USS2_Master1_PwmOutput() 启动USS2PWM输出 USS_Power_OFF() //三路电源开 USS_Power_ON() //三路电源关 USS_F_Power_ON(); //F路电源开 USS_F_Power_OFF(); //F路电源关 USS_R_Power_ON(); //R路电源开 USS_R_Power_OFF(); //R路电源关 USS_S_Power_ON(); //S路电源开 USS_S_Power_OFF(); //S路电源关 void Uss_Master1ResetSet(void); /*GPIO输出高电平*/E521.42复位引脚 void Uss_Master1ResetClear(void); /*GPIO输出低电平*/E521.42复位引脚 void Uss_Master2ResetSet(void); /*GPIO输出高电平*/E521.42复位引脚 void Uss_Master2ResetClear(void); /*GPIO输出低电平*/E521.42复位引脚 bool Uss_Master1Intb(void); /*master1 intb input，返回bool型，0是低电平，1是高电平*/ bool Uss_Master2Intb(void); /*master2 intb input，返回bool型，0是低电平，1是高电平*/ Uss_Master1intbIsr() uss1intb中断服务函数,E521.42中断 Uss_Master2intbIsr() uss2intb中断服务函数,E521.42中断

超声开发需求

超声MCU集成方案

USS驱动策略

1 项目需求分析 ：

1.基于tc399硬件平台驱动8路UPA雷达以及4路APA雷达
2.基于底软以及API接口开发
3.雷达的直接测量需要考虑到互相之间的干扰
4.雷达的间接测量需要考虑到测量周期以及实时性
雷达外设相关软硬件接口

引脚定义 TC399端口 功能定义
USS1_CLKREF B12-P13.12 500KHz PWM OUT
USS1_RSTB F12-P10.0 GPIO-OUT
USS1_CS G11-P10.4 QSPI1_CS_CH8
USS1_CLK F10-P10.2 QSPI1_CLK
USS1_SPI_MOSI F11-P10.3 QSPI1_MOSI
USS1_SPI_MISO G12_P10.1 QSPI1_MISO
USS1_RFC B11-13.14 ICU Interrupt
USS1_INTB A15-B13_7 ICU Interrupt
USS1_DCR1B AB19-P33.15 ICU Interrupt
USS1_DCR2B B13-13.10 ICU Interrupt

USS2_CLKREF T21-P22_10 500KHz PWM OUT
USS2_RSTB T22-P22_11 GPIO-OUT
USS2_CS W21-P22_4 QSPI0_CS_CH12
USS2_CLK V22- P22_7 QSPI0_CLK
USS2_SPI_MOSI W22-P22_5 QSPI0_MOSI
USS2_SPI_MISO V22- P22_6 QSPI0_MISO
USS2_RFC N25_ P20_0 ICU Interrupt
USS2_INTB U22_P22_9 ICU Interrupt
USS2_DCR1B P22-P21_0 ICU Interrupt
USS2_DCR2B B5-P10_14 ICU Interrupt

VIP_USS_EN L22-P20_6 GPIO-OUT
VIP_USS_INT L24_P20_7 GPIO-IN
VIP_PWR_I2C_SDA K19-P15_5 I2C_SDA
VIP_PWR_I2C_SCL F22_p15_4 I2C_SCL
雷达探头连接

5. E521.42 SPI协议
   
   
   
   
   

6. 标准测量驱动状态机
   

7. 直接测量策略

12颗雷达共分为四组
CH1: F2 UPA, R2 UPA, F1 APA
CH2: F3 UPA, R3 UPA, R1 APA
CH3: F4 UPA, R4 UPA, F6 APA
CH4: F5 UPA, R5 UPA, R6 APA
每个测量周期只开启一组雷达的直接测量，保证测量时不会出现互相干扰的情况；
该种模式的优势为符合雷达的硬件连接，每次测量仅需要一次测量指令

以前系统为例，距离数据结构如下，绿色为直接测量数据

9. 间接测量策略

12颗雷达共分为四组
CH1: F2 UPA, R2 UPA, F1 APA
CH2: F3 UPA, R3 UPA, R1 APA
CH3: F4 UPA, R4 UPA, F6 APA
CH4: F5 UPA, R5 UPA, R6 APA
每个测量周期开启一组雷达的直接测量，同时开启其他组雷达的间接测量，每次测量不仅获取该组探头的直接测量数据，也获取相邻探头的间接测量数据，构成障碍物和两雷达之间的三角结构，从而计算出障碍物在空间上的具体位置。
该种模式的优势为满足算法的数据输入需求，但每次测量需要下发两条SPI指令

以前系统为例，距离数据结构如下，绿色为直接测量数据，白色为间接测量数据

如图示，超声雷达分布分为三个系统：前系统（Front），后系统（Rear）和侧系统（Side）。前系统和后系统使用UPA雷达，官方推荐工作频率为55.5KHz，侧系统使用APA雷达，官方推荐工作频率为48KHz。三个系统相互独立，互相之间没有干扰。系统内部需要考虑同频干扰的问题。

前，后，侧三个系统具有独立性，可以配置三组独立的状态机，但又由于三个系统的雷达都相互接在同一个MASTER芯片上，无法完全独立，顾采用固定周期的方式进行测量指令下发，在每个测量周期开始时，计算出每个雷达的工作状态，采用统一下发指令的方式。

模块架构（2022.3.30)

AVP模块架构 – 核间布署

驱动和算法接口

•超声探头原始输出：

/// @brief collect complete data set from one sensor, including monitoring Data
typedef struct {  
diagData_t diagData;
  echo_t  echoData[MAX_DSI_ECHOS]; 
 nfd_t   nfdData;  
uint8_t echoCnt;  
uint16_t monCnt;  
uint16_t monLen;  
uint16_t monCh1[MAXMON];  
uint16_t monCh2[MAXMON];  
uint16_t monCh3[MAXMON];  
uint16_t monCh4[MAXMON];
} sensorData_t;


/// @brief Dignostic data
typedef struct {  
int8_t     chipTemp;  
uint16_t   vSUP;  
uint16_t   noiseSum;  
uint8_t   noiseCount;  // Burst  
uint8_t   burstLength;  
uint16_t   vTank1;  
uint16_t   vTank2;  // Ring  
uint8_t   ringCount; 
 uint16_t   ringSamples;  
uint16_t   rtm;
} diagData_t;


/// @brief Near Field Data
typedef struct {  
bool nfd1_flag;  
bool nfd2_flag;  
uint8_t nfd1_echo;  
uint8_t nfd2_echo;  
uint8_t nfd3_echo;  
uint8_t nfd_result;
} nfd_t;


/// @ Echo Data for standard or advanced path
typedef struct {  
uint16_t echoAmp;  
uint16_t echoTimeStamp;  
uint8_t  echoType;  
uint8_t  confidence;
} echo_t;

/// @brief collect complete data set from one sensor, including monitoring Data
typedef struct {
diagData_t diagData;
echo_t echoData[MAX_DSI_ECHOS];
nfd_t nfdData;
uint8_t echoCnt;
uint16_t monCnt;
uint16_t monLen;
uint16_t monCh1[MAXMON];
uint16_t monCh2[MAXMON];
uint16_t monCh3[MAXMON];
uint16_t monCh4[MAXMON];
} sensorData_t;

/// @brief Dignostic data
typedef struct {
int8_t chipTemp;
uint16_t vSUP;
uint16_t noiseSum;
uint8_t noiseCount; // Burst
uint8_t burstLength;
uint16_t vTank1;
uint16_t vTank2; // Ring
uint8_t ringCount;
uint16_t ringSamples;
uint16_t rtm;
} diagData_t;

/// @brief Near Field Data
typedef struct {
bool nfd1_flag;
bool nfd2_flag;
uint8_t nfd1_echo;
uint8_t nfd2_echo;
uint8_t nfd3_echo;
uint8_t nfd_result;
} nfd_t;

/// @ Echo Data for standard or advanced path
typedef struct {
uint16_t echoAmp;
uint16_t echoTimeStamp;
uint8_t echoType;
uint8_t confidence;
} echo_t;

根据原始回波信息转换得到的障碍物信息：

typedef struct{  
uint16_t Amp;//幅值  
uint16_t TimeDuration;//时间 发波到收到回波  
uint16_t width;//回波宽度   回波强度得到目标回波宽度  
uint8_t  confidence;//置信度  
uint16_t distance;//距离  根据时间算出的目标距离cm
} object_t;

typedef struct {    
uint64_t timestamp;  
object_t objData[MAX_DSI_OBJECTS];  
uint8_t objCnt;
} objData_t;

typedef struct{
uint16_t Amp;//幅值
uint16_t TimeDuration;//时间 发波到收到回波
uint16_t width;//回波宽度 回波强度得到目标回波宽度
uint8_t confidence;//置信度
uint16_t distance;//距离 根据时间算出的目标距离cm
} object_t;

typedef struct {
uint64_t timestamp;
object_t objData[MAX_DSI_OBJECTS];
uint8_t objCnt;
} objData_t;

驱动滤波算法之后的输出

typedef struct {    
    uint64_t timestamp;  
    uint16_t distance;  
    uint16_t Amp;
    uint16_t wide;
    uint8_t confidence;
} ConfirmInfo_t;

typedef struct {
uint64_t timestamp;
uint16_t distance;
uint16_t Amp;
uint16_t wide;
uint8_t confidence;
} ConfirmInfo_t;
状态机

•1. tc399MCAL初始化，包括 GPIO/PWM/SPI/IIC 等；TC399的 PWM 产生500kHz 给 E521.42的 CLKREF；TC399产生复位脉冲给 E521.42的 RESB，复位 E521.42；TC399等待 E521.42的 RFC（ready for communication），直到高电平；TC399通过 SPI 配置 E521.42寄存器；TC399通过 SPI 发送 TDMA Scheme 到 E521.42；
•2. TC399通过 SPI 读取 E521.42状态（read IC status）；
•3. TC399通过 SPI 发送 CRM 命令，E524.17进入开始 std 测量，并进入 PDCM 模式；
•4. TC399通过 SPI 发送 BRC 命令，E521.42开始进行 PDCM 数据采集；
•5. TC399等待 DCB1B 为低电平，然后通过 SPI 读取 PDCM 数据；
•6. 判断 PDCM 数据采集是否完毕，如未完毕回到5；
•7. 完成 PDCM 数据采集， TC399将结果发送出来，然后切换通道执行3，反复循环执行，实现12个探头的数据循环获取。

超声部分状态机

包含模块
•UssWorkIdleProc
•UssWorkInitProc
•UssWorkResetSonarProc
•UssWorkStartMeasureProc
•UssWorkPdcmProc
•UssWorkDealDataProc
•UssWorkObjectInfoDealProc
•UssWorkSwitchChannelProc

AVP_SWC_USS_Init初始化

•初始化中断服务函数
•复位E521,42
•等待ref
•启动PWM输出
•配置 E521.42寄存器
•通过 SPI 发送 TDMA Scheme 到 E521.42，配置不同地址的 E524.17占用 DSI3总线的时间段（不同地址占用的时间不同）

UssWorkIdleProc空闲任务

•初始化起始通道、设置ID、查询ID、初始DSI通道、初始spi用于循环读取
•初始化设置电源通道1导通

UssWorkInitProc初始化各探头ID

•0、使用ID1-6循环查找设备
•1、判断是否读取到ID，未读取到则返回0继续查找设备，判断ID与期望ID符合，不符合则进入状态2，符合则进入状态5
•2、设置ID进入542.17的RAM中
•3、将设置ID从ram写入到eeprom中
•4、保存eeprom中的ID
•5、切换通道或电源进行下一个探头ID初始化，如所有ID初始化完成则跳出UssWorkInitProc进入下一状态

UssWorkInitProc初始化各探头ID

•1：写配置参数，从EEPROM复制参数到RAM，密钥：0xAA
•2：允许EEPROM编程操作，密钥：0x66
•4：将配置参数从RAM写入EEPROM中，密钥：0x33
•8：将从机ID号从RAM写入EEPROM中，密钥：0x55
UssWorkStartMeasureProc启动采样
•E524.17发波命令，通道写寄存器设置启动测量为真
UssWorkPdcmProc获取PDCM数据
•1、执行PDCM帧获取，默认PDCM_FRAME_NUM = 20帧
•2、collectDataPDCM_Poll读取后存储到pdcm_response_scheme，直到所有帧存储完成，切换状态
•3、processDataPDCM将12个探头帧数据进行处理，获取到所有数据返回true，探头帧数据根据帧类型解析，从一个semsor收集完整的数据集，包括监测数据存放在sensorData中，包括诊断数据diagData_t、标准模式回波数据echo_t、场数据nfd_t等
UssWorkDealDataProc数据处理
•1、processObjData得到objData，每个探头包含8个障碍物信息包含
§uint16_t Amp;//幅值
§uint16_t TimeStamp;//时间 发波到收到回波ms
§uint16_t width;//回波宽度 回波强度得到目标回波宽度
§uint8_t confidence;//置信度
§uint16_t distance;//距离 根据时间算出的目标距离cm， distance = TimeStamp/2340/100100
对于间接回波数据，初步设计每个探头处理4个位置数据，一发三收，参考小鹏的设计可只保留前方中间2个及后方中间2个探头数据供算法使用
•2、ringTimeUpdate
3、checkRingTimeErr

UssWorkObjectInfoDealProc障碍物信息处理

•1、聚类，过滤 跟踪 确认得到的障碍物信息
•2、输出u16ObjConfirmDistance记录每个传感器的实际距离数据
SP_PK_SensorPreProc_process感知预处理
•1、获取DR、控车和超声输出ObjData作为输入，数据如下
•2、输出经过障碍物具体位置算法、同频干扰抑制算法、噪声滤波算法后的位置信息及超声数据

SP_PK_SensorPreProc_process输入数据

SP_PK_SensorPreProc_process输出数据

SP_PK_SonarObj_process障碍物信息处理

•1、障碍物信息处理，获取SP_PK_SensorPreProc_process的输出数据，进行算法处理，实现避障功能

SP_PK_SonarObj_process所需输入

SP_PK_SonarObj_process输出

SP_PK_SonarPLD_process
•1、PLD模块获取Obj模块的输出进行算法处理实现PLD功能

SP_PK_SonarPLD_process输入

SP_PK_SonarPLD_process输出

DR移植过程总结

DR输入参数分析

/* VCU-方向盘-脉冲输入信号 */
typedef struct tVM_VcuData
{
long long i64Timestamp;  // COMMON_VEHICLE_DATA 下 time_stamp
int nMoveState;/*运动方向 1:前进,-1:后退, 0:其他COMMON_VEHICLE_DATA 下 vehicle_movement_state */ 
float fSteerAngleD;/* 方向盘转角 deg 左+ 右-  COMMON_VEHICLE_DATA 下 steer_angle  */
int nPulseFL; // COMMON_VEHICLE_DATA FLWhlDistPlsCntr
int nPulseFR; // COMMON_VEHICLE_DATA FRWhlDistPlsCntr
int nPulseRL; // COMMON_VEHICLE_DATA RLWhlDistPlsCntr
int nPulseRR; // COMMON_VEHICLE_DATA RRWhlDistPlsCntr
float v_speed;
int spd_valid_signal;
}tVM_VcuData; 

/* VCU-方向盘-脉冲输入信号 */
typedef struct tVM_VcuData
{
long long i64Timestamp; // COMMON_VEHICLE_DATA 下 time_stamp
int nMoveState;/*运动方向 1:前进,-1:后退, 0:其他COMMON_VEHICLE_DATA 下 vehicle_movement_state /
float fSteerAngleD;/ 方向盘转角 deg 左+ 右- COMMON_VEHICLE_DATA 下 steer_angle */
int nPulseFL; // COMMON_VEHICLE_DATA FLWhlDistPlsCntr
int nPulseFR; // COMMON_VEHICLE_DATA FRWhlDistPlsCntr
int nPulseRL; // COMMON_VEHICLE_DATA RLWhlDistPlsCntr
int nPulseRR; // COMMON_VEHICLE_DATA RRWhlDistPlsCntr
float v_speed;
int spd_valid_signal;
}tVM_VcuData;

DR输出参数分析

typedef struct _tComDR_Embed_Output
{
int64_tnTimestamp;// vcu timestamp
tPOSE3FsCurPose;// current comdr odometry pose
tDPOSE3FsDetPose;// pose offset from last frame
float32_tfTurnR;// Turning radius
float32_tfFwdDist;// Forward distance +Forward, -Backward
float32_t   fSpdFiltered;   // filtered speed using pulse
float32_t   fOdo;    // accumulated odometry distance
uint8_tnVehMoveSt;// 0-still, 1-forward, 2-backward
}tComDR_Embed_Output;

typedef struct _tComDR_Embed_Output
{
int64_tnTimestamp;// vcu timestamp
tPOSE3FsCurPose;// current comdr odometry pose
tDPOSE3FsDetPose;// pose offset from last frame
float32_tfTurnR;// Turning radius
float32_tfFwdDist;// Forward distance +Forward, -Backward
float32_t fSpdFiltered; // filtered speed using pulse
float32_t fOdo; // accumulated odometry distance
uint8_tnVehMoveSt;// 0-still, 1-forward, 2-backward
}tComDR_Embed_Output;

需标定参数

typedef struct tVM_Params
{
int   nVehicleType;//车辆ID
int   nMaxPulse;    // 最大脉冲数              DR需求
float fDistPerPulse;// 单个脉冲距离（后轮，m） DR需求
float fVehLen;// 车辆长度（m）
float fVehWid;// 车辆宽度（m）- 不带后视镜
float fRearViewMirrorX; // 
float fRearViewMirrorY; // 
float fMirrorLen;
float fAxisLen;// 车辆前后轴距离（m）          DR需求
float fFrontAxis2Bumper; //front axis to front bumper
float fRearAxis2Bumper;  //rear axis to rear bumper
float Rr_min; // minimum turning radius based on the axis middle (m)
float EPS_Spd; // steering spd (angles/s)
float EPS_Angle_Max; // max steering angle in automatic control mode
float EPS_Angle_Min;     // min steering angle in automatic control mode
float EPS_Delaydt; // delay time of eps response

float EPS_Offset; // offset of the eps angle
float Vehicle_StartUp_Delay_Time;

double dLfParam[4];    ///< wheel angle to radius parameter, left front
double dRfParam[4];    ///< wheel angle to radius parameter, right front
double dLrParam[4];    ///< wheel angle to radius parameter, left rear
double dRrParam[4];    ///< wheel angle to radius parameter, right rear

double dLfParamA2S[4];    ///< radius parameter to wheel angle, left front
double dRfParamA2S[4];    ///< radius parameter to wheel angle, right front
double dLrParamA2S[4];    ///< radius parameter to wheel angle, left rear
double dRrParamA2S[4];    ///< radius parameter to wheel angle, right rear

USS_MODEL sonar;

float Car_TurningModel_Pfac_ROU_C[24];
float Car_TurningModel_Nfac_ROU_C[24];
float Car_TurningModel_fac_EPS_C[24];
}tVM_Params;

typedef struct tVM_Params
{
int nVehicleType;//车辆ID
int nMaxPulse; // 最大脉冲数 DR需求
float fDistPerPulse;// 单个脉冲距离（后轮，m） DR需求
float fVehLen;// 车辆长度（m）
float fVehWid;// 车辆宽度（m）- 不带后视镜
float fRearViewMirrorX; //
float fRearViewMirrorY; //
float fMirrorLen;
float fAxisLen;// 车辆前后轴距离（m） DR需求
float fFrontAxis2Bumper; //front axis to front bumper
float fRearAxis2Bumper; //rear axis to rear bumper
float Rr_min; // minimum turning radius based on the axis middle (m)
float EPS_Spd; // steering spd (angles/s)
float EPS_Angle_Max; // max steering angle in automatic control mode
float EPS_Angle_Min; // min steering angle in automatic control mode
float EPS_Delaydt; // delay time of eps response

float EPS_Offset; // offset of the eps angle
float Vehicle_StartUp_Delay_Time;

double dLfParam[4]; ///< wheel angle to radius parameter, left front
double dRfParam[4]; ///< wheel angle to radius parameter, right front
double dLrParam[4]; ///< wheel angle to radius parameter, left rear
double dRrParam[4]; ///< wheel angle to radius parameter, right rear

double dLfParamA2S[4]; ///< radius parameter to wheel angle, left front
double dRfParamA2S[4]; ///< radius parameter to wheel angle, right front
double dLrParamA2S[4]; ///< radius parameter to wheel angle, left rear
double dRrParamA2S[4]; ///< radius parameter to wheel angle, right rear

USS_MODEL sonar;

float Car_TurningModel_Pfac_ROU_C[24];
float Car_TurningModel_Nfac_ROU_C[24];
float Car_TurningModel_fac_EPS_C[24];
}tVM_Params;

VM_motion.c文件移植过程及问题

1.暂时屏蔽dim3.h引用，改为dim3_tdf.h引用；

2.dim3_tdf.h编译错误由文件编码格式导致，改UTF-8保存文档；

3.C++下，多态的同名函数，修改函数名；

4.C++下，多态的同名函数，修改函数名如setConfig，calcDistancePulse等；

5.函数内变量应该先定义再初始化，再使用，修改原for（int i；；）等；

6.关于M_API void cVM_Motion::movePointTo(tDIM2F& sDstPoint, tDIM2F& sSrcPoint)
等下的transframe递归函数的改写，，待移植；

7.关于calVehSpeedViaPulse下的队列的改写，待移植；
将comDr_main 改为comDr_interface接口层

Makefile添加

INCLUDE += -I…/work/AVP_SWC/DR/algo_core
INCLUDE += -I…/work/AVP_SWC/DR/algo_interface
INCLUDE += -I…/work/AVP_SWC/DR/spaceos_interface

COBJS += …\work\AVP_SWC\DR\algo_core\vm_motion.o
COBJS += …\work\AVP_SWC\DR\algo_interface\comdr_interface.o

Space_tmp1 添加：

…\work\AVP_SWC\DR\algo_interface\comdr_interface.o

c++转c问题点

1）eCOMDR_ERR cComDR_Main::loadGtPose(int nMode, std::string logpath)

加载真值表原因，之前在视频回放时，用来学习更优坐标曲线而加入的，MCU移植不适用

2）movePointTo函数，调用transframe坐标系转换方法，非递归函数，是C++的多态；

3）关于float cVM_Motion::calVehSpeedViaPulse(tVM_VcuData sVehdata)
由于autosar不能使用动态分配内存，所以关于队列的使用，需要改成数组，具体大小，具体使用方法问蒋博。

4）dim3 算法移植问题，关于坐标移植，需要调用dim3库，dim3库主要是针对坐标转换做的四则运算算法，需要针对实际应用到的坐标转换的四则运算移植至MCU端。

DR算法总结

1）方向盘转角转换成前轮轴心偏角：calcAxisAngle

dArgsS2A[16];
/* 车辆方向盘-前轮偏向角 映射曲线 */

2）根据方向盘转角与挡位信号计算车辆行驶的转弯半径：calcMotionRadius

3）：calcDistancePulse：相当于每两次值之间取平均值

4）：calcDistancePulseThres：

阿克曼转向几何(Ackerman Turning Geometry)是一种为了解决交通工具转弯时，内外转向轮路径指向的圆心不同的几何学。
在单车模型中，将转向时左/右前轮偏角假设为同一角度，虽然通常两个角度大致相等，但实际并不是，通常情况下，内侧轮胎转角更大。如下图所示，δ o 和δ i 分别为外侧前轮和内侧前轮偏角，当车辆右转时，右前轮胎为内侧轮胎，其转角δ i 较左前轮胎转角δ o 更大。ℓ w 为轮距，L为轴距，后轮两轮胎转角始终为0 °

航位推算（DR）算法

搭建测试环境

1）在SpaceOS中新建项目，将MCU代码添加并编译；

2）在新建项目中，添加Framework功能，即引用SpaceOS接口函数，读取配置参数，can数据等，并运行整个程序；
相关编译错误记录
1：引用#include “frameworkbase.h” ；
2：不允许 dllimport 函数 的定义 ；在属性预编译添加预编译宏定义；
3：将工程生成配置类型为，（动态库.DLL）
4：将/modl=uss_mcu_algo_test.dll（/后不能有空格）添加至fra_envir_path_apa.cfg中（为 什么时APA因为在config.cfg中配置）；
3）在fra_envir_path_apa.cfg中将测试视频文件修改为，file=mp4/20211123_110149.mp4输入文件，进行测试；
4）在原ComDr接口和McuDr接口上，添加输出结果打印，打印结果进行对比，进行测试；
5）变更测试输入视频文件，进行对比验证，查看两个测试结果是否正常，误差是否可接受范围；
6）由于测试结果相似度大，因此屏蔽自己程序核心算法代码，得到Mcu测试结果明显错误，验证MCU算法是在独立运行，得到离线测试OK结果。

校准参数：所有参数从长城项目SpaceOS的globe.json中得到

宏定义

输入参数初始化：由于方向盘转角与半径拟合曲线数据太多，未定义宏定义，直接赋值的方式初始化。

MCAL测试方法

测试点

中断测试
使用超声探头的地接杜邦线触发对应的调试点进行测试，对于需要高电平的intb引脚使用e521.42的20脚高电平测试
2022.6.8的测试结果如下，末尾为使用的测试点：

void Uss_Master1Dcr1bIsr(void){u8t_ist_cnt[0]++;}//常规高电平，低电平可获取，中断正常触发3805
void Uss_Master1Dcr2bIsr(void){u8t_ist_cnt[1]++;}//常规高电平，低电平可获取，中断正常触发3806
void Uss_Master2Dcr1bIsr(void){u8t_ist_cnt[2]++;}//常规高电平，低电平可获取，中断正常触发3842
void Uss_Master2Dcr2bIsr(void){u8t_ist_cnt[3]++;}//常规高电平，低电平可获取，中断正常触发3830
void Uss_Master1RfcIsr(void){u8t_ist_cnt[4]++;}//常规高电平，低电平可获取，中断不触发3808
void Uss_Master2RfcIsr(void){u8t_ist_cnt[5]++;}//常规高电平，低电平可获取，中断正常3831
void Uss_Master1intbIsr(void){u8t_ist_cnt[6]++;}//常规低电平，高电平可获取，中断不触发3803
void Uss_Master2intbIsr(void){u8t_ist_cnt[7]++;}//常规低电平，高电平可获取，中断正常触发3828

void Uss_Master1Dcr1bIsr(void){u8t_ist_cnt[0]++;}//常规高电平，低电平可获取，中断正常触发3805
void Uss_Master1Dcr2bIsr(void){u8t_ist_cnt[1]++;}//常规高电平，低电平可获取，中断正常触发3806
void Uss_Master2Dcr1bIsr(void){u8t_ist_cnt[2]++;}//常规高电平，低电平可获取，中断正常触发3842
void Uss_Master2Dcr2bIsr(void){u8t_ist_cnt[3]++;}//常规高电平，低电平可获取，中断正常触发3830
void Uss_Master1RfcIsr(void){u8t_ist_cnt[4]++;}//常规高电平，低电平可获取，中断不触发3808
void Uss_Master2RfcIsr(void){u8t_ist_cnt[5]++;}//常规高电平，低电平可获取，中断正常3831
void Uss_Master1intbIsr(void){u8t_ist_cnt[6]++;}//常规低电平，高电平可获取，中断不触发3803
void Uss_Master2intbIsr(void){u8t_ist_cnt[7]++;}//常规低电平，高电平可获取，中断正常触发3828

实车FOV标定

背景：整车FOV需要实车标定，以确定当前参数性能指标是否满足fov要求，分为水平标定和垂直标定
所需资源：10cm10cm网格标定步，单张尺寸4m2m,至少2张；icu3.0板子、劳德巴赫、实车、空旷场地面积(车长+6m)*(车宽+10m)。
水平标定：
垂直放置pvc管子，在不同的网格位置进行探测
Eg:将管子放置在网格中，以前向他们头为例，以两个中间的前向探头中心点为原点，向两侧延伸，将pvc管放置在各个小格子中前向四个探头获取到dis数据时记为1未获取到时为空，制作如下图的标定表即为前向FOV标定。
对于侧向即以两个侧向雷达的中点垂直地面为原点，后向雷达以两个中间的后向探头中心点为原点。
对于距离小于设置值而无法探测到的称之为盲点，超出最远距离探测不到的边线为FOV最远距离，当超出最远距离或盲点时目前的处理为均报出最远距离值。个人期望设置为不同的宏值，方便算法应用做不同处理。

垂直标定：
水平放置pvc管子，在不同的高度和距离位置进行探测

FOV如何确定
E542.17手册中探头性能：
UPA Sensor FOV

实测最远距离
APA的FOV
实测最远距离
待补充
根据需求文档中说明
追势超声系统标定验收标准V1.2.docx
FOV要求为
1.1 探测距离
UPA探头：3m
APA探头：5m

探头位置

来源：
V71超声位置0502.xlsx
全场景滤波算法标定

TTE超声开发

车模

探头位置命名

坐标系定义
坐标：车辆后轴中心为原点，车头方向为Y，车头方向右侧垂线为X
TTE输出信号
TTE Transmit Signal

OBJ信息
PDC_Obj_01_type 障碍物类型
PDC_Obj_01_Confidence 置信度
PDC_Obj_01_height 高度属性
PDC_Obj_01_height_Confidence 高度属性的置信度
PDC_Obj_01_Startpt1_x 点1的x坐标
PDC_Obj_01_Startpt1_y 点1的y坐标
PDC_Obj_01_Endpt2_x 点2的x坐标
PDC_Obj_01_Endpt2_y 点2的y坐标
PDC_Obj_01_TimeStamp 时间戳
PLD信息
PDC_PSLeft1 车位ID
PDC_PSLeft1_LatRefType 侧面障碍物高/低类型
PDC_PSLeft1_Length 车位长度
PDC_PSLeft1_Psdepth 车位深度
PDC_PSLeft1_Obj1Start_xPos 第一个障碍物的起点x坐标
PDC_PSLeft1_Obj1Start_yPos 第一个障碍物的起点y坐标
PDC_PSLeft1_Obj1StartPoint_Status 第一个障碍物起点状态（是否真实检测到）
PDC_PSLeft1_Obj1End_xPos 第一个障碍物的终点x坐标
PDC_PSLeft1_Obj1End_yPos 第一个障碍物的终点y坐标
PDC_PSLeft1_Obj1EndPoint_Status 第一个障碍物终点状态（是否真实检测到）
PDC_PSLeft1_SlotObjStartPt_xPos 车位深度侧障碍物的起点x坐标
PDC_PSLeft1_SlotObjStartPt_yPos 车位深度侧障碍物的起点y坐标
PDC_PSLeft1_SlotObjStartPt_Status 车位深度侧障碍物起点状态
PDC_PSLeft1_SlotObjEndPt_xPos 车位深度侧障碍物的终点x坐标
PDC_PSLeft1_SlotObjEndPt_yPos 车位深度侧障碍物的终点y坐标
PDC_PSLeft1_SlotObjEndPt_Status 车位深度侧障碍物终点状态
PDC_PSLeft1_Obj2Start_xPos 第二个障碍物的起点x坐标
PDC_PSLeft1_Obj2Start_yPos 第一个障碍物的起点y坐标
PDC_PSLeft1_Obj2StartPoint_Status 第一个障碍物起点状态（是否真实检测到）
PDC_PSLeft1_Obj2End_xPos 第一个障碍物的终点x坐标
PDC_PSLeft1_Obj2End_yPos 第一个障碍物的终点y坐标
PDC_PSLeft1_Obj2EndPoint_Status 第一个障碍物终点状态（是否真实检测到）
PSLeft1_SyncTimeStamp 时间戳
TTE输入信号

诊断信号

超声诊断测试方法：
SPI通信错误：CRC判断故意错误，模拟进行spi通信错误，
主芯片过温：判断对应寄存器的值，给芯片加温或降温，温度阈值是多少
主节点DSI欠压：
主节点欠压：通过芯片引脚加压，确认是哪个引脚，把电压拉低，
主节点CLKREF失效：

算法移植方案

修改pld输入接口结构体增加TTE_PLD参数
增加TTE输出PLD数据转换函数，用于使用TTE算法时的输出
void SP_PK_SonarPLD_ConvertTTESlots(const ussIf_SlotInfo_t tteSonarSlots[SLOT_SIDE_NUM][EACH_SIDE_SLOT_NUM], float dr[3],
TTE_SonarSlotArray* pLeftSonarSlots, TTE_SonarSlotArray* pRightSonarSlots)
修改SP_PK_SonarPLD_Data_Input结构体增加leftSonarSlotsFromTTE和rightSonarSlotsFromTTE

    typedef struct _SP_PK_SonarPLD_Data_Input
    {
        uint64 u64Timestamp;
        PK_ModuleComType eSonarPldCom;
        int PK_SR_GPS_Loss_Flag;
        uint8 roadAttrRD;
        uint8 isParkArea;
        int CDU_SCU_SuperParkActive_CCP;
        float Rte_CurPos_Nvm[4];
        FusionObj_T Rte_Fusion_ObsInfo_A_Left;
        FusionObj_T Rte_Fusion_ObsInfo_A_Rear_Left;
        FusionObj_T Rte_Fusion_ObsInfo_A_Right;
        FusionObj_T Rte_Fusion_ObsInfo_A_Rear_Right;
        TTE_SonarSlotArray leftSonarSlotsFromTTE;
        TTE_SonarSlotArray rightSonarSlotsFromTTE;
    } SP_PK_SonarPLD_Data_Input;

typedef struct _SP_PK_SonarPLD_Data_Input
{
    uint64 u64Timestamp;
    PK_ModuleComType eSonarPldCom;
    int PK_SR_GPS_Loss_Flag;
    uint8 roadAttrRD;
    uint8 isParkArea;
    int CDU_SCU_SuperParkActive_CCP;
    float Rte_CurPos_Nvm[4];
    FusionObj_T Rte_Fusion_ObsInfo_A_Left;
    FusionObj_T Rte_Fusion_ObsInfo_A_Rear_Left;
    FusionObj_T Rte_Fusion_ObsInfo_A_Right;
    FusionObj_T Rte_Fusion_ObsInfo_A_Rear_Right;
    TTE_SonarSlotArray leftSonarSlotsFromTTE;
    TTE_SonarSlotArray rightSonarSlotsFromTTE;
} SP_PK_SonarPLD_Data_Input;

追势算法与TTE算法共用SP_PK_SonarPLD_Data_Output，都通过void SP_PK_SonarPLD_process_10ms()函数进行数据更新
使用宏 TTE 控制是否使用TTE代码

#ifdef _TTE_
    // TODO: get input
    ussIf_SlotInfo_t tteSonarSlots[SLOT_SIDE_NUM][EACH_SIDE_SLOT_NUM];
    ConvertTTESlots(tteSonarSlots, input->Rte_CurPos_Nvm, &input->leftSonarSlotsFromTTE, &input->rightSonarSlotsFromTTE);
#endif

#ifdef TTE
// TODO: get input
ussIf_SlotInfo_t tteSonarSlots[SLOT_SIDE_NUM][EACH_SIDE_SLOT_NUM];
ConvertTTESlots(tteSonarSlots, input->Rte_CurPos_Nvm, &input->leftSonarSlotsFromTTE, &input->rightSonarSlotsFromTTE);
#endif

针对对毫末提出IPC需求事项，设计简化接口SP_PK_SonarPLD_Data_MiniOutput相关结构体如下:

//PLD输出IPC接口
typedef struct {
    SlotObj_T slotobj;//车位框信息
    VehPos_T targpos;// 当前车辆信息
    sint8 slotobj_attr[5];
    PK_SlotShapeType slotshap;//车位类型

    uint64 u64SlotId;//车位ID

    uint8 uSlotType;    // 1 - enclosed, 2 - forward open, 3 - backward open

}SlotInfo_T_SHRINK;

#define SLOT_ARRAY_MAX_NUM_SHRINK 6

// 
typedef struct
{
    SlotInfo_T_SHRINK multiArray[SLOT_ARRAY_MAX_NUM_SHRINK];
    int multiNum;
}Multi_Slot_Array_T_SHRINK;
// 
typedef struct SP_PK_SonarPLD_Data_MiniOutput{
        SP_PK_SonarPLD_DiagInfo diaginfo;
        PK_ModuleStateType eSonarPldState;
        Multi_Slot_Array_T_SHRINK leftSlots;
        Multi_Slot_Array_T_SHRINK rightSlots;
}SP_PK_SonarPLD_Data_MiniOutput;

//PLD输出IPC接口
typedef struct {
SlotObj_T slotobj;//车位框信息
VehPos_T targpos;// 当前车辆信息
sint8 slotobj_attr[5];
PK_SlotShapeType slotshap;//车位类型

uint64 u64SlotId;//车位ID

uint8 uSlotType;    // 1 - enclosed, 2 - forward open, 3 - backward open

}SlotInfo_T_SHRINK;

#define SLOT_ARRAY_MAX_NUM_SHRINK 6

//
typedef struct
{
SlotInfo_T_SHRINK multiArray[SLOT_ARRAY_MAX_NUM_SHRINK];
int multiNum;
}Multi_Slot_Array_T_SHRINK;
//
typedef struct SP_PK_SonarPLD_Data_MiniOutput{
SP_PK_SonarPLD_DiagInfo diaginfo;
PK_ModuleStateType eSonarPldState;
Multi_Slot_Array_T_SHRINK leftSlots;
Multi_Slot_Array_T_SHRINK rightSlots;
}SP_PK_SonarPLD_Data_MiniOutput;

TTE实际传输库位数为左右各3个，为保障扩展性PLD库位数定义为左右各6个，据此申请毫末提供SlotInfo_T multiArray[6];结构体左右各两个进行数据传输。
PROC模块调整超声数据的结构体，完善雷达相关信息后的结构体如下：

// 雷达相关信息
typedef struct {
    uint64  Ts;//时间戳,用以区分雷达值是否为有效值
    Point_T Pos;//障碍物点
    Point_T Pos_loc;//车身坐标系障碍物点
    uint16  Dist;//距离值
    uint16   Peak;//能量值
    uint16   Width;//波宽
    uint8 Confidence;//置信度
} URadar_InfoType;
// 三角测量相关信息,主要是前后四个探头
typedef struct {
    uint64  Ts;//时间戳
    Point_T Pos;//
    Point_T Pos1;//
    Point_T Pos2;//
    uint16  Dist;//
    uint16   Peak;
    uint16   Width;
    uint8 Confidence;//置信度
    uint8   PosVd;
    uint8   Pos1Vd;
    uint8   Pos2Vd;
} URadar_TriInfoType;

// 超声波雷达的结构体
typedef struct {
    uint64 Timestamp;//时间戳
    uint16 Distance;//距离值
    uint16  PeakLevel;//能量值
    uint16  WaveWidth;//波宽
    uint8 Confidence;//置信度
} U_RadarDataType;

// 超声波雷达的结构体
typedef struct {
    uint64 Timestamp;//时间戳
    uint16 Distance;//距离  mm
    uint16  PeakLevel;//能量值
    uint16  WaveWidth;//波宽
    uint8 Confidence;//置信度
    uint8  Status;//状态值,需要深入看下
} URadar_DataType;

// 雷达相关信息
typedef struct {
uint64 Ts;//时间戳,用以区分雷达值是否为有效值
Point_T Pos;//障碍物点
Point_T Pos_loc;//车身坐标系障碍物点
uint16 Dist;//距离值
uint16 Peak;//能量值
uint16 Width;//波宽
uint8 Confidence;//置信度
} URadar_InfoType;
// 三角测量相关信息,主要是前后四个探头
typedef struct {
uint64 Ts;//时间戳
Point_T Pos;//
Point_T Pos1;//
Point_T Pos2;//
uint16 Dist;//
uint16 Peak;
uint16 Width;
uint8 Confidence;//置信度
uint8 PosVd;
uint8 Pos1Vd;
uint8 Pos2Vd;
} URadar_TriInfoType;

// 超声波雷达的结构体
typedef struct {
uint64 Timestamp;//时间戳
uint16 Distance;//距离值
uint16 PeakLevel;//能量值
uint16 WaveWidth;//波宽
uint8 Confidence;//置信度
} U_RadarDataType;

// 超声波雷达的结构体
typedef struct {
uint64 Timestamp;//时间戳
uint16 Distance;//距离 mm
uint16 PeakLevel;//能量值
uint16 WaveWidth;//波宽
uint8 Confidence;//置信度
uint8 Status;//状态值,需要深入看下
} URadar_DataType;

申请毫末修改ipc中原数据类型PeakLevel、WaveWidth从uint8改为uint16，新增各结构体中uint8 Confidence;//置信度
OBJ模块TTE提供的数量为20个，按32个申请，这个需要跟毫末讨论确认
详细需求详见
追势ipc需求文档

PROC移植处理

需要将TTE提供的探头数据转换到proc输入中，目前只需要12个直接回波数据
preProc_InputData.CurrUltrasData[i]有24组数据，目前需求为前12个存储探头直接回波

移植问题记录

1.编译器版本不统一（tasking版本需同步为6.2r2）
已知差异点：lsl文件中如果是6.3r1此部分需要添加代码

    heap "heap"
    (
        min_size = (LCF_HEAP_SIZE),
        fixed,
        align = 8
    );

heap "heap"
(
    min_size = (LCF_HEAP_SIZE),
    fixed,
    align = 8
);

超声数据bin文件录制

1. tar -zcvf spaceos_0325_1.tar.gz ./logdata/ //压缩文件
2. scp .\avp.tgz root@192.168.1.12:/opt/data/lk/
   3.scp root@192.168.1.12:/opt/data/lk/spaceos/spaceos_0325_1.tar.gz ./
   启动底盘数据
3. cd /var/data/output/
4. ./start_all.sh
5. 启动spaceos
6. cd /opt/data/lk/spaceos/
7. ./start.sh

ssh-keygen -R 192.168.1.12
超声数据bin文件回放
1.准备2个文件
.bin文件
xxx.mp4
2.修改

windows下

Linux下
将.bin文件放在与start.sh同目录下
1.找到文件
fra_envir_path_GDC_ICU3.cfg
2.修改文件中内容
LOAD_PARAM: “/modl=libfw_sdk.so /modl=libfw_bev.so /modl=libstitch.so /modl=libhmi_icu3.so /modl=libhmi_p7.so /modl=libnetwork_connector.so /modl=libsegparse.so /modl=libparkoccupy.so /modl=libcomdr.so /cfg=comdr_config.cfg /modl=libspacenet.so /modl=libpld.so /modl=libhmihub.so /modl=libSP_PK_ObjAvoid.so /modl=libSP_PK_PathFollow.so /modl=libSP_PK_PathPlan.so /modl=libSP_PK_SensorFusion.so /modl=libSP_PK_SlotDetect.so /modl=libSP_PK_SonarProc.so /modl=libSP_PK_StateManager_all.so /modl=libSP_PK_VehicleControl.so /modl=libapa_debug_monitor.so /modl=libslam.so /modl=libavp_pathplan.so /modl=libtsml.so /modl=libmapservice.so /modl=libSPM.so /modl=liblocalization.so /modl=libavp_pathfollow.so /modl=libplanning.so /modl=librouting.so /modl=libvision_perception.so /cfg=config_dat.cfg /cfg=pk_occupy_config.cfg /cfg=PLD_config.cfg /cfg=apa_config.cfg /cfg=path_plan_config.cfg /cfg=apapf_config.cfg /cfg=sp_pk_objavoid_config.cfg /cfg=sp_pk_pathfollow_config.cfg /cfg=sp_pk_pathplan_config.cfg /cfg=sp_pk_sensorfusion_config.cfg /cfg=sp_pk_slotdetect_config.cfg /cfg=sp_pk_sonarproc_config.cfg /cfg=sp_pk_statemanager_all_config.cfg /cfg=sp_pk_vehiclecontrol_config.cfg /cfg=sys_scheduler_config.cfg /cfg=tsml_config.cfg /modl=libdata_visualization.so /modl=libavp_behaviorDecision.so /cfg=avp_behavior_config.cfg /modl=libspace_tracking.so /modl=libSP_PK_Pas.so /cfg=avp_pathplan_config.cfg /modl=xlibmp4_playback.so /file=xtest.mp4”
以回放视频test.mp4为例，将最后的
xlibmp4_playback.so /file=xtest.mp4"修改为 libmp4_playback.so /file=test.mp4"
3.打开回放工具路径
spaceos\tools\xiaopeng_p7_H5\SPACEOS_H5_Data\StreamingAssets\P_configuration.TXT
把IP改成板子的ip 192.168.1.12
4.打开H5.exe查看回放信息

数据接入及测试方法方案

SOC端超声数据结构体

编译分支IPC数据入口

SOC端数据通过SP_PK_SensorPreProc_Mcu_Data_Output_DB接入spaceos

SPSystem_UpdateIPCData中获取超声数据

COMMON_VEHICLE_DATA m_vehicleCanInfo使用超声数据（供PAS调用，仅需超声PROC中的距离）

目前长城项目中通过SPECIAL_VEHICLE_DATA结构体将数据写死，未接入上传的超声数据

OBJ数据和PLD数据也已接入，接入方式类似，代码量较大

PROC应用超声数据

代码分支sp_pk_slotdetect_spaceos_interface.cpp文件中DebugShow函数读取超声PROC数据并绘制曲线

OBJ生成障碍物信息应用超声数据

OBJ数据绘制

PLD生成库位应用超声数据

PLD绘制

防撞雷达PAS应用超声数据(仅PROC数据)
PAS回放代码

PAS模块应用.

应用CAN调试需求梳理

需要仿照算法的debugshow和PAS的防撞雷达界面，进行超声距离画线、障碍物显示、库位显示等功能。目前can资源为32帧*64字节数据传输，可将PROC、OBJ和PLD数据通过CANOE输出到PC，需要PC完成一个HMI功能，对数据进行处理和绘制显示，测试过程中通过查看相应绘制效果进行调试
一个雷达场景代码示例
需要追势自己改接口
GitHub - SNC6SI/radarVisualization
改signalhelper.cpp 和h
https://github.com/SNC6SI/radarVisualization
使用git clone 下载
TTECAN调试参考
TTE自己设计了DBC文件，使用DBC获取log数据
所提需求为32帧64字节数据 毫末已满足
使用canfd02线束接口 can配置500 2000 85 85

铁将军出差计划梳理
1.相关人员
目标：
1.FOV设置3m和5m（水平、垂直要求按规范内容调整）
追势超声系统标定验收标准V1.2.docx
整理优化设置参数和标定结果（2天）
2.算法按规范场景测试
各场景符合规范要求，铁将军滤波算法优化(前后雷达2天，侧雷达2天)
测试项参考测试用例
长城-SONOR-软件-自测-离线-001
3.追势OBJ和PLD算法性能初步测试
避障效果
A过程
B过程
目标：通过BIN文件录制数据回放，算法人员初步分析确认改进方向（1天）
泊车效果
目标：通过BIN文件录制数据回放，算法人员初步分析确认改进方向（1天）
风险：铁将军出差日期视付款和出差进度，追势OBJ和PLD算法性能初步测试依赖毫末IPC提供（新版IPC预计7.15提供，铁将军出差事宜预计7月18日开始）
MCU完整架构（20220718）
TC399MCU设计文档

Canoe log回灌can总线方法总结
CANoe12
CANoe操作流程

protoc文件的使用

生成cc文件

文件路径
/home/lxpizza/workspace/icu3_0/icu3_0/modules/spaceos_launcher_a2

pcap包录制

控制器网络报文抓包

SOC代码修改

更新protocol的pb文件

RPC宏开启

IPC接口转换

启用RPC
//ADD BY ZC
#define ENABLE_HAOMORPC
//END

根据protoc调整代码，修改错误命名增加删除差异项

TTE缓存需求

超声波感知需要存储的数据总结如下:
为了便于后续问题的调查处理，（比如障碍物误报、漏报，车位误检、漏检，故障诊断等），超声波感知软件需保存一部分数据到FLASH或上传云端服务器。
超声波感知每40毫秒需要存储约6Kbyte数据。
需要SOC端开辟20Mbyte的空间（比如，在DDR也可以）用于缓存感知数据。
当满足以下任一条件，需要SOC把缓存的数据转存到Flash或上传云端服务器：

1. 当系统从找车位状态进入泊车状态；
2. 当系统从泊车状态退出。

针对此需求 对ipc要求较高
方案一 ：增加mcu comdr数据上传ipc，存储追势超声数据，可以解析出TTE接口数据
方案二：增加TTE接口ipc，直接存储TTE，需要毫末新增较多接口

rpc_sample的代码

超声接入PAS

测试流程

1.测试首先启动毫末产出
可以进入毫末产出路径下依次输入
bash
source env.sh
cli_client
data hz -n 999999

查看超声数据uss_proc_data持续刷新表明产出ok

运行rpc_sample脚本./start_rpc_sample
脚本代码如下，同时需要rpc_sample执行文件
cat start.sh

将前8行复制，完成环境变量配置
然后运行rpc_sample
输出2次rpc uss failed表示已设置uss产出完成

正常启动spaceos
./start.sh

目前毫末产出已成功写入databank

打印信息开关

1.ipc_interface.cpp中关闭打印，有2处

2.SP_AUTOPILOT_INFS.cpp中关闭调试信息，有2处

防止程序进入playback

将/opt/data/mxj/spaceos/space_autopilot/data/config/路径下fra_envir_path_GDC_ICU3.cfg文件中/modl=libmp4_playback.so屏蔽，临时修改为/modl=xlibmp4_playback.so

毫末产出处理
毫末产出使用方法

MCU或CAN的loading统计方法

TTE采用直接查看canoe statistics,如下图为12.61%

超声APA功能调试
启动产出
参考毫末产出使用方法
启动rpc
查看产出脚本代码
Cat start.sh
复制环境变量并启动
执行rpc_server

启动spaceos

打开H5

选择apa进入

算法20220809PLD联调记录

1.超声库位左侧坐标也为正值，文档说明中应为负值。

坐标系定义
坐标：车辆后轴中心为原点，车头方向为Y，车头方向右侧垂线为X
2.库位代码和文档中注释不一致，实测应为文档标注角点顺序
typedef struct {
ussIf_UINT16 ID ; //Parking slot ID
ussIf_SINT8 SlotType ; //Parking slot Type
ussIf_SINT8 LatRefType ; //Parking slot lateral refer-ence type
ussIf_SINT16 Length ; //Parking slot length
ussIf_SINT16 Psdepth ; //0=no valid ps>0=PS Depth

ussIf_SlotPoint_t SlotPtPos[APA_SLOT_POINT_NUM];    
//Description of SlotPtPos:
//SlotPtPos[0]  is Obj1StartPos;//The start position of  object1 when searching slot
//SlotPtPos[1]  is Obj1EndPos ; //The end position of  object1 when searching slot
//SlotPtPos[2]  is Obj2StartPos ; //The start position of object2 when searching slot
//SlotPtPos[3]  is Obj2EndPos ; //The end position of object2 when searching slot
//SlotPtPos[4]  is SlotObjStartPtPos ; //The depth position of  object1 when searching slot
//SlotPtPos[5]  is SlotObjEndPtPos ; //The depth position of  object2 when searching slot

ussIf_UINT64    TimeStamp ; //Time synchronization mes-sage-Synchronization time    

} ussIf_SlotInfo_t;