P30测距声呐测试程序——STM32版
P30测距声呐简介
产品图片
官方论坛：BlueRobots 水下机器人社区 - 交流论坛
P30回声测距原理
P30 通过换能器发射 115kHz 脉冲，然后侦听并测量返回的声能强度。声波在水中传播时会反射或反射回固体物体，然后传播回P30。
电源参数
| 电源电压 | 5.5V DC |
|---|---|
| 通讯协议 | UART串口通信 |
| TTL逻辑电平 | 3.3V~5V DC |
| 功耗 | 100mA（5V） |
线序颜色
| 红色 | VCC |
|---|---|
| 黑色 | GND |
| 绿色 | Rx |
| 白色 | Tx |
声学参数
| 频率 | 115kHZ |
|---|---|
| 波束宽度 | 30° |
| 工作范围 | 0.5m~30m |
| 分辨率@30m | 15cm |
| 分辨率@2m | 1cm |
P30测距声呐数据读取（STM32为例）
通讯方式
P30测距声呐采用串口通讯TTL电平进行通讯，波特率默认为115200。为了获取数据，以HEX格式向P30测距声呐发送命令后，会得到其响应的回讯，通过对回传数据的解析可以得到响应的数据。
通讯协议
每条消息都由信息头，主要负载信息以及校验码组成，其二进制格式如下：
字节0： 每次发送以及接收都为 0x42。
字节1： 每次发送以及接收都为 0x52。
字节2-3： 数据长度位，采用小端模式。例数据长度为 0x02,存储方式为 02 00。
字节4-5： 命令 ID,同样采用小端模式。1200为获取版本信息命令 ,16进制表示为04 B0，存储方式为 B0 04。
字节6： 每次发送以及接收都为 00。
字节7： 每次发送以及接受都为 00。
字节8-n： 数据内容。
字节n+1,n+2： 校验和方式 ,即对校验位之前的所有字节进行求和，同样采取小端模式。
注： 小端模式，是指数据的高字节保存在内存的高地址中，而数据的低字节保存在内存的低地址中，这种存储模式将地址的高低和数据位权有效地结合起来，高地址部分权值高，低地址部分权值低。
获取示例
获取单次简单数据(命令 ID:1211)
42 52 00 00 BB 04 00 00 53 01
高度计反馈
42 52 05 00 BB 04 00 00 55 21 00 00 37 05 02
获取版本信息(命令 ID:1200)
42 52 00 00 B0 04 00 00 48 01
高度计反馈
42 52 06 00 B0 04 00 00 01 01 03 00 18 00 6B 01
获取范围信息(命令 ID:1204)
42 52 00 00 B4 04 00 00 4C 01
高度计反馈
42 52 08 00 B4 04 00 00 00 00 00 00 C3 32 00 00 49 02
获取当前设置声速(命令 ID:1203)
42 52 00 00 B3 04 00 00 4B 01
高度计反馈
42 52 04 00 B3 04 00 00 60 E3 16 00 A8 02(1500000mm/s)
设置示例
设置声速信息(命令 ID:1002)
42 52 04 00 EA 03 00 00 C0 5C 15 00 B6 02(设置声速为 1400m/s)
高度计无反馈
注: 高度计每次上电后声速都会默认被设置为 1500m/s
设置连续发送命令(命令 ID:1400)
42 52 02 00 78 05 00 00 14 05 2C 01
高度计开始连续发送命令
设置取消连续发送命令(命令 ID:1401)
42 52 02 00 79 05 00 00 14 05 2D 01
高度计无反馈
设置Ping-Enable(命令 ID:1006)
启用或禁用声学测量
42 52 01 00 EE 03 00 00 01 87 01
数据解析
1001 set_range
设置声学测量的扫描范围。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u32 | scan_start | 起始扫描距离。 | 毫米 |
| u32 | scan_length | 扫描范围的长度。 | 毫米 |
1002 set_speed_of_sound
设置用于距离计算的声音速度。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u32 | 声速设置 | 测量介质中的声音速度。约1,500,000毫米/秒的水。 | 毫米/秒 |
1203 speed_of_sound
获取距离计算的声音速度。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u32 | 声音的速度 | 测量介质中的声音速度。约1,500,000毫米/秒的水 | 毫米/秒 |
1204 range
获取声学测量的扫描范围。设备返回的测量值将在范围内（scan_start，scan_start + scan_length）。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u32 | scan_start | 距环能器起点距离，以mm为单位。 | 毫米 |
| u32 | scan_length | 扫描范围的长度。 | 毫米 |
1208 transmit_duration
获取声音传输的持续时间。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u16 | 发送持续时间 | 声脉冲持续时间。 | us |
1211 distance_simple
获取置信度估计值与目标的距离。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u32 | 距离 | 到目标的距离 | mm |
| u8 | 置信度 | 对距离测量的置信度 | ％ |
1212 distance
置信度估计值与目标的距离。还提供了测量过程中的相关设置参数。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u32 | 距离 | 为最近的声学测量确定的当前返回距离。 | ms |
| u16 | 置信度 | 对最新范围测量的信心。 ％ | |
| u16 | 发送持续时间 | 声音传输/激活过程中的声音脉冲长度。 | us |
| u32 | ping_number | 自启动以来的脉冲/测量计数。 | |
| u32 | scan_start | 扫描区域的起点，距离换能器以mm为单位。 | mm |
| u32 | scan_length | 扫描区域的长度。 | mm |
| u32 | gain_setting | 当前的增益设置。0：0.6，1：1.8，2：5.5，3：12.9，4：30.2，5：66.1，6：144 |
1400 Continuous_start
启动连续持续发送数据命令。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u16 | ID | 选择P30需要持续发送数据的消息ID，例如ID1300为持续发送样本信息命令ID。 |
1401 Continuous_stop
停止持续发送数据命令。
| 类型 | 名称 | 描述 | 单位 |
|---|---|---|---|
| u16 | ID | 选择P30需要停止持续发送数据的消息ID |
更多请查看官方网页：P30 消息 - Ping Protocol 声呐通信协议 (searobotix.com)
调试程序
main.c
#include "stm32f10x.h"
#include "bsp_usart_usb.h"
#include "bsp_usart_sonar.h"
/**
* @brief 主函数
* @param 无
* @retval 无
*/
int main(void)
{
/*初始化USART 配置模式为 115200 8-N-1，中断接收*/
USART1_Config();
USART2_Config();
Sonar_RequestData(); //获取声呐数据
while(1)
{
}
}
bsp_usart_sonar.c
/**
******************************************************************************
* @file bsp_usart_sonar.c
* @date 2021-07-09
* @brief 配置声呐所使用的串口，将收到的数据存储并计算出实际值。
声呐型号为P30，使用串口通讯，连接STM32103的串口2，即PA2和PA3。连线如下：
绿线(Rx)——PA2(Tx)
白线(Tx)——PA3(Rx)
红线——VCC（5V）
黑线——GND
声呐型号及资料：https://searobotix.com/p30-sonar/
【注意】默认波特率为115200。
*/
#include "bsp_usart_sonar.h"
#include "bsp_usart_usb.h"
uint8_t RXcount=0;
uint8_t SonarData[15];
/**
* @brief 配置嵌套向量中断控制器NVIC
* @param 无
* @retval 无
*/
static void NVIC_Configuration(void)
{
NVIC_InitTypeDef NVIC_InitStructure;
/* 嵌套向量中断控制器组选择 */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
/* 配置USART为中断源 */
NVIC_InitStructure.NVIC_IRQChannel = DEBUG2_USART2_IRQ;
/* 抢断优先级*/
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
/* 子优先级 */
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
/* 使能中断 */
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
/* 初始化配置NVIC */
NVIC_Init(&NVIC_InitStructure);
}
/**
* @brief USART GPIO 配置,工作参数配置
* @param 无
* @retval 无
*/
void USART2_Config(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
// 打开串口GPIO的时钟
DEBUG2_USART_GPIO_APBxClkCmd(DEBUG2_USART_GPIO_CLK, ENABLE);
// 打开串口外设的时钟
DEBUG2_USART_APBxClkCmd(DEBUG2_USART_CLK, ENABLE);
// 将USART Tx的GPIO配置为推挽复用模式
GPIO_InitStructure.GPIO_Pin = DEBUG2_USART_TX_GPIO_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(DEBUG2_USART_TX_GPIO_PORT, &GPIO_InitStructure);
// 将USART Rx的GPIO配置为浮空输入模式
GPIO_InitStructure.GPIO_Pin = DEBUG2_USART_RX_GPIO_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(DEBUG2_USART_RX_GPIO_PORT, &GPIO_InitStructure);
// 配置串口的工作参数
// 配置波特率
USART_InitStructure.USART_BaudRate = DEBUG2_USART_BAUDRATE;
// 配置 针数据字长
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
// 配置停止位
USART_InitStructure.USART_StopBits = USART_StopBits_1;
// 配置校验位
USART_InitStructure.USART_Parity = USART_Parity_No ;
// 配置硬件流控制
USART_InitStructure.USART_HardwareFlowControl =
USART_HardwareFlowControl_None;
// 配置工作模式，收发一起
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
// 完成串口的初始化配置
USART_Init(DEBUG2_USARTx, &USART_InitStructure);
// 串口中断优先级配置
NVIC_Configuration();
// 使能串口接收中断
USART_ITConfig(DEBUG2_USARTx, USART_IT_RXNE, ENABLE);
// 使能串口
USART_Cmd(DEBUG2_USARTx, ENABLE);
}
/**
* @brief 【声呐串口中断处理函数】
当声呐回传数据时，将收到的数据存储在数组SonarData[]中，并调用声呐数据处理函数Sonar_ConvertData()对数据进行处理并将处理结果
输出至串口调试助手窗口。
* @param 无
* @retval 无
*/
void DEBUG2_USART2_IRQHandler(void)
{
uint8_t ucTemp;
if(USART_GetITStatus(DEBUG2_USARTx,USART_IT_RXNE)!=RESET) //如果串口接收到数据
{
ucTemp = USART_ReceiveData(DEBUG2_USARTx); //读取接收到的数据
if( CheckData(ucTemp) ) //调用数据校对函数，确认接受到的数据是数据包的前两位，校对无误则开始存储数据
{
SonarData[RXcount] = ucTemp; //数据存储进数组SonarData[]
RXcount++; //接收数据总数自增
USART_ClearITPendingBit(DEBUG2_USARTx, USART_IT_RXNE); //清除数据接收标志位
if (RXcount == 15) //数据包接收完毕
{
//Usart_SendArray(DEBUG1_USARTx,SonarData ,15); //输出接收的原始数据
Sonar_ConvertData(); //对数据进行计算处理并输出
}
}
}
}
/**
* @brief 【声呐数据请求函数】
发送请求声呐数据的对应指令，向声呐请求回传数据。
* @param 无
* @retval 无
*/
void Sonar_RequestData( void )
{
uint8_t request_data[10]={0x42, 0x52, 0x00, 0x00, 0xBB, 0x04, 0x00, 0x00, 0x53, 0x01};
Usart_SendArray(DEBUG2_USARTx,request_data,10);
}
/**
* @brief 【声呐数据检查函数】
用于检查接收到的前两个数据是否为数据包的前两位，即第一位为42，第二位为52，如果是，则说明接收到的数据是数据包的开头，即可将数据存储进数组。
* @param num:校对的数据序号；data：校对的数据
* @retval 1：校对无误；0：数据错误。
*/
int CheckData( uint8_t data)
{
if(RXcount==0)
{
if(data==0x42) return 1;
else return 0;
}
if(RXcount==1)
{
if(data==0x52) return 1;
else return 0;
}
else return 1;
}
/**
* @brief 【声呐数据处理函数】
对声呐回传的数据进行处理，并通过串口调试助手显示其数据处理后的结果
* @param 无
* @retval 无
*/
void Sonar_ConvertData(void)
{
/*
声呐回传数据解析
SonarData[0~1]为0x42,0x52，是指令识别符号
SonarData[2~3]为回传数据长度。采用小端模式，即先存储低位，后存储高位。例如数据长度为5时，存储为0x05,0x00。
SonarData[4~5]为命令ID。例如请求单次简单数据（包含距离和置信度）时，命令ID为1211，16进制表示为0x04BB，则存储为0xBB，0x04。
SonarData[6~7]为通讯设备ID，均为0x00。
SonarData[8~11]当请求单次简单数据时，此四个数据为距离，采用小端模式存储，单位为mm。
SonarData[12]当请求单次简单数据时，此数据为置信度，单位为%。
SonarData[末两位]为消息校验和。
*/
int SonarDistance; //距离
uint8_t SonarConfidence; //置信度
SonarDistance = ( ((uint32_t)SonarData[8]) |
((uint32_t)SonarData[9]<<8) |
((uint32_t)SonarData[10]<<16) |
((uint32_t)SonarData[11]<<24) );
SonarConfidence = SonarData[12];
printf ("距离为： %d mm \n",SonarDistance );
printf ("置信度为： %d \n",SonarConfidence);
}
bsp_usart_sonar.h
#ifndef __BSP_USART_SONAR_H
#define __BSP_USART_SONAR_H
#include "stm32f10x.h"
#include <stdio.h>
#include "usart_function.h"
// 串口2-USART2
#define DEBUG2_USARTx USART2
#define DEBUG2_USART_CLK RCC_APB1Periph_USART2
#define DEBUG2_USART_APBxClkCmd RCC_APB1PeriphClockCmd
#define DEBUG2_USART_BAUDRATE 115200
// USART GPIO 引脚宏定义
#define DEBUG2_USART_GPIO_CLK (RCC_APB2Periph_GPIOA)
#define DEBUG2_USART_GPIO_APBxClkCmd RCC_APB2PeriphClockCmd
#define DEBUG2_USART_TX_GPIO_PORT GPIOA
#define DEBUG2_USART_TX_GPIO_PIN GPIO_Pin_2
#define DEBUG2_USART_RX_GPIO_PORT GPIOA
#define DEBUG2_USART_RX_GPIO_PIN GPIO_Pin_3
#define DEBUG2_USART2_IRQ USART2_IRQn
#define DEBUG2_USART2_IRQHandler USART2_IRQHandler
void USART2_Config(void);
int CheckData( uint8_t data);
void Sonar_ConvertData(void);
void Sonar_RequestData( void );
#endif /* __BSP_USART_SONAR_H */
bsp_usart_usb.c
/**
******************************************************************************
* @file bsp_usart_usb.c
* @author fire
* @version V1.0
* @date 2013-xx-xx
* @brief 重定向c库printf函数到usart端口
******************************************************************************
*/
#include "bsp_usart_usb.h"
/**
* @brief 配置嵌套向量中断控制器NVIC
* @param 无
* @retval 无
*/
static void NVIC_Configuration(void)
{
NVIC_InitTypeDef NVIC_InitStructure;
/* 嵌套向量中断控制器组选择 */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
/* 配置USART为中断源 */
NVIC_InitStructure.NVIC_IRQChannel = DEBUG1_USART1_IRQ;
/* 抢断优先级*/
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
/* 子优先级 */
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
/* 使能中断 */
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
/* 初始化配置NVIC */
NVIC_Init(&NVIC_InitStructure);
}
/**
* @brief USART GPIO 配置,工作参数配置
* @param 无
* @retval 无
*/
void USART1_Config(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
// 打开串口GPIO的时钟
DEBUG1_USART_GPIO_APBxClkCmd(DEBUG1_USART_GPIO_CLK, ENABLE);
// 打开串口外设的时钟
DEBUG1_USART_APBxClkCmd(DEBUG1_USART_CLK, ENABLE);
// 将USART Tx的GPIO配置为推挽复用模式
GPIO_InitStructure.GPIO_Pin = DEBUG1_USART_TX_GPIO_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(DEBUG1_USART_TX_GPIO_PORT, &GPIO_InitStructure);
// 将USART Rx的GPIO配置为浮空输入模式
GPIO_InitStructure.GPIO_Pin = DEBUG1_USART_RX_GPIO_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(DEBUG1_USART_RX_GPIO_PORT, &GPIO_InitStructure);
// 配置串口的工作参数
// 配置波特率
USART_InitStructure.USART_BaudRate = DEBUG1_USART_BAUDRATE;
// 配置 针数据字长
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
// 配置停止位
USART_InitStructure.USART_StopBits = USART_StopBits_1;
// 配置校验位
USART_InitStructure.USART_Parity = USART_Parity_No ;
// 配置硬件流控制
USART_InitStructure.USART_HardwareFlowControl =
USART_HardwareFlowControl_None;
// 配置工作模式，收发一起
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
// 完成串口的初始化配置
USART_Init(DEBUG1_USARTx, &USART_InitStructure);
// 串口中断优先级配置
NVIC_Configuration();
// 使能串口接收中断
USART_ITConfig(DEBUG1_USARTx, USART_IT_RXNE, ENABLE);
// 使能串口
USART_Cmd(DEBUG1_USARTx, ENABLE);
}
void DEBUG1_USART1_IRQHandler(void)
{
uint8_t ucTemp;
if(USART_GetITStatus(DEBUG1_USARTx,USART_IT_RXNE)!=RESET)
{
ucTemp = USART_ReceiveData(DEBUG1_USARTx);
USART_SendData(DEBUG1_USARTx,ucTemp);
}
}
bsp_usart_usb.h
#ifndef __BSP_USART_USB_H
#define __BSP_USART_USB_H
#include "stm32f10x.h"
#include <stdio.h>
#include "usart_function.h"
/**
* 串口宏定义，不同的串口挂载的总线和IO不一样，移植时需要修改这几个宏
* 1-修改总线时钟的宏，uart1挂载到apb2总线，其他uart挂载到apb1总线
* 2-修改GPIO的宏
*/
// 串口1-USART1
#define DEBUG1_USARTx USART1
#define DEBUG1_USART_CLK RCC_APB2Periph_USART1
#define DEBUG1_USART_APBxClkCmd RCC_APB2PeriphClockCmd
#define DEBUG1_USART_BAUDRATE 9600
// USART GPIO 引脚宏定义
#define DEBUG1_USART_GPIO_CLK (RCC_APB2Periph_GPIOA)
#define DEBUG1_USART_GPIO_APBxClkCmd RCC_APB2PeriphClockCmd
#define DEBUG1_USART_TX_GPIO_PORT GPIOA
#define DEBUG1_USART_TX_GPIO_PIN GPIO_Pin_9
#define DEBUG1_USART_RX_GPIO_PORT GPIOA
#define DEBUG1_USART_RX_GPIO_PIN GPIO_Pin_10
#define DEBUG1_USART1_IRQ USART1_IRQn
#define DEBUG1_USART1_IRQHandler USART1_IRQHandler
void USART1_Config(void);
#endif /* __BSP_USART_USB_H */
usart_function.c
#include "stm32f10x.h"
#include <stdio.h>
#include "usart_function.h"
/***************** 发送一个字节 **********************/
void Usart_SendByte( USART_TypeDef * pUSARTx, uint8_t ch)
{
/* 发送一个字节数据到USART */
USART_SendData(pUSARTx,ch);
/* 等待发送数据寄存器为空 */
while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);
}
/****************** 发送8位的数组 ************************/
void Usart_SendArray( USART_TypeDef * pUSARTx, uint8_t *array, uint16_t num)
{
uint8_t i;
for(i=0; i<num; i++)
{
/* 发送一个字节数据到USART */
Usart_SendByte(pUSARTx,array[i]);
}
/* 等待发送完成 */
while(USART_GetFlagStatus(pUSARTx,USART_FLAG_TC)==RESET);
}
/***************** 发送字符串 **********************/
void Usart_SendString( USART_TypeDef * pUSARTx, char *str)
{
unsigned int k=0;
do
{
Usart_SendByte( pUSARTx, *(str + k) );
k++;
} while(*(str + k)!='\0');
/* 等待发送完成 */
while(USART_GetFlagStatus(pUSARTx,USART_FLAG_TC)==RESET)
{}
}
/***************** 发送一个16位数 **********************/
void Usart_SendHalfWord( USART_TypeDef * pUSARTx, uint16_t ch)
{
uint8_t temp_h, temp_l;
/* 取出高八位 */
temp_h = (ch&0XFF00)>>8;
/* 取出低八位 */
temp_l = ch&0XFF;
/* 发送高八位 */
USART_SendData(pUSARTx,temp_h);
while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);
/* 发送低八位 */
USART_SendData(pUSARTx,temp_l);
while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);
}
///重定向c库函数printf到串口，重定向后可使用printf函数
int fputc(int ch, FILE *f)
{
/* 发送一个字节数据到串口 */
USART_SendData(DEBUG1_USARTx, (uint8_t) ch);
/* 等待发送完毕 */
while (USART_GetFlagStatus(DEBUG1_USARTx, USART_FLAG_TXE) == RESET);
return (ch);
}
///重定向c库函数scanf到串口，重写向后可使用scanf、getchar等函数
int fgetc(FILE *f)
{
/* 等待串口输入数据 */
while (USART_GetFlagStatus(DEBUG1_USARTx, USART_FLAG_RXNE) == RESET);
return (int)USART_ReceiveData(DEBUG1_USARTx);
}
usart_function.h
#include "stm32f10x.h"
#include <stdio.h>
#include "bsp_usart_usb.h"
#ifndef __USART_FUNCTION_H
#define __USART_FUNCTION_H
void Usart_SendByte( USART_TypeDef * pUSARTx, uint8_t ch);
void Usart_SendString( USART_TypeDef * pUSARTx, char *str);
void Usart_SendHalfWord( USART_TypeDef * pUSARTx, uint16_t ch);
void Usart_SendArray( USART_TypeDef * pUSARTx, uint8_t *array, uint16_t num);
#endif /* __USART_FUNCTION_H */
usart_function.h
#include "stm32f10x.h"
#include <stdio.h>
#include "bsp_usart_usb.h"
#ifndef __USART_FUNCTION_H
#define __USART_FUNCTION_H
void Usart_SendByte( USART_TypeDef * pUSARTx, uint8_t ch);
void Usart_SendString( USART_TypeDef * pUSARTx, char *str);
void Usart_SendHalfWord( USART_TypeDef * pUSARTx, uint16_t ch);
void Usart_SendArray( USART_TypeDef * pUSARTx, uint8_t *array, uint16_t num);
#endif /* __USART_FUNCTION_H */