CameraC328R.cpp

/**
 * Copyright 2009
 * Sean Voisen <http://gizmologi.st>
 * Beatriz da Costa <http://beatrizdacosta.net>
 *
 * Thanks to Jerome Despatis <jerome@despatis.com> for initial testing, 
 * improvements and debugging.
 *
 * VERSION 003
 * 2009/09/19 - John Jarvis <http://jarv.org>
 *   - fixed issue with last data packet being less than packageSize in getJPEGPicture
 *   - added checksum validation for detecting errors
 *
 * Based on the .NET driver authored by Pavel Bansky <http://bansky.net>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "CameraC328R.h"

/******************************************************************************
 * Constants
 *****************************************************************************/
static const byte CMD_PREFIX = 0xAA;
static const byte CMD_SYNC = 0x0D;
static const byte CMD_ACK = 0x0E;
static const byte CMD_NAK = 0x0F;
static const byte CMD_INITIAL = 0x01;
static const byte CMD_DATA = 0x0A;
static const byte CMD_RESET = 0x08;
static const byte CMD_POWEROFF = 0x09;
static const byte CMD_BAUDRATE = 0x07;
static const byte CMD_PACKAGESIZE = 0x06;
static const byte CMD_SNAPSHOT = 0x05;
static const byte CMD_GETPICTURE = 0x04;
static const byte CMD_LIGHTFREQ = 0x13;

static const uint16_t LAST_JPEG_ACK = 0xF0F0;
static const byte RAW_ACK = 0x0A;

/******************************************************************************
 * Methods
 *****************************************************************************/

/**
 * Constructor.
 */
CameraC328R::CameraC328R( const HardwareSerial& sserial )
  : _serial( sserial )
{
  _packageSize = DEFAULT_PACKAGE_SIZE;
}

/**
 * Performs synchronization with the camera. Synchronization will be attempted
 * up to MAX_SYNC_ATTEMPTS. Before you can do anything with the camera, you
 * must synchronize! Call this method first.
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::sync()
{
  uint8_t attempts = 0;
  bool success;

  // Create the sync command
  createCommand( CMD_SYNC, 0, 0, 0, 0 );

  while( attempts < MAX_SYNC_ATTEMPTS )
  {
    // Send a SYNC command
    sendCommand();

    // Wait for ACK response
    success = waitForACK( RESPONSE_DELAY, CMD_SYNC );

    // Make sure it is an ACK
    if( success )
    {
      // Now wait for a SYNC
      success = waitForResponse( RESPONSE_DELAY );
      if( success && _receive_cmd[1] == CMD_SYNC )
      {
        // All good, flush the buffer
        _serial.flush();

        // Now send an ACK
        createCommand( CMD_ACK, CMD_SYNC, 0, 0, 0 );
        sendCommand();

        return true;
      }
    }

    attempts++;
  }

  return false;
}

/**
 * Sets up the camera's initial image size and color type. You will
 * want to call this method after synchronization to set up your photo
 * parameters before taking a snapshot.
 *
 * @param colorType The color type setting for all photos. This should
 * be a value from the ColorType enumeration.
 * @param previewResolution The resolution to use for preview images.
 * This should be a value from the PreviewResolution enumeration.
 * @param jpegResolution The resolution to use for JPEG photos. This
 * should be a value from the JPEGResolution enumeration.
 *
 * @return True if successful, false otherwise.
 */
bool CameraC328R::initial( ColorType colorType, PreviewResolution previewResolution, JPEGResolution jpegResolution )
{
  createCommand( CMD_INITIAL, 0, colorType, previewResolution, jpegResolution );
  sendCommand();

  if( waitForACK( RESPONSE_DELAY, CMD_INITIAL ) )
  {
    return true;
  }

  return false;
}

