diff options
| -rw-r--r-- | camera.c | 410 |
1 files changed, 274 insertions, 136 deletions
@@ -1,81 +1,162 @@ #include "camera.h" #include "legato.h" +#include <termios.h> COMPONENT_INIT { } -// File descriptor is returned in the fd pointer -le_result_t fd_openCam (int *fd) { - *fd = le_tty_Open(TTY_PATH, O_RDWR | O_NOCTTY | O_NDELAY); - le_result_t baudRes = le_tty_SetBaudRate(*fd, LE_TTY_SPEED_38400); - le_result_t rawRes = le_tty_SetRaw(*fd, CAM_BUFF_SIZE, TTY_TIMEOUT); - return baudRes == LE_OK && rawRes == LE_OK ? LE_OK : LE_FAULT; -} - -void fd_closeCam (int fd) { - le_tty_Close(fd); -} - -le_result_t fd_resetCamTty (int *fd) { - fd_closeCam(*fd); - return fd_openCam(fd); -} - +/** + * Convert an integer baud rate to a speed_t + */ +speed_t fd_convertBaud (int baud) { + speed_t b; + switch (baud) { + case 50: b = B50; break; + case 75: b = B75; break; + case 110: b = B110; break; + case 134: b = B134; break; + case 150: b = B150; break; + case 200: b = B200; break; + case 300: b = B300; break; + case 600: b = B600; break; + case 1200: b = B1200; break; + case 1800: b = B1800; break; + case 2400: b = B2400; break; + case 9600: b = B9600; break; + case 19200: b = B19200; break; + case 38400: b = B38400; break; + case 57600: b = B57600; break; + case 115200: b = B115200; break; + case 230400: b = B230400; break; + default: + b = -2; + } + return b; +} + +/** + * Open a serial connection on device + * + * Lifted from here + * https://github.com/WiringPi/WiringPi/blob/master/wiringPi/wiringSerial.c + */ +int fd_openSerial (const char *device, int baud) { + struct termios options; + speed_t binaryBaud = fd_convertBaud(baud); + int status, fd; + if ((fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK)) == -1) + return -1 ; + fcntl(fd, F_SETFL, O_RDWR); + tcgetattr(fd, &options); + cfmakeraw(&options); + cfsetispeed(&options, binaryBaud); + cfsetospeed(&options, binaryBaud); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_oflag &= ~OPOST; + + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 100; // Ten seconds (100 deciseconds) + + tcsetattr(fd, TCSANOW | TCSAFLUSH, &options); + + ioctl(fd, TIOCMGET, &status); + + status |= TIOCM_DTR; + status |= TIOCM_RTS; + + ioctl(fd, TIOCMSET, &status); + usleep(10000); // 10mS + return fd; +} + +/** + * Open a serial connection to the camera + */ +int fd_openCam () { + return fd_openSerial(TTY_PATH, CAM_BAUD_RATE); +} + +/** + * Close the serial connection to the camera + */ +int fd_closeCam (int fd) { + return close(fd); +} + +/** + * Read a byte from fd into data + */ ssize_t fd_getByte (int fd, uint8_t *data) { return read(fd, data, 1); } +/** + * Check how many bytes are available + * on the serial connection described by fd + */ int fd_dataAvail (int fd, int *data) { return ioctl(fd, FIONREAD, data); } -void sendCommand (Camera *cam, uint8_t cmd, uint8_t args[], uint8_t nArgs) { - // TODO figure out the longest possible command - // as I doubt it's anywhere near 100 - // - // The other (and probably better option) here would - // be to call malloc - uint8_t toWrite[100] = { VC0706_PREFIX, cam->serialNum, cmd }; +/** + * Send a command sequence to cam + * Should not be called directly (use cam_runCommand) + */ +void cam_sendCommand (Camera *cam, uint8_t cmd, uint8_t args[], uint8_t nArgs) { int start = 3; int end = nArgs + start; + uint8_t *toWrite = malloc(end); + *toWrite = VC0706_PREFIX; + *(toWrite + 1) = cam->serialNum; + *(toWrite + 2) = cmd; for (int i = start; i < end; i++) { - toWrite[i] = args[i - start]; + *(toWrite + i) = args[i - start]; }; - for (int i = 0; i < end; i++) { - LE_DEBUG("toSend[%d]=0x%x", i, toWrite[i]); - } - write(cam->fd, &toWrite[0], end); + write(cam->fd, toWrite, end); + free(toWrite); } -bool runCommand (Camera *cam, uint8_t cmd, uint8_t args[], uint8_t nArgs, uint8_t respLen, bool flushFlag) { +/** + * Run a command described by cmd and args on cam + */ +bool cam_runCommand (Camera *cam, uint8_t cmd, uint8_t args[], uint8_t nArgs, uint8_t respLen, bool flushFlag) { if (flushFlag) { - readResponse(cam, 100, 10); + cam_readResponse(cam, 100, 10); } - sendCommand(cam, cmd, args, nArgs); - uint8_t actual = readResponse(cam, respLen, 200); - LE_DEBUG("Expected: %d, Actual: %d", respLen, actual); - if (actual != respLen) + cam_sendCommand(cam, cmd, args, nArgs); + if (cam_readResponse(cam, respLen, 200) != respLen) return false; - if (!verifyResponse(cam, cmd)) + if (!cam_verifyResponse(cam, cmd)) return false; return true; } -bool runCommandFlush (Camera *cam, uint8_t cmd, uint8_t args[], uint8_t nArgs, uint8_t respLen) { - return runCommand(cam, cmd, args, nArgs, respLen, true); +/** + * Flush buffer before running command + */ +bool cam_runCommandFlush (Camera *cam, uint8_t cmd, uint8_t args[], uint8_t nArgs, uint8_t respLen) { + return cam_runCommand(cam, cmd, args, nArgs, respLen, true); } -// Reads from the camera and returns how many bytes it read -uint8_t readResponse (Camera *cam, uint8_t nBytes, uint8_t timeout) { +/** + * Read a response from cam + */ +uint8_t cam_readResponse (Camera *cam, uint8_t nBytes, uint8_t timeout) { uint8_t counter = 0; cam->bufferLen = 0; int avail = 0; uint8_t data; while ((counter < timeout) && (cam->bufferLen < nBytes)) { - // data is returned in avail pointer - int availRes = fd_dataAvail(cam->fd, &avail); + // available data is returned in avail pointer + bool availSuccess = fd_dataAvail(cam->fd, &avail) == 0; // this case covers no data available // or when fd_dataAvail fails - if (avail <= 0 || availRes != 0) { + if (avail <= 0 || !availSuccess) { usleep(1000); counter++; continue; @@ -88,20 +169,12 @@ uint8_t readResponse (Camera *cam, uint8_t nBytes, uint8_t timeout) { return cam->bufferLen; } -void printBuffer (Camera *cam) { - LE_DEBUG("Printing cam buffer"); - for(int i = 0; i < NUM_ARRAY_MEMBERS(cam->buff); i++) { - LE_DEBUG("buff[%d]=%x", i, cam->buff[i]); - } -} - -bool verifyResponse (Camera *cam, uint8_t cmd) { +/** + * Verify that a given response is valid + */ +bool cam_verifyResponse (Camera *cam, uint8_t cmd) { // If any of these are not equal than // the command failed - LE_DEBUG("buff[0] correct? %d", cam->buff[0] == VC0706_RESP_PREFIX); - LE_DEBUG("buff[1] correct? %d", cam->buff[1] == cam->serialNum); - LE_DEBUG("buff[2] correct? %d", cam->buff[2] == cmd); - LE_DEBUG("buff[3] correct? %d", cam->buff[3] == 0x0); return cam->buff[0] == VC0706_RESP_PREFIX && cam->buff[1] == cam->serialNum && @@ -109,59 +182,80 @@ bool verifyResponse (Camera *cam, uint8_t cmd) { cam->buff[3] == 0x0; } -bool cameraFrameBuffCtrl (Camera *cam, uint8_t cmd) { +/** + * Helper for commands related + * to controlling the frame buffer + */ +bool cam_frameBuffCtrl (Camera *cam, uint8_t cmd) { uint8_t args[] = { 0x1, cmd }; - return runCommandFlush(cam, VC0706_FBUF_CTRL, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_FBUF_CTRL, args, sizeof(args), 5); } -bool takePicture (Camera *cam) { +/** + * Stop the frame to take a photo + */ +bool cam_takePicture (Camera *cam) { cam->frameptr = 0; - return cameraFrameBuffCtrl(cam, VC0706_STOPCURRENTFRAME); + return cam_frameBuffCtrl(cam, VC0706_STOPCURRENTFRAME); } -bool reset (Camera *cam) { +/** + * Reset cam + */ +bool cam_reset (Camera *cam) { uint8_t args[] = { 0x0 }; - return runCommandFlush(cam, VC0706_RESET, args, sizeof(args), 5); + bool runRes = cam_runCommandFlush(cam, VC0706_RESET, args, sizeof(args), 5); + sleep(2); // wait for camera to come back online + return runRes; } -bool TVon (Camera *cam) { +/** + * Enable the analog TV out + */ +bool cam_tvOn (Camera *cam) { uint8_t args[] = { 0x1, 0x1 }; - return runCommandFlush(cam, VC0706_TVOUT_CTRL, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_TVOUT_CTRL, args, sizeof(args), 5); } -bool TVOff (Camera *cam) { +/** + * Disable the analog TV out + */ +bool cam_tvOff (Camera *cam) { uint8_t args[] = { 0x1, 0x0 }; - return runCommandFlush(cam, VC0706_TVOUT_CTRL, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_TVOUT_CTRL, args, sizeof(args), 5); } -uint8_t* readPicture (Camera *cam, uint8_t n) { +/** + * Request a block of size n data from cam + */ +uint8_t* cam_readPicture (Camera *cam, uint8_t n) { uint8_t args[] = { 0x0C, 0x0, 0x0A, 0, 0, cam->frameptr >> 8, cam->frameptr & 0xFF, 0, 0, 0, n, CAM_DELAY >> 8, CAM_DELAY & 0xFF }; - LE_DEBUG("frameptr: %d", cam->frameptr); - if (!runCommand(cam, VC0706_READ_FBUF, args, sizeof(args), 5, false)) { // don't flush - LE_DEBUG("Failed in runCommand"); + if (!cam_runCommand(cam, VC0706_READ_FBUF, args, sizeof(args), 5, false)) // don't flush return NULL; - } - int imgDataRead = readResponse(cam, n + 5, CAM_DELAY); - LE_DEBUG("Image data read: %d", imgDataRead); - if (imgDataRead == 0) { - LE_DEBUG("Failed in readResponse"); + if (cam_readResponse(cam, n + 5, CAM_DELAY) == 0) return NULL; - } cam->frameptr += n; return &(cam->buff[0]); } -bool resumeVideo (Camera *cam) { - return cameraFrameBuffCtrl(cam, VC0706_RESUMEFRAME); +/** + * Resume frame capture + */ +bool cam_resumeVideo (Camera *cam) { + return cam_frameBuffCtrl(cam, VC0706_RESUMEFRAME); } -uint32_t frameLength (Camera *cam) { +/** + * Get the length of the frame + * currently on cam + */ +uint32_t cam_frameLength (Camera *cam) { uint8_t args[] = { 0x01, 0x00 }; - if (!runCommandFlush(cam, VC0706_GET_FBUF_LEN, args, sizeof(args), 9)) + if (!cam_runCommandFlush(cam, VC0706_GET_FBUF_LEN, args, sizeof(args), 9)) return 0; uint32_t len; @@ -176,92 +270,124 @@ uint32_t frameLength (Camera *cam) { return len; } -char* getVersion (Camera *cam) { +/** + * Get the version from cam + */ +char* cam_getVersion (Camera *cam) { uint8_t args[] = { 0x00 }; - sendCommand(cam, VC0706_GEN_VERSION, args, sizeof(args)); - if (!readResponse(cam, CAM_BUFF_SIZE, 200)) { - LE_DEBUG("Failed to get version, returning empty string"); - return 0; - } + cam_sendCommand(cam, VC0706_GEN_VERSION, args, sizeof(args)); + if (!cam_readResponse(cam, CAM_BUFF_SIZE, 200)) + return NULL; cam->buff[cam->bufferLen] = 0; return (char*)&(cam->buff[5]); } -uint8_t available (Camera *cam) { +/** + * Get the length of cam's buffer + */ +uint8_t cam_available (Camera *cam) { return cam->bufferLen; } -uint8_t getDownsize (Camera *cam) { +/** + * Get cam's current downsize + */ +uint8_t cam_getDownsize (Camera *cam) { uint8_t args[] = { 0x0 }; - if (!runCommandFlush(cam, VC0706_DOWNSIZE_STATUS, args, sizeof(args), 6)) + if (!cam_runCommandFlush(cam, VC0706_DOWNSIZE_STATUS, args, sizeof(args), 6)) return -1; return cam->buff[5]; } -bool setDownsize (Camera *cam, uint8_t newSize) { +/** + * Set cam's downsize + */ +bool cam_setDownsize (Camera *cam, uint8_t newSize) { uint8_t args[] = { 0x01, newSize }; - return runCommandFlush(cam, VC0706_DOWNSIZE_CTRL, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_DOWNSIZE_CTRL, args, sizeof(args), 5); } -uint8_t getImageSize (Camera *cam) { +/** + * Get cam's current image size + */ +uint8_t cam_getImageSize (Camera *cam) { uint8_t args[] = { 0x4, 0x4, 0x1, 0x00, 0x19 }; - if (!runCommandFlush(cam, VC0706_READ_DATA, args, sizeof(args), 6)) + if (!cam_runCommandFlush(cam, VC0706_READ_DATA, args, sizeof(args), 6)) return -1; return cam->buff[5]; } -bool setImageSize (Camera *cam, uint8_t x) { +/** + * Set cam's image size to x + */ +bool cam_setImageSize (Camera *cam, uint8_t x) { uint8_t args[] = { 0x05, 0x04, 0x01, 0x00, 0x19, x }; - return runCommandFlush(cam, VC0706_WRITE_DATA, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_WRITE_DATA, args, sizeof(args), 5); } -bool getMotionDetect (Camera *cam) { +bool cam_getMotionDetect (Camera *cam) { uint8_t args[] = { 0x0 }; - if (!runCommandFlush(cam, VC0706_COMM_MOTION_STATUS, args, sizeof(args), 6)) + if (!cam_runCommandFlush(cam, VC0706_COMM_MOTION_STATUS, args, sizeof(args), 6)) return false; return cam->buff[5]; } -uint8_t getMotionStatus(Camera *cam, uint8_t x) { +uint8_t cam_getMotionStatus (Camera *cam, uint8_t x) { uint8_t args[] = { 0x01, x }; - return runCommandFlush(cam, VC0706_MOTION_STATUS, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_MOTION_STATUS, args, sizeof(args), 5); } -bool motionDetected (Camera *cam) { - if (readResponse(cam, 4, 200) != 4) +/** + * Return true if cam detected motion + */ +bool cam_motionDetected (Camera *cam) { + if (cam_readResponse(cam, 4, 200) != 4) return false; - if (!verifyResponse(cam, VC0706_COMM_MOTION_DETECTED)) + if (!cam_verifyResponse(cam, VC0706_COMM_MOTION_DETECTED)) return false; return true; } -bool setMotionDetect (Camera *cam, bool flag) { - if (!setMotionStatus(cam, VC0706_MOTIONCONTROL, VC0706_UARTMOTION, VC0706_ACTIVATEMOTION)) +/** + * Enable/disable motion detection on cam + */ +bool cam_setMotionDetect (Camera *cam, bool flag) { + if (!cam_setMotionStatus(cam, VC0706_MOTIONCONTROL, VC0706_UARTMOTION, VC0706_ACTIVATEMOTION)) return false; uint8_t args[] = { 0x1, flag }; - return runCommandFlush(cam, VC0706_MOTION_STATUS, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_MOTION_STATUS, args, sizeof(args), 5); } -bool setMotionStatus (Camera *cam, uint8_t x, uint8_t d1, uint8_t d2) { +bool cam_setMotionStatus (Camera *cam, uint8_t x, uint8_t d1, uint8_t d2) { uint8_t args[] = { 0x03, x, d1, d2 }; - return runCommandFlush(cam, VC0706_MOTION_CTRL, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_MOTION_CTRL, args, sizeof(args), 5); } -uint8_t getCompression (Camera *cam) { +/** + * Get cam's current level of compression + */ +uint8_t cam_getCompression (Camera *cam) { uint8_t args[] = { 0x4, 0x1, 0x1, 0x12, 0x04 }; - runCommandFlush(cam, VC0706_READ_DATA, args, sizeof(args), 6); + cam_runCommandFlush(cam, VC0706_READ_DATA, args, sizeof(args), 6); return cam->buff[5]; } -bool setCompression (Camera *cam, uint8_t c) { +/** + * Set cam's level of compression to c + */ +bool cam_setCompression (Camera *cam, uint8_t c) { uint8_t args[] = { 0x5, 0x1, 0x1, 0x12, 0x04, c }; - return runCommandFlush(cam, VC0706_WRITE_DATA, args, sizeof(args), 5); + return cam_runCommandFlush(cam, VC0706_WRITE_DATA, args, sizeof(args), 5); } -bool getPTZ(Camera *cam, uint16_t *w, uint16_t *h, uint16_t *wz, uint16_t *hz, uint16_t *pan, uint16_t *tilt) { +/** + * Get cam's current PTZ values + * All values returned in pointers + */ +bool cam_getPTZ(Camera *cam, uint16_t *w, uint16_t *h, uint16_t *wz, uint16_t *hz, uint16_t *pan, uint16_t *tilt) { uint8_t args[] = { 0x0 }; - if (!runCommandFlush(cam, VC0706_GET_ZOOM, args, sizeof(args), 16)) + if (!cam_runCommandFlush(cam, VC0706_GET_ZOOM, args, sizeof(args), 16)) return false; *w = cam->buff[5]; *w <<= 8; @@ -290,27 +416,37 @@ bool getPTZ(Camera *cam, uint16_t *w, uint16_t *h, uint16_t *wz, uint16_t *hz, u return true; } -bool setPTZ (Camera *cam, uint16_t wz, uint16_t hz, uint16_t pan, uint16_t tilt) { +/** + * Set cam's PTZ settings + */ +bool cam_setPTZ (Camera *cam, uint16_t wz, uint16_t hz, uint16_t pan, uint16_t tilt) { uint8_t args[] = { 0x08, wz >> 8, wz, hz >> 8, wz, pan >> 8, pan, tilt >> 8, tilt }; - return !runCommandFlush(cam, VC0706_SET_ZOOM, args, sizeof(args), 5); + return !cam_runCommandFlush(cam, VC0706_SET_ZOOM, args, sizeof(args), 5); } -uint8_t getImageBlockSize (int jpgLen) { +/** + * Return how much data should be requested + * from cam based on jpgLen + */ +uint8_t cam_getImageBlockSize (int jpgLen) { return CAM_BLOCK_SIZE < jpgLen ? CAM_BLOCK_SIZE : jpgLen; } -bool readImageBlock (Camera *cam, FILE *filePtr) { - int jpgLen = frameLength(cam); +/** + * Read the entire frame buffer from cam + * into filePtr + */ +bool cam_readImageBlocks (Camera *cam, FILE *filePtr) { + int jpgLen = cam_frameLength(cam); bool success = true; while (jpgLen > 0) { - uint8_t bytesToRead = getImageBlockSize(jpgLen); - LE_DEBUG("jpgLen: %d, bytesToRead: %d", jpgLen, bytesToRead); - uint8_t *buff = readPicture(cam, bytesToRead); + uint8_t bytesToRead = cam_getImageBlockSize(jpgLen); + uint8_t *buff = cam_readPicture(cam, bytesToRead); if (buff == NULL) { LE_ERROR("Failed to read image data"); success = false; @@ -318,20 +454,16 @@ bool readImageBlock (Camera *cam, FILE *filePtr) { } fwrite(buff, sizeof(*buff), bytesToRead, filePtr); jpgLen -= bytesToRead; - // TODO figure out why resetting the resetting - // the serial connection after reading a block - // keeps the camera responsive - // - // This may be a limitation in le_tty - // so using Linux system calls may be worthwhile - fd_resetCamTty(&(cam->fd)); } - fclose(filePtr); - printBuffer(cam); return success; } -bool readImageToFile (Camera *cam, char *path) { +/** + * Read the image from cam into a file + * described by path + */ +bool cam_readImageToFile (Camera *cam, char *path) { + bool success = false; char writePath[100]; // e.g /mnt/sd/<timestamp>.jpg sprintf(writePath, "%s/%d.jpg", path, (int)time(0)); @@ -339,21 +471,27 @@ bool readImageToFile (Camera *cam, char *path) { FILE *filePtr = fopen(writePath, "w"); if (filePtr != NULL) { LE_INFO("Got valid file pointer"); - return readImageBlock(cam, filePtr); + success = cam_readImageBlocks(cam, filePtr); } else { LE_ERROR("Invalid file pointer for %s", writePath); - return false; } + LE_INFO("Done writing image"); + fclose(filePtr); + return success; } -bool snapshotToFile (Camera *cam, char *path, uint8_t imgSize) { - setImageSize(cam, imgSize); +/** + * Take a photo on cam and write it to a file + * described by path + */ +bool cam_snapshotToFile (Camera *cam, char *path, uint8_t imgSize) { + cam_setImageSize(cam, imgSize); LE_INFO("Taking photo..."); - bool photoTaken = takePicture(cam); + bool photoTaken = cam_takePicture(cam); if (photoTaken) { LE_INFO("Photo taken"); - return readImageToFile(cam, path); + return cam_readImageToFile(cam, path); } else { LE_ERROR("Failed to take photo"); |