├── DWA.cpp
├── DWA.h
├── LineExtractionFromLaser.cpp
├── LineExtractionFromLaser.h
├── README.md
├── laser_filters.cpp
├── laser_filters.h
├── main.cpp
├── rs232.cpp
└── rs232.h


/DWA.cpp:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/githjk/DynamicWindowApproach/de5029e79cf1257b7d0697d76bc03d89f7248fad/DWA.cpp


--------------------------------------------------------------------------------
/DWA.h:
--------------------------------------------------------------------------------
 1 | #include <vector>
 2 | #include <queue>
 3 | #include <thread>
 4 | 
 5 | class DWA{
 6 | 	typedef std::vector<std::vector<int>> cluster_vector;
 7 | 	typedef std::vector<std::vector<std::pair<float, float>>> VMap;
 8 | public:
 9 | 	DWA(){
10 | 		errno_t err;
11 | 		err = fopen_s(&fptr, "Log.txt", "w");
12 | 		if (err != 0){
13 | 			printf("open file failure...\n");
14 | 		}
15 | 		head_direction = 90;
16 | 		targetMask();
17 | 	}
18 | 	~DWA(){
19 | 		fclose(fptr);
20 | 	}
21 | 	void twoThread();
22 | 	volatile bool flag = false;
23 | 	volatile int head_direction ;
24 | private:
25 | 	struct arima_laserscan
26 | 	{
27 | 		std::vector<float> ranges;
28 | 		std::vector<float> angles;
29 | 	};
30 | 	struct slope_range
31 | 	{
32 | 		float r_right;
33 | 		float r_left;
34 | 		float r;
35 | 		float y;
36 | 	};
37 | 	void targetMask(){
38 | 		for (int i = 0; i < 101; i++){
39 | 			if (i < 50)
40 | 				mask.push_back((float)i / 50.0F);
41 | 			else if (i == 50)
42 | 				mask.push_back(1);
43 | 			else
44 | 				mask.push_back((float)(100 - i) / 50.0F);
45 | 		}
46 | 	}
47 | 	void robotHead(int);
48 | 	void motorThread(int);
49 | 	void laserThread(int);
50 | 	int cal_DWA(arima_laserscan);
51 | 	cluster_vector cluster(arima_laserscan&);
52 | 	std::queue<arima_laserscan> global_laserscan;
53 | 	std::vector<float> mask;
54 | 	int count_frame = 0;
55 | 	int count_obs, count_backup;
56 | 	int pre_v = 50;
57 | 	volatile float robot_head = 90.0F;
58 | 	float time_lapsed = 0;
59 | 	bool Lock = false, trap = false, Lclose = false, Rclose = false;
60 | 	FILE* fptr;
61 | };


