rtu_linux_modules/dh_takephoto/takephoto.c

#include "takephoto.h"
#include "stb_image_write.h"

// 模块名称
static const char MODULE_NAME[] = "HrTakePhoto";

// 相机类型
static unsigned int device_type =  interfaceTypeGige | interfaceTypeUsb3;

// 制造厂商
static const char * const manufacturer = "Huaray Technology";

// 触发模式
static const char * const setTriggerMode = "Off"; /// 默认: "Off",  可选: "On"

// 曝光取图
#define AE_WAIT_MAX_MS     180000 // 最大曝光稳定等待的时间, 默认3分钟, 单位: ms
#define AE_EXP_TIME_EPS_US     50 // 曝光时间稳定判定的误差, 防抖&区间, 单位: us
#define AE_EXP_STABLE_FRAMES    4 // 相机处于自动曝光模式时, 累计曝光值稳定的帧数
static int waitAEDone = 1; // 是否等待自动曝光完成: "1"-等待(默认), "0"-不等待//

// 保存照片
static int SavePhoto(HANDLE hCam, IMV_Frame *pFrame, DHImgType imgType, const char *imgPath);

// 拍照环境
typedef struct
{
  HANDLE      hCam;           // 打开相机的句柄
  uint32_t    camType;        // 相机类型:U、网
  char manuName[MAX_LINE_CHARS]; //制造厂商名称
  DHImgType   saveImgType;    // 保存图像的类型
  const char *saveImgPath;    // 保存图像的路径
  unsigned int imgWidth;      // 拍照图像宽度px
  unsigned int imgHeight;     // 拍照图像高度px
  bool        isExposureAuto; // 自动曝光:是/否
  struct timespec expTime0;   // 曝光开始的时间
  float       lastExpTime;    // 上次的曝光时长(us)
  int         expStableCnt;   // 连续曝光稳定帧计数
  HANDLE      hESig;          // 任务结束的通知
  int         rCode;          // 任务结束返回值
  void       *pUser;          // 可选的用户数据
} PthotoProcCtx;

// 非标准的"Huaray Technology"制造商名称, 但也可视为华睿的
static const char * const huaray_manu_aliases[] = { "Machine Vision", NULL };
static bool is_huaray_manu_alias(const char *name)
{
  for(int i = 0; huaray_manu_aliases[i] != NULL; i++)
  {
    if(xstrcasecmp(name, huaray_manu_aliases[i]) == 0) return true;
  }
  return false;
}

static void OnFrameReceived(IMV_Frame *pFrame, void *pUser) // 数据帧的回调, 完成一次拍照任务
{
  PthotoProcCtx *ctx = (PthotoProcCtx *)pUser; int ret = IMV_OK;
  if(NULL == pFrame) return;

  // 1, 打印该帧信息
  sw_log_info("[%s] +++GetOneFrame+++, Width[%u], Height[%u], FrameNum[%llu], FrameLen[%u], PixelType[0x%08X]", MODULE_NAME,
              pFrame->frameInfo.width, pFrame->frameInfo.height, pFrame->frameInfo.blockId, pFrame->frameInfo.size,
              (unsigned int)pFrame->frameInfo.pixelFormat);

  // 2, 自动曝光模式
  if(ctx->isExposureAuto && waitAEDone != 0)
  {
    double curExpTime = 0.0;
    ret = IMV_GetDoubleFeatureValue(ctx->hCam, "ExposureTime", &curExpTime);
    if(IMV_OK != ret) goto end_p;

    struct timespec now; clock_gettime(CLOCK_MONOTONIC, &now);
    long elapsed = (now.tv_sec - ctx->expTime0.tv_sec)*1000 + (now.tv_nsec-ctx->expTime0.tv_nsec)/(1000*1000); // 计算时间差, 单位: ms
    sw_log_info("[%s] +++GetOneFrame+++, 等待曝光完成, ExposureTime = %.2fus, ElapsedTime = %ldms", MODULE_NAME, curExpTime, elapsed);

    if(fabsf(curExpTime - ctx->lastExpTime) <= AE_EXP_TIME_EPS_US) ctx->expStableCnt++;
    else ctx->expStableCnt = 0;

    if(ctx->expStableCnt < AE_EXP_STABLE_FRAMES && elapsed < AE_WAIT_MAX_MS)
    { // 曝光未稳定, 也未超时, 则继续等下一帧
      ctx->lastExpTime = curExpTime; if(0 == ctx->expStableCnt) ctx->expStableCnt = 1;
      return;
    }
  }

  // 3, 导出图像文件
  if(ctx->pUser) { sw_thrd_destroy(ctx->pUser, WAITTHRD_SAFEEXIT_TIMEOUT); ctx->pUser = NULL; }
  ctx->imgWidth = pFrame->frameInfo.width; ctx->imgHeight = pFrame->frameInfo.height;
  ret = SavePhoto(ctx->hCam, pFrame, ctx->saveImgType, ctx->saveImgPath);

  // 4, 设置拍照完成
end_p:
  ctx->rCode = ret; sw_signal_give(ctx->hESig);
}

