源码是当时从谷歌上搜的,记不清那位朋友写的了,自己当时改了些地方,实现了一标签传入数据双基站的交点解析,即根据传入的俩个基站的信息花园求出的交点坐标,坐标是二维的正方形的结构。不过也是大体的解析,有些地方处理的比较粗,可以拿去玩玩。
大体思路:
1、当时做测试时,只实现了单对单的数据流,所以结了俩个串口,用到了俩个线程一直去读获取的基站与标签的距离信息。完全可以将信息添加一个格式,整合一块来读取,那样省力又省时。
2、硬件环境 选定A坐标为原点(0,0)B 为(X,0),X为获取的输入参数。 读串口即(/dev/tty_station1 与 /dev/tty_station2)来获取标签C的实时距离基站的位置。
3、打开管道 /dev/FIFO_UWB、为了输出解析出的坐标点到管道。
4、 输入B的X轴坐标,基站A坐标为原点(0,0)基站B 为(X,0)
5、根据双线程读取双基站与标签C的距离信息,结合定点坐标 解析出标签C的坐标信息。
two_round.c
#include"./read_usart.h"
#include<stdio.h>
#include<math.h>
#include <unistd.h>
#include<fcntl.h>
#include<stdlib.h>
#include<string.h>
#include<sys/stat.h>
extern double distanceRadiusStationFirst;
extern double distanceRadiusStationSecond;
char sendMsg[1024];
char FlagRecData = 0;
struct point_t {
double x, y;
};
struct circle_t {
struct point_t center;
double r;
};
int double_equals(double const a, double const b)
{
static const double ZERO = 1e-9;
return fabs(a - b) < ZERO;
}
double distance_sqr(struct point_t const* a, struct point_t const* b)
{
return (a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y);
}
double distance(struct point_t const* a, struct point_t const* b)
{
return sqrt(distance_sqr(a, b));
}
int insect(struct circle_t circles[], struct point_t points[])
{
double d, a, b, c, p, q, r;
double cos_value[2], sin_value[2];
if (double_equals(circles[0].center.x, circles[1].center.x)
&& double_equals(circles[0].center.y, circles[1].center.y)
&& double_equals(circles[0].r, circles[1].r)) {
return -1;
}
d = distance(&circles[0].center, &circles[1].center);
if (d > circles[0].r + circles[1].r
|| d < fabs(circles[0].r - circles[1].r)) {
return 0;
}
a = 2.0 * circles[0].r * (circles[0].center.x - circles[1].center.x);
b = 2.0 * circles[0].r * (circles[0].center.y - circles[1].center.y);
c = circles[1].r * circles[1].r - circles[0].r * circles[0].r
- distance_sqr(&circles[0].center, &circles[1].center);
p = a * a + b * b;
q = -2.0 * a * c;
if (double_equals(d, circles[0].r + circles[1].r)
|| double_equals(d, fabs(circles[0].r - circles[1].r))) {
cos_value[0] = -q / p / 2.0;
sin_value[0] = sqrt(1 - cos_value[0] * cos_value[0]);
points[0].x = circles[0].r * cos_value[0] + circles[0].center.x;
points[0].y = circles[0].r * sin_value[0] + circles[0].center.y;
if (!double_equals(distance_sqr(&points[0], &circles[1].center),
circles[1].r * circles[1].r)) {
points[0].y = circles[0].center.y - circles[0].r * sin_value[0];
}
return 1;
}
r = c * c - b * b;
cos_value[0] = (sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;
cos_value[1] = (-sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;
sin_value[0] = sqrt(1 - cos_value[0] * cos_value[0]);
sin_value[1] = sqrt(1 - cos_value[1] * cos_value[1]);
points[0].x = circles[0].r * cos_value[0] + circles[0].center.x;
points[1].x = circles[0].r * cos_value[1] + circles[0].center.x;
points[0].y = circles[0].r * sin_value[0] + circles[0].center.y;
points[1].y = circles[0].r * sin_value[1] + circles[0].center.y;
if (!double_equals(distance_sqr(&points[0], &circles[1].center),
circles[1].r * circles[1].r)) {
points[0].y = circles[0].center.y - circles[0].r * sin_value[0];
}
if (!double_equals(distance_sqr(&points[1], &circles[1].center),
circles[1].r * circles[1].r)) {
points[1].y = circles[0].center.y - circles[0].r * sin_value[1];
}
if (double_equals(points[0].y, points[1].y)
&& double_equals(points[0].x, points[1].x)) {
if (points[0].y > 0) {
points[1].y = -points[1].y;
} else {
points[0].y = -points[0].y;
}
}
return 2;
}
int main()
{
struct circle_t circles[2];
struct point_t points[2];
int ret,fd_uwb;
if((fd_uwb = open("/dev/FIFO_UWB",O_RDWR)) < 0)
{
ret=mkfifo("/dev/FIFO_UWB",0777);
if(ret<0)
{
printf("creat fifo FIFO_UWB failure\n");
return -1;
}
fd_uwb = open("/dev/FIFO_UWB",O_RDWR);//打开管道文件 通过一直读管道来获取数据
}
if(fd_uwb<0)
{
printf("open fifo FIFO_UWB failure\n");
return -1;
}
circles[1].center.x = 0;//基站A坐标(0,0)
circles[1].center.y = 0;
//circles[0].center.x = 200;//基站B坐标(b,0) cm
circles[0].center.y = 0;
pthread_t thread_read,thread_read_station2;
if(start_thread_func(com_read, &thread_read) != 0)
{
printf("error to leave/n");
return -1;
}
if(start_thread_func(com_read_station2, &thread_read_station2) != 0)
{
printf("error to leave/n");
return -1;
}
printf("开始获取线程读取的距离信息写入半径R\n");
usleep(10);
printf("写入基站B的x轴坐标\n");
scanf("%lf",&circles[0].center.x);
while(1)
{
circles[0].r = 100 * distanceRadiusStationFirst;
circles[1].r = 100 * distanceRadiusStationSecond;
//printf("circles[0].r : %f",circles[0].r);
//printf("circles[1].r : %f \n",circles[1].r);
/* while (!(circles[0].r = 100 * distanceRadiusSta tionFirst) && !(circles[1].r = 100 * distanceRadiusStationSecond))
{
sleep(1);
printf("wait for data....\n");
// FlagRecData = 1;
}
if(FlagRecData) {
printf("rec data success!\n");
FlagRecData = 0;
}*/
switch (insect(circles, points))
{
case -1:
printf("THE CIRCLES ARE THE SAME/n");
break;
case 0:
printf("NO INTERSECTION/n");
break;
case 1:
memset(sendMsg,0,strlen(sendMsg));
printf("(%.3lf %.3lf)\n", points[0].x, points[0].y);
sprintf(sendMsg,"%f,%f\n",points[0].x,points[0].y);
write(fd_uwb,sendMsg,strlen(sendMsg));
//write(fd_uwb,"\r",1);
break;
case 2:
memset(sendMsg,0,strlen(sendMsg));
printf("(%.3lf %.3lf)\n", points[0].x, points[0].y);// (%.3lf %.3lf) ,points[1].x, points[1].y
sprintf(sendMsg,"%f,%f\n",points[0].x,points[0].y);
write(fd_uwb,sendMsg,strlen(sendMsg));
//write(fd_uwb,"\r",1);
default:
break;
}
sleep(1);
}
stop_thread_func(&thread_read);
stop_thread_func(&thread_read_station2);
close(fd_uwb);
return 0;
}
read.usart.c
#include <pthread.h>
#include <stdio.h>
#include <sys/time.h>
#include <string.h>
#include<termios.h>
#include<sys/stat.h>
#include<fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include "./read_usart.h"
volatile double distanceRadiusStationFirst;
volatile double distanceRadiusStationSecond;
//Data_Usart Data;
void* com_read(void* pstatu)
{
//printtid();
//int i=0;
int fd,num;
struct termios oldtio,newtio;
char buf[R_BUF_LEN];
printf("start com_read_station1...\n");
/*打开PC机的COM1通信端口*/
fd=open(STATION_FIRST,O_RDWR | O_NOCTTY | O_NONBLOCK/*| O_NDELAY*/);
if(fd<0)
{
perror(STATION_FIRST);
exit(1);
}
/*将目前终端机的结构保存至oldtio结构*/
tcgetattr(fd,&oldtio);
/*清除newtio结构,重新设置通信协议*/
bzero(&newtio,sizeof(newtio));
/*通信协议设为8N1,8位数据位,N没有效验,1位结束位*/
newtio.c_cflag = BAUDRATE |CS8|CLOCAL|CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
/*设置为正规模式*/
newtio.c_lflag=ICANON;
/*清除所有队列在串口的输入*/
tcflush(fd,TCIFLUSH); /*新的termios的结构作为通信端口的参数*/
tcsetattr(fd,TCSANOW,&newtio);
printf("reading...\n");
while(1)
{
num = read(fd,buf, R_BUF_LEN);
buf[R_BUF_LEN-1] = 0;
if(num > 0 && num <= R_BUF_LEN)
{
buf[num]=0;
sscanf(buf,"DIST1#%lf#m",&distanceRadiusStationFirst);
if(distanceRadiusStationFirst<=0.0) distanceRadiusStationFirst = 0.0;
printf("rec data from station1 : %3.2lf m\n ",distanceRadiusStationFirst);
//printf("%s", buf);
fflush(stdout);
usleep(10000);
}
}
printf("close...\n");
close(fd);
/*恢复旧的通信端口参数*/
tcsetattr(fd,TCSANOW,&oldtio);
}
void* com_read_station2(void* pstatu)
{
//printtid();
//int i=0;
int fd_station2,num;
struct termios oldtio,newtio;
char buf_station2[R_BUF_LEN];
printf("start com_read_station2...\n");
/*打开PC机的COM1通信端口*/
fd_station2=open(STATION_SECOND,O_RDWR | O_NOCTTY | O_NONBLOCK/*| O_NDELAY*/);
if(fd_station2<0)
{
perror(STATION_SECOND);
exit(1);
}
/*将目前终端机的结构保存至oldtio结构*/
tcgetattr(fd_station2,&oldtio);
/*清除newtio结构,重新设置通信协议*/
bzero(&newtio,sizeof(newtio));
/*通信协议设为8N1,8位数据位,N没有效验,1位结束位*/
newtio.c_cflag = BAUDRATE |CS8|CLOCAL|CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
/*设置为正规模式*/
newtio.c_lflag=ICANON;
/*清除所有队列在串口的输入*/
tcflush(fd_station2,TCIFLUSH); /*新的termios的结构作为通信端口的参数*/
tcsetattr(fd_station2,TCSANOW,&newtio);
printf("reading...\n");
while(1)
{
num = read(fd_station2,buf_station2, R_BUF_LEN);
buf_station2[R_BUF_LEN-1] = 0;
if(num > 0 && num <= R_BUF_LEN)
{
buf_station2[num]=0;
sscanf(buf_station2,"DIST2@%lf@m",&distanceRadiusStationSecond);
if(distanceRadiusStationSecond <= 0) distanceRadiusStationSecond = 0;
printf("rec data from station2 : %3.2lf m\n ",distanceRadiusStationSecond);
//printf("%s", buf_station2);
fflush(stdout);
usleep(10000);
}
}
printf("close...\n");
close(fd_station2);
/*恢复旧的通信端口参数*/
tcsetattr(fd_station2,TCSANOW,&oldtio);
}
/*
开始线程
thread_fun 线程函数
pthread 线程函数所在pthread变量
par 线程函数参数
COM_STATU 线程函数状态控制变量 1:运行 0:退出
*/
int start_thread_func(void*(*func)(void*), pthread_t* pthread)
{
memset(pthread, 0, sizeof(pthread_t));
int temp;
/*创建线程*/
if((temp = pthread_create(pthread, NULL, func, NULL)) != 0)
printf("线程创建失败!\n");
else
{
//int id = pthread_self();
printf("线程%u被创建\n", (unsigned int)*pthread);
}
return temp;
}
/*
结束线程
pthread 线程函数所在pthread变量
COM_STATU 线程函数状态控制变量 1:运行 0:退出
*/
int stop_thread_func(pthread_t* pthread)
{
printf("prepare stop thread %u\n",(unsigned int)*pthread);
if(*pthread !=0)
{
pthread_join(*pthread, NULL);
}
return 0;
}
read.usart.h
#ifndef READ_USART_H
#define READ_USART_H
#include <pthread.h>
#define BAUDRATE B115200
#define STATION_FIRST "/dev/tty_station1"
#define STATION_SECOND "/dev/tty_station2"
#define R_BUF_LEN (256)
// typedef struct {
// double distanceRadiusStationFirst;
// double distanceRadiusStationSecond;
// } Data_Usart;
// extern Data_Usart Data;
extern volatile double distanceRadiusStationFirst;
extern volatile double distanceRadiusStationSecond;
void* com_read(void* pstatu);
void* com_read_station2(void* pstatu);
int start_thread_func(void*(*func)(void*), pthread_t* pthread);
int start_thread_func(void*(*func)(void*), pthread_t* pthread);
int stop_thread_func(pthread_t* pthread);
#endif