Cameras - 42 not 37

Well, as my last post detailed, I got a certain distance with cameras and then hit a brick wall trying to get hardware serial communications working. I've decided as my next step to get the much simpler linksprite sample code and get it working on an Arduino Uno.

The LinkSprite sample just consists of a few functions to send specific commands to the camera - reset, take photo and read data. Internally these quite literally just pump bytes of data down the serial port (they don't even wait for a response). The read data one is the only slightly clever bit, as it sends the address it wants to read as well. I won't bother showing these ones though. What is here, is my cleaned up and made useful version of the main loop:

void loop() 
{
  //send reset command
  SendResetCmd();
  delay(4000);                              
 
  //send take photo command
  Serial.println("Taking photo");
  SendTakePhotoCmd();

  //drain any bytes out of the serial buffer that will have come back from the previous 2 commands
  while(mySerial.available()>0)
  {
    incomingbyte=mySerial.read();
    printbyte(incomingbyte);
  }   
  Serial.println();
  
  //begin reading the photo
  Serial.println("Reading data");
  while(!EndFlag)
  {  
    //send command to read data
    SendReadDataCmd();
    
    //delay 25ms - and just hope data arrives within this time??   
    delay(25);
    
    //read whole response (5 byte response+32 byte data) into 'all' buffer
    int pos=0;
    int count=0;
    byte all[64];
    while(mySerial.available()>0)
    {
      //get response
      all[pos]=mySerial.read();
      
      //if within the actual data range
      if((pos>5)&&(pos<37)&&(!EndFlag))
      {
        //check for EOF marker (0xFF followed by 0xD9)
        if((all[pos-1]==0xFF)&&(all[pos]==0xD9))      //Check if the picture is over
          EndFlag=1;                           
        //increment bytes read count
        count++;
      }
      
      //move forwards 1 byte
      pos++;
    }
    
    //check we actually got data
    if(pos > 0)
    {
      //print out the response bytes (should always be 0x76 0x00 0x32 0x00 0x00)
      for(j=0; j < 5; j++)
      {
         printbyte(all[j]);
      }
      Serial.println();
      
      //print out the data bytes
      for(j=0;j<count;j++)
      {   
         printbyte(all[j+5]);
      }                                       //Send jpeg picture over the serial port
      Serial.println();
    }
    else
    {
      //if didn't get anything, print a warning
      Serial.println("No data!");
    }
  }      
  while(1);
}

If you've done much communications code the first thing you might see and scream in horror at is the delay(25) just after reading data. This classic noob error is basically assuming that after 25ms the data will definately be around nice and ready to use. Still, I decide to not change any key logic yet. What I do change is what gets printed. The purpose of this round is to debug what's going wrong, so rather than try to get nice images across, I print out every command response, followed by the data it provides.

Taking photo
0x76 0x00 0x26 ... response from first commands ... 0x36 0x00 0x00 
Reading data
0x76 0x00 0x32 0x00 0x00 
0xFF 0xD8 0xFF ... image data ... 0x00 0x0D 0x00 
0x76 0x00 0x32 0x00 0x00 
0x12 0x0B 0x51 ... image data ... 0x08 0x05 0x05 
0x76 0x00 0x32 0x00 0x00 
0x04 0x05 0x0A ... image data ... 0x12 0x13 0x14 
0x76 0x00 0x32 0x00 0x00 
0x15 0x15 0x0D ... image data ... 0x14 0x14 0x14 

What you see here is the initial photo being taken, followed by me draining all data from the serial buffer. Then I begin reading data in 32 byte chunks. Each read returns a response (0x76 0x00 0x32 0x00 0x00) followed by 32 bytes of image data. All good, but already just by searching through the log I see some weirdness:

0x76 0x00 0x32 0x00 0x00 
0xC4 0x2D 0x82 ... image data ... 0x54 0x75 0xAB 
0x76 0x00 0x32 0x00 0x00 
                                <---- where's the image data?
0x90 0x34 0xC5 0xB3 0x51        <---- and this is not a good response! 
0xB1 0x2D 0xD7 ... image data ... 0x00 0x32 0x00 
0x76 0x00 0x32 0x00 0x00 

Here I've already lost 32 whole bytes of image data, and suspiciously the next response is invalid. Now if I had been doing mutithreaded programming for years (and I have - yay!) I'd put this instantly down to that silly 'delay(25)' I mentioned earlier. I change this to:

    //delay until correct amount of data is here
    while(mySerial.available() < 37);

And the data coming through becomes clean as a whistle. OK. I'm back to being able to get a clean image out on the Arduino Uno using SoftwareSerial. This time though I have much simpler and debuggable code. Lets see what happens when we hook it up to a hardware serial port on the MmBot's Arduino Mega.

First up, in the spirit of sensible debugging, I take things one step at a time and get the camera working over software serial on the Arduino Mega. For this I have to tweak the code to run off GPIO pins 12  and 13, rather than 2 or 3, as not all pins support interrupts (and therefore software serial) on the Mega. After some fiddling and getting bits mixed up, I eventually attain the same result as I got from the Uno.

Now for hardware serial. This is a simple case of connecting the camera to the pins for hardware serial port 2, and changing all references to 'mySerial' in the earlier code to be 'Serial2'. I run it and lo and behold... it works. Hmmm. So what's different is the next question. It could be a timing issue, but the problem was pretty much 100% reliable on Thursday, with or without loads of debug prints to slow things down. Just to be sure, I adjust the code to only print things out if it doesn't get what it expects with a simple if statement:

      //print out the response bytes if wrong (should always be 0x76 0x00 0x32 0x00 0x00)
      if(pos != 37 || all[0] != 0x76 || all[1] != 0x00 || all[2] != 0x32 || all[3] != 0x00 || all[4] != 0x00)
      {
          Serial.print("Erroneous data received at 0x");
          Serial.println(a,HEX);
          Serial.print(pos);
          Serial.println(" bytes read");
          for(j=0;j<5;j++)
          {   
             printbyte(all[j]);
          }                                       
          Serial.println();
          for(j=5;j<pos;j++)
          {   
             printbyte(all[j]);
          }                                       
          Serial.println();
        }

I also add a little 'Serial.println("Done")' at the end so I know when it's finished.

Basically what I've done is made it run as fast as it possibly can. Unless something goes wrong the code will now just keep trying to grab data from the camera. Instantly the whole thing stops working - 'done' never gets printed out. This means that some bit of code has got stuck in an infinite loop somewhere. There's only 1 potential infinite loop in the code though - remember my 'wait for 37 bytes':

    //delay until correct amount of data is here
    while(mySerial.available() < 37);

If it's blocking somewhere it'll be here, so I add a timeout warning:

    //delay until correct amount of data is here
    unsigned long timems = millis();
    while(Serial2.available() < 37)
    {
      if((millis()-timems) > 1000)
      {
        Serial.print("Been waiting for data for ages now, only got ");
        Serial.print(Serial2.available());
        Serial.print(" current address 0x");
        Serial.print(a,HEX);
        Serial.println("");
        timems = millis();
      }
    }

This continuously checks if 1000ms have passed (i.e. 1s) and prints out a warning followed by some data. Once this is in I run the program again and lo and behold, this starts getting printed out in the serial monitor:

Reading data
Been waiting for data for ages now, only got 10 current address 0x40
Been waiting for data for ages now, only got 10 current address 0x40
Been waiting for data for ages now, only got 10 current address 0x40

Stuck as expected. Not just stuck either - stuck 0x40 (aka 64) bytes in - exactly where it was getting stuck on Thursday. So lets reason this out - what do I know?

• I've sent off and received 2 requests for data (as I'm now on address 0x40)
• All the requests I did receive were apparently valid - the response was correct and I got 37 bytes
• The serial transmit buffer can't be full, as I never send more than a few command bytes before waiting for a response
• The serial receive buffer can't be full unless for some reason the camera suddenly decided to send me massive amounts of data, which I doubt
• Curiously, the camera has actually sent me 10 bytes back - that'll be 5 for the response, plus another 5

