amiro-os / include / amiro / Lidar.h @ 753ccd04
History | View | Annotate | Download (7.27 KB)
1 | 58fe0e0b | Thomas Schöpping | #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_ */ |