--------------------------------------------------------------------------------
/LineExtractionFromLaser.cpp:
--------------------------------------------------------------------------------
  1 | #include "LineExtractionFromLaser.h"
  2 | 
  3 | using namespace LnExt;
  4 | 
  5 | CLineExtractionFromLaser::CLineExtractionFromLaser()
  6 | {
  7 |    m_minLineLengthM = 1.5; // (warning) TBD
  8 |    m_inlierRatio = 0.25F; // inlier / total (warning) TBD
  9 |    m_minPtInCluster = (int)(LASER_ANGLE * m_inlierRatio / 2); // 1/2 #inliers
 10 |    m_maxTrialCnt = (int)(log(1 - 0.999) / log(1 - m_inlierRatio*m_inlierRatio) + 0.5);
 11 |    m_maxDistToLine = 0.1F; // (unit: meter) if the distance between a point and the computed line is smaller than this threshold, consider this point as an inlier
 12 | }
 13 | 
 14 | 
 15 | CLineExtractionFromLaser::~CLineExtractionFromLaser()
 16 | {
 17 | }
 18 | 
 19 | bool CLineExtractionFromLaser::LineExtract(const ArimaLaserscan laserScans, const ClusterVector clusters, int& sIdx, int& eIdx)
 20 | {
 21 |    // find largest cluster
 22 |    int largestClusterIdx = -1;
 23 |    int largestClusterNum = 0;
 24 |    for (unsigned int i = 0; i < clusters.size(); i++)
 25 |    {
 26 |       if (clusters[i].size() > largestClusterNum)
 27 |       {
 28 |          largestClusterIdx = i;
 29 |          largestClusterNum = (int)clusters[i].size();
 30 |       }
 31 |    }
 32 |    // RANSAC
 33 |    bool hasValidLineSegment = false;
 34 |    int bestFrom = -1;
 35 |    int bestTo = -1;
 36 |    int bestInlierNum = -1;
 37 |    //
 38 |    if (largestClusterNum > m_minPtInCluster) // apply RANSAC only in the largest cluster which has more laser scans than "minPtInCluster" 
 39 |    {
 40 |       for (int t = 0; t < m_maxTrialCnt; t++)
 41 |       {
 42 |          // select points from the largest cluster
 43 |          int from = rand() % clusters[largestClusterIdx].size();
 44 |          from = clusters[largestClusterIdx][from];
 45 |          int to = rand() % clusters[largestClusterIdx].size();
 46 |          to = clusters[largestClusterIdx][to];
 47 |          while (from == to)
 48 |          {
 49 |             to = rand() % clusters[largestClusterIdx].size();
 50 |             to = clusters[largestClusterIdx][to];
 51 |          }
 52 | 
 53 |          // compute an equation for line
 54 |          cv::Point2f pt1 = CvtLaserScanToXY(DEG_TO_RAD(laserScans.angles[from]), laserScans.ranges[from]);
 55 |          cv::Point2f pt2 = CvtLaserScanToXY(DEG_TO_RAD(laserScans.angles[to]), laserScans.ranges[to]);
 56 | 
 57 |          float theta = 0;
 58 |          float rho = 0;
 59 |          ComputeLineEq(pt1, pt2, theta, rho);
 60 | 
 61 |          // compute the distance between laser scans in the largest cluster and the computed line, and find the line segment (sp <---> ep)
 62 |          int inlierCnt = 0;
 63 |          cv::Point2f sp = pt1;
 64 |          cv::Point2f ep = pt2;
 65 |          // [20151203_Curtis] only laser scans in the largest cluster can vote
 66 |          for (unsigned int i = 0; i < clusters[largestClusterIdx].size(); i++)
 67 |          {
 68 |             int idx = clusters[largestClusterIdx][i];
 69 |             cv::Point2f pt = CvtLaserScanToXY(DEG_TO_RAD(laserScans.angles[idx]), laserScans.ranges[idx]);
 70 |             float dist = ComputeDistFromPtToLine(theta, rho, pt);
 71 |             if (dist < m_maxDistToLine)
 72 |             {
 73 |                inlierCnt++;
 74 |                bool updateSP = (InnerProduct(pt, ep, sp) < 0);
 75 |                bool updateEP = (InnerProduct(pt, sp, ep) < 0);
 76 |                if (updateSP)
 77 |                {
 78 |                   sp = pt;
 79 |                   from = idx;
 80 |                }
 81 |                if (updateEP)
 82 |                {
 83 |                   ep = pt;
 84 |                   to = idx;
 85 |                }
 86 |                if (updateSP && updateEP)
 87 |                {
 88 |                   printf("===========================\n");
 89 |                   printf("Warning: check what happens\n");
 90 |                   printf("===========================\n");
 91 |                }
 92 |             }
 93 |          }
 94 | 
 95 |          // check if the found line segment is long enough and has enough inliers
 96 |          // besides, try to find as many as inliers instead the longest distance
 97 |          float dist = ComputeDistBetweenTwoPt(sp, ep);
 98 |          if (inlierCnt > m_minPtInCluster && dist > m_minLineLengthM && inlierCnt > bestInlierNum)
 99 |          {
100 |             hasValidLineSegment = true;
101 |             bestFrom = from;
102 |             bestTo = to;
103 |             bestInlierNum = inlierCnt;
104 |          }
105 |       }
106 |    }
107 |  
108 |    //
109 |    sIdx = bestFrom;
110 |    eIdx = bestTo;
111 |    return hasValidLineSegment;
112 | }
113 | 
114 | // ------------------------------------------------------
115 | // function to compute equation of a line from two points
116 | // ------------------------------------------------------
117 | void CLineExtractionFromLaser::ComputeLineEq(const cv::Point2f sp, const cv::Point2f ep, float& thetaRad, float& rho)
118 | {
119 |    // use "x*cos(thetaRad) + y*sin(thetaRad) = c" as line equation   
120 |    thetaRad = atan2f(sp.x - ep.x, sp.y - ep.y);
121 |    rho = sp.x * cos(thetaRad) + sp.y * sin(thetaRad);
122 |    return;
123 | }
124 | 
125 | // ----------------------------------------------------------
126 | // function to compute the (x, y) position of a given laser scan
127 | // ----------------------------------------------------------
128 | cv::Point2f CLineExtractionFromLaser::CvtLaserScanToXY(const float angleRad, const float rangeM, const float angularOffset, const float scale, const cv::Point2f offset)
129 | {
130 |    cv::Point2f xyPt;
131 |    // 1). add angularOffset to angle
132 |    float angle = angleRad + angularOffset;
133 |    // 2). multiple scale when compute (x, y)
134 |    xyPt.x = rangeM * cos(angle) * scale;
135 |    xyPt.y = rangeM * sin(angle) * scale;
136 |    // 3). add offset to computed (x, y)
137 |    xyPt.x += offset.x;
138 |    xyPt.y += offset.y;
139 | 
140 |    return xyPt;
141 | }
142 | 
143 | inline float CLineExtractionFromLaser::ComputeDistFromPtToLine(const float thetaRad, const float rho, cv::Point2f pt)
144 | {
145 |    // dist(ax + by + c = 0, (x0, y0)) = |ax0 + by0 + c| / sqrt(a * a + b * b)
146 |    return abs(pt.x * cos(thetaRad) + pt.y * sin(thetaRad) - rho);
147 | }
148 | 
149 | inline float CLineExtractionFromLaser::InnerProduct(const cv::Point2f ptL, const cv::Point2f ptR, const cv::Point2f ptO)
150 | {
151 |    //  ptL     PtR
152 |    //    \    /    : compute the inner product of ptO->ptL and ptO->ptR 
153 |    //      ptO 
154 |    return ((ptL.x - ptO.x) * (ptR.x - ptO.x) + (ptL.y - ptO.y) * (ptR.y - ptO.y));
155 | }
156 | 
157 | inline float CLineExtractionFromLaser::ComputeDistBetweenTwoPt(const cv::Point2f pt1, const cv::Point2f pt2)
158 | {
159 |    return sqrt(pow(pt1.x - pt2.x, 2) + pow(pt1.y - pt2.y, 2));
160 | }
161 | 