This rings a bell. My gut says that if I print out those 10 bytes that I have received, it'll be 2 copies of the same response. Lets see...

I add:

        while(Serial2.available())
          printbyte(Serial2.read());
        Serial.println("");

To that timeout code, so now if over 1 second passes I actually drain the serial buffer and print out what I do have so far. And shock horror, we print out...

Reading data
Been waiting for data for ages now, only got 10 current address 0x40
0x76 0x00 0x32 0x00 0x00 0x76 0x00 0x0A 0x01 0x00 

Well what do you know. Not quite an exact repetition, but I definitely get the response back, then at least 2 bytes of the same  response again (0x76 0x00). Having checked the manual there appears to be no response that contains 0x0A, so my gut says that's the start of the image. I'm now thinking there's a bug in the camera software. If you request the next read too soon after receiving the previous, it fails to communicate properly.

Adding a 10ms delay before executing the next read changes the behaviour again. This time I start getting too much data - still after 0x40. As a result, the program no longer blocks and starts printing out information about dodgy data:

Erroneous data received at 0x40
38 bytes read
0x76 0x00 0x32 0x00 0x00 
0x76 0x00 0x32 0x00 0x00 0x12 0x0B 0x51 0x04 0x51 0x04 0x00 0x00 0xFF 0xDB 0x00 0x84 0x00 0x07 0x04 0x05 0x06 0x05 0x04 0x07 0x06 0x05 0x06 0x07 0x07 0x07 0x08 0x0A 
Erroneous data received at 0x60
38 bytes read
0x10 0x0B 0x0A 0x09 0x76 
0x00 0x32 0x00 0x00 0x76 0x00 0x32 0x00 0x00 0x09 0x0A 0x14 0x0E 0x0F 0x0C 0x10 0x18 0x15 0x19 0x18 0x17 0x15 0x17 0x16 0x1A 0x1D 0x25 0x20 0x1A 0x1C 0x23 0x1C 0x16 
Erroneous data received at 0x80
38 bytes read
0x17 0x21 0x2C 0x21 0x23 
0x27 0x28 0x2A 0x76 0x00 0x32 0x00 0x00 0x76 0x00 0x32 0x00 0x00 0x2A 0x2A 0x19 0x1F 0x2E 0x31 0x2E 0x29 0x31 0x25 0x29 0x2A 0x28 0x01 0x07 0x07 0x07 0x0A 0x09 0x0A 
Erroneous data received at 0xA0

