camera.c

一个机器人开发的相关嵌入式开发源码

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *  camera.c - interface code for C328 camera module
 *    Copyright (C) 2005-2006  Surveyor Corporation
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details (www.gnu.org/licenses)
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#include "camera.h"

unsigned char initial_80[] =   { 0xAA, 0x01, 0x00, 0x07, 0x01, 0x01 };
unsigned char initial_160[] =  { 0xAA, 0x01, 0x00, 0x07, 0x03, 0x03 };
unsigned char initial_320[] =  { 0xAA, 0x01, 0x00, 0x07, 0x05, 0x05 };
unsigned char initial_640[] =  { 0xAA, 0x01, 0x00, 0x07, 0x07, 0x07 };
unsigned char reset[] =        { 0xAA, 0x08, 0x01, 0x00, 0x00, 0xFF };
unsigned char set_baud_921k[] ={ 0xAA, 0x07, 0x01, 0x01, 0x00, 0x00 };
unsigned char get_preview[] =  { 0xAA, 0x04, 0x05, 0x00, 0x00, 0x00 };  
unsigned char set_pkg_size[] = { 0xAA, 0x06, 0x08, 0x00, 0x02, 0x00 };
unsigned char sync[] =         { 0xAA, 0x0D, 0x00, 0x00, 0x00, 0x00 };
unsigned char ack[] =          { 0xAA, 0x0E, 0x00, 0x00, 0x00, 0x00 };
  
unsigned char recv_buf[6];

/*****************************************************************************
 *
 * Description:
 *    Wait for ACK from C328
 *
 ****************************************************************************/
unsigned char
wait_for_ack(unsigned char *rbuf)
{
  unsigned int ix, t0;
  unsigned char rChar;
  
  // look for ACK
  for (ix=0; ix<6; ix++)
    rbuf[ix] = 0;
  ix = 0;
  t0 = TIMER0_TC;
    
  while (ix < 6 && ((TIMER0_TC-t0) < 100)) 
  {  
    if (uart1GetChar(&rChar) == TRUE) 
    {
      //uart0SendChar('a');
      //printNumber(16, 2, FALSE, '0', rChar);
      if (ix == 0 && rChar != 0xAA)
        continue;
      else
          rbuf[ix++] = rChar; 
    }
  }
  
  if ((ix < 6) || (rbuf[1] != 0x0E))
    return 0;
  else
    return rbuf[2];
}

/*****************************************************************************
 *
 * Description:
 *    Wait for SYNC from C328
 *
 ****************************************************************************/
unsigned char
wait_for_sync(unsigned char *rbuf)
{
  unsigned int ix, t0;
  unsigned char rChar;
  
  for (ix=0; ix<6; ix++)
    rbuf[ix] = 0;
  ix = 0;
  t0 = TIMER0_TC;
    
  while (ix < 6 && ((TIMER0_TC - t0) < 100)) // 100 millisecond timeout
  {  
    if (uart1GetChar(&rChar) == TRUE) 
    {
      //uart0SendChar('s');
      //printNumber(16, 2, FALSE, '0', rChar);
      if (ix == 0 && rChar != 0xAA)
        continue;
      else
          rbuf[ix++] = rChar; 
    }
  }
  
  if ((ix < 6) || (rbuf[1] != 0x0D))
    return 0;
  else
    return 1;
}

/*****************************************************************************
 *
 * Description:
 *    Wait for DATA packet from C328
 *
 ****************************************************************************/
unsigned int
wait_for_data(unsigned char *rbuf)
{
  unsigned int ix, t0;
  unsigned char rChar;
  
  for (ix=0; ix<6; ix++)
    rbuf[ix] = 0;
  ix = 0;
  t0 = TIMER0_TC;
    
  while (ix < 6 && ((TIMER0_TC - t0) < 250)) // 200 millisecond timeout
  {  
    if (uart1GetChar(&rChar) == TRUE) 
    {
      //uart0SendChar('d');
      //printNumber(16, 2, FALSE, '0', rChar);
      if (ix == 0 && rChar != 0xAA)
        continue;
      else
          rbuf[ix++] = rChar; 
    }
  }
  
  if ((ix < 6) || (rbuf[1] != 0x0A))
    return 0;
  else
    return (rbuf[3] + 256*(unsigned int)rbuf[4]);
}


/*****************************************************************************
 *
 * Description:
 *    reset C328
 *
 ****************************************************************************/

