File content as of revision 9:fbd80a803857:

#include "Y201.h"
#include "rtos.h"

const int Y201::resetSeq       [4] = {0x56,0x00,0x26,0x00};
const int Y201::resetSeqAck    [4] = {0x76,0x00,0x26,0x00};
const int Y201::takePicSeq     [5] = {0x56,0x00,0x36,0x01,0x00};
const int Y201::takePicSeqAck  [5] = {0x76,0x00,0x36,0x00,0x00};
const int Y201::set160x120     [9] = {0x56,0x00,0x31,0x05,0x04,0x01,0x00,0x19,0x22};
const int Y201::set320x240     [9] = {0x56,0x00,0x31,0x05,0x04,0x01,0x00,0x19,0x11};
const int Y201::set640x480     [9] = {0x56,0x00,0x31,0x05,0x04,0x01,0x00,0x19,0x00};
const int Y201::setSizeAck     [5] = {0x76,0x00,0x31,0x00,0x00};
const int Y201::readFileSize   [5] = {0x56,0x00,0x34,0x01,0x00};
const int Y201::readFileSizeAck[7] = {0x76,0x00,0x34,0x00,0x04,0x00,0x00};
const int Y201::readFileHead   [8] = {0x56,0x00,0x32,0x0C,0x00,0x0A,0x00,0x00};
const int Y201::readFileAck    [5] = {0x76,0x00,0x32,0x00,0x00};

void Y201::trash() {
    // wait for trash...
    while(readable()) {
        int x = getc();
    }
}

bool Y201::readImage(int startAddress, int readLen, uint8_t *readBuffer) {
   putSeq(readFileHead,readFileHeadLen);
   putc(startAddress>>8); // Start Address MSB
   putc(startAddress&255); // Start Address LSB
   putc(0x00);
   putc(0x00);
   putc(readLen>>8); // Length of file MSB
   putc(readLen&255); // length of file LSB
   putc(0x00); // Interval MSB
   putc(0x0A); // Interval LSB
   int nread = 0;
   if(waitFor(readFileAck,readFileAckLen)) {
      Thread::wait(1);
      Timer t;
      t.start();
      while(nread<readLen) {
         if(readable()) {
            uint8_t c = getc();
           // fprintf(stdout, "[%02x]", c);
            readBuffer[nread] = c;
            nread++;
            t.reset();
         }
         else
         {
           if(t.read_ms()>3000)
           {
             fprintf(stdout, "Blocked!\n Missed %d bytes over %d\nLast byte read is %02x\n", readLen - nread, readLen, readBuffer[nread-1]);
             return false;
           }
         }
     }
     Thread::wait(1);
  } else {
     return false;
  }
  return true;
}

bool Y201::waitFor(const int *seq, const int seqLen) {
    int spos = 0;
    long timeout = 100;
    Timer timer;
    timer.start();
    while(spos<seqLen) {
        if(readable()) {
            int c = getc();
            if(seq[spos]==c) {
                spos++;
            } else {
                return false;
            }
        } else {
            if(timer.read_ms()>timeout) {
                return false;
            }
        }
    }
    return true;
}

bool Y201::waitForInt(int bytes, int *fileSize) {
   int spos = 0;
   long timeout = 100;
   Timer timer;
   timer.start();
   *fileSize = 0;
   while(spos<bytes) {
        if(readable()) {
            uint8_t val = getc();
            if(spos==0) {
                *fileSize += (val<<8);
            } else {
                *fileSize += val;
            }
            
            spos++;
            
        } else {
            if(timer.read_ms()>timeout) {
                return false;
            }
        }
    }
    return true;
}

void Y201::putSeq(const int *seq, int seqLen) {
    while(seqLen--) {
        putc(*seq++);
    }
}

Y201::Y201(PinName tx, PinName rx, const char *name) : MODSERIAL(tx,rx,name) {
    baud(38400);
}

bool Y201::setImageSize(Y201ImageSize size) {
    switch(size) {
        case Y201::e640x480:
            putSeq(set640x480,setSizeLen);
        break;
        
        case Y201::e160x120:
            putSeq(set160x120,setSizeLen);
        break;
        
        case Y201::e320x240:
            putSeq(set320x240,setSizeLen);
        break;
    }
    return waitFor(setSizeAck,setSizeAckLen);
}

bool Y201::takePicture() {
   putSeq(takePicSeq,takePicSeqLen);
   Thread::wait(50);
   return waitFor(takePicSeqAck,takePicSeqAckLen);
}

bool Y201::readImageSize(int *fileSize) {
   putSeq(readFileSize,readFileSizeLen);
   bool ret = waitFor(readFileSizeAck,readFileSizeAckLen);
   if(!ret)
      return false;
  return waitForInt(2,fileSize);
}

bool Y201::reset() {
  putSeq(resetSeq,resetSeqLen);
  bool ret = waitFor(resetSeqAck,resetSeqAckLen);

  // wait for trash
  int count = 3;
  while(count) {
    count--;
    if(readable()) {
        while(readable()) {
            int c = getc();
        }
    }
    Thread::wait(1000);
  }
  return ret;
}