【UWB 定位】 简单双圆交点定位

源码是当时从谷歌上搜的，记不清那位朋友写的了，自己当时改了些地方，实现了一标签传入数据双基站的交点解析，即根据传入的俩个基站的信息花园求出的交点坐标，坐标是二维的正方形的结构。不过也是大体的解析，有些地方处理的比较粗，可以拿去玩玩。

 

大体思路：

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