static int smartp_verbose(int fd, const char *dev)
{
int rc;
unsigned char buf[256];
struct hidraw_devinfo info;
memset(&info, 0x0, sizeof(info));
memset(buf, 0x0, sizeof(buf));
/* Get Raw Name */
rc = ioctl(fd, HIDIOCGRAWNAME(256), buf);
if (rc < 0) {
err("%s: failed to get raw name\n", dev);
return -errno;
}
printf("Raw name: %s\n", buf);
/* Get Physical Location */
rc = ioctl(fd, HIDIOCGRAWPHYS(256), buf);
if (rc < 0) {
err("%s: failed to get physical location\n", dev);
return -errno;
}
printf("Raw phys: %s\n", buf);
/* Get Raw Info */
rc = ioctl(fd, HIDIOCGRAWINFO, &info);
if (rc < 0) {
err("%s: failed to get raw info\n", dev);
return -errno;
}
printf("Raw info: bustype %d (%s), vendor 0x%04hx, product 0x%04hx\n",
info.bustype, bus_str(info.bustype), info.vendor, info.product);
return 0;
}
int main(int argc, char **argv)
{
int fd;
int i, res, desc_size = 0, count = 1;
char buf[256];
struct hidraw_report_descriptor rpt_desc;
struct hidraw_devinfo info;
memset(&rpt_desc, 0x0, sizeof(rpt_desc));
memset(&info, 0x0, sizeof(info));
memset(buf, 0x0, sizeof(buf));
memset(&update_info, 0x0, sizeof(update_info));
fd = open("/dev/hidraw0", O_RDWR);
if (fd < 0) {
perror("Unable to open /dev/hidraw0");
return 1;
} else {
perror("open /dev/hidraw0 success!");
/* Get Raw Info */
res = ioctl(fd, HIDIOCGRAWINFO, &info);
if (res < 0) {
perror("HIDIOCGRAWINFO");
} else {
printf("Raw Info:\n");
printf("\tbustype: %d (%s)\n",
info.bustype, bus_str(info.bustype));
printf("\tvendor: 0x%04hx\n", info.vendor);
printf("\tproduct: 0x%04hx\n", info.product);
}
update_info.write_fd = fd;
update_info.read_fd = fd;
}
init_data();
write_start();
update_info.keep_polling = 1;
poll_event_thread(NULL);
update_info.keep_polling = -1;
///update_info.rtkbt_thread_id = rtkbt_create_thread(poll_event_thread, NULL);
return 0;
}
void SendCameraCommand(char * device_path, uint8_t Command, uint8_t Length, uint8_t CommandValue) {
int fd;
int i, res, desc_size = 0;
char buf[256];
struct hidraw_report_descriptor rpt_desc;
struct hidraw_devinfo info;
char *device = device_path;
/* Open the Device with non-blocking reads. In real life,
don't use a hard coded path; use libudev instead. */
fd = open(device, O_RDWR|O_NONBLOCK);
if (fd < 0) {
perror("Unable to open device");
return;
}
memset(&rpt_desc, 0x0, sizeof(rpt_desc));
memset(&info, 0x0, sizeof(info));
memset(buf, 0x0, sizeof(buf));
/* Get Report Descriptor Size */
res = ioctl(fd, HIDIOCGRDESCSIZE, &desc_size);
if (res < 0) {
perror("HIDIOCGRDESCSIZE");
} else {
#if DEBUG
printf("Report Descriptor Size: %d\n", desc_size);
#endif
}
/* Get Report Descriptor */
rpt_desc.size = desc_size;
res = ioctl(fd, HIDIOCGRDESC, &rpt_desc);
if (res < 0) {
perror("HIDIOCGRDESC");
} else {
#if DEBUG
printf("Report Descriptor:\n");
for (i = 0; i < rpt_desc.size; i++)
printf("%hhx ", rpt_desc.value[i]);
puts("\n");
#endif
}
/* Get Raw Name */
res = ioctl(fd, HIDIOCGRAWNAME(256), buf);
if (res < 0) {
perror("HIDIOCGRAWNAME");
} else {
#if DEBUG
printf("Raw Name: %s\n", buf);
#endif
}
/* Get Physical Location */
res = ioctl(fd, HIDIOCGRAWPHYS(256), buf);
if (res < 0) {
perror("HIDIOCGRAWPHYS");
} else {
#if DEBUG
printf("Raw Phys: %s\n", buf);
#endif
}
/* Get Raw Info */
res = ioctl(fd, HIDIOCGRAWINFO, &info);
if (res < 0) {
perror("HIDIOCGRAWINFO");
} else {
#if DEBUG
printf("Raw Info:\n");
printf("\tbustype: %d (%s)\n",
info.bustype, bus_str(info.bustype));
printf("\tvendor: 0x%04hx\n", info.vendor);
printf("\tproduct: 0x%04hx\n", info.product);
#endif
}
char CommandVal_hex[10] = "\0";
sprintf(CommandVal_hex,"%X",CommandValue);
//Set the Report Number
//SET_O_ZOOM,2,30
buf[0] = 0x00;
buf[1] = FX320_CAMERA_CONTROL; /* Report Number */
buf[2] = FX320_CAMERA_COMMAND; /* Report Number */
buf[3] = (Length+1); /* length in hex */
buf[4] = Command; /* Command in ASCII */
if (1 == Length) {
/* Hex value of CommandValue in ASCII */
buf[5] = CommandVal_hex[0];
} else if (Length == 2) {
if(CommandVal_hex[1] != 0x00) {
/* Hex value of CommandValue in ASCII */
buf[5] = CommandVal_hex[0];
/* Hex value of CommandValue in ASCII */
buf[6] = CommandVal_hex[1];
} else {
/* Pad extra zero in ASCII */
buf[5] = 0x30;
/* Hex value of CommandValue in ASCII */
buf[6] = CommandVal_hex[0];
}
}
/* Send a Report to the Device */
int ret = write(fd, buf, BUFFER_LENGTH);
#if DEBUG
printf("ret = %d\n", ret);
#endif
close(fd);
return;
}
int
main(int argc, char **argv)
{
int fd;
int i, res, desc_size = 0;
char buf[256], *dev;
struct hidraw_report_descriptor rpt_desc;
struct hidraw_devinfo info;
/* Usage check */
if (argc != 3)
{
fprintf(stderr, "Usage: %s <hidraw-dev> <SITL IP>\n", argv[0]);
return 1;
}
/* Device is argv[1] */
dev = argv[1];
/* IP addr is argv[2] */
pwm_init(argv[2]);
/* Open the Device with non-blocking reads. In real life,
don't use a hard coded path; use libudev instead. */
fd = open(dev, O_RDWR|O_NONBLOCK);
if (fd < 0)
{
perror("Unable to open device");
return 1;
}
memset(&rpt_desc, 0x0, sizeof(rpt_desc));
memset(&info, 0x0, sizeof(info));
memset(buf, 0x0, sizeof(buf));
/* Get Report Descriptor Size */
res = ioctl(fd, HIDIOCGRDESCSIZE, &desc_size);
if (res < 0)
{
perror("HIDIOCGRDESCSIZE");
}
else
{
printf("Report Descriptor Size: %d\n", desc_size);
}
/* Get Report Descriptor */
rpt_desc.size = desc_size;
res = ioctl(fd, HIDIOCGRDESC, &rpt_desc);
if (res < 0)
{
perror("HIDIOCGRDESC");
}
else
{
printf("Report Descriptor:\n");
for (i = 0; i < rpt_desc.size; i++)
{
printf("%hhx ", rpt_desc.value[i]);
}
puts("\n");
}
/* Get Raw Name */
res = ioctl(fd, HIDIOCGRAWNAME(256), buf);
if (res < 0)
{
perror("HIDIOCGRAWNAME");
}
else
{
printf("Raw Name: %s\n", buf);
}
/* Get Physical Location */
res = ioctl(fd, HIDIOCGRAWPHYS(256), buf);
if (res < 0)
{
perror("HIDIOCGRAWPHYS");
}
else
{
printf("Raw Phys: %s\n", buf);
}
/* Get Raw Info */
res = ioctl(fd, HIDIOCGRAWINFO, &info);
if (res < 0)
{
perror("HIDIOCGRAWINFO");
}
else
{
printf("Raw Info:\n");
printf("\tbustype: %d (%s)\n",
info.bustype, bus_str(info.bustype));
printf("\tvendor: 0x%04hx\n", info.vendor);
printf("\tproduct: 0x%04hx\n", info.product);
}
/* Set Feature */
buf[0] = 0x9; /* Report Number */
buf[1] = 0xff;
buf[2] = 0xff;
buf[3] = 0xff;
res = ioctl(fd, HIDIOCSFEATURE(4), buf);
if (res < 0)
{
perror("HIDIOCSFEATURE");
}
else
{
printf("ioctl HIDIOCGFEATURE returned: %d\n", res);
}
/* Get Feature */
buf[0] = 0x9; /* Report Number */
res = ioctl(fd, HIDIOCGFEATURE(256), buf);
if (res < 0)
{
perror("HIDIOCGFEATURE");
}
else
{
printf("ioctl HIDIOCGFEATURE returned: %d\n", res);
printf("Report data (not containing the report number):\n\t");
for (i = 0; i < res; i++)
{
printf("%hhx ", buf[i]);
}
puts("\n");
}
/* Send a Report to the Device */
buf[0] = 0x1; /* Report Number */
buf[1] = 0x77;
res = write(fd, buf, 2);
if (res < 0)
{
printf("Error: %d\n", errno);
perror("write");
}
else
{
printf("write() wrote %d bytes\n", res);
}
/* Get a report from the device */
res = read(fd, buf, 16);
if (res < 0)
{
perror("read");
}
else
{
printf("read() read %d bytes:\n\t", res);
for (i = 0; i < res; i++)
{
printf("%hhx ", buf[i]);
}
puts("\n");
}
/* OP loop */
while (1)
{
/* Send a Report to the Device */
buf[0] = 0x0; /* Report Number */
buf[1] = 0x0;
res = write(fd, buf, 2);
if (res < 0)
{
printf("Error: %d\n", errno);
perror("write");
}
/* Get a report from the device */
res = read(fd, buf, 16);
if (res < 0)
{
perror("read");
}
else
{
pwm_send(buf, res);
}
usleep(20 * 1000);
}
close(fd);
return 0;
}