static int FrameSoftTrigger(unsigned long wParam, unsigned long lParam) // 线程回调函数, 触发相机拍照
{
  PthotoProcCtx *ctx = (PthotoProcCtx *)wParam;
  void *hThrd = ctx->pUser; int slot_ms = 100, tick_ms, ret;

  while(sw_thrd_isAlive(hThrd))
  {
    sw_log_info("[%s] 触发一次拍照", MODULE_NAME);
    ret = IMV_ExecuteCommandFeature(ctx->hCam, "TriggerSoftware");
    if(IMV_OK != ret) { sw_log_error("[%s] 触发拍照失败, errCode=%d!!", MODULE_NAME,  ret); break; }
    else tick_ms = 0;
    while(sw_thrd_isAlive(hThrd) && tick_ms < 1000) { sw_thrd_delay(slot_ms); tick_ms += slot_ms; }
  }

  return -1;
}

// 单次拍照时, 如果相机处于自动曝光模式, 是否等待曝光稳定后再拍照
int DH_SetWaitAEDoneForTakePhoto(int waitAE)
{
  waitAEDone = (waitAE != 0);
  return 0;
}

// 单次执行相机拍照, 并保存到文件, 成功返回: 0值, 失败返回:非0值
// "imgType"     - 获取图像类型
// "saveImgPath" - 保存的文件名
// "timeout"     - 等待超时时间, 单位：秒: < 0 表示无超时;
//                 无论超时怎么设置, 首次尝试拍照一定会执行
//                 , 但时间不确定; 拍照成功、超时或发生错误
//                 时会自动结束任务(相机持续无数据的时间).
// "pImgMark"    - 输出本次拍照图像的水印信息, 可以设置NULL
int DH_TakePhoto(DHImgType imgType, const char *saveImgPath, int timeout, DHImgMark *pImgMark)
{
  int fd; char runDir[MAX_PATH_CHARS] = { 0 }, lockFile[MAX_PATH_CHARS+32] = { 0 };
  int ret, index = 0; DHImgMark imgMark = { 0 }; HANDLE hCam = NULL; PthotoProcCtx ctx = { 0 };
  IMV_DeviceList devList = { 0 }; IMV_DeviceInfo *pDevInfo = NULL; IMV_String exposureMode =  { 0 };

  // 1, 占用锁定, 避免同时间拍照
  xGetSelfRunningInfo(runDir, NULL);
  if(runDir[strlen(runDir)-1] == '/') sprintf(lockFile, "%s%s", runDir, "status/");
  else sprintf(lockFile, "%s/%s", runDir, "status/");
  if(!sw_dir_exists(lockFile)) sw_dir_create(lockFile);
  strcat(lockFile, "dh_takephoto.lock");

  fd = open(lockFile, O_CREAT | O_RDWR | __O_CLOEXEC, 0666);
  if(-1 == fd) { return -1; }

  if(-1 == flock(fd, LOCK_EX | LOCK_NB)) { close(fd); return -2; }

  // 2, 查找相机, 获取其设备信息
  ret = IMV_EnumDevices(&devList, device_type);
  if(IMV_OK != ret)
  {
    sw_log_error("[%s] 枚举相机,执行错误, errCode=%d!!", MODULE_NAME, ret);
    goto end_p;
  }
  if(devList.nDevNum < 1)
  {
    ret = -3;
    sw_log_error("[%s] 没有相机,数量=%u!!", MODULE_NAME, devList.nDevNum);
    goto end_p;
  }

findp:
  pDevInfo = &devList.pDevInfo[index++];
  if(!pDevInfo)
  {
    ret = -4;
    sw_log_error("[%s] unexpected internal error, failed to obtain camera information!!", MODULE_NAME);
    goto end_p;
  }

  if(pDevInfo->nCameraType == typeU3vCamera) ctx.camType = interfaceTypeUsb3;
  else if(pDevInfo->nCameraType == typeGigeCamera) ctx.camType = interfaceTypeGige;
  else { // SDK出错, 我只枚举了U口和G口相机, 不可能出现其他类型
    ret = -5;
    sw_log_error("[%s] unexpected internal error, unknown camera type(%u)!!", MODULE_NAME, pDevInfo->nCameraType);
    goto end_p;
  }
  strcpy(ctx.manuName, pDevInfo->vendorName);
  strcpy(imgMark.camModelName, pDevInfo->modelName);
  strcpy(imgMark.camSerialNum, pDevInfo->serialNumber);

  if(xstrcasecmp(ctx.manuName, manufacturer) != 0 && !is_huaray_manu_alias(ctx.manuName))
  {
    if(index < devList.nDevNum) goto findp;
    ret = -6;
    sw_log_error("[%s] 没有找到 \"%s\" 的相机!!", MODULE_NAME, manufacturer);
    goto end_p;
  }

  // 3, 打开相机, 设置其触发模式
  unsigned int cameraIndex = (index - 1);
  ret = IMV_CreateHandle(&hCam, modeByIndex, (void*)&cameraIndex);
  if(IMV_OK == ret) ret = IMV_Open(hCam);
  if(IMV_OK != ret)
  {
    sw_log_error("[%s] 打开相机时发生错误, errCode=%d!!", MODULE_NAME,  ret);
    goto end_p;
  }

  if(IMV_OK == ret && xstrcasecmp(setTriggerMode, "On") == 0) ret = IMV_SetEnumFeatureSymbol(hCam, "TriggerSelector", "FrameStart"); // 设置触发条件
  if(IMV_OK == ret && xstrcasecmp(setTriggerMode, "On") == 0) ret = IMV_SetEnumFeatureSymbol(hCam, "TriggerSource", "Software");     // 设置软件触发
  if(IMV_OK == ret) ret = IMV_SetEnumFeatureSymbol(hCam, "TriggerMode", setTriggerMode); // 设置触发模式
  if(IMV_OK != ret)
  {
    sw_log_error("[%s] 设置相机时发生错误, errCode=%d!!", MODULE_NAME,  ret);
    goto end_p;
  }

  // 4, 开始拍照, 等待完成后输出
  ret = IMV_GetEnumFeatureSymbol(hCam, "ExposureAuto", &exposureMode);
  if(IMV_OK != ret)
  {
    sw_log_error("[%s] 获取曝光模式时出错, errCode=%d!!", MODULE_NAME,  ret);
    goto end_p;
  }
  if(xstrcasecmp(exposureMode.str, "Off") != 0) { ctx.isExposureAuto = true; clock_gettime(CLOCK_MONOTONIC, &ctx.expTime0); }
  else ctx.isExposureAuto = false;

  ctx.hCam = hCam;
  ctx.saveImgType = imgType;
  ctx.saveImgPath = saveImgPath;
  ctx.hESig = sw_signal_create();
  if(!ctx.hESig)
  {
    ret = -7;
    sw_log_error("[%s] SIG信号量创建失败!!", MODULE_NAME);
    goto end_p;
  }
  ctx.rCode = IMV_OK;

  ret = IMV_AttachGrabbing(hCam, OnFrameReceived, (void *)&ctx);
  if(IMV_OK != ret)
  {
    sw_log_error("[%s] 注册回调时发生错误, errCode=%d!!", MODULE_NAME,  ret);
    goto end_p;
  }

  ret = IMV_StartGrabbing(hCam);
  if(IMV_OK != ret)
  {
    sw_log_error("[%s] 相机取流时发生错误, errCode=%d!!", MODULE_NAME,  ret);
    goto end_p;
  }

  HANDLE hThrd = NULL;
  if(0 != xstrcasecmp(setTriggerMode, "On")) goto waitp;

  hThrd = sw_thrd_create("PhotoProc", THREAD_DEFAULT_PRIORITY, THREAD_DEFAULT_STACK_SIZE, FrameSoftTrigger, (unsigned long)&ctx, 0);
  if(!hThrd)
  {
    ret = -8;
    sw_signal_destroy(ctx.hESig);
    sw_log_error("[%s] 触发线程-创建失败!!", MODULE_NAME);
    goto end_p;
  }
  ctx.pUser = (void *)hThrd; sw_thrd_resume(hThrd);

waitp:
  ret = sw_signal_wait(ctx.hESig, timeout*1000); // 阻塞等待拍照任务结束或超时
  if(0 == ret) ret = ctx.rCode;
  else
  {
    ret = -9;
    sw_log_error("[%s] 拍照过程-等待超时!!", MODULE_NAME);
  }

  if(ctx.pUser) sw_thrd_destroy(ctx.pUser, WAITTHRD_SAFEEXIT_TIMEOUT);
  sw_signal_destroy(ctx.hESig);

  // 5, 成功拍照, 输出相机的信息
  if(IMV_OK == ret)
  {
    double curExpTime = 0.0;
    IMV_GetDoubleFeatureValue(hCam, "ExposureTime", &curExpTime);
    imgMark.imgExposureTime = curExpTime;
    imgMark.imgWidth = ctx.imgWidth; imgMark.imgHeight = ctx.imgHeight;
    if(pImgMark) memcpy(pImgMark, &imgMark, sizeof(DHImgMark));
  }

  // 6, 任务结束, 释放相关的资源
end_p:
  if(hCam)
  {
    IMV_StopGrabbing(hCam);
    IMV_Close(hCam);
    IMV_DestroyHandle(hCam);
  }

  flock(fd, LOCK_UN);
  close(fd);
  sw_file_delete(lockFile);

  return ret;
}