void camera_reset(unsigned char *sv)
{
  int ix, t0;
  
  for (ix=0; ix<3; ix++) 
  {
    uart1SendChars(reset, 6);
    if (wait_for_ack(recv_buf) == 0x08) 
    {
      // uart0SendString("reset\n");
      break;
    }
    else
      delayUs(1000000);
  }
  
  // set up camera
  t0 = TIMER0_TC;
  while (1)
  {
    delayUs(10000);
    // uart0SendChar('.');
    
    // send SYNC
    uart1SendChars(sync, 6);

    if (wait_for_ack(recv_buf) != 0x0D) 
      continue;
    
    if (wait_for_sync(recv_buf)) 
    {
      // send ACK back to camera
      ack[2] = 0x0D;
      uart1SendChars(ack, 6);
      break;
    }
  }

  uart1SendChars(set_baud_921k, 6);
  while (wait_for_ack(recv_buf) != 0x07)
  {
    // uart0SendChar(',');
    delayUs(10000);
    uart1SendChars(set_baud_921k, 6);
  }

  initUart1(B921600((CRYSTAL_FREQUENCY * PLL_FACTOR) / VPBDIV_FACTOR), UART_8N1, UART_FIFO_16);
  
  delayUs(100000);

  switch (sv[3])
  {
    case '1':
      uart1SendChars(initial_80, 6);
      while (wait_for_ack(recv_buf) != 0x01) 
        uart1SendChars(initial_80, 6);
      break;
    case '3':
      uart1SendChars(initial_160, 6);
      while (wait_for_ack(recv_buf) != 0x01) 
        uart1SendChars(initial_160, 6);
      break;
    case '5':
      uart1SendChars(initial_320, 6);
      while (wait_for_ack(recv_buf) != 0x01) 
        uart1SendChars(initial_320, 6);
      break;
    }

  uart1SendChars(set_pkg_size, 6);
  while (wait_for_ack(recv_buf) != 0x06) 
    uart1SendChars(set_pkg_size, 6);

}


/*****************************************************************************
 *
 * Description:
 *    Set C328 JPEG resolution
 *
 ****************************************************************************/
unsigned int camera_set_resolution(unsigned char cx)
{
  int t0;

  switch (cx)
  {
    case '1':   
      uart1SendChars(initial_80, 6);
      t0 = TIMER0_TC;
      while (wait_for_ack(recv_buf) != 0x01) 
      {
        uart1SendChars(initial_80, 6);
        if ((TIMER0_TC - t0) > 1000)
          return RESTART;
      }
      return SUCCESS;
    case '3':
      uart1SendChars(initial_160, 6);
      t0 = TIMER0_TC;
      while (wait_for_ack(recv_buf) != 0x01) 
      {
        uart1SendChars(initial_160, 6);
        if ((TIMER0_TC - t0) > 1000)
          return RESTART;
      }
      return SUCCESS;      
    case '5':
      uart1SendChars(initial_320, 6);
      t0 = TIMER0_TC;
      while (wait_for_ack(recv_buf) != 0x01) 
      {
        uart1SendChars(initial_320, 6);
        if ((TIMER0_TC - t0) > 1000)
          return RESTART;
      }
      return SUCCESS;      
    default:
      return RESTART;
  }
}


/*****************************************************************************
 *
 * Description:
 *    grab JPEG frame from C328
 *
 ****************************************************************************/
int camera_grab_frame(unsigned char *cam_buf)
{
  unsigned int jpeg_size, jpeg_pkg, jpeg_runt, last_pkg;

  unsigned int ix, jx, t0;
  unsigned char rxChar;  
  
  jpeg_size = 0;
    
  uart1SendChars(get_preview, 6);
  rxChar = wait_for_ack(recv_buf);
  if (rxChar == 0x04) 
  {
    t0 = TIMER0_TC;
    while (!(jpeg_size=wait_for_data(recv_buf)))
      if ((TIMER0_TC - t0) > 500)
        return RETRY;
  }
  else if (rxChar == 0x05)  // bug in older C328 firmware returns jpeg_size too soon
  {
    jpeg_size = recv_buf[3] + 256*(unsigned int)recv_buf[4];  
  }      
  else 
    return RETRY;
    
  // get picture after first clearing buffer
  for (ix=0; ix<C328_BUFSIZE; ix++)
    cam_buf[ix] = 0;
      
  jpeg_size = (jpeg_size + 3) & 0xFFFFFFFC;  // size is always a multiple of 4

  ack[2] = 0;
  uart1SendChars(ack, 6);
    
  ix = 0;
  last_pkg = jpeg_size / 506;
  jpeg_runt = (jpeg_size % 506) + 6;
  for (jpeg_pkg=0; jpeg_pkg<(last_pkg+1); jpeg_pkg++)
  {
    if (jpeg_pkg < last_pkg)
    {
      jx = 0;
      t0 = TIMER0_TC;
      while ((jx < 512) && ((TIMER0_TC - t0) < 500))
      //while ((jx < 512))
      {
        if (uart1GetChar(&rxChar))
        {
          jx++;
          if ((jx > 4) && (jx < 511))
          {
            cam_buf[ix++] = rxChar;
          }
        }
      }
    }
    else
    {
      jx = 0;
      t0 = TIMER0_TC;
      while ((jx < jpeg_runt) && ((TIMER0_TC - t0) < 500))
      //while ((jx < jpeg_runt))
      {
        if (uart1GetChar(&rxChar))
        {
          jx++;
          if ((jx > 4) && (jx < (jpeg_runt-1)))
          {
            cam_buf[ix++] = rxChar;
          }
          if ((cam_buf[ix-2] == 0xFF) && (cam_buf[ix-1] == 0xD9))  // end of image
          {
            while (jx++ < jpeg_runt)  // send nulls to fill out frame
              cam_buf[ix++] = 0x00;
            break;
          }
        }
      }
    }
  
    ack[4] = jpeg_pkg + 1;
    uart1SendChars(ack, 6);
  }

  ack[4] = 0xF0;
  ack[5] = 0xF0;
  uart1SendChars(ack, 6);
  ack[4] = 0;
  ack[5] = 0;  // reset ack packet
  

  return (int)jpeg_size;
}