/**
 * Sets the light frequency setting.
 *
 * @param frequencyType The frequency type to use. This should be a
 * value from the FrequencyType enumeration.
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::setLightFrequency( FrequencyType frequencyType )
{
  createCommand( CMD_LIGHTFREQ, (byte)frequencyType, 0, 0, 0 );
  sendCommand();

  if( waitForACK( RESPONSE_DELAY, CMD_LIGHTFREQ ) )
  {
    return true;
  }

  return false;
}

/**
 * Sets the size of the package for image data.
 *
 * @param size The package size to use, in bytes. This can be a value between 64
 * and 512.
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::setPackageSize( uint16_t size )
{
  createCommand( CMD_PACKAGESIZE, 0x08, (byte)(size & 0xFF), (byte)(size >> 8), 0 );
  sendCommand();

  if( waitForACK( RESPONSE_DELAY, CMD_PACKAGESIZE ) )
  {
    // Store package size in instance for future reference
    _packageSize = size;
    return true;
  }

  return false;
}

/**
 * Takes a snapshot with the camera and stores it in the camera's
 * data buffer. Once a snapshot has been taken, use getJPEGPicture or
 * getRawPicture to retrieve the photo data.
 *
 * @param snapshotType The snapshot type, compressed or uncompressed. This
 * should be a value from the SnapshotType enumeration.
 * @param skipFrames The number of frames to drop before compression occurs.
 * 0 keeps the first frame, 1 the second, and so forth.
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::snapshot( SnapshotType snapshotType, uint16_t skipFrames )
{
  createCommand( CMD_SNAPSHOT, snapshotType, (byte)(skipFrames & 0xFF), (byte)(skipFrames >> 8), 0 );
  sendCommand();

  if( waitForACK( RESPONSE_DELAY, CMD_SNAPSHOT ) )
  {
    return true;
  }

  return false;
}

/**
 * Gets a snapshot from the camera in compressed JPEG format.
 * Use snapshot to take a picture first.
 *
 * @param pictureType The picture type. Should be either JPEG or snapshot
 * type from the PictureType enumeration.
 * @param processDelay The time wait to process the picture with the camera's
 * on board JPEG compression. Larger pictures can take up to 1 second.
 * @param callback This is a pointer reference to a function that returns void.
 * Because the Atmega's memory is limited, and its built-in EEPROM is even smaller,
 * use this callback to handle the chunks of image data as they arrive. Each time
 * a package containing image data is transmitted, this callback will be called.
 * You will probably want to write the data to external memory of some sort,
 * appending it each time.
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::getJPEGPicture( PictureType pictureType, uint16_t processDelay, void (*callback)( uint16_t picSize, uint16_t packPicDataSize, uint16_t packCount, byte* pack))
{
  uint16_t pictureSize = 0;

  if( !getPicture( pictureType, processDelay, pictureSize ) )
    return false;

  // Keeps track of the number of errors, will wait up till MAX_ERRORS
  uint8_t errorCount = 0;
  // Keeps track of the number of packages received
  uint16_t packageCount = 0;
  // The size of the picture data within the package
  uint16_t packagePictureDataSize = 0;
  // The size of the picture ID within the package
  uint16_t packagePictureID = 0;


  // Current byte position in the ENTIRE image
  uint16_t pictureDataPosition = 0;

  // Wait for MAX_ERRORS before aborting
  while( pictureDataPosition < pictureSize && errorCount < MAX_ERRORS )
  {
    sendACK( 0, packageCount );  // send the first ack
    // Give it time to process and transmit data
    delay( PACKAGE_DELAY );

    uint8_t package_header[PACKAGE_DATA_START];
    // Wait for the package header
    if(!waitForResponse( processDelay, package_header, sizeof(package_header)))
    {
    	errorCount = MAX_ERRORS;
    	break;
    }


    // This is the ID
    packagePictureID = package_header[1] << 8;
    packagePictureID |= package_header[0];

    // Size of the data payload
    packagePictureDataSize = package_header[3] << 8;
    packagePictureDataSize |= package_header[2];

    // Read the image data
    uint8_t package_data[packagePictureDataSize];
    if (!waitForResponse( processDelay, package_data, sizeof(package_data)))
    {
    	errorCount = MAX_ERRORS;
    	break;
    }
    // Read the checksum
    uint8_t chksum_data[PACKAGE_DATA_END_OFFSET];
    if (!waitForResponse( processDelay, chksum_data, sizeof(chksum_data)))
    {
    	errorCount = MAX_ERRORS;
    	break;
    }

    uint8_t chksum_calc = 0;

    for (uint8_t i = 0; i < sizeof(package_header) ; i++)
    {
    	chksum_calc += package_header[i];
    }

    for (uint8_t i = 0; i < sizeof(package_data); i++)
    {
    	chksum_calc += package_data[i];
    }


    if (chksum_calc == chksum_data[0])
    {
		// Call the callback
		pictureDataPosition += packagePictureDataSize;
		callback( pictureSize, packagePictureDataSize, packageCount, package_data);
		packageCount++;
    } else {
    	//invalid checksum try again
    	errorCount++;
    }

  }

  // Send last ACK
  sendACK( 0, LAST_JPEG_ACK );

  if( errorCount < MAX_ERRORS )
  {
    return true;
  }
  else
  {
    return false;
  }
}

/**
 * Resets the camera.
 *
 * @param completeReset True to completely reset the camera, otherwise
 * a "soft" reset (reset the camera state machine) will occur
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::reset( bool completeReset )
{
  createCommand( CMD_RESET, completeReset ? 0x00 : 0x01, 0, 0, 0xFF );
  sendCommand();

  if( waitForACK( RESPONSE_DELAY, CMD_RESET ) )
  {
    return true;
  }

  return false;
}

/**
 * Sets the baud rate to use with the camera. This is useful if
 * you need to change the baud rate AFTER synchronization.
 *
 * @param baudRate The baud rate to use. This should be a value from
 * the BaudRate enumeration.
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::setBaudRate( BaudRate baudRate )
{
  createCommand( CMD_BAUDRATE, (byte)baudRate, 0x01, 0, 0 );
  sendCommand();

  if( waitForACK( RESPONSE_DELAY, CMD_BAUDRATE ) )
  {
    return true;
  }

  return false;
}

/**
 * Gets a raw, uncompressed photo from the camera. Unlike getJPEGPicture,
 * this method returns the ENTIRE photo in one large data package. This
 * means that the photo must be temporarily stored in the Atmega's
 * memory. Use this method only if you have memory to spare.
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::getRawPicture( PictureType pictureType, byte pictureBuffer[], uint16_t &bufferSize, uint16_t processDelay )
{
  uint16_t pictureSize = 0;

  if( !getPicture( pictureType, processDelay, pictureSize ) )
    return false;

  if( pictureSize > bufferSize )
    return false;
  else
    bufferSize = pictureSize;

  // Wait for the package
  if( waitForResponse( processDelay, pictureBuffer, pictureSize ) ) {
    sendACK( RAW_ACK );
    return true;
  }

  return false;
}

/**
 * Power off the camera. The camera will be unusable after calling
 * this method until you successfully complete synchronization.
 *
 * @return True if successful, false otherwise
 */