--------------------------------------------------------------------------------
/LineExtractionFromLaser.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | #include <vector>
 3 | #include <opencv2/imgproc/imgproc.hpp>
 4 | 
 5 | namespace LnExt
 6 | {
 7 | 
 8 | 
 9 | 
10 | ///////////////////////
11 | #define LASER_ANGLE 120 // 120 or 180
12 | 
13 | #ifndef M_PI
14 | #define M_PI 3.14159265358979323846F
15 | #endif
16 | 
17 | #ifndef DEG_TO_RAD
18 | #define DEG_TO_RAD(deg) ((deg) / 180.0F * M_PI)
19 | #endif
20 |    
21 | 
22 |    //////////////////////
23 |    struct ArimaLaserscan
24 |    {
25 |       std::vector<float> ranges; // meter
26 |       std::vector<float> angles; // deg
27 |    };
28 |    typedef std::vector<std::vector<int>> ClusterVector;
29 | 
30 | 
31 |    //////////////////////////////
32 |    class CLineExtractionFromLaser
33 |    {
34 |    private:
35 |       float m_minLineLengthM;
36 |       float m_inlierRatio;
37 |       int m_minPtInCluster;
38 |       int m_maxTrialCnt;
39 |       float m_maxDistToLine;
40 |    private:
41 |       void ComputeLineEq(const cv::Point2f sp, const cv::Point2f ep, float& thetaRad, float& rho);
42 |       cv::Point2f CvtLaserScanToXY(const float angleRad, const float rangeM, const float angularOffset = 0, const float scale = 1, const cv::Point2f offset = cv::Point2f(0, 0));
43 |       inline float ComputeDistFromPtToLine(const float thetaRad, const float rho, cv::Point2f pt);
44 |       inline float InnerProduct(const cv::Point2f ptL, const cv::Point2f ptR, const cv::Point2f ptO);
45 |       inline float ComputeDistBetweenTwoPt(const cv::Point2f pt1, const cv::Point2f pt2);
46 |    public:
47 |       CLineExtractionFromLaser();
48 |       ~CLineExtractionFromLaser();
49 |       bool LineExtract(const ArimaLaserscan laserScans, const ClusterVector clusters, int& sIdx, int& eIdx);
50 |    };
51 | }
52 | 
53 | 
54 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Dynamic Window Approach
2 | Reference paper: [The Dynamic Window Approach to Collision Avoidance](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf)
3 | 
4 | #[Flow chart](https://drive.google.com/open?id=0B5JJtltbMGfBS2drZkRqaS15QW8)
5 | 
6 | 


--------------------------------------------------------------------------------
/laser_filters.cpp:
--------------------------------------------------------------------------------
  1 | #include "laser_filters.h"
  2 | 
  3 | 
  4 | 
  5 | Laser_Filters::Laser_Filters() 
  6 | {
  7 |    ;
  8 | }
  9 |    
 10 | Laser_Filters::~Laser_Filters()
 11 | {
 12 |    ;
 13 | }
 14 | 
 15 | bool Laser_Filters::median_filter(std::vector<float>& laser_scans_angle_in, std::vector<float>& laser_scans_dist_in, std::vector<float>& laser_scans_angle_out, std::vector<float>& laser_scans_dist_out)
 16 | {
 17 |    laser_scans_dist_out = laser_scans_dist_in;
 18 |    laser_scans_angle_out = laser_scans_angle_in;
 19 |    
 20 |    //
 21 |    int median_filter_radius = 2;
 22 |    for(int i = 0; i < laser_scans_dist_in.size(); i++)
 23 |    {
 24 |       if((i < median_filter_radius) || (i >= laser_scans_dist_in.size()-median_filter_radius))
 25 |       {
 26 |          laser_scans_dist_out[i] = laser_scans_dist_in[i];
 27 |       }
 28 |       else
 29 |       {
 30 |          float median = find_median(i, laser_scans_dist_in, median_filter_radius);
 31 |          if(median == -1)
 32 |          {
 33 |             laser_scans_dist_out[i] = 0;
 34 |             return 0;   
 35 |          }
 36 |          else
 37 |          {
 38 |             laser_scans_dist_out[i] = median;
 39 |          }
 40 |       }
 41 |    }
 42 |    
 43 |    return 1;
 44 | }
 45 | 
 46 | float Laser_Filters::find_median(int index, std::vector<float>& laser_scans_dist_in, int radius)
 47 | {
 48 |    int i = 0;
 49 |    int j = 0;
 50 |    int l = 0;
 51 |    int m = radius*2;
 52 |   
 53 |    // check the boundary of laser_scans_dist_in
 54 |    if((index - radius < 0) || (index + radius >= laser_scans_dist_in.size()))
 55 |    {
 56 |       return -1;
 57 |    }
 58 |    
 59 |    // copy data from laser_scans_dist_in
 60 |    float* laser_scans_dist_local = new float[radius*2+1]; 
 61 |    for(i = -radius; i <= radius; i++)
 62 |    {
 63 |       laser_scans_dist_local[i+radius] = laser_scans_dist_in[index+i];
 64 |    }
 65 |    
 66 |    // find the median
 67 |    float x;
 68 |    while(l < m)
 69 |    {
 70 |       x = laser_scans_dist_local[radius];
 71 |       i = l;
 72 |       j = m;
 73 |       do
 74 |       {
 75 |          while(laser_scans_dist_local[i] < x) i++;
 76 |          while(x < laser_scans_dist_local[j]) j--;
 77 |          if(i<=j)
 78 |          {
 79 |             // swap
 80 |             float temp = laser_scans_dist_local[i];
 81 |             laser_scans_dist_local[i] = laser_scans_dist_local[j];
 82 |             laser_scans_dist_local[j] = temp;
 83 |             i++; 
 84 |             j--;
 85 |          }
 86 |       } while (i <= j);
 87 |       if(j < radius) l = i;
 88 |       if(radius < i) m = j;
 89 |    }  
 90 |    
 91 |    // debug - make sure the returned variable is median
 92 |    /*
 93 |    for(i = -radius; i <= radius; i++)
 94 |    {
 95 |       if(i > 0)
 96 |       {
 97 |          if(laser_scans_dist_local[radius] > laser_scans_dist_local[radius+i])
 98 |          {
 99 |             ROS_ERROR("*** [laser_filters] find incorrect median ***");
100 |          }
101 |       }
102 |       else if(i < 0)
103 |       {
104 |          if(laser_scans_dist_local[radius] < laser_scans_dist_local[radius+i])
105 |          {
106 |             ROS_ERROR("*** [laser_filters] find incorrect median ***");
107 |          }      
108 |       }
109 |    }
110 |    */ 
111 |   
112 |    // delete and return
113 |    float ret_val = laser_scans_dist_local[radius];
114 |    delete [] laser_scans_dist_local;
115 |    return ret_val;
116 | }
117 | 
118 | 
119 | 


