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;
}
