1/*
2 * Author: Jon Trulson <jtrulson@ics.com>
3 * Copyright (c) 2015 Intel Corporation.
4 *
5 * Thanks to Seeed Studio for a working arduino sketch
6 *
7 * Permission is hereby granted, free of charge, to any person obtaining
8 * a copy of this software and associated documentation files (the
9 * "Software"), to deal in the Software without restriction, including
10 * without limitation the rights to use, copy, modify, merge, publish,
11 * distribute, sublicense, and/or sell copies of the Software, and to
12 * permit persons to whom the Software is furnished to do so, subject to
13 * the following conditions:
14 *
15 * The above copyright notice and this permission notice shall be
16 * included in all copies or substantial portions of the Software.
17 *
18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
19 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
20 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
21 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
22 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
23 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
24 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
25 */
26
27#include <iostream>
28#include <string>
29#include <stdexcept>
30#include <errno.h>
31
32#include "grovescam.h"
33
34using namespace upm;
35using namespace std;
36
37static const int maxRetries = 100;
38
39GROVESCAM::GROVESCAM(int uart, uint8_t camAddr)
40{
41  m_ttyFd = -1;
42
43  // save our shifted camera address, we'll need it a lot
44  m_camAddr = (camAddr << 5);
45
46  m_picTotalLen = 0;
47
48  if ( !(m_uart = mraa_uart_init(uart)) )
49    {
50      throw std::invalid_argument(std::string(__FUNCTION__) +
51                                  ": mraa_uart_init() failed");
52      return;
53    }
54
55  // This requires a recent MRAA (1/2015)
56  const char *devPath = mraa_uart_get_dev_path(m_uart);
57
58  if (!devPath)
59    {
60      throw std::runtime_error(std::string(__FUNCTION__) +
61                               ": mraa_uart_get_dev_path() failed");
62      return;
63    }
64
65  // now open the tty
66  if ( (m_ttyFd = open(devPath, O_RDWR)) == -1)
67    {
68      throw std::runtime_error(std::string(__FUNCTION__) +
69                               ": open of " +
70                               string(devPath) + " failed:" +
71                               string(strerror(errno)));
72      return;
73    }
74}
75
76GROVESCAM::~GROVESCAM()
77{
78  if (m_ttyFd != -1)
79    close(m_ttyFd);
80}
81
82bool GROVESCAM::dataAvailable(unsigned int millis)
83{
84  if (m_ttyFd == -1)
85    return false;
86
87  struct timeval timeout;
88
89  if (millis == 0)
90    {
91      // no waiting
92      timeout.tv_sec = 0;
93      timeout.tv_usec = 0;
94    }
95  else
96    {
97      timeout.tv_sec = millis / 1000;
98      timeout.tv_usec = (millis % 1000) * 1000;
99    }
100
101  int nfds;
102  fd_set readfds;
103
104  FD_ZERO(&readfds);
105
106  FD_SET(m_ttyFd, &readfds);
107
108  if (select(m_ttyFd + 1, &readfds, NULL, NULL, &timeout) > 0)
109    return true;                // data is ready
110  else
111    return false;
112}
113
114int GROVESCAM::readData(uint8_t *buffer, int len)
115{
116  if (m_ttyFd == -1)
117    return(-1);
118
119  int rv = read(m_ttyFd, (char *)buffer, len);
120
121  if (rv < 0)
122    {
123      throw std::runtime_error(std::string(__FUNCTION__) +
124                               ": read() failed: " +
125                               string(strerror(errno)));
126      return rv;
127    }
128
129  return rv;
130}
131
132int GROVESCAM::writeData(uint8_t *buffer, int len)
133{
134  if (m_ttyFd == -1)
135    return(-1);
136
137  // first, flush any pending but unread input
138
139  tcflush(m_ttyFd, TCIFLUSH);
140
141  int rv = write(m_ttyFd, (char *)buffer, len);
142
143  if (rv < 0)
144    {
145      throw std::runtime_error(std::string(__FUNCTION__) +
146                               ": write() failed: " +
147                               string(strerror(errno)));
148      return rv;
149    }
150
151  tcdrain(m_ttyFd);
152
153  return rv;
154}
155
156bool GROVESCAM::setupTty(speed_t baud)
157{
158  if (m_ttyFd == -1)
159    return(false);
160
161  struct termios termio;
162
163  // get current modes
164  tcgetattr(m_ttyFd, &termio);
165
166  // setup for a 'raw' mode.  81N, no echo or special character
167  // handling, such as flow control.
168  cfmakeraw(&termio);
169
170  // set our baud rates
171  cfsetispeed(&termio, baud);
172  cfsetospeed(&termio, baud);
173
174  // make it so
175  if (tcsetattr(m_ttyFd, TCSAFLUSH, &termio) < 0)
176    {
177      throw std::runtime_error(std::string(__FUNCTION__) +
178                               ": tcsetattr() failed: " +
179                               string(strerror(errno)));
180      return false;
181    }
182
183  return true;
184}
185
186void GROVESCAM::drainInput()
187{
188  uint8_t ch;
189
190  while (dataAvailable(0))
191    readData(&ch, 1);
192}
193
194bool GROVESCAM::init()
195{
196  const unsigned int pktLen = 6;
197  uint8_t cmd[pktLen] = {0xaa, static_cast<uint8_t>(0x0d|m_camAddr), 0x00,
198                         0x00, 0x00, 0x00};
199  uint8_t resp[pktLen];
200  int retries = 0;
201
202  while (true)
203    {
204      if (retries++ > maxRetries)
205        {
206          throw std::runtime_error(std::string(__FUNCTION__) +
207                                   ": maximum retries exceeded");
208          return false;
209        }
210
211      writeData(cmd, pktLen);
212
213      if (!dataAvailable(500))
214        continue;
215
216      if (readData(resp, pktLen) != pktLen)
217        continue;
218
219      if (resp[0] == 0xaa
220          && resp[1] == (0x0e | m_camAddr)
221          && resp[2] == 0x0d
222          && resp[4] == 0
223          && resp[5] == 0)
224        {
225          if (readData(resp, pktLen) != pktLen)
226            continue;
227          else
228            {
229              if (resp[0] == 0xaa
230                  && resp[1] == (0x0d | m_camAddr)
231                  && resp[2] == 0
232                  && resp[3] == 0
233                  && resp[4] == 0
234                  && resp[5] == 0)
235                break;
236            }
237        }
238    }
239
240  cmd[1] = 0x0e | m_camAddr;
241  cmd[2] = 0x0d;
242  writeData(cmd, pktLen);
243
244  return true;
245}
246
247bool GROVESCAM::preCapture(PIC_FORMATS_T fmt)
248{
249  const unsigned int pktLen = 6;
250  uint8_t cmd[pktLen] = { 0xaa, static_cast<uint8_t>(0x01 | m_camAddr), 0x00,
251                          0x07, 0x00, static_cast<uint8_t>(fmt) };
252  uint8_t resp[pktLen];
253  int retries = 0;
254
255  while (true)
256    {
257      if (retries++ > maxRetries)
258        {
259          throw std::runtime_error(std::string(__FUNCTION__) +
260                                   ": maximum retries exceeded");
261          return false;
262        }
263
264      drainInput();
265
266      writeData(cmd, pktLen);
267
268      if (!dataAvailable(100))
269        continue;
270
271      if (readData(resp, pktLen) != pktLen)
272        continue;
273
274      if (resp[0] == 0xaa
275          && resp[1] == (0x0e | m_camAddr)
276          && resp[2] == 0x01
277          && resp[4] == 0
278          && resp[5] == 0) break;
279    }
280
281  return true;
282}
283
284bool GROVESCAM::doCapture()
285{
286  const unsigned int pktLen = 6;
287  uint8_t cmd[pktLen] = { 0xaa, static_cast<uint8_t>(0x06 | m_camAddr), 0x08,
288                          MAX_PKT_LEN & 0xff, (MAX_PKT_LEN >> 8) & 0xff, 0};
289  uint8_t resp[pktLen];
290  int retries = 0;
291
292  m_picTotalLen = 0;
293
294  while (true)
295    {
296      if (retries++ > maxRetries)
297        {
298          throw std::runtime_error(std::string(__FUNCTION__) +
299                                   ": maximum retries exceeded");
300          return false;
301        }
302
303      drainInput();
304      writeData(cmd, pktLen);
305      usleep(100000);
306
307      if (!dataAvailable(100))
308        continue;
309
310      if (readData(resp, pktLen) != pktLen)
311        continue;
312
313      if (resp[0] == 0xaa
314          && resp[1] == (0x0e | m_camAddr)
315          && resp[2] == 0x06
316          && resp[4] == 0
317          && resp[5] == 0)
318        break;
319    }
320
321  cmd[1] = 0x05 | m_camAddr;
322  cmd[2] = 0;
323  cmd[3] = 0;
324  cmd[4] = 0;
325  cmd[5] = 0;
326
327  retries = 0;
328  while (true)
329    {
330      if (retries++ > maxRetries)
331        {
332          throw std::runtime_error(std::string(__FUNCTION__) +
333                                   ": maximum retries exceeded");
334          return false;
335        }
336
337      drainInput();
338      writeData(cmd, pktLen);
339      if (readData(resp, pktLen) != pktLen)
340        continue;
341
342      if (resp[0] == 0xaa
343          && resp[1] == (0x0e | m_camAddr)
344          && resp[2] == 0x05
345          && resp[4] == 0
346          && resp[5] == 0)
347        break;
348    }
349
350  cmd[1] = 0x04 | m_camAddr;
351  cmd[2] = 0x01;
352
353  retries = 0;
354  while (true)
355    {
356      if (retries++ > maxRetries)
357        {
358          throw std::runtime_error(std::string(__FUNCTION__) +
359                                   ": maximum retries exceeded");
360          return false;
361        }
362
363      drainInput();
364      writeData(cmd, 6);
365
366      if (readData(resp, pktLen) != pktLen)
367        continue;
368
369      if (resp[0] == 0xaa
370          && resp[1] == (0x0e | m_camAddr)
371          && resp[2] == 0x04
372          && resp[4] == 0
373          && resp[5] == 0)
374        {
375          if (!dataAvailable(1000))
376            continue;
377
378          if (readData(resp, pktLen) != pktLen)
379            continue;
380
381          if (resp[0] == 0xaa
382              && resp[1] == (0x0a | m_camAddr)
383              && resp[2] == 0x01)
384            {
385              m_picTotalLen = (resp[3]) | (resp[4] << 8) | (resp[5] << 16);
386              break;
387            }
388        }
389    }
390
391  return true;
392}
393
394bool GROVESCAM::storeImage(const char *fname)
395{
396  if (!fname)
397    {
398      throw std::invalid_argument(std::string(__FUNCTION__) +
399                                  ": filename is NULL");
400      return false;
401    }
402
403  if (!m_picTotalLen)
404    {
405      throw std::runtime_error(std::string(__FUNCTION__) +
406                    ": Picture length is zero, you need to capture first.");
407
408      return false;
409    }
410
411  FILE *file = fopen(fname, "wb");
412
413  if (!file)
414    {
415      throw std::runtime_error(std::string(__FUNCTION__) +
416                               ": fopen() failed: " +
417                               string(strerror(errno)));
418      return false;
419    }
420
421  /// let the games begin...
422  const unsigned int pktLen = 6;
423  unsigned int pktCnt = (m_picTotalLen) / (MAX_PKT_LEN - 6);
424  if ((m_picTotalLen % (MAX_PKT_LEN-6)) != 0)
425    pktCnt += 1;
426
427  uint8_t cmd[pktLen] = { 0xaa, static_cast<uint8_t>(0x0e | m_camAddr), 0x00,
428                          0x00, 0x00, 0x00 };
429  uint8_t pkt[MAX_PKT_LEN];
430  int retries = 0;
431
432  for (unsigned int i = 0; i < pktCnt; i++)
433    {
434      cmd[4] = i & 0xff;
435      cmd[5] = (i >> 8) & 0xff;
436
437      retries = 0;
438
439    retry:
440
441      usleep(10000);
442
443      drainInput();
444      writeData(cmd, pktLen);
445
446      if (!dataAvailable(1000))
447        {
448          if (retries++ > maxRetries)
449            {
450              throw std::runtime_error(std::string(__FUNCTION__) +
451                                       ": timeout, maximum retries exceeded");
452              return false;
453            }
454          goto retry;
455        }
456
457      uint16_t cnt = readData(pkt, MAX_PKT_LEN);
458
459      unsigned char sum = 0;
460      for (int y = 0; y < cnt - 2; y++)
461        {
462          sum += pkt[y];
463        }
464      if (sum != pkt[cnt-2])
465        {
466          if (retries++ <= maxRetries)
467            goto retry;
468          else
469            {
470              throw std::runtime_error(std::string(__FUNCTION__) +
471                                       ": cksum error, maximum retries exceeded");
472              return false;
473            }
474        }
475
476      fwrite((const uint8_t *)&pkt[4], cnt - 6, 1, file);
477    }
478
479  cmd[4] = 0xf0;
480  cmd[5] = 0xf0;
481  writeData(cmd, pktLen);
482
483  fclose(file);
484
485  // reset the pic length to 0 for another run.
486  m_picTotalLen = 0;
487
488  return true;
489}
490