Subversion Repositories svnkaklik

Rev

Rev 367 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 367 Rev 369
Line 30... Line 30...
30 
#include <sys/ioctl.h>
 30 
#include <sys/ioctl.h>
31 
#include <sys/types.h>
 31 
#include <sys/types.h>
32 
#include <sys/stat.h>
 32 
#include <sys/stat.h>
33 
#include <fcntl.h>
 33 
#include <fcntl.h>
34 
#include "geocalc.h"
 34 
#include "geocalc.h"
- 
 
 35 
#include "track.h"
35 
 
 36 
 
36 
using namespace std;
 37 
using namespace std;
37 
 
 38 
 
38 
#define CMPS03_SOFTWARE_REVISION 0x0
 39 
#define CMPS03_SOFTWARE_REVISION 0x0
39 
#define SRF02_SOFTWARE_REVISION 0x0
 40 
#define SRF02_SOFTWARE_REVISION 0x0
Line 85... Line 86...
85 
		cerr << "Could not open /dev/i2c-0." << endl;
 86 
		cerr << "Could not open /dev/i2c-0." << endl;
86 
		return -1;
 87 
		return -1;
87 
	}
 88 
	}
88 
 
 89 
 
89 
    pthread_create(&thread_1, NULL, print_tele, NULL);
 90 
    pthread_create(&thread_1, NULL, print_tele, NULL);
- 
 
 91 
    pthread_create(&thread_3, NULL, gps, NULL);
90 
//    pthread_create(&thread_2, NULL, sensors, NULL);
 92 
//    pthread_create(&thread_2, NULL, sensors, NULL);
91 
 
 93 
 
92 
    char Buf[64];
 94 
    char Buf[64];
93 
    command=0;
 95 
    command=0;
94 
 
 96 
 
95 
    while(true)
 97 
    while(true)
96 
    {
 98 
    {
97 
        switch (command)
 99 
        switch (command)
98 
        {
 100 
        {
99 
            case 'f':   // forward
 101 
            case 'f':   // forward
100 
                go(M1, 20);
 102 
                go(M1, 70);
101 
                go(M2, 20);
 103 
                go(M2, 70);
102 
                command=0;
 104 
                command=0;
103 
                break;
 105 
                break;
104 
            case 'b':   // backward
 106 
            case 'b':   // backward
105 
                go(M1, -20);
 107 
                go(M1, -70);
106 
                go(M2, -20);
 108 
                go(M2, -70);
107 
                command=0;
 109 
                command=0;
108 
                break;
 110 
                break;
109 
            case 's':   // stop
 111 
            case 'v':   // stop
110 
                go(M1, 0);
 112 
                go(M1, 0);
111 
                go(M2, 0);
 113 
                go(M2, 0);
112 
                command=0;
 114 
                command=0;
113 
                break;
 115 
                break;
- 
 
 116 
            case 's':   // stop
- 
 
 117 
                go(M1, 1);
- 
 
 118 
                go(M2, 1);
- 
 
 119 
                command=0;
- 
 
 120 
                break;
114 
            case 'g':
 121 
            case 'g':
115 
usleep(180000);
 122 
usleep(180000);
116 
                I2C_addr(US_Addr);
 123 
                I2C_addr(US_Addr);
117 
                Buf[0]=0x0;
 124 
                Buf[0]=0x0;
118 
                Buf[1]=0x51;
 125 
                Buf[1]=0x51;
Line 149... Line 156...
149 
    };
 156 
    };
150 
 
 157 
 
151 
	close(file);
 158 
	close(file);
152 
    pthread_join(thread_1, NULL);
 159 
    pthread_join(thread_1, NULL);
153 
    pthread_join(thread_2, NULL);
 160 
    pthread_join(thread_2, NULL);
- 
 
 161 
    pthread_join(thread_3, NULL);
154 
 
 162 
 
155 
	return 0;
 163 
	return 0;
156 
}
 164 
}
157 
 
 165 
 
158 
 
 166 
 
Line 170... Line 178...
170 
        pRouraO = fopen("/home/ble/pipe","w");
 178 
        pRouraO = fopen("/home/ble/pipe","w");
171 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
 179 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
172 
 
 180 
 
173 
    pthread_mutex_lock(&mutex);
 181 
    pthread_mutex_lock(&mutex);
174 
   	fprintf(pRouraO,"%f N %f E\n", n, e);
 182 
   	fprintf(pRouraO,"%f N %f E\n", n, e);
175 
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, 49.266667, 14.716667));
 183 
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
176 
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, 49.266667, 14.716667));
 184 
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));
177 
    pthread_mutex_unlock(&mutex);
 185 
    pthread_mutex_unlock(&mutex);
178 
 
 186 
 
179 
        fclose(pRouraO);
 187 
        fclose(pRouraO);
180 
    }
 188 
    }
181 
}
 189 
}
182 
 
 190 
 
183 
void *gps(void *unused)
 191 
void *gps(void *unused)
184 
{
 192 
{
185 
    FILE *pRS232;
 193 
    FILE *pRS232;
186 
    double pomN, pomE;
 194 
    double N, E, pomN, pomE, nn, ee;
187 
 
 195 
 
188 
    pRS232 = fopen("/dev/ttyS1","r");
 196 
    pRS232 = fopen("/dev/ttyS1","r");
189 
 
 197 
 
190 
    while(true)
 198 
    while(true)
191 
    {
 199 
    {
192 
        fscanf(pRS232,"$GPGGA,*,%lf,N,%lf,E", &pomN, &pomE);
 200 
        fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
- 
 
 201 
        nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
- 
 
 202 
        pomN=(N-nn*100)/60+nn;
- 
 
 203 
        ee=ldiv((long)E,100).quot;
193 
        pthread_mutex_lock(&mutex);
 204 
        pomE=(E-ee*100)/60+ee;
- 
 
 205 
        pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
194 
        n=pomN; e=pomE;
 206 
        n=pomN; e=pomE;
195 
        pthread_mutex_unlock(&mutex);
 207 
        pthread_mutex_unlock(&mutex);
196 
        usleep(500000);
 208 
        usleep(500000);                     // NMEA nechodi castejc nez 1x za 1s
197 
    }
 209 
    }
198 
}
 210 
}
199 
 
 211 
 
200 
/*
 212 
/*
201 
void *sensors(void *unused)
 213 
void *sensors(void *unused)