// 保存给定的数据帧到BMP图像文件, 成功返回: 0值, 失败返回: 非0值
static int SaveFrameToBmp(IMV_HANDLE hCam, IMV_Frame *pFrame, const char *path)
{
  IMV_PixelConvertParam stPixelConvertParam; int ret = IMV_OK;
  unsigned char *pDstBuf = NULL; unsigned int nDstBufSize = 0;

  nDstBufSize = sizeof(unsigned char) * pFrame->frameInfo.width * pFrame->frameInfo.height * 3;
  pDstBuf = (unsigned char*)malloc(nDstBufSize);
  if(!pDstBuf) return -18;

  memset(&stPixelConvertParam, 0, sizeof(stPixelConvertParam));
	stPixelConvertParam.nWidth = pFrame->frameInfo.width;
	stPixelConvertParam.nHeight = pFrame->frameInfo.height;
	stPixelConvertParam.ePixelFormat = pFrame->frameInfo.pixelFormat;
	stPixelConvertParam.pSrcData = pFrame->pData;
	stPixelConvertParam.nSrcDataLen = pFrame->frameInfo.size;
	stPixelConvertParam.nPaddingX = pFrame->frameInfo.paddingX;
	stPixelConvertParam.nPaddingY = pFrame->frameInfo.paddingY;
	stPixelConvertParam.eBayerDemosaic = demosaicNearestNeighbor;
	stPixelConvertParam.eDstPixelFormat = gvspPixelRGB8;
	stPixelConvertParam.pDstBuf = pDstBuf;
	stPixelConvertParam.nDstBufSize = nDstBufSize;

  ret = IMV_PixelConvert(hCam, &stPixelConvertParam);
  if(IMV_OK != ret) goto ret_p;

  ret = stbi_write_bmp(path, pFrame->frameInfo.width, pFrame->frameInfo.height, 3, pDstBuf);
  if(1 == ret) ret = IMV_OK;
  else ret = -19;

ret_p:
  if(pDstBuf) free(pDstBuf);
  return ret;
}

