amiro-os / include / amiro / Lidar.h @ 753ccd04
History | View | Annotate | Download (7.27 KB)
1 |
#ifndef LIDAR_H_
|
---|---|
2 |
#define LIDAR_H_
|
3 |
|
4 |
// Top view
|
5 |
/* __________
|
6 |
// / | \
|
7 |
// / O \
|
8 |
// / H O K U Y O \
|
9 |
// | |
|
10 |
// | |
|
11 |
// \ U R G /
|
12 |
// scannedData[NUMBER_OF_STEPS] = END_STEP \/ \/ STARTING_STEP = scannedData[0]
|
13 |
// \__________/
|
14 |
*/
|
15 |
|
16 |
#include <string.h> // memcpy |
17 |
#include <stdlib.h> |
18 |
|
19 |
#include <amiro/bus/spi/HWSPIDriver.hpp> |
20 |
#include <amiro/Constants.h> // CAN::LIGHT_RING_ID |
21 |
#include <ch.hpp> |
22 |
#include <hal.h> |
23 |
#include <chprintf.h> |
24 |
#include <evtimer.h> |
25 |
|
26 |
// DO CONFIGURATION HERE : START
|
27 |
// Important: SD_SPEED_PREFIX + SD_SPEED has to have 6 characters: e.g. 019200 or 115200
|
28 |
#define SD_SPEED_PREFIX // Prefix < 0 | 0 | | | | > |
29 |
#define SD_SPEED 250000 // Baudrate < 19200 | 57600 | 115200 | 250000 | 500000 | 750000 > |
30 |
#define UPDATE_LIDAR_PERIOD_MSEC MS2ST(1000) // Periodic sensor value fetch in ms |
31 |
// Scan acquire message : START
|
32 |
// Uncomment the specific character decoding to make it available
|
33 |
#define USE_2CHAR_CODE
|
34 |
// #define USE_3CHAR_CODE // TODO Not implemented yet
|
35 |
#define DATA_ACQ_2CHAR_CODE "MS" |
36 |
#define DATA_ACQ_3CHAR_CODE "MD" |
37 |
// Important: STARTING_STEP_PREFIX + STARTING_STEP has to have 4 characters: e.g. 0005 0044 0320
|
38 |
#define STARTING_STEP_PREFIX 00 |
39 |
#define STARTING_STEP 44 |
40 |
// Important: END_STEP_PREFIX + END_STEP has to have 4 characters: e.g. 0005 0725 0320
|
41 |
#define END_STEP_PREFIX 0 |
42 |
#define END_STEP 725 |
43 |
#define CLUSTER_COUNT 00 |
44 |
#define SCAN_INTERVALL 0 |
45 |
#define NUMBER_OF_INTERVALL 01 |
46 |
// Scan acquire message : END
|
47 |
// DO CONFIGURATION HERE : END
|
48 |
|
49 |
// Internal calculations and definitions
|
50 |
#if defined(USE_2CHAR_CODE) && defined(USE_3CHAR_CODE)
|
51 |
# error("USE_2CHAR_CODE and USE_3CHAR_CODE are defined. Only one of them is allowed!") |
52 |
#elif !defined(USE_2CHAR_CODE) && !defined(USE_3CHAR_CODE)
|
53 |
# error("USE_2CHAR_CODE and USE_3CHAR_CODE are not defined. Define one of them!") |
54 |
#elif defined(USE_2CHAR_CODE)
|
55 |
# define DATA_ACQ_CODE DATA_ACQ_2CHAR_CODE
|
56 |
# define NUMBER_OF_CHARACTERS (END_STEP - STARTING_STEP + 1) * 2 |
57 |
#elif defined(USE_3CHAR_CODE)
|
58 |
# define DATA_ACQ_CODE DATA_ACQ_3CHAR_CODE
|
59 |
# define NUMBER_OF_CHARACTERS (END_STEP - STARTING_STEP + 1) * 3 |
60 |
#endif
|
61 |
#define NUMBER_OF_STEPS (END_STEP - STARTING_STEP + 1) |
62 |
|
63 |
#define STR_HELPER(x) #x |
64 |
#define STR(x) STR_HELPER(x)
|
65 |
|
66 |
#define LF "\n" |
67 |
|
68 |
namespace amiro { |
69 |
|
70 |
class Lidar : public chibios_rt::BaseStaticThread<256> {
|
71 |
public:
|
72 |
/**
|
73 |
* Possible setups for the LIDAR: POWER_ONLY (The LIDAR will be powererd, but no serial IO is performed [Need for USB IO]); SERIAL_IO (Power and serial communication via lidar driver)
|
74 |
*/
|
75 |
enum SETUP {POWER_ONLY, SERIAL_IO};
|
76 |
|
77 |
/**
|
78 |
* Writes the last scan into the given array
|
79 |
*
|
80 |
* @param scanData Array of size NUMBER_OF_STEPS which will be override
|
81 |
* with the scanned data
|
82 |
*
|
83 |
* @return True if scanData holds the current data, False if no data have been copied
|
84 |
*/
|
85 |
bool_t getScan(uint16_t (&scanData)[NUMBER_OF_STEPS]); |
86 |
|
87 |
/**
|
88 |
* Returns the value NUMBER_OF_STEPS which is the amount of datapoints per scan
|
89 |
*
|
90 |
* @return Amount of data in one scan
|
91 |
*/
|
92 |
uint16_t getNumberOfValues(); |
93 |
|
94 |
protected:
|
95 |
virtual msg_t main(); |
96 |
|
97 |
/**
|
98 |
* Do a scan and update the datapoints in scanData
|
99 |
*/
|
100 |
msg_t updateSensorVal(); |
101 |
|
102 |
/**
|
103 |
* Print received data until "\n\n" was received
|
104 |
*/
|
105 |
void printData();
|
106 |
|
107 |
/**
|
108 |
* Receive data until "\n\n" and compare it to the given string
|
109 |
* Note: If the command did not work, use printData() instead
|
110 |
* and compare the reply with chapter "9. Response to Invalid Commands"
|
111 |
*
|
112 |
* @param compareString String to compare with
|
113 |
*
|
114 |
* @return True if string was equal, false if not
|
115 |
*/
|
116 |
bool_t checkDataString(const char compareString[]); |
117 |
/**
|
118 |
* Send the command which let the LIDAR send its details
|
119 |
*/
|
120 |
void printDetails();
|
121 |
|
122 |
/**
|
123 |
* Send the command which let the LIDAR send its specifications
|
124 |
*/
|
125 |
void printSpecification();
|
126 |
|
127 |
/**
|
128 |
* Send the command which let the LIDAR send its information
|
129 |
*/
|
130 |
void printInformation();
|
131 |
|
132 |
/**
|
133 |
* Tells you, if the sensor is ready for work
|
134 |
*
|
135 |
* @return True for operability, False if not
|
136 |
*/
|
137 |
inline bool_t getIsReady();
|
138 |
|
139 |
/**
|
140 |
* Do the two character encoding after the manual
|
141 |
*
|
142 |
* @param char1 High character
|
143 |
* @param char2 Low character
|
144 |
*
|
145 |
* @return Encoded data
|
146 |
*/
|
147 |
inline uint16_t twoCharacterEncoding(uint8_t &char1, uint8_t &char2);
|
148 |
|
149 |
/**
|
150 |
* Blocking function which writes the received character to the given variable
|
151 |
*
|
152 |
* @param data Container for the variable
|
153 |
* @param timeoutMs Timeout in millisecond
|
154 |
*
|
155 |
* @return True if data was received, False if timed out (NOTE timeoutMs should by much smaller than UPDATE_LIDAR_PERIOD_MSEC)
|
156 |
*/
|
157 |
bool_t getData(uint8_t &data, uint32_t timeoutMs); |
158 |
|
159 |
/**
|
160 |
* Container for the scanned data
|
161 |
*
|
162 |
* @brief The "+1" character is for holding the last sum which is transmitted
|
163 |
* by the LIDAR. It is not used, but it makes the algorithm much easier
|
164 |
* we we do not care about it
|
165 |
*/
|
166 |
static uint8_t scannedData[NUMBER_OF_CHARACTERS + 1]; |
167 |
|
168 |
/**
|
169 |
* Flushes the input queue of the SD2 serial device
|
170 |
*/
|
171 |
inline void flushSD2InputQueue(); |
172 |
|
173 |
/**
|
174 |
* Possible states for the receiver state machine
|
175 |
*/
|
176 |
enum STATE {SEND_SCAN_CMD, DATA_VALID_CHECK, DATA_START_CHECK, DATA_RECORD, DATA_DECODE, DATA_SHOW, FAIL, FINISH};
|
177 |
|
178 |
/**
|
179 |
* Current state of the machine
|
180 |
*/
|
181 |
STATE step = SEND_SCAN_CMD; |
182 |
|
183 |
/**
|
184 |
* This is the status which comes after the command echo "\n99b\n"
|
185 |
*/
|
186 |
const uint8_t validScanTag[5] = {10,57,57,98,10}; |
187 |
|
188 |
/**
|
189 |
* Index for the received status
|
190 |
*/
|
191 |
uint8_t checkStatusIdx = 0;
|
192 |
|
193 |
/**
|
194 |
* Index for the received status
|
195 |
*/
|
196 |
|
197 |
/**
|
198 |
* Hold the board id
|
199 |
*/
|
200 |
uint8_t boardId; |
201 |
|
202 |
/**
|
203 |
* Hold the setup configuration for the lidar
|
204 |
*/
|
205 |
SETUP setup; |
206 |
|
207 |
/**
|
208 |
* Holds the current input
|
209 |
*/
|
210 |
uint8_t newInput = 0xFF;
|
211 |
|
212 |
/**
|
213 |
* Holds the last input
|
214 |
*/
|
215 |
uint8_t lastInput = 0xFF;
|
216 |
|
217 |
/**
|
218 |
* Counter for the received characters of the data points
|
219 |
*/
|
220 |
uint16_t dataIdx = 0;
|
221 |
|
222 |
/**
|
223 |
* Temporary variable which hold the amount of receved bytes
|
224 |
*/
|
225 |
uint32_t dataCounter = 0;
|
226 |
|
227 |
/**
|
228 |
* Holds the information, if scannedData holds the last sensor scan
|
229 |
*/
|
230 |
bool_t isReady = false;
|
231 |
|
232 |
/**
|
233 |
* Holds the information, if the datachunk was not transmitted completely
|
234 |
*/
|
235 |
bool_t isFail = false;
|
236 |
|
237 |
private:
|
238 |
|
239 |
EvTimer evtimer; |
240 |
|
241 |
public:
|
242 |
/**
|
243 |
* Constructor
|
244 |
*
|
245 |
* @param boardId Board ID which can be found in ControllerAreaNetwork.h.
|
246 |
* It is used to setup the LIDAR for exactly this board
|
247 |
*/
|
248 |
Lidar(const uint8_t boardId, Lidar::SETUP setup);
|
249 |
|
250 |
/**
|
251 |
* Constructor
|
252 |
*
|
253 |
* Disable the LIDAR
|
254 |
* TODO Why "virtual ~Lidar() = 0;"
|
255 |
*/
|
256 |
~Lidar(); |
257 |
|
258 |
}; |
259 |
|
260 |
} |
261 |
#endif /* LIDAR_H_ */ |