You can see on the first error I receive more data than expected, and end up with 2 whole copies of the command response. From this point on we're basically screwed, as we will always be playing catchup with this out of sync serial buffer. Timing had an effect though, so I change the 10ms delay to a 10s delay (ridiculously long) and the problem is still around. Time for more thinking. What do I know now:

• Adding a time delay doesn't solve the problem, but does affect it in a minor way
• It still always dies at exactly 0x40 bytes received, plus 2*5 bytes in responses
• That's a total of 74 bytes received
• Potentially irrelevant, but the Arduino hardware serial port has 128 byte receive buffer
• I also work out that I send 16 bytes per request, so that's 0x20 bytes sent
• The problem seems a lot less severe when I'm sending data out over Serial1 - still occurs sometimes though

Response length is the common denominator here. For some reason things are going wrong in between me requesting data and me getting it back. So next up I try putting a 100ms delay between request and actually attempting to read data. Now something interesting happens - see if you can spot it...

Reading data
Erroneous data received at 0x20
42 bytes read
0x76 0x00 0x32 0x00 0x00 
0xFF 0xD8 0xFF 0xFE 0x00 0x24 0x63 ... bla bla bla ... 0x00 0xF0 0x00 0x40 0x01 0x1A 0x00 0x32 0x76 0x00 0x32 0x00 0x00 
Erroneous data received at 0x40
42 bytes read
0x76 0x00 0x32 0x00 0x00 
0x12 0x0B 0x51 0x04 0x51 0x04 0x00 ... bla bla bla ... 0x07 0x07 0x08 0x0A 0x10 0x0B 0x0A 0x09 0x76 0x00 0x32 0x00 0x00 
Erroneous data received at 0x60
42 bytes read
0x76 0x00 0x32 0x00 0x00 
0x09 0x0A 0x14 0x0E 0x0F 0x0C 0x10 ... bla bla bla ... 0x17 0x21 0x2C 0x21 0x23 0x27 0x28 0x2A 0x76 0x00 0x32 0x00 0x00 
Erroneous data received at 0x80

Suddenly the going wrong is much more reliable. I get 42 bytes (i.e. the 32 data + 10) back every time. More importantly though, now that I'm waiting for the bit that is apparently going wrong to tell me everything it has to say, I realise I've got things the wrong way round...

• I thought it was sending me the response twice, followed by the message
• It was actually sending me a response, followed by a message, followed by the response again

If you only wait for what you need both of these appear identical, as you simply start reading the end of the last message as the start of the next one. I go and read the manual more carefully and spot the subtley worded explanation - you can expect the response to appear at the start and end of your image data after a read. Here it is:

None of the example code had to handle this. By using software serial they ran slow enough to always end up with a full 42 bytes, and just ignored the end of the message (to the extent of not even acknowledging it's existence). However, hardware serial runs fast enough to start reading the message before the end tag has arrived, and thus all existing example code (I could find anyway) breaks down.

I change the bit of code that waits for 37 bytes of data to wait for 42 bytes, and set the error check to verify 42 were received. Everything works. Wow. That was hard. Here's the final higher resolution image sent over hardware serial. i.e. MmBots first sight....

MmBot's first sight

I'm glad it was me.

Time for a rest. 6 hours is a long time to work out it should be 42 not 37. That's programming for you though. Next up I'll wrap the camera in my own nicer api, do some performance tests and hopefully get 2 camera shots (1 from each eye) being sent from MmBot to PC.

-Chris