bool CameraC328R::powerOff()
{
  createCommand( CMD_POWEROFF, 0, 0, 0, 0 );
  sendCommand();

  if( waitForACK( RESPONSE_DELAY, CMD_POWEROFF ) )
  {
    return true;
  }

  return false;
}

/**
 * @private
 *
 * Used by getJPEGPicture and getRawPicture to start the get picture
 * process. This method is used primarily to get the size of the picture
 * that is going to be transmitted by the camera.
 */
bool CameraC328R::getPicture( PictureType pictureType, uint16_t processDelay, uint16_t &pictureSize )
{
  pictureSize = 0;

  createCommand( CMD_GETPICTURE, pictureType, 0, 0, 0 );
  sendCommand();

  // Give the camera some time for processing
  delay( processDelay );

  if( !waitForACK( processDelay, CMD_GETPICTURE ) )
    return false;

  if( waitForResponse( processDelay ) && _receive_cmd[1] == CMD_DATA )
  {
    // Set the picture size for future reference
    pictureSize = _receive_cmd[5] << 8;
    pictureSize |= _receive_cmd[4] << 8;
    pictureSize |= _receive_cmd[3];

    return true;
  }

  return false;
}

/**
 * @private
 *
 * Waits for an ACK.
 *
 * @return True if got an ACK for the specified command, false otherwise
 */
bool CameraC328R::waitForACK( uint32_t timeout, uint8_t cmdId )
{
  bool success = waitForResponse( timeout );

  // TODO: We are ignoring NAKs here. Should we do something for this
  // specific case?
  if( success && _receive_cmd[1] == CMD_ACK && _receive_cmd[2] == cmdId )
  {
    return true;
  }

  return false;
}

/**
 * @private
 *
 * Sends the command that is stored in _command.
 */
void CameraC328R::sendCommand()
{
  uint8_t i;

  for( i = 0; i < CMD_SIZE; i++ )
  {
    _serial.print( _command[i], BYTE );
  }
}

/**
 * @private
 *
 * Simple utility method for populating the _command array.
 */
void CameraC328R::createCommand( const byte cmd, byte param1, byte param2, byte param3, byte param4 )
{
  _command[0] = CMD_PREFIX;
  _command[1] = cmd;
  _command[2] = param1;
  _command[3] = param2;
  _command[4] = param3;
  _command[5] = param4;
}

/**
 * @private
 *
 * Sends and ACK.
 */
void CameraC328R::sendACK( const byte cmd )
{
  sendACK( cmd, 0 );
}

/**
 * @private
 *
 * Sends an ACK. Used when getting data packages to send an
 * ACK for each numbered package.
 */
void CameraC328R::sendACK( const byte cmd, uint16_t packageId )
{
  createCommand( CMD_ACK, cmd, 0, (byte)(packageId & 0xFF), (byte)(packageId >> 8) );
  sendCommand();
}

/**
 * @private
 *
 * Waits for a response up to timeout, and stores response in the
 * _receive_cmd array.
 *
 * @return True if response received, false otherwise
 */
bool CameraC328R::waitForResponse( uint32_t timeout )
{
  return waitForResponse( timeout, _receive_cmd, CMD_SIZE );
}

/**
 * @private
 *
 * Waits for a response up to timeout, and stores response in the
 * buffer array provided.
 *
 * @param timeout The timeout
 * @param buffer The buffer within which to store the response
 * @param bufferLength The length of the buffer provided
 *
 * @return True if response received, false otherwise
 */
bool CameraC328R::waitForResponse( uint32_t timeout, byte buffer[], uint16_t bufferLength )
{
  uint8_t byteCnt = 0;
  unsigned long time = millis();

  while( millis() - time <= timeout )
  {
    while( _serial.available() > 0 )
    {
      buffer[byteCnt] = _serial.read();
      byteCnt++;

      if( byteCnt == bufferLength )
      {
        return true;
      }
    }
  }

  if( byteCnt > 0 )
  {
    return true;
  }

  return false;
}