void ControlMode::outputControlTimerCallback(const ros::TimerEvent& e) { //NODELET_INFO_STREAM(__PRETTY_FUNCTION__); control_mode_outputPtr control_out(new control_mode_output); switch (state) { case ControlModeTypes::STANDBY: case ControlModeTypes::ACTIVE: output_pub.publish(control_out); break; default: break; } }
int main(int argc, char **argv) { FILE *pFile = NULL; int size; long lSize; unsigned char *buffer; unsigned char *buf2 = calloc(300, sizeof(unsigned char)); if (argc < 2) { fprintf(stderr, "Missing upload filename \n\n"); fprintf(stderr, "Usage: \n"); fprintf(stderr, "ISP808 <uploadfile> \n"); exit(1);; } pFile = fopen(argv[1], "rb"); if (pFile == NULL) { fprintf(stderr, "ERROR: Unable to open file %s \n", argv[1]); exit(1); } // obtain file size: fseek(pFile, 0, SEEK_END); lSize = ftell(pFile); rewind(pFile); // allocate memory to contain the whole file: buffer = calloc(lSize, sizeof(unsigned char)); if (buffer == NULL) { fputs("Memory error", stderr); exit(2); } size = fread(buffer, 1, lSize, pFile); if (size != lSize) { fputs("Reading error", stderr); exit(3); } libusb_context *context; libusb_init(&context); libusb_set_debug(context, 3); device_handle = libusb_open_device_with_vid_pid(context, VID, PID); if (!device_handle) { fprintf(stderr, "ERROR: Unable to find device " "(Vendor ID = %04x, Product ID = %04x)\n", VID, PID); fprintf(stderr, "Press the Mode Button and connect the cam to USB \n"); return 1; } libusb_claim_interface(device_handle, 2); libusb_set_interface_alt_setting(device_handle, 0, 0); usleep(100); // Get the installed version fprintf(stdout, "\nRead 808 Version \n"); GetVersion(); GetVersion1(); GetVersion2(); fprintf(stdout, "\nStart Uploading the file: %s \n",argv[1]); char defout[] = { 0x00, 0x80, 0x04, 0x00, 0x2d, 0x10, 0x00, 0x00 }; control_in_vendor_device(0x70, 0, 0, 5); usleep(100); control_out(0xfd, 0, 0x4f3, defout, 8); usleep(200); // Fix me // Download the code fprintf(stdout, "\nDownload Code.... \n"); bulk_write(3, buffer, lSize); usleep(200); char defout2[] = { 0x00, 0x80, 0x04, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00 }; control_out(0xfd, 0, 0x4f1, defout2, 0x10); usleep(100); // Dont know why bulk_write(3, buf2, 256); fprintf(stdout, "\nDownload Ready.... \n"); usleep(100); if (pFile) fclose(pFile); free(buffer); libusb_release_interface(device_handle, 0); libusb_close(device_handle); libusb_exit(context); return 0; }