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;
}
bool RazerHydra::init(const char *device)
{
int res;
uint8_t buf[256];
struct hidraw_report_descriptor rpt_desc;
struct hidraw_devinfo info;
hidraw_fd = open(device, O_RDWR | O_NONBLOCK);
if (hidraw_fd < 0)
{
ROS_ERROR("couldn't open hidraw device");
return false;
}
ROS_INFO("opened hydra");
memset(&rpt_desc, 0x0, sizeof(rpt_desc));
memset(&info, 0x0, sizeof(info));
memset(buf, 0x0, sizeof(buf));
/*
// Get Report Descriptor Size
ROS_DEBUG("Getting Report Descriptor Size");
res = ioctl(hidraw_fd, HIDIOCGRDESCSIZE, &desc_size);
if (res < 0)
perror("HIDIOCGRDESCSIZE");
else
printf("Report Descriptor Size: %d\n", desc_size);
// Get Report Descriptor
ROS_DEBUG("Getting Report Descriptor");
rpt_desc.size = desc_size;
res = ioctl(hidraw_fd, HIDIOCGRDESC, &rpt_desc);
if (res < 0) {
perror("HIDIOCGRDESC");
} else {
printf("Report Descriptor:\n");
for (size_t i = 0; i < rpt_desc.size; i++)
printf("%hhx ", rpt_desc.value[i]);
puts("\n");
}
*/
/* Get Raw Name */
ROS_DEBUG("Getting Raw Name");
res = ioctl(hidraw_fd, HIDIOCGRAWNAME(256), buf);
if (res < 0)
perror("HIDIOCGRAWNAME");
else
printf("Raw Name: %s\n", buf);
// set feature to start it streaming
memset(buf, 0x0, sizeof(buf));
buf[6] = 1;
buf[8] = 4;
buf[9] = 3;
buf[89] = 6;
int attempt = 0;
for (; attempt < 50; attempt++)
{
res = ioctl(hidraw_fd, HIDIOCSFEATURE(91), buf);
if (res < 0)
{
ROS_ERROR("unable to start streaming");
perror("HIDIOCSFEATURE");
usleep(50000);
}
else
{
ROS_INFO("started streaming");
break;
}
}
ROS_INFO("%d attempts", attempt);
return attempt < 60;
}
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;
}
int main (int argc, char *argv[]) {
int i,j;
if (argc != 2) {
fprintf (stderr, "Usage: %s hidraw\n", argv[0]);
return EXIT_FAILURE;
}
int fd;
if (-1 == (fd = open (argv[1], O_RDWR))) {
perror ("open");
return EXIT_FAILURE;
}
struct hidraw_devinfo info;
if (-1 == ioctl (fd, HIDIOCGRAWINFO, &info)) {
perror ("ioctl HIDIOCGRAWINFO");
return EXIT_FAILURE;
}
char name[256];
if (-1 == ioctl (fd, HIDIOCGRAWNAME(sizeof (name)), name)) {
perror ("ioctl HIDIOCGRAWNAME");
return EXIT_FAILURE;
}
printf ("%04hx:%04hx %s\n", info.vendor, info.product, name);
struct hidraw_report_descriptor desc;
if (-1 == ioctl (fd, HIDIOCGRDESCSIZE, &desc.size)) {
perror ("ioctl HIDIOCGRDESCSIZE");
return EXIT_FAILURE;
}
if (-1 == ioctl (fd, HIDIOCGRDESC, &desc)) {
perror ("ioctl HIDIOCGRDESC");
return EXIT_FAILURE;
}
int found_report[2][2] = { { 0, 0 }, { 0, 0 } };
const int report_ids[2] = { 0x10, 0x11 };
const int report_counts[2] = { 6, 19 };
#define GLOBAL_STACK_SIZE 16
struct global_t global_stack[GLOBAL_STACK_SIZE];
int global_stack_index = 0;
struct global_t global_state;
memset (&global_state, 0, sizeof (struct global_t));
struct local_t local_state;
memset (&local_state, 0, sizeof (struct local_t));
i = 0;
while (i < desc.size) {
int type, tag, data_len;
const unsigned char *item_data;
i += parse_report_item (&desc.value[i], &type, &tag, &data_len, &item_data);
switch (type) {
case 0: // Main
switch (tag) {
case 8: // Input
case 9: // Output
if (global_state.usage_page == 0xFF00 && local_state.usage_count == 1 && local_state.usage[0] >= 1 && local_state.usage[0] <= 2) {
int report_dir = tag == 8 ? 0 : 1;
int report_type = local_state.usage[0] - 1;
if (global_state.report_id != report_ids[report_type]) {
fprintf (stderr, "Invalid report ID\n");
break;
}
if (global_state.report_count != report_counts[report_type]) {
fprintf (stderr, "Invalid report count\n");
break;
}
if (global_state.report_size != 8) {
fprintf (stderr, "Invalid report size\n");
break;
}
if (parse_int (item_data, data_len) != 0) {
fprintf (stderr, "Invalid report flags\n");
break;
}
found_report[report_type][report_dir] = 1;
}
break;
case 11: // Feature
break;
case 10: // Collection
case 12: // End Collection
break;
default:
fprintf (stderr, "Invalid main tag %d\n", tag);
return EXIT_FAILURE;
}
memset (&local_state, 0, sizeof (struct local_t));
break;
case 1: // Global
switch (tag) {
case 0: // Usage Page
global_state.usage_page = parse_int (item_data, data_len);
break;
case 1: // Logical Minimum
case 2: // Logical Maximum
case 3: // Physical Minimum
case 4: // Physical Maximmum
case 5: // Unit Exponent
case 6: // Unit
break;
case 7: // Report Size
global_state.report_size = parse_int (item_data, data_len);
break;
case 8: // Report ID
global_state.report_id = parse_int (item_data, data_len);
break;
case 9: // Report Count
global_state.report_count = parse_int (item_data, data_len);
break;
case 10: // Push
memcpy (&global_state, &global_stack[global_stack_index++], sizeof (struct global_t));
break;
case 11: // Pop
memcpy (&global_stack[--global_stack_index], &global_state, sizeof (struct global_t));
break;
default:
fprintf (stderr, "Invalid global tag %d\n", tag);
return EXIT_FAILURE;
}
break;
case 2: // Local
switch (tag) {
case 0: // Usage
local_state.usage[local_state.usage_count++] = parse_int (item_data, data_len);
break;
case 1: // Usage Minimum
local_state.usage_min = parse_int (item_data, data_len);
break;
case 2: // Usage Maximum
local_state.usage_max = parse_int (item_data, data_len);
break;
case 3: // Designator Index
case 4: // Designator Minimum
case 5: // Designator Maximum
case 7: // String Index
case 8: // String Minimum
case 9: // String Maximum
case 10: // Delimiter
break;
default:
fprintf (stderr, "Invalid local tag %d\n", tag);
return EXIT_FAILURE;
}
break;
default:
fprintf (stderr, "Invalid item type %d\n", type);
return EXIT_FAILURE;
}
}
for (i = 0; i < 2; ++i) {
for (j = 0; j < 2; ++j) {
if (!found_report[i][j]) {
fprintf (stderr, "Missing logitech HID report\n");
return EXIT_FAILURE;
}
}
}
printf ("This device supports Logitech HID reports.\n");
return EXIT_SUCCESS;
}
struct shuttlexpress *check_for_shuttlexpress(char *dev_filename) {
struct shuttlexpress *s;
struct hidraw_devinfo devinfo;
char name[100];
int r;
printf("%s: checking %s\n", modname, dev_filename);
s = (struct shuttlexpress *)calloc(1, sizeof(struct shuttlexpress));
if (s == NULL) {
fprintf(stderr, "%s: out of memory!\n", modname);
return NULL;
}
s->device_file = dev_filename;
s->fd = open(s->device_file, O_RDONLY);
if (s->fd < 0) {
fprintf(stderr, "%s: error opening %s: %s\n", modname, s->device_file, strerror(errno));
if (errno == EACCES) {
fprintf(stderr, "%s: make sure you have read permission on %s, read the shuttlexpress(1) manpage for more info\n", modname, s->device_file);
}
goto fail0;
}
r = ioctl(s->fd, HIDIOCGRAWINFO, &devinfo);
if (r < 0) {
fprintf(stderr, "%s: error with ioctl HIDIOCGRAWINFO on %s: %s\n", modname, s->device_file, strerror(errno));
goto fail1;
}
if (devinfo.vendor != VENDOR_ID) {
fprintf(stderr, "%s: dev %s has unexpected Vendor ID 0x%04x (expected Contour Design, 0x%04x)\n", modname, s->device_file, devinfo.vendor, VENDOR_ID);
goto fail1;
}
if (devinfo.product != PRODUCT_ID) {
fprintf(stderr, "%s: dev %s has unexpected Product ID 0x%04x (expected ShuttleXpress, 0x%04x)\n", modname, s->device_file, devinfo.product, PRODUCT_ID);
goto fail1;
}
r = ioctl(s->fd, HIDIOCGRAWNAME(99), name);
if (r < 0) {
fprintf(stderr, "%s: error with ioctl HIDIOCGRAWNAME on %s: %s\n", modname, s->device_file, strerror(errno));
goto fail1;
}
printf("%s: found %s on %s\n", modname, name, s->device_file);
s->hal = (struct shuttlexpress_hal *)hal_malloc(sizeof(struct shuttlexpress_hal));
if (s->hal == NULL) {
fprintf(stderr, "%s: ERROR: unable to allocate HAL shared memory\n", modname);
goto fail1;
}
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_0), hal_comp_id, "%s.%d.button-0", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_0_not), hal_comp_id, "%s.%d.button-0-not", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_1), hal_comp_id, "%s.%d.button-1", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_1_not), hal_comp_id, "%s.%d.button-1-not", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_2), hal_comp_id, "%s.%d.button-2", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_2_not), hal_comp_id, "%s.%d.button-2-not", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_3), hal_comp_id, "%s.%d.button-3", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_3_not), hal_comp_id, "%s.%d.button-3-not", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_4), hal_comp_id, "%s.%d.button-4", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_bit_newf(HAL_OUT, &(s->hal->button_4_not), hal_comp_id, "%s.%d.button-4-not", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_s32_newf(HAL_OUT, &(s->hal->counts), hal_comp_id, "%s.%d.counts", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_float_newf(HAL_OUT, &(s->hal->spring_wheel_f), hal_comp_id, "%s.%d.spring-wheel-f", modname, num_devices);
if (r != 0) goto fail1;
r = hal_pin_s32_newf(HAL_OUT, &(s->hal->spring_wheel_s32), hal_comp_id, "%s.%d.spring-wheel-s32", modname, num_devices);
if (r != 0) goto fail1;
*s->hal->button_0 = 0;
*s->hal->button_0_not = 1;
*s->hal->button_1 = 0;
*s->hal->button_1_not = 1;
*s->hal->button_2 = 0;
*s->hal->button_2_not = 1;
*s->hal->button_3 = 0;
*s->hal->button_3_not = 1;
*s->hal->button_4 = 0;
*s->hal->button_4_not = 1;
*s->hal->counts = 0;
*s->hal->spring_wheel_f = 0.0;
*s->hal->spring_wheel_s32 = 0;
return s;
fail1:
close(s->fd);
fail0:
free(s);
return NULL;
}