// 保存给定的数据帧到JPG图像文件, 成功返回: 0值, 失败返回: 非0值
static int SaveFrameToJpg(IMV_HANDLE hCam, IMV_Frame *pFrame, const char *path)
{
  IMV_PixelConvertParam stPixelConvertParam; int ret = IMV_OK;
  unsigned char *pDstBuf = NULL; unsigned int nDstBufSize = 0;

  nDstBufSize = sizeof(unsigned char) * pFrame->frameInfo.width * pFrame->frameInfo.height * 3;
  pDstBuf = (unsigned char*)malloc(nDstBufSize);
  if(!pDstBuf) return -18;

  memset(&stPixelConvertParam, 0, sizeof(stPixelConvertParam));
	stPixelConvertParam.nWidth = pFrame->frameInfo.width;
	stPixelConvertParam.nHeight = pFrame->frameInfo.height;
	stPixelConvertParam.ePixelFormat = pFrame->frameInfo.pixelFormat;
	stPixelConvertParam.pSrcData = pFrame->pData;
	stPixelConvertParam.nSrcDataLen = pFrame->frameInfo.size;
	stPixelConvertParam.nPaddingX = pFrame->frameInfo.paddingX;
	stPixelConvertParam.nPaddingY = pFrame->frameInfo.paddingY;
	stPixelConvertParam.eBayerDemosaic = demosaicNearestNeighbor;
	stPixelConvertParam.eDstPixelFormat = gvspPixelRGB8;
	stPixelConvertParam.pDstBuf = pDstBuf;
	stPixelConvertParam.nDstBufSize = nDstBufSize;

  ret = IMV_PixelConvert(hCam, &stPixelConvertParam);
  if(IMV_OK != ret) goto ret_p;

  ret = stbi_write_jpg(path, pFrame->frameInfo.width, pFrame->frameInfo.height, 3, pDstBuf, 90);
  if(1 == ret) ret = IMV_OK;
  else ret = -19;

ret_p:
  if(pDstBuf) free(pDstBuf);
  return ret;
}

