Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
navdata.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Dino Hensen, Vincent van Hoek
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 #include <stdio.h>
31 #include <stdlib.h>
32 #include <fcntl.h> // for O_RDWR, O_NOCTTY, O_NONBLOCK
33 #include <termios.h> // for baud rates and options
34 #include <unistd.h>
35 #include <string.h>
36 #include <math.h>
37 #include "navdata.h"
38 
39 int nav_fd;
40 
42 {
43  port = malloc(sizeof(navdata_port));
44 
45  nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
46  if (nav_fd == -1)
47  {
48  perror("navdata_init: Unable to open /dev/ttyO1 - ");
49  return 1;
50  } else {
51  port->isOpen = 1;
52  }
53 
54  fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
55  //set port options
56  struct termios options;
57  //Get the current options for the port
58  tcgetattr(nav_fd, &options);
59  //Set the baud rates to 460800
60  cfsetispeed(&options, B460800);
61  cfsetospeed(&options, B460800);
62 
63  options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
64  options.c_iflag = 0; //clear input options
65  options.c_lflag = 0; //clear local options
66  options.c_oflag &= ~OPOST; //clear output options (raw output)
67 
68  //Set the new options for the port
69  tcsetattr(nav_fd, TCSANOW, &options);
70 
71  // stop acquisition
72  uint8_t cmd=0x02;
73  write(nav_fd, &cmd, 1);
74 
75  // start acquisition
76  cmd=0x01;
77  write(nav_fd, &cmd, 1);
78 
79  navdata = malloc(sizeof(measures_t));
82 
83  port->bytesRead = 0;
84  port->totalBytesRead = 0;
85  port->packetsRead = 0;
86  port->isInitialized = 1;
87 
89 
90  return 0;
91 }
92 
94 {
95  port->isOpen = 0;
96  close(nav_fd);
97 }
98 
100 {
101  int newbytes = 0;
102 
103  if (port->isInitialized != 1)
104  navdata_init();
105 
106  if (port->isOpen != 1)
107  return;
108 
110 
111  // because non-blocking read returns -1 when no bytes available
112  if (newbytes > 0)
113  {
114  port->bytesRead += newbytes;
115  port->totalBytesRead += newbytes;
116  }
117 
118 }
119 
121 {
122  navdata_read();
123 
124  // while there is something interesting to do...
125  while (port->bytesRead >= 60)
126  {
127  if (port->buffer[0] == NAVDATA_START_BYTE)
128  {
129  // if checksum is OK
130  if ( 1 ) // we dont know how to calculate the checksum
131 // if ( navdata_checksum() == 0 )
132  {
136  port->packetsRead++;
137 // printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
139  }
140  navdata_CropBuffer(60);
141  }
142  else
143  {
144  // find start byte, copy all data from startbyte to buffer origin, update bytesread
145  uint8_t * pint;
146  pint = memchr(port->buffer, NAVDATA_START_BYTE, port->bytesRead);
147 
148  if (pint != NULL) {
149  port->bytesRead -= pint - port->buffer;
150  navdata_CropBuffer(pint - port->buffer);
151  } else {
152  // if the start byte was not found, it means there is junk in the buffer
153  port->bytesRead = 0;
154  }
155  }
156  }
157 }
158 
159 void navdata_CropBuffer(int cropsize)
160 {
161  if (port->bytesRead - cropsize < 0) {
162  // TODO think about why the amount of bytes read minus the cropsize gets below zero
163  printf("BytesRead - Cropsize may not be below zero...");
164  return;
165  }
166 
167  memmove(port->buffer, port->buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
168  port->bytesRead -= cropsize;
169 }
170 
172 
173  if (navdata->ultrasound > 10000) {
175  }
176 
177  int16_t ultrasoundHeight = 0;
178 
179  ultrasoundHeight = (navdata->ultrasound - 880) / 26.553;
180 
181  previousUltrasoundHeight = ultrasoundHeight;
182 
183  return ultrasoundHeight;
184 }
185 
186 // The checksum should be calculated here: we don't know the algorithm
188  navdata_cks = 0;
190  navdata_cks += navdata->ax;
191  navdata_cks += navdata->ay;
192  navdata_cks += navdata->az;
193  navdata_cks += navdata->vx;
194  navdata_cks += navdata->vy;
195  navdata_cks += navdata->vz;
212  navdata_cks += navdata->mx;
213  navdata_cks += navdata->my;
214  navdata_cks += navdata->mz;
215 // navdata_cks += navdata->chksum;
216 
217  return 0; // we dont know how to calculate the checksum
218 }
int16_t mz
Definition: navdata.h:86
unsigned short uint16_t
Definition: types.h:16
int16_t gradient
Definition: navdata.h:77
uint16_t us_curve_time
Definition: navdata.h:70
uint8_t buffer[NAVDATA_BUFFER_SIZE]
Definition: navdata.h:45
uint16_t us_fin_echo
Definition: navdata.h:66
uint16_t us_association_echo
Definition: navdata.h:67
uint8_t isOpen
Definition: navdata.h:41
uint32_t totalBytesRead
Definition: navdata.h:43
int16_t vx
Definition: navdata.h:57
uint16_t us_debut_echo
Definition: navdata.h:65
uint16_t ay
Definition: navdata.h:54
uint16_t bytesRead
Definition: navdata.h:42
int16_t vz
Definition: navdata.h:59
uint16_t az
Definition: navdata.h:55
int16_t my
Definition: navdata.h:85
uint32_t sum_echo
Definition: navdata.h:76
int32_t pressure
Definition: navdata.h:81
signed short int16_t
Definition: types.h:17
uint16_t us_curve_value
Definition: navdata.h:71
int16_t mx
Definition: navdata.h:84
uint16_t nu_trame
Definition: navdata.h:51
unsigned char uint8_t
Definition: types.h:14
uint32_t packetsRead
Definition: navdata.h:44
uint16_t us_distance_echo
Definition: navdata.h:68
int16_t temperature_pressure
Definition: navdata.h:82
uint16_t us_curve_ref
Definition: navdata.h:72
uint16_t temperature_gyro
Definition: navdata.h:61
int16_t vy
Definition: navdata.h:58
uint16_t flag_echo_ini
Definition: navdata.h:79
uint16_t nb_echo
Definition: navdata.h:74
uint16_t ultrasound
Definition: navdata.h:63
uint16_t ax
Definition: navdata.h:53
uint16_t temperature_acc
Definition: navdata.h:60
uint8_t isInitialized
Definition: navdata.h:40