--------------------------------------------------------------------------------
/laser_filters.h:
--------------------------------------------------------------------------------
 1 | // [20150825_Curtis] remove all member variables so that this class only provides functions for filtering laser scans
 2 | 
 3 | #include <vector>
 4 | 
 5 | class Laser_Filters
 6 | {
 7 | public:
 8 |    Laser_Filters(); 
 9 |    ~Laser_Filters();
10 |    bool median_filter(std::vector<float>& laser_scans_angle_in, std::vector<float>& laser_scans_dist_in, std::vector<float>& laser_scans_angle_out, std::vector<float>& laser_scans_dist_out);
11 |    
12 | private:
13 |    float find_median(int index, std::vector<float>& laser_scans_dist_in, int radius = 2); // find the median between laser_scans_dist_in[index-radius] and laser_scans_dist_in[index+radius]
14 | };
15 | 
16 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | #include "DWA.h"
 3 | #include "rs232.h"
 4 | #include "laser_filters.h"
 5 | #include <conio.h>
 6 | #include <thread>
 7 | #include <Mutex>
 8 | #pragma warning (disable : 4996)
 9 | 
10 | 
11 | void main(int argc, const char** argv){
12 | 	DWA *_dwa = new DWA();
13 | 	std::thread t(&DWA::twoThread,_dwa);
14 | 	//std::thread t = _dwa->exeThread();
15 | 	while (1){
16 | 		if (kbhit() != 0){
17 | 			int c = getch();
18 | 			switch (c){
19 | 			case 27://ESC
20 | 				_dwa->flag = true; break;
21 | 			case 100://d
22 | 				_dwa->head_direction = 0; break;
23 | 			case 101://e
24 | 				_dwa->head_direction = 70; break;
25 | 			case 119://w
26 | 				_dwa->head_direction = 90; break;
27 | 			case 113://q
28 | 				_dwa->head_direction = 110; break;
29 | 			case 97://a
30 | 				_dwa->head_direction = 180; break;
31 | 			case 115://s for stop
32 | 				_dwa->head_direction = 360; break;
33 | 			case 120://x for backup
34 | 				_dwa->head_direction = 300; break;
35 | 			}
36 | 		}
37 | 		if (_dwa->flag)
38 | 			break;
39 | 	}
40 | 	t.join();
41 | 	system("pause");
42 | 	return;
43 | }


