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 
34 using namespace upm;
35 using namespace std;
36 
37 static const int maxRetries = 100;
38 
GROVESCAM(int uart,uint8_t camAddr)39 GROVESCAM::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 
~GROVESCAM()76 GROVESCAM::~GROVESCAM()
77 {
78   if (m_ttyFd != -1)
79     close(m_ttyFd);
80 }
81 
dataAvailable(unsigned int millis)82 bool 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 
readData(uint8_t * buffer,int len)114 int 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 
writeData(uint8_t * buffer,int len)132 int 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 
setupTty(speed_t baud)156 bool 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 
drainInput()186 void GROVESCAM::drainInput()
187 {
188   uint8_t ch;
189 
190   while (dataAvailable(0))
191     readData(&ch, 1);
192 }
193 
init()194 bool 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 
preCapture(PIC_FORMATS_T fmt)247 bool 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 
doCapture()284 bool 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 
storeImage(const char * fname)394 bool 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