Statistics
| Branch: | Tag: | Revision:

amiro-os / components / Lidar.cpp @ 58fe0e0b

History | View | Annotate | Download (9.53 KB)

1
#include <amiro/Lidar.h>
2

    
3
using namespace chibios_rt;
4
using namespace amiro;
5

    
6
uint8_t Lidar::scannedData[NUMBER_OF_CHARACTERS + 1] = {};
7

    
8
Lidar::Lidar(const uint8_t boardId, Lidar::SETUP setup)
9
    : BaseStaticThread<256>(),
10
      boardId(boardId),
11
      setup(setup) {
12

    
13
}
14

    
15
Lidar::
16
~Lidar() {
17

    
18
  // Power down the LIDAR
19
  // TODO Is it correct anyway
20
  switch (boardId) {
21
    case(CAN::LIGHT_RING_ID):
22
      palWritePad(GPIOB, GPIOB_LASER_EN, PAL_LOW);
23
      break;
24
    default:
25
      break;
26
  }
27
  this->isReady = false;
28
};
29

    
30
void Lidar::flushSD2InputQueue() {
31
  chSysLock();
32
  chIQResetI(&SD2.iqueue);
33
  chSysUnlock();
34
}
35

    
36
msg_t Lidar::main(void) {
37

    
38

    
39
  switch (this->boardId) {
40
    case(CAN::LIGHT_RING_ID): {
41

    
42
      // Power up the LIDAR
43
      palWritePad(GPIOB, GPIOB_LASER_EN, PAL_HIGH);
44
      BaseThread::sleep(MS2ST(5000));
45

    
46
      // Setup the driver and lidar, if we want to communicate with it
47
      if (setup != SETUP::POWER_ONLY) {
48
        // Flush the queue because there is a "0" in it
49
        flushSD2InputQueue();
50

    
51
        // Configure LIDAR serial interface speed
52
//       chprintf((BaseSequentialStream*) &SD1, "Speed switch to " STR(SD_SPEED) "\n");
53
        chprintf((BaseSequentialStream*) &SD2, "SS" STR(SD_SPEED_PREFIX) STR(SD_SPEED) LF);
54

    
55
        // Check if the switch went well, otherwise terminate the thread
56
        if (checkDataString("SS" STR(SD_SPEED_PREFIX) STR(SD_SPEED) "\n00P\n\n")) {
57
          chprintf((BaseSequentialStream*) &SD1, "Lidar speed switch OK\n");
58
          // Configure serial interface of STM32
59
          sdStop(&SD2);
60
          SerialConfig sdLidarconf = { SD_SPEED, 0, 0, 0 };
61
          sdStart(&SD2, &sdLidarconf);
62
        } else {
63
          chprintf((BaseSequentialStream*) &SD1, "Lidar speed switch NOT OK: Terminating Lidar \n");
64
          palWritePad(GPIOB, GPIOB_LASER_EN, PAL_LOW);
65
          return -1;
66
        }
67
      }
68
      break;
69
    }
70
    default:
71
      break;
72
  }
73

    
74
  evtInit(&this->evtimer, UPDATE_LIDAR_PERIOD_MSEC);
75

    
76
  EvtSource *eventTimerEvtSource = reinterpret_cast<EvtSource *>(&this->evtimer.et_es);
77

    
78
  EvtListener eventTimerEvtListener;
79

    
80
  // TODO Does PERIODIC_TIMER_ID has something to do with ControllerAreNetwork.h ?
81
  eventTimerEvtSource->registerOne(&eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
82

    
83
  evtStart(&this->evtimer);
84

    
85
  this->setName("Lidar");
86

    
87
  while (!this->shouldTerminate()) {
88

    
89
    eventmask_t eventMask = this->waitOneEvent(ALL_EVENTS);
90
    switch (eventMask) {
91
      case EVENT_MASK(CAN::PERIODIC_TIMER_ID):
92
//         printDetails();
93
        if (setup != SETUP::POWER_ONLY) {
94
          updateSensorVal();
95
        }
96
        break;
97
    }
98
  }
99

    
100
  evtStop(&this->evtimer);
101
  eventTimerEvtSource->unregister(&eventTimerEvtListener);
102

    
103
  return RDY_OK;
104
}
105

    
106
bool_t Lidar::getScan(uint16_t (&scannedData)[NUMBER_OF_STEPS]) {
107
  if (this->isReady && !this->isFail) {
108
    chSysLock();
109
      memcpy(&scannedData, &(Lidar::scannedData[0]), NUMBER_OF_CHARACTERS);
110
      this->isReady = false;
111
    chSysUnlock();
112
    return true;
113
  } else {
114
    return false;
115
  }
116
}
117

    
118
uint16_t Lidar::getNumberOfValues() {
119
  return NUMBER_OF_STEPS;
120
}
121

    
122
uint16_t Lidar::twoCharacterEncoding(uint8_t &char1, uint8_t &char2) {
123
  return uint16_t((((char1 - 0x30) & 0b00111111) << 6) | ((char2 - 0x30) & 0b00111111));
124
}
125

    
126
bool_t Lidar::getData(uint8_t &data, uint32_t timeoutMs) {
127

    
128
//     data = sdGet(&SD2);
129
    msg_t dataTmp = chIQGetTimeout(&SD2.iqueue, MS2ST(timeoutMs));
130
    if  (dataTmp == Q_TIMEOUT || dataTmp == Q_RESET) {
131
      return false;
132
    } else {
133
      data = uint8_t(dataTmp);
134
      ++this->dataCounter;
135
      return true;
136
    }
137
}
138

    
139
msg_t Lidar::updateSensorVal() {
140

    
141
  chSysLock();
142
    // Set this flag so that the everyone knows, that scannedData will be filed right now
143
    this->isReady = false;
144
    // Reset the FAIL flag
145
    this->isFail = false;
146
  chSysUnlock();
147

    
148
  while (!this->isReady) {
149
    switch(step) {
150
      case SEND_SCAN_CMD:
151
        flushSD2InputQueue();
152
//                     Read (725-(44-1))=682 values with two-character encoding what makes 1364 datapoints
153
//                     1456 in total:
154
//                     16 (Cmd-Echo) + ? (Remaining Scans) + 1 (LF) + ? (String Characters) + 1 (LF) + 2 (Status) + 1 (Sum) + 1 (LF) + ? (Timestamp) + 1 (LF) + 1364 (Data for a certain setup) + 1 (Sum) + 1 (LF) + 1 (LF)
155
        chprintf((BaseSequentialStream*) &SD2,DATA_ACQ_CODE STR(STARTING_STEP_PREFIX) STR(STARTING_STEP) STR(END_STEP_PREFIX) STR(END_STEP) STR(CLUSTER_COUNT) STR(SCAN_INTERVALL) STR(NUMBER_OF_INTERVALL) LF);
156
        step = DATA_VALID_CHECK;
157
        this->dataCounter = 0;
158
        break;
159
      case DATA_VALID_CHECK:
160
        if(getData(this->newInput, 200)) {
161
          // TODO check for last data, if scan has a few errors
162
          // HACK The timeout fix this issue at least, because after a while there
163
          //      wont be any transmitts
164
          if (this->newInput == validScanTag[this->checkStatusIdx]) {
165
            if (++this->checkStatusIdx == 5) {
166
              // scan_data is without error: Found the sting "\n99b\n"
167
              step = DATA_START_CHECK;
168
              this->checkStatusIdx = 0;
169
            }
170
          } else {
171
            this->checkStatusIdx = 0;
172
          }
173
        } else {
174
          step = FAIL;
175
        }
176
        break;
177
      case DATA_START_CHECK:
178
        // Check for the next linefeed to start the record
179
        if(getData(this->newInput, 200)) {
180
          if (this->newInput == 10) {
181
            step = DATA_RECORD;
182
            this->dataIdx = 0;
183
          }
184
        } else {
185
          step = FAIL;
186
        }
187
        break;
188
      case DATA_RECORD:
189
        if(getData(this->newInput, 200)) {
190
          if (this->lastInput == 10 && this->newInput == 10) {
191
            // end of data
192
            this->lastInput = 0x0;  // Just to delete this->lastInput value
193
            step = DATA_DECODE;
194
          } else if  (this->newInput != 10) {
195
            Lidar::scannedData[this->dataIdx++] = this->newInput;
196
            this->lastInput = this->newInput;
197
          } else {  // this->lastInput != 10 && this->newInput == 10
198
            // This is the case, where we know that the last character was the checksum.
199
            // Therefore, we need to decrement the counter
200
            --this->dataIdx;
201
            this->lastInput = this->newInput;
202
          }
203
          //chprintf((BaseSequentialStream*) &SD1, "%d ", this->newInput);
204
        } else {
205
          step = FAIL;
206
        }
207
        break;
208
      case DATA_DECODE:
209
        // Decode the recorded data
210
        for (uint16_t idx=0; idx < NUMBER_OF_CHARACTERS; idx += 2) {
211
          //TODO Check if +1 is right
212
          *((uint16_t*) &(Lidar::scannedData[idx])) = twoCharacterEncoding(Lidar::scannedData[idx],Lidar::scannedData[idx+1]);
213
        }
214
        // To print out the data in this thread, choose "step = DATA_SHOW"
215
        step = FINISH;
216
        break;
217
      case DATA_SHOW:
218
        // Show the decoded data
219
        chprintf((BaseSequentialStream*) &SD1, "\n%d", this->dataCounter);
220
        for (uint32_t idx=0; idx < this->dataIdx; idx+=2) {
221
          chprintf((BaseSequentialStream*) &SD1, "\n%d", *((uint16_t*) &(Lidar::scannedData[idx])));
222
        }
223
        step = FINISH;
224
        break;
225
      case FAIL:
226
        // Set the FAIL flag
227
        chSysLock();
228
          // Set this flag so that the everyone knows, that scannedData holds wrong information
229
          this->isFail = true;
230
        chSysUnlock();
231
        break;
232
      case FINISH:
233
        // Clean up
234
        this->step = SEND_SCAN_CMD;
235
        chSysLock();
236
          // Set this flag so that the everyone knows, that scannedData holds the actual data
237
          this->isReady = true;
238
        chSysUnlock();
239
        break;
240
      default:
241
        break;
242
    }
243
  }
244

    
245
  return RDY_OK;
246
}
247

    
248
void Lidar::printData() {
249
  uint8_t lastInput = 0xFF, newInput = 0xFF;
250
  while (true) {
251
    if(getData(newInput, 200)) {
252
      if (lastInput == 10 && newInput == 10) {
253
        return;
254
      } else {
255
        chprintf((BaseSequentialStream*) &SD1, "%c", newInput);
256
      }
257
      lastInput = newInput;
258
    } else {
259
      chprintf((BaseSequentialStream*) &SD1, "TIMEOUT\n", newInput);
260
      return;
261
    }
262
  }
263
}
264

    
265
bool_t Lidar::checkDataString(const char compareString[]) {
266
  uint8_t lastInput = 0xFF, newInput = 0xFF, recIdx = 0;
267
  bool_t dataOk = true, processFlag = true;
268
  while (processFlag) {
269
    if (getData(newInput, 200)) {
270
      // End of data received, quit processing
271
      if (lastInput == 10 && newInput == 10) {
272
        processFlag = false;
273
      }
274
      // Compare data if the characters before have been ok
275
      // Otherwise do not compare anymore, because of possible
276
      // segmentation fault
277
      if (dataOk) {
278
        if (newInput != compareString[recIdx++]) {
279
          dataOk = false;
280
        }
281
      }
282
      // Save the received character
283
      lastInput = newInput;
284
    } else {
285
      processFlag = false;
286
      dataOk = false;
287
    }
288
  }
289
  return dataOk;
290
}
291

    
292
void Lidar::printDetails() {
293

    
294
  chprintf((BaseSequentialStream*) &SD1, "Print sensor details:\n");
295

    
296
  // Tell the sensor to transmit its details
297
  chprintf((BaseSequentialStream*) &SD2, "VV\n");
298

    
299
  // Print the transmitted data
300
  printData();
301
}
302

    
303
void Lidar::printSpecification() {
304

    
305
  chprintf((BaseSequentialStream*) &SD1, "Print sensor specification:\n");
306

    
307
  // Tell the sensor to transmit its specifications
308
  chprintf((BaseSequentialStream*) &SD2, "PP\n");
309

    
310
  // Print the transmitted data
311
  printData();
312
}
313

    
314
void Lidar::printInformation() {
315
  chprintf((BaseSequentialStream*) &SD1, "Print sensor information:\n");
316

    
317
  // Tell the sensor to transmit its information
318
  chprintf((BaseSequentialStream*) &SD2, "II\n");
319

    
320
  // Print the transmitted data
321
  printData();
322
}
323

    
324
bool_t Lidar::getIsReady() {
325
  return this->isReady;
326
}