--------------------------------------------------------------------------------
/rs232.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | ***************************************************************************
  3 | *
  4 | * Author: Teunis van Beelen
  5 | *
  6 | * Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015 Teunis van Beelen
  7 | *
  8 | * Email: teuniz@gmail.com
  9 | *
 10 | ***************************************************************************
 11 | *
 12 | * This program is free software; you can redistribute it and/or modify
 13 | * it under the terms of the GNU General Public License as published by
 14 | * the Free Software Foundation version 2 of the License.
 15 | *
 16 | * This program is distributed in the hope that it will be useful,
 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 19 | * GNU General Public License for more details.
 20 | *
 21 | * You should have received a copy of the GNU General Public License along
 22 | * with this program; if not, write to the Free Software Foundation, Inc.,
 23 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 24 | *
 25 | ***************************************************************************
 26 | *
 27 | * This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
 28 | *
 29 | ***************************************************************************
 30 | */
 31 | 
 32 | /* Last revision: October 10, 2015 */
 33 | 
 34 | /* For more info and how to use this library, visit: http://www.teuniz.net/RS-232/ */
 35 | 
 36 | 
 37 | #include "rs232.h"
 38 | #include <stdio.h>
 39 | 
 40 | 
 41 | 
 42 | #if defined(__linux__) || defined(__FreeBSD__)   /* Linux & FreeBSD */
 43 | 
 44 | 
 45 | int Cport[38],
 46 | error;
 47 | 
 48 | struct termios new_port_settings,
 49 | 	old_port_settings[38];
 50 | 
 51 | char comports[38][16]={"/dev/ttyS0","/dev/ttyS1","/dev/ttyS2","/dev/ttyS3","/dev/ttyS4","/dev/ttyS5",
 52 | "/dev/ttyS6","/dev/ttyS7","/dev/ttyS8","/dev/ttyS9","/dev/ttyS10","/dev/ttyS11",
 53 | "/dev/ttyS12","/dev/ttyS13","/dev/ttyS14","/dev/ttyS15","/dev/ttyUSB0",
 54 | "/dev/ttyUSB1","/dev/ttyUSB2","/dev/ttyUSB3","/dev/ttyUSB4","/dev/ttyUSB5",
 55 | "/dev/ttyAMA0","/dev/ttyAMA1","/dev/ttyACM0","/dev/ttyACM1",
 56 | "/dev/rfcomm0","/dev/rfcomm1","/dev/ircomm0","/dev/ircomm1",
 57 | "/dev/cuau0","/dev/cuau1","/dev/cuau2","/dev/cuau3",
 58 | "/dev/cuaU0","/dev/cuaU1","/dev/cuaU2","/dev/cuaU3"};
 59 | 
 60 | int RS232_OpenComport(int comport_number, int baudrate, const char *mode)
 61 | {
 62 | 	int baudr,
 63 | 		status;
 64 | 
 65 | 	if((comport_number>37)||(comport_number<0))
 66 | 	{
 67 | 		printf("illegal comport number\n");
 68 | 		return(1);
 69 | 	}
 70 | 
 71 | 	switch(baudrate)
 72 | 	{
 73 | 	case      50 : baudr = B50;
 74 | 		break;
 75 | 	case      75 : baudr = B75;
 76 | 		break;
 77 | 	case     110 : baudr = B110;
 78 | 		break;
 79 | 	case     134 : baudr = B134;
 80 | 		break;
 81 | 	case     150 : baudr = B150;
 82 | 		break;
 83 | 	case     200 : baudr = B200;
 84 | 		break;
 85 | 	case     300 : baudr = B300;
 86 | 		break;
 87 | 	case     600 : baudr = B600;
 88 | 		break;
 89 | 	case    1200 : baudr = B1200;
 90 | 		break;
 91 | 	case    1800 : baudr = B1800;
 92 | 		break;
 93 | 	case    2400 : baudr = B2400;
 94 | 		break;
 95 | 	case    4800 : baudr = B4800;
 96 | 		break;
 97 | 	case    9600 : baudr = B9600;
 98 | 		break;
 99 | 	case   19200 : baudr = B19200;
100 | 		break;
101 | 	case   38400 : baudr = B38400;
102 | 		break;
103 | 	case   57600 : baudr = B57600;
104 | 		break;
105 | 	case  115200 : baudr = B115200;
106 | 		break;
107 | 	case  230400 : baudr = B230400;
108 | 		break;
109 | 	case  460800 : baudr = B460800;
110 | 		break;
111 | 	case  500000 : baudr = B500000;
112 | 		break;
113 | 	case  576000 : baudr = B576000;
114 | 		break;
115 | 	case  921600 : baudr = B921600;
116 | 		break;
117 | 	case 1000000 : baudr = B1000000;
118 | 		break;
119 | 	case 1152000 : baudr = B1152000;
120 | 		break;
121 | 	case 1500000 : baudr = B1500000;
122 | 		break;
123 | 	case 2000000 : baudr = B2000000;
124 | 		break;
125 | 	case 2500000 : baudr = B2500000;
126 | 		break;
127 | 	case 3000000 : baudr = B3000000;
128 | 		break;
129 | 	case 3500000 : baudr = B3500000;
130 | 		break;
131 | 	case 4000000 : baudr = B4000000;
132 | 		break;
133 | 	default      : printf("invalid baudrate\n");
134 | 		return(1);
135 | 		break;
136 | 	}
137 | 
138 | 	int cbits=CS8,
139 | 		cpar=0,
140 | 		ipar=IGNPAR,
141 | 		bstop=0;
142 | 
143 | 	if(strlen(mode) != 3)
144 | 	{
145 | 		printf("invalid mode \"%s\"\n", mode);
146 | 		return(1);
147 | 	}
148 | 
149 | 	switch(mode[0])
150 | 	{
151 | 	case '8': cbits = CS8;
152 | 		break;
153 | 	case '7': cbits = CS7;
154 | 		break;
155 | 	case '6': cbits = CS6;
156 | 		break;
157 | 	case '5': cbits = CS5;
158 | 		break;
159 | 	default : printf("invalid number of data-bits '%c'\n", mode[0]);
160 | 		return(1);
161 | 		break;
162 | 	}
163 | 
164 | 	switch(mode[1])
165 | 	{
166 | 	case 'N':
167 | 	case 'n': cpar = 0;
168 | 		ipar = IGNPAR;
169 | 		break;
170 | 	case 'E':
171 | 	case 'e': cpar = PARENB;
172 | 		ipar = INPCK;
173 | 		break;
174 | 	case 'O':
175 | 	case 'o': cpar = (PARENB | PARODD);
176 | 		ipar = INPCK;
177 | 		break;
178 | 	default : printf("invalid parity '%c'\n", mode[1]);
179 | 		return(1);
180 | 		break;
181 | 	}
182 | 
183 | 	switch(mode[2])
184 | 	{
185 | 	case '1': bstop = 0;
186 | 		break;
187 | 	case '2': bstop = CSTOPB;
188 | 		break;
189 | 	default : printf("invalid number of stop bits '%c'\n", mode[2]);
190 | 		return(1);
191 | 		break;
192 | 	}
193 | 
194 | 	/*
195 | 	http://pubs.opengroup.org/onlinepubs/7908799/xsh/termios.h.html
196 | 
197 | 	http://man7.org/linux/man-pages/man3/termios.3.html
198 | 	*/
199 | 
200 | 	Cport[comport_number] = open(comports[comport_number], O_RDWR | O_NOCTTY | O_NDELAY);
201 | 	if(Cport[comport_number]==-1)
202 | 	{
203 | 		perror("unable to open comport ");
204 | 		return(1);
205 | 	}
206 | 
207 | 	/* lock access so that another process can't also use the port */
208 | 	if(flock(Cport[comport_number], LOCK_EX | LOCK_NB) != 0)
209 | 	{
210 | 		close(Cport[comport_number]);
211 | 		perror("Another process has locked the comport.");
212 | 		return(1);
213 | 	}
214 | 
215 | 	error = tcgetattr(Cport[comport_number], old_port_settings + comport_number);
216 | 	if(error==-1)
217 | 	{
218 | 		close(Cport[comport_number]);
219 | 		perror("unable to read portsettings ");
220 | 		return(1);
221 | 	}
222 | 	memset(&new_port_settings, 0, sizeof(new_port_settings));  /* clear the new struct */
223 | 
224 | 	new_port_settings.c_cflag = cbits | cpar | bstop | CLOCAL | CREAD;
225 | 	new_port_settings.c_iflag = ipar;
226 | 	new_port_settings.c_oflag = 0;
227 | 	new_port_settings.c_lflag = 0;
228 | 	new_port_settings.c_cc[VMIN] = 0;      /* block untill n bytes are received */
229 | 	new_port_settings.c_cc[VTIME] = 0;     /* block untill a timer expires (n * 100 mSec.) */
230 | 
231 | 	cfsetispeed(&new_port_settings, baudr);
232 | 	cfsetospeed(&new_port_settings, baudr);
233 | 
234 | 	error = tcsetattr(Cport[comport_number], TCSANOW, &new_port_settings);
235 | 	if(error==-1)
236 | 	{
237 | 		close(Cport[comport_number]);
238 | 		perror("unable to adjust portsettings ");
239 | 		return(1);
240 | 	}
241 | 
242 | 	if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
243 | 	{
244 | 		perror("unable to get portstatus");
245 | 		return(1);
246 | 	}
247 | 
248 | 	status |= TIOCM_DTR;    /* turn on DTR */
249 | 	status |= TIOCM_RTS;    /* turn on RTS */
250 | 
251 | 	if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
252 | 	{
253 | 		perror("unable to set portstatus");
254 | 		return(1);
255 | 	}
256 | 
257 | 	return(0);
258 | }
259 | 
260 | 
261 | int RS232_PollComport(int comport_number, unsigned char *buf, int size)
262 | {
263 | 	int n;
264 | 
265 | 	n = read(Cport[comport_number], buf, size);
266 | 
267 | 	return(n);
268 | }
269 | 
270 | 
271 | int RS232_SendByte(int comport_number, unsigned char byte)
272 | {
273 | 	int n;
274 | 
275 | 	n = write(Cport[comport_number], &byte, 1);
276 | 	if(n<0)  return(1);
277 | 
278 | 	return(0);
279 | }
280 | 
281 | 
282 | int RS232_SendBuf(int comport_number, unsigned char *buf, int size)
283 | {
284 | 	return(write(Cport[comport_number], buf, size));
285 | }
286 | 
287 | 
288 | void RS232_CloseComport(int comport_number)
289 | {
290 | 	int status;
291 | 
292 | 	if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
293 | 	{
294 | 		perror("unable to get portstatus");
295 | 	}
296 | 
297 | 	status &= ~TIOCM_DTR;    /* turn off DTR */
298 | 	status &= ~TIOCM_RTS;    /* turn off RTS */
299 | 
300 | 	if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
301 | 	{
302 | 		perror("unable to set portstatus");
303 | 	}
304 | 
305 | 	tcsetattr(Cport[comport_number], TCSANOW, old_port_settings + comport_number);
306 | 	close(Cport[comport_number]);
307 | 
308 | 	flock(Cport[comport_number], LOCK_UN); /* free the port so that others can use it. */
309 | }
310 | 
311 | /*
312 | Constant  Description
313 | TIOCM_LE        DSR (data set ready/line enable)
314 | TIOCM_DTR       DTR (data terminal ready)
315 | TIOCM_RTS       RTS (request to send)
316 | TIOCM_ST        Secondary TXD (transmit)
317 | TIOCM_SR        Secondary RXD (receive)
318 | TIOCM_CTS       CTS (clear to send)
319 | TIOCM_CAR       DCD (data carrier detect)
320 | TIOCM_CD        see TIOCM_CAR
321 | TIOCM_RNG       RNG (ring)
322 | TIOCM_RI        see TIOCM_RNG
323 | TIOCM_DSR       DSR (data set ready)
324 | 
325 | http://man7.org/linux/man-pages/man4/tty_ioctl.4.html
326 | */
327 | 
328 | int RS232_IsDCDEnabled(int comport_number)
329 | {
330 | 	int status;
331 | 
332 | 	ioctl(Cport[comport_number], TIOCMGET, &status);
333 | 
334 | 	if(status&TIOCM_CAR) return(1);
335 | 	else return(0);
336 | }
337 | 
338 | int RS232_IsCTSEnabled(int comport_number)
339 | {
340 | 	int status;
341 | 
342 | 	ioctl(Cport[comport_number], TIOCMGET, &status);
343 | 
344 | 	if(status&TIOCM_CTS) return(1);
345 | 	else return(0);
346 | }
347 | 
348 | int RS232_IsDSREnabled(int comport_number)
349 | {
350 | 	int status;
351 | 
352 | 	ioctl(Cport[comport_number], TIOCMGET, &status);
353 | 
354 | 	if(status&TIOCM_DSR) return(1);
355 | 	else return(0);
356 | }
357 | 
358 | void RS232_enableDTR(int comport_number)
359 | {
360 | 	int status;
361 | 
362 | 	if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
363 | 	{
364 | 		perror("unable to get portstatus");
365 | 	}
366 | 
367 | 	status |= TIOCM_DTR;    /* turn on DTR */
368 | 
369 | 	if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
370 | 	{
371 | 		perror("unable to set portstatus");
372 | 	}
373 | }
374 | 
375 | void RS232_disableDTR(int comport_number)
376 | {
377 | 	int status;
378 | 
379 | 	if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
380 | 	{
381 | 		perror("unable to get portstatus");
382 | 	}
383 | 
384 | 	status &= ~TIOCM_DTR;    /* turn off DTR */
385 | 
386 | 	if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
387 | 	{
388 | 		perror("unable to set portstatus");
389 | 	}
390 | }
391 | 
392 | void RS232_enableRTS(int comport_number)
393 | {
394 | 	int status;
395 | 
396 | 	if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
397 | 	{
398 | 		perror("unable to get portstatus");
399 | 	}
400 | 
401 | 	status |= TIOCM_RTS;    /* turn on RTS */
402 | 
403 | 	if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
404 | 	{
405 | 		perror("unable to set portstatus");
406 | 	}
407 | }
408 | 
409 | void RS232_disableRTS(int comport_number)
410 | {
411 | 	int status;
412 | 
413 | 	if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
414 | 	{
415 | 		perror("unable to get portstatus");
416 | 	}
417 | 
418 | 	status &= ~TIOCM_RTS;    /* turn off RTS */
419 | 
420 | 	if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
421 | 	{
422 | 		perror("unable to set portstatus");
423 | 	}
424 | }
425 | 
426 | 
427 | #else         /* windows */
428 | 
429 | 
430 | HANDLE Cport[16];
431 | 
432 | 
433 | char comports[16][10] = { "\\\\.\\COM1", "\\\\.\\COM2", "\\\\.\\COM3", "\\\\.\\COM4",
434 | "\\\\.\\COM5", "\\\\.\\COM6", "\\\\.\\COM7", "\\\\.\\COM8",
435 | "\\\\.\\COM9", "\\\\.\\COM10", "\\\\.\\COM11", "\\\\.\\COM12",
436 | "\\\\.\\COM13", "\\\\.\\COM14", "\\\\.\\COM15", "\\\\.\\COM16" };
437 | 
438 | char mode_str[128];
439 | 
440 | 
441 | int RS232_OpenComport(int comport_number, int baudrate, const char *mode)
442 | {
443 | 	if ((comport_number > 15) || (comport_number < 0))
444 | 	{
445 | 		printf("illegal comport number\n");
446 | 		return(1);
447 | 	}
448 | 
449 | 	switch (baudrate)
450 | 	{
451 | 		//case     110 : strcpy_s(mode_str, sizeof(mode_str),"baud=110");
452 | 		//               break;
453 | 		//case     300 : strcpy_s(mode_str, "baud=300");
454 | 		//               break;
455 | 		//case     600 : strcpy_s(mode_str, "baud=600");
456 | 		//               break;
457 | 		//case    1200 : strcpy_s(mode_str, "baud=1200");
458 | 		//               break;
459 | 		//case    2400 : strcpy(mode_str, "baud=2400");
460 | 		//               break;
461 | 		//case    4800 : strcpy(mode_str, "baud=4800");
462 | 		//               break;
463 | 		case    9600: strcpy_s(mode_str, sizeof(mode_str), "baud=9600");
464 | 						break;
465 | 		case   19200: strcpy_s(mode_str, sizeof(mode_str), "baud=19200");
466 | 		               break;
467 | 		//case   38400 : strcpy(mode_str, "baud=38400");
468 | 		//               break;
469 | 		//case   57600 : strcpy(mode_str, "baud=57600");
470 | 		//               break;
471 | 	case  115200: strcpy_s(mode_str, sizeof(mode_str), "baud=115200");
472 | 		break;
473 | 	case  128000: strcpy_s(mode_str, sizeof(mode_str), "baud=128000");
474 | 		break;
475 | 	case  256000: strcpy_s(mode_str, sizeof(mode_str), "baud=256000");
476 | 		break;
477 | 	case  500000: strcpy_s(mode_str, sizeof(mode_str), "baud=500000");
478 | 		break;
479 | 	case 1000000: strcpy_s(mode_str, sizeof(mode_str), "baud=1000000");
480 | 		break;
481 | 	case 230400: strcpy_s(mode_str, sizeof(mode_str), "baud=230400");
482 | 		break;
483 | 	default: printf("invalid baudrate\n");
484 | 		return(1);
485 | 		break;
486 | 	}
487 | 
488 | 	if (strlen(mode) != 3)
489 | 	{
490 | 		printf("invalid mode \"%s\"\n", mode);
491 | 		return(1);
492 | 	}
493 | 
494 | 	switch (mode[0])
495 | 	{
496 | 	case '8': strcat_s(mode_str, sizeof(mode_str), " data=8");
497 | 		break;
498 | 	case '7': strcat_s(mode_str, sizeof(mode_str), " data=7");
499 | 		break;
500 | 	case '6': strcat_s(mode_str, sizeof(mode_str), " data=6");
501 | 		break;
502 | 	case '5': strcat_s(mode_str, sizeof(mode_str), " data=5");
503 | 		break;
504 | 	default: printf("invalid number of data-bits '%c'\n", mode[0]);
505 | 		return(1);
506 | 		break;
507 | 	}
508 | 
509 | 	switch (mode[1])
510 | 	{
511 | 	case 'N':
512 | 	case 'n': strcat_s(mode_str, sizeof(mode_str), " parity=n");
513 | 		break;
514 | 	case 'E':
515 | 	case 'e': strcat_s(mode_str, sizeof(mode_str), " parity=e");
516 | 		break;
517 | 	case 'O':
518 | 	case 'o': strcat_s(mode_str, sizeof(mode_str), " parity=o");
519 | 		break;
520 | 	default: printf("invalid parity '%c'\n", mode[1]);
521 | 		return(1);
522 | 		break;
523 | 	}
524 | 
525 | 	switch (mode[2])
526 | 	{
527 | 	case '1': strcat_s(mode_str, sizeof(mode_str), " stop=1");
528 | 		break;
529 | 	case '2': strcat_s(mode_str, sizeof(mode_str), " stop=2");
530 | 		break;
531 | 	default: printf("invalid number of stop bits '%c'\n", mode[2]);
532 | 		return(1);
533 | 		break;
534 | 	}
535 | 
536 | 	strcat_s(mode_str, sizeof(mode_str), " dtr=on rts=on");
537 | 
538 | 	/*
539 | 	http://msdn.microsoft.com/en-us/library/windows/desktop/aa363145%28v=vs.85%29.aspx
540 | 
541 | 	http://technet.microsoft.com/en-us/library/cc732236.aspx
542 | 	*/
543 | 
544 | 	Cport[comport_number] = CreateFileA(comports[comport_number],
545 | 		GENERIC_READ | GENERIC_WRITE,
546 | 		0,                          /* no share  */
547 | 		NULL,                       /* no security */
548 | 		OPEN_EXISTING,
549 | 		0,                          /* no threads */
550 | 		NULL);                      /* no templates */
551 | 
552 | 	if (Cport[comport_number] == INVALID_HANDLE_VALUE)
553 | 	{
554 | 		printf("unable to open comport\n");
555 | 		return(1);
556 | 	}
557 | 
558 | 	DCB port_settings;
559 | 	memset(&port_settings, 0, sizeof(port_settings));  /* clear the new struct  */
560 | 	port_settings.DCBlength = sizeof(port_settings);
561 | 
562 | 	if (!BuildCommDCBA(mode_str, &port_settings))
563 | 	{
564 | 		printf("unable to set comport dcb settings\n");
565 | 		CloseHandle(Cport[comport_number]);
566 | 		return(1);
567 | 	}
568 | 
569 | 	if (!SetCommState(Cport[comport_number], &port_settings))
570 | 	{
571 | 		printf("unable to set comport cfg settings\n");
572 | 		CloseHandle(Cport[comport_number]);
573 | 		return(1);
574 | 	}
575 | 
576 | 	COMMTIMEOUTS Cptimeouts;
577 | 
578 | 	Cptimeouts.ReadIntervalTimeout = MAXDWORD;
579 | 	Cptimeouts.ReadTotalTimeoutMultiplier = 0;
580 | 	Cptimeouts.ReadTotalTimeoutConstant = 0;
581 | 	Cptimeouts.WriteTotalTimeoutMultiplier = 0;
582 | 	Cptimeouts.WriteTotalTimeoutConstant = 0;
583 | 
584 | 	if (!SetCommTimeouts(Cport[comport_number], &Cptimeouts))
585 | 	{
586 | 		printf("unable to set comport time-out settings\n");
587 | 		CloseHandle(Cport[comport_number]);
588 | 		return(1);
589 | 	}
590 | 
591 | 	return(0);
592 | }
593 | 
594 | 
595 | int RS232_PollComport(int comport_number, unsigned char *buf, int size)
596 | {
597 | 	int n;
598 | 
599 | 	/* added the void pointer cast, otherwise gcc will complain about */
600 | 	/* "warning: dereferencing type-punned pointer will break strict aliasing rules" */
601 | 
602 | 	ReadFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL);
603 | 
604 | 	return(n);
605 | }
606 | 
607 | 
608 | int RS232_SendByte(int comport_number, unsigned char byte)
609 | {
610 | 	int n;
611 | 
612 | 	WriteFile(Cport[comport_number], &byte, 1, (LPDWORD)((void *)&n), NULL);
613 | 
614 | 	if (n < 0)  return(1);
615 | 
616 | 	return(0);
617 | }
618 | 
619 | 
620 | int RS232_SendBuf(int comport_number, unsigned char *buf, int size)
621 | {
622 | 	int n;
623 | 
624 | 	if (WriteFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL))
625 | 	{
626 | 		return(n);
627 | 	}
628 | 
629 | 	return(-1);
630 | }
631 | 
632 | 
633 | void RS232_CloseComport(int comport_number)
634 | {
635 | 	CloseHandle(Cport[comport_number]);
636 | }
637 | 
638 | /*
639 | http://msdn.microsoft.com/en-us/library/windows/desktop/aa363258%28v=vs.85%29.aspx
640 | */
641 | 
642 | int RS232_IsDCDEnabled(int comport_number)
643 | {
644 | 	int status;
645 | 
646 | 	GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status));
647 | 
648 | 	if (status&MS_RLSD_ON) return(1);
649 | 	else return(0);
650 | }
651 | 
652 | 
653 | int RS232_IsCTSEnabled(int comport_number)
654 | {
655 | 	int status;
656 | 
657 | 	GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status));
658 | 
659 | 	if (status&MS_CTS_ON) return(1);
660 | 	else return(0);
661 | }
662 | 
663 | 
664 | int RS232_IsDSREnabled(int comport_number)
665 | {
666 | 	int status;
667 | 
668 | 	GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status));
669 | 
670 | 	if (status&MS_DSR_ON) return(1);
671 | 	else return(0);
672 | }
673 | 
674 | 
675 | void RS232_enableDTR(int comport_number)
676 | {
677 | 	EscapeCommFunction(Cport[comport_number], SETDTR);
678 | }
679 | 
680 | 
681 | void RS232_disableDTR(int comport_number)
682 | {
683 | 	EscapeCommFunction(Cport[comport_number], CLRDTR);
684 | }
685 | 
686 | 
687 | void RS232_enableRTS(int comport_number)
688 | {
689 | 	EscapeCommFunction(Cport[comport_number], SETRTS);
690 | }
691 | 
692 | 
693 | void RS232_disableRTS(int comport_number)
694 | {
695 | 	EscapeCommFunction(Cport[comport_number], CLRRTS);
696 | }
697 | 
698 | 
699 | #endif
700 | 
701 | 
702 | void RS232_cputs(int comport_number, unsigned char *text)  /* sends a string to serial port */
703 | {
704 | 	while (*text != 0)   RS232_SendByte(comport_number, *(text++));
705 | }
706 | 
707 | 
708 | 