// 保存给定的数据帧到指定格式("BMP"/"JPG")的图像文件,成功返回0值
static int SavePhoto(HANDLE hCam, IMV_Frame *pFrame, DHImgType imgType, const char *imgPath)
{
  char *path1 = (char *)imgPath, *path2 = NULL; int ret = IMV_OK; char ext[5];

  if(!hCam || !pFrame || !path1 || strlen(path1) <= 0) return -15;

  switch(imgType)
  {
   case IMG_TYPE_BMP: // 保存为.bmp格式
    strcpy(ext, ".bmp");
    break;
   case IMG_TYPE_JPG: // 保存为.jpg格式
    strcpy(ext, ".jpg");
    break;
   default: return -16;
  }

  if(!xstrcasestr(path1, "right", ext))
  {
    path2 = (char *)sw_heap_malloc(strlen(path1) + sizeof(ext));
    if(path2) sprintf(path2, "%s%s", path1, ext);
    else return -17;
    if(imgType == IMG_TYPE_BMP) ret = SaveFrameToBmp(hCam, pFrame, path2);
    if(imgType == IMG_TYPE_JPG) ret = SaveFrameToJpg(hCam, pFrame, path2);
    sw_heap_free(path2);
  }
  else
  {
    if(imgType == IMG_TYPE_BMP) ret = SaveFrameToBmp(hCam, pFrame, path1);
    if(imgType == IMG_TYPE_JPG) ret = SaveFrameToJpg(hCam, pFrame, path1);
  }

  return ret;
}

// 获取系统当前USBFS内存大小(MB), 成功返回: >=0, 失败返回: <0值
int DH_GetSysUsbfsMemCurrentSize()
{
  const char *usbfsFile = "/sys/module/usbcore/parameters/usbfs_memory_mb";
  char buf[MAX_LINE_CHARS]; int val = -1;
  FILE *fp = fopen(usbfsFile, "r");
  if(fp)
  {
    if(fgets(buf, sizeof(buf), fp)) sscanf(buf, "%d", &val);
    fclose(fp);
  }
  return val;
}

// 设置系统新的USBFS内存大小(MB), 成功返回: 0值, 失败返回: <0值
int DH_SetSysUsbfsMemSize(int val)
{
  const char *usbfsFile = "/sys/module/usbcore/parameters/usbfs_memory_mb";
  char buf[MAX_LINE_CHARS]; sprintf(buf, "%d\n", val);
  if(val >= 0 && sw_file_update(usbfsFile, "w", buf, strlen(buf)) == strlen(buf)) return 0;
  else return -1;
}

// 获取当前已连接的海康相机的数量, 成功返回: >=0, 失败返回: <0值
int DH_GetCameraCount()
{
  IMV_DeviceList devList = { 0 };
  int ret;

  ret = IMV_EnumDevices(&devList, device_type);
  if(IMV_OK != ret)
  {
    sw_log_error("[%s] 枚举相机,执行错误, errCode=%d!!", MODULE_NAME, ret);
    return -1;
  }

  ret = devList.nDevNum;

  if(ret > 0)
  {
    int index = 0, cnt = 0; IMV_DeviceInfo *pDevInfo; char name[MAX_LINE_CHARS];
findp:
    pDevInfo = &devList.pDevInfo[index++];

    name[0] = '\0';
    if(pDevInfo && (pDevInfo->nCameraType == typeU3vCamera || pDevInfo->nCameraType == typeGigeCamera)) strcpy(name, pDevInfo->vendorName);

    if(xstrcasecmp(name, manufacturer) == 0 || is_huaray_manu_alias(name)) cnt++;
    if(index < devList.nDevNum) goto findp;
    else ret = cnt;
  }

  return ret;
}