BWS2000倾角传感器c++测试代码
2023-12-20 15:30:49
使用瑞芬的倾角传感器配置的时候，数据手册一下就配置好了，但是BWS2000倾角传感器总是出错，这里进行一下记录出现的问题与解决方式。
1.初步测试
在配置BWS2000倾角传感器读取帧数据的时候，总是出现一个问题，就是进行输出的时候，得到的数据与实际设置的频率并不是不一致的。
其中，command_R1是对于输出频率的设置，command_R2是对于波特率的设置，command_R3保存设置命令。
问题一：注意当COM口大于10的时候，需要在前面添加\\\\.\\，否则会出现连接不上的情况，具体原因见博客：https://www.cnblogs.com/pandamohist/p/13681042.html
问题二：读出来的数据是16进制的，因此需要进行自己转换，
转换过程代码如下所示（这个代码不是最优方式，比较麻烦，懒得再思考了）：
if (sizeof(response_R) >= 13 && response_R[0] == 0x77 && response_R[1] == 0x10)//&& response[3] == 0x84)
{
//x轴
int hexValues[] = { response_R[5] & 0xFF, response_R[6] & 0xFF, response_R[7] & 0xFF };
int numValues = sizeof(hexValues) / sizeof(hexValues[0]);
std::stringstream ss;
ss << std::setfill('0') << std::setw(2) << std::hex << hexValues[0];
//std::cout << "String representation: " << numValues << std::endl;
for (int i = 1; i < numValues; i++) {
ss << "." << std::setw(2) << std::hex << hexValues[i];
}
std::string resultString = ss.str();
std::cout << "X轴 String representation: " << resultString << std::endl;
std::string inputString = resultString;
std::string outputString;
size_t firstDotIndex = inputString.find('.');
size_t secondDotIndex = inputString.find('.', firstDotIndex + 1);
if (firstDotIndex != std::string::npos && secondDotIndex != std::string::npos) {
outputString = inputString.substr(0, secondDotIndex) + inputString.substr(secondDotIndex + 1);
}
else {
outputString = inputString;
}
double resultDouble = std::stod(outputString);
if (response_R[4] == 0x00)
{
Sensor_Angle_RX0 = resultDouble;
}
else
{
Sensor_Angle_RX0 = -resultDouble;
}
std::cout << "x轴角度" << std::fixed << std::setprecision(4) << resultDouble << std::endl;
//Y轴
int hexValues1[] = { response_R[9] & 0xFF, response_R[10] & 0xFF, response_R[11] & 0xFF };
int numValues1 = sizeof(hexValues1) / sizeof(hexValues1[0]);
// Convert hexadecimal values to a string "01.0102"
std::stringstream ss1;
ss1 << std::setfill('0') << std::setw(2) << std::hex << hexValues1[0];
std::cout << "String representation: " << numValues << std::endl;
for (int i = 1; i < numValues1; i++) {
ss1 << "." << std::setw(2) << std::hex << hexValues1[i];
}
std::string resultString1 = ss1.str();
//std::cout << "Y轴 String representation: " << resultString1 << std::endl;
std::string inputString1 = resultString1;
std::string outputString1;
size_t firstDotIndex1 = inputString1.find('.');
size_t secondDotIndex1 = inputString1.find('.', firstDotIndex1 + 1);
if (firstDotIndex1 != std::string::npos && secondDotIndex1 != std::string::npos) {
outputString1 = inputString1.substr(0, secondDotIndex1) + inputString1.substr(secondDotIndex1 + 1);
}
else {
outputString1 = inputString1;
}
double resultDouble1 = std::stod(outputString1);
if (response_R[8] == 0x00)
{
Sensor_Angle_RY0 = resultDouble1;
}
else
{
Sensor_Angle_RY0 = -resultDouble1;
}
std::cout << "y轴: " << std::fixed << std::setprecision(4) << resultDouble1 << std::endl;
}我写的测试代码如下所示：
#include <Windows.h>
#include <sstream>
#include <iomanip>
#include <iostream>
#include <chrono>
#include <thread>
#include <Windows.h>
int main() {
HANDLE hSerial;
DCB dcbSerialParams = { 0 };
COMMTIMEOUTS timeouts = { 0 };
DWORD bytesRead, bytesWritten, bytesWritten1, bytesWritten2, bytesWritten3, response_R1;
char command_R[] = { 0x77, 0x04, 0x00, 0x04, 0x08 };//读X、Y轴角度 发送命令： 77 04 00 04 08
char response_R[18] = { 18 };
double Sensor_Angle_RX0;
double Sensor_Angle_RY0;
// 设置角度输出频率模式 77 05 00 0C 06（100Hz 05 50Hz 04 25Hz） 11 100Hz
char command_R1[] = { 0x77, 0x05, 0x00, 0x0C, 0x04, 0x11 };//25Hz
// 发送命令： 77 05 00 0B 04（115200 03 19200 02 9600） 12 115200 2
char command_R2[] = { 0x77, 0x05, 0x00, 0x0B, 0x02, 0x12 };
// 保存设置命令： 77 04 00 0A 0E
char command_R3[] = { 0x77, 0x04, 0x00, 0x0A, 0x0E };
char dataReceived[100];
wchar_t portName[] = L"\\\\.\\COM11"; // Note the 'L' before the string
hSerial = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
if (hSerial == INVALID_HANDLE_VALUE) {
if (GetLastError() == ERROR_FILE_NOT_FOUND) {
std::cout << "Serial port not available." << std::endl;
}
return 1;
}
dcbSerialParams.DCBlength = sizeof(dcbSerialParams);
if (!GetCommState(hSerial, &dcbSerialParams)) {
std::cout << "Error getting serial port state." << std::endl;
CloseHandle(hSerial);
return 1;
}
dcbSerialParams.BaudRate = CBR_9600;
dcbSerialParams.ByteSize = 8;
dcbSerialParams.StopBits = ONESTOPBIT;
dcbSerialParams.Parity = NOPARITY;
if (!SetCommState(hSerial, &dcbSerialParams)) {
std::cout << "Error setting serial port state." << std::endl;
CloseHandle(hSerial);
return 1;
}
timeouts.ReadIntervalTimeout = 50;
timeouts.ReadTotalTimeoutConstant = 50;
timeouts.ReadTotalTimeoutMultiplier = 10;
timeouts.WriteTotalTimeoutConstant = 50;
timeouts.WriteTotalTimeoutMultiplier = 10;
if (!SetCommTimeouts(hSerial, &timeouts)) {
std::cout << "Error setting timeouts." << std::endl;
CloseHandle(hSerial);
return 1;
}
if (!WriteFile(hSerial, command_R1, sizeof(command_R1), &bytesWritten1, NULL)) {
std::cout << "Error writing to serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
//std::cout << "Data sent successfully." << std::endl;
/*
if (!ReadFile(hSerial, dataReceived, sizeof(dataReceived), &bytesRead, NULL)) {
std::cout << "Error reading from serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
if (!WriteFile(hSerial, command_R2, sizeof(command_R2), &bytesWritten2, NULL)) {
std::cout << "Error writing to serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
if (!ReadFile(hSerial, dataReceived, sizeof(dataReceived), &bytesRead, NULL)) {
std::cout << "Error reading from serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
if (!WriteFile(hSerial, command_R3, sizeof(command_R3), &bytesWritten3, NULL)) {
std::cout << "Error writing to serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
int i = 0;
while (1)
{
for (int i = 0; i < sizeof(response_R); i++) {
printf("%02X ", response_R[i]);
}
std::cout << std::endl;
std::cout << i++ << response_R<< std::endl;
/*
//右倾角传感器
if (sizeof(response_R) >= 13 && response_R[0] == 0x77 && response_R[1] == 0x10)//&& response[3] == 0x84)
{
*/
//x轴
// Given hexadecimal values
int hexValues[] = { response_R[5] & 0xFF, response_R[6] & 0xFF, response_R[7] & 0xFF };
int numValues = sizeof(hexValues) / sizeof(hexValues[0]);
// Convert hexadecimal values to a string "01.0102"
std::stringstream ss;
ss << std::setfill('0') << std::setw(2) << std::hex << hexValues[0];
//std::cout << "String representation: " << numValues << std::endl;
for (int i = 1; i < numValues; i++) {
ss << "." << std::setw(2) << std::hex << hexValues[i];
}
std::string resultString = ss.str();
//std::cout << "X轴 String representation: " << resultString << std::endl;
std::string inputString = resultString;
std::string outputString;
size_t firstDotIndex = inputString.find('.');
size_t secondDotIndex = inputString.find('.', firstDotIndex + 1);
if (firstDotIndex != std::string::npos && secondDotIndex != std::string::npos) {
outputString = inputString.substr(0, secondDotIndex) + inputString.substr(secondDotIndex + 1);
}
else {
outputString = inputString;
}
// Convert the string "01.0102" to double 01.0102
double resultDouble = std::stod(outputString);
if (response_R[4] == 0x00)
{
Sensor_Angle_RX0 = resultDouble;
}
else
{
Sensor_Angle_RX0 = -resultDouble;
}
std::cout << "x轴角度" << std::fixed << std::setprecision(4) << resultDouble << std::endl;
//Y轴
int hexValues1[] = { response_R[9] & 0xFF, response_R[10] & 0xFF, response_R[11] & 0xFF };
int numValues1 = sizeof(hexValues1) / sizeof(hexValues1[0]);
std::stringstream ss1;
ss1 << std::setfill('0') << std::setw(2) << std::hex << hexValues1[0];
//std::cout << "String representation: " << numValues << std::endl;
for (int i = 1; i < numValues1; i++) {
ss1 << "." << std::setw(2) << std::hex << hexValues1[i];
}
std::string resultString1 = ss1.str();
std::string inputString1 = resultString1;
std::string outputString1;
size_t firstDotIndex1 = inputString1.find('.');
size_t secondDotIndex1 = inputString1.find('.', firstDotIndex1 + 1);
if (firstDotIndex1 != std::string::npos && secondDotIndex1 != std::string::npos) {
outputString1 = inputString1.substr(0, secondDotIndex1) + inputString1.substr(secondDotIndex1 + 1);
}
else {
outputString1 = inputString1;
}
double resultDouble1 = std::stod(outputString1);
if (response_R[8] == 0x00)
{
Sensor_Angle_RY0 = resultDouble1;
}
else
{
Sensor_Angle_RY0 = -resultDouble1;
}
std::cout << "y轴: " << std::fixed << std::setprecision(4) << resultDouble1 << std::endl;
}
getchar();
CloseHandle(hSerial);
return 0;
}
在上面的代码之中，我是使用的自动输出的方式，配置的25Hz进行输出，但是出现的问题就是当把下面的代码进行打开之后：
if (sizeof(response_R) >= 13 && response_R[0] == 0x77 && response_R[1] == 0x10)//&& response[3] == 0x84)
{
//x轴
int hexValues[] = { response_R[5] & 0xFF, response_R[6] & 0xFF, response_R[7] & 0xFF };
int numValues = sizeof(hexValues) / sizeof(hexValues[0]);
// Convert hexadecimal values to a string "01.0102"
std::stringstream ss;
ss << std::setfill('0') << std::setw(2) << std::hex << hexValues[0];
//std::cout << "String representation: " << numValues << std::endl;
for (int i = 1; i < numValues; i++) {
ss << "." << std::setw(2) << std::hex << hexValues[i];
}
std::string resultString = ss.str();
//std::cout << "X轴 String representation: " << resultString << std::endl;
std::string inputString = resultString;
std::string outputString;
size_t firstDotIndex = inputString.find('.');
size_t secondDotIndex = inputString.find('.', firstDotIndex + 1);
if (firstDotIndex != std::string::npos && secondDotIndex != std::string::npos) {
outputString = inputString.substr(0, secondDotIndex) + inputString.substr(secondDotIndex + 1);
}
else {
outputString = inputString;
}
double resultDouble = std::stod(outputString);
if (response_R[4] == 0x00)
{
Sensor_Angle_RX0 = resultDouble;
}
else
{
Sensor_Angle_RX0 = -resultDouble;
}
std::cout << "x轴角度" << std::fixed << std::setprecision(4) << resultDouble << std::endl;
//Y轴
int hexValues1[] = { response_R[9] & 0xFF, response_R[10] & 0xFF, response_R[11] & 0xFF };
int numValues1 = sizeof(hexValues1) / sizeof(hexValues1[0]);
std::stringstream ss1;
ss1 << std::setfill('0') << std::setw(2) << std::hex << hexValues1[0];
//std::cout << "String representation: " << numValues << std::endl;
for (int i = 1; i < numValues1; i++) {
ss1 << "." << std::setw(2) << std::hex << hexValues1[i];
}
std::string resultString1 = ss1.str();
std::string inputString1 = resultString1;
std::string outputString1;
size_t firstDotIndex1 = inputString1.find('.');
size_t secondDotIndex1 = inputString1.find('.', firstDotIndex1 + 1);
if (firstDotIndex1 != std::string::npos && secondDotIndex1 != std::string::npos) {
outputString1 = inputString1.substr(0, secondDotIndex1) + inputString1.substr(secondDotIndex1 + 1);
}
else {
outputString1 = inputString1;
}
// Convert the string "01.0102" to double 01.0102
double resultDouble1 = std::stod(outputString1);
if (response_R[8] == 0x00)
{
Sensor_Angle_RY0 = resultDouble1;
}
else
{
Sensor_Angle_RY0 = -resultDouble1;
}
std::cout << "y轴: " << std::fixed << std::setprecision(4) << resultDouble1 << std::endl;
}发现明显输出的频率是和25Hz是不一致的。
然后直接将相应的报文进行打印，调用
for (int i = 0; i < sizeof(response_R); i++) {
printf("%02X ", response_R[i]);
}输出如下结果：
真正想用的的数据是0x77 0x10 0x00数据之后的，也就是尽量是要求每一帧的数据都是这个情况，由于设置了自动输出的频率为25Hz，所以你读取相应的response是不知道在哪一个时刻开始的，需要进行相应的频率进行设置。
2.应答测试
在每个帧信号之后，输出一个应答信号，检测是否输出正确？
#include <Windows.h>
#include <iostream>
#include <iomanip>
#include <iostream>
#include <chrono>
#include <thread>
#include <Windows.h>
#include <iostream>
#include <iomanip>
double convertStringToDouble(const std::string& str) {
double result;
std::istringstream stream(str);
stream >> result;
return result;
}
int main() {
HANDLE hSerial;
DCB dcbSerialParams = { 0 };
COMMTIMEOUTS timeouts = { 0 };
DWORD bytesRead, bytesWritten, bytesWritten1, bytesWritten2, bytesWritten3, response_R1;
char command_R[] = { 0x77, 0x04, 0x00, 0x04, 0x08 };//读X、Y轴角度 发送命令： 77 04 00 04 08
char response_R[18] = { 18 };
double Sensor_Angle_RX0;
double Sensor_Angle_RY0;
// 设置角度输出频率模式 77 05 00 0C 06（100Hz 05 50Hz 04 25Hz） 11 100Hz
char command_R1[] = { 0x77, 0x05, 0x00, 0x0C, 0x00, 0x11 };//25Hz
// 发送命令： 77 05 00 0B 04（115200 03 19200 02 9600） 12 115200 2
char command_R2[] = { 0x77, 0x05, 0x00, 0x0B, 0x02, 0x12 };
// 保存设置命令： 77 04 00 0A 0E
char command_R3[] = { 0x77, 0x04, 0x00, 0x0A, 0x0E };
char dataReceived[20];
wchar_t portName[] = L"\\\\.\\COM10"; // Note the 'L' before the string
hSerial = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
if (hSerial == INVALID_HANDLE_VALUE) {
if (GetLastError() == ERROR_FILE_NOT_FOUND) {
std::cout << "Serial port not available." << std::endl;
}
return 1;
}
dcbSerialParams.DCBlength = sizeof(dcbSerialParams);
if (!GetCommState(hSerial, &dcbSerialParams)) {
std::cout << "Error getting serial port state." << std::endl;
CloseHandle(hSerial);
return 1;
}
dcbSerialParams.BaudRate = CBR_9600;
dcbSerialParams.ByteSize = 8;
dcbSerialParams.StopBits = ONESTOPBIT;
dcbSerialParams.Parity = NOPARITY;
if (!SetCommState(hSerial, &dcbSerialParams)) {
std::cout << "Error setting serial port state." << std::endl;
CloseHandle(hSerial);
return 1;
}
timeouts.ReadIntervalTimeout = 500;
timeouts.ReadTotalTimeoutConstant = 500;
timeouts.ReadTotalTimeoutMultiplier = 100;
timeouts.WriteTotalTimeoutConstant = 500;
timeouts.WriteTotalTimeoutMultiplier = 100;
if (!SetCommTimeouts(hSerial, &timeouts)) {
std::cout << "Error setting timeouts." << std::endl;
CloseHandle(hSerial);
return 1;
}
if (!WriteFile(hSerial, command_R1, sizeof(command_R1), &bytesWritten1, NULL)) {
std::cout << "Error writing to serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
std::cout << "输出频率 " << std::endl;
if (!ReadFile(hSerial, dataReceived, sizeof(dataReceived), &bytesRead, NULL)) {
std::cout << "Error reading from serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
for (int i = 0; i < sizeof(dataReceived); i++) {
printf("%02X ", dataReceived[i]);
}
if (!WriteFile(hSerial, command_R2, sizeof(command_R2), &bytesWritten2, NULL)) {
std::cout << "Error writing to serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
std::cout << " 发送命令" << std::endl;
if (!ReadFile(hSerial, dataReceived, sizeof(dataReceived), &bytesRead, NULL)) {
std::cout << "Error reading from serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
for (int i = 0; i < sizeof(dataReceived); i++) {
printf("%02X ", dataReceived[i]);
}
if (!WriteFile(hSerial, command_R3, sizeof(command_R3), &bytesWritten3, NULL)) {
std::cout << "Error writing to serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
if (!ReadFile(hSerial, dataReceived, sizeof(dataReceived), &bytesRead, NULL)) {
std::cout << "Error reading from serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
std::cout << "保存设置 " << std::endl;
for (int i = 0; i < sizeof(dataReceived); i++) {
printf("%02X ", dataReceived[i]);
}
std::cout << " " << std::endl;
std::cout << "开始采集 " << std::endl;
int i = 0;
while (1)
{
if (!WriteFile(hSerial, command_R, sizeof(command_R), &bytesWritten, NULL)) {
std::cout << "Error writing to serial port." << std::endl;
CloseHandle(hSerial);
return 1;
}
if (!ReadFile(hSerial, response_R, sizeof(response_R), &response_R1, NULL)) {
std::cout << "Error reading from serial port." << std::endl;
//CloseHandle(hSerial);
//return 1;
}
std::cout << i++ << std::endl;
for (int i = 0; i < sizeof(response_R); i++) {
printf("%02X ", response_R[i]);
}
std::cout << std::endl;
/*
//右倾角传感器
if (sizeof(response_R) >= 13 && response_R[0] == 0x77 && response_R[1] == 0x10)//&& response[3] == 0x84)
{
*/
//x轴
int hexValues[] = { response_R[5] & 0xFF, response_R[6] & 0xFF, response_R[7] & 0xFF };
int numValues = sizeof(hexValues) / sizeof(hexValues[0]);
std::stringstream ss;
ss << std::setfill('0') << std::setw(2) << std::hex << hexValues[0];
//std::cout << "String representation: " << numValues << std::endl;
for (int i = 1; i < numValues; i++) {
ss << "." << std::setw(2) << std::hex << hexValues[i];
}
std::string resultString = ss.str();
std::string inputString = resultString;
std::string outputString;
size_t firstDotIndex = inputString.find('.');
size_t secondDotIndex = inputString.find('.', firstDotIndex + 1);
if (firstDotIndex != std::string::npos && secondDotIndex != std::string::npos) {
outputString = inputString.substr(0, secondDotIndex) + inputString.substr(secondDotIndex + 1);
}
else {
outputString = inputString;
}
double resultDouble = std::stod(outputString);
if (response_R[4] == 0x00)
{
Sensor_Angle_RX0 = resultDouble;
}
else
{
Sensor_Angle_RX0 = -resultDouble;
}
std::cout << "x轴角度" << std::fixed << std::setprecision(4) << resultDouble << std::endl;
//Y轴
int hexValues1[] = { response_R[9] & 0xFF, response_R[10] & 0xFF, response_R[11] & 0xFF };
int numValues1 = sizeof(hexValues1) / sizeof(hexValues1[0]);
std::stringstream ss1;
ss1 << std::setfill('0') << std::setw(2) << std::hex << hexValues1[0];
//std::cout << "String representation: " << numValues << std::endl;
for (int i = 1; i < numValues1; i++) {
ss1 << "." << std::setw(2) << std::hex << hexValues1[i];
}
std::string resultString1 = ss1.str();
std::string inputString1 = resultString1;
std::string outputString1;
size_t firstDotIndex1 = inputString1.find('.');
size_t secondDotIndex1 = inputString1.find('.', firstDotIndex1 + 1);
if (firstDotIndex1 != std::string::npos && secondDotIndex1 != std::string::npos) {
outputString1 = inputString1.substr(0, secondDotIndex1) + inputString1.substr(secondDotIndex1 + 1);
}
else {
outputString1 = inputString1;
}
double resultDouble1 = std::stod(outputString1);
if (response_R[8] == 0x00)
{
Sensor_Angle_RY0 = resultDouble1;
}
else
{
Sensor_Angle_RY0 = -resultDouble1;
}
std::cout << "y轴: " << std::fixed << std::setprecision(4) << resultDouble1 << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(400));
}
getchar();
CloseHandle(hSerial);
return 0;
}
当设置波特率为9600，应答模式的情况下，可以见到输出完完全全正确：
但是我需要高频率的输出，见到技术手册里面存在这样一句话，
给定的软件是可以正常运行的。
3.解决高频率输出
当我把频率直接设置成115200的时候，发现还是存在错误，应答信号直接出错了。
我想了很久，然后技术手册之中有一句话是这样说的，
每次变更通信波特率成功之后，会以原来的波特率发送应答信号，然后立即改变通信波特率。
也就是说初始的时候，波特率还是应该设置为9600，发送command_R2完毕之后，才会发生改变。因此，下面这个代码值是不能够发生变化的
只需要将command_R2的值进行变化就是可以的。
运行1348次数据仍然是正确的。
4.界面化设计
此处使用的应答模式
因为代码之中使用自动输出模式，会出现检测代码的问题，前面已经提到过。
后面的思路是在界面之中设定一个定时器，以50Hz的方式进行写读，不会出现读出来的帧数据出错的问题，规避了自动输出的局限性。之后使用线程对于这个定时器进行实时调用就可以了。
文章来源:https://blog.csdn.net/m0_47489229/article/details/135087726
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！