--------------------------------------------------------------------------------
/rs232.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | ***************************************************************************
 3 | *
 4 | * Author: Teunis van Beelen
 5 | *
 6 | * Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015 Teunis van Beelen
 7 | *
 8 | * Email: teuniz@gmail.com
 9 | *
10 | ***************************************************************************
11 | *
12 | * This program is free software; you can redistribute it and/or modify
13 | * it under the terms of the GNU General Public License as published by
14 | * the Free Software Foundation version 2 of the License.
15 | *
16 | * This program is distributed in the hope that it will be useful,
17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19 | * GNU General Public License for more details.
20 | *
21 | * You should have received a copy of the GNU General Public License along
22 | * with this program; if not, write to the Free Software Foundation, Inc.,
23 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
24 | *
25 | ***************************************************************************
26 | *
27 | * This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
28 | *
29 | ***************************************************************************
30 | */
31 | 
32 | /* Last revision: October 10, 2015 */
33 | 
34 | /* For more info and how to use this libray, visit: http://www.teuniz.net/RS-232/ */
35 | 
36 | 
37 | #ifndef rs232_INCLUDED
38 | #define rs232_INCLUDED
39 | 
40 | #ifdef __cplusplus
41 | extern "C" {
42 | #endif
43 | 
44 | #include <stdio.h>
45 | #include <string.h>
46 | 
47 | 
48 | 
49 | #if defined(__linux__) || defined(__FreeBSD__)
50 | 
51 | #include <termios.h>
52 | #include <sys/ioctl.h>
53 | #include <unistd.h>
54 | #include <fcntl.h>
55 | #include <sys/types.h>
56 | #include <sys/stat.h>
57 | #include <limits.h>
58 | #include <sys/file.h>
59 | 
60 | #else
61 | 
62 | #include <windows.h>
63 | 
64 | #endif
65 | 
66 | 	int RS232_OpenComport(int, int, const char *);
67 | 	int RS232_PollComport(int, unsigned char *, int);
68 | 	int RS232_SendByte(int, unsigned char);
69 | 	int RS232_SendBuf(int, unsigned char *, int);
70 | 	void RS232_CloseComport(int);
71 | 	void RS232_cputs(int, unsigned char);
72 | 	int RS232_IsDCDEnabled(int);
73 | 	int RS232_IsCTSEnabled(int);
74 | 	int RS232_IsDSREnabled(int);
75 | 	void RS232_enableDTR(int);
76 | 	void RS232_disableDTR(int);
77 | 	void RS232_enableRTS(int);
78 | 	void RS232_disableRTS(int);
79 | 
80 | 
81 | #ifdef __cplusplus
82 | } /* extern "C" */
83 | #endif
84 | 
85 | #endif
86 | 
87 | 
88 | 


--------------------------------------------------------------------------------