00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <assert.h>
00038 #include <iostream>
00039 #include <fstream>
00040
00041 #include <string.h>
00042 #include <stdlib.h>
00043
00044 #include <ros/console.h>
00045 #include <ros/time.h>
00046
00047 #include <wge100_camera/ipcam_packet.h>
00048 #include <wge100_camera/host_netutil.h>
00049 #include <wge100_camera/wge100lib.h>
00050
00051 uint16_t checksum(uint16_t *data)
00052 {
00053 uint16_t sum = 0;
00054 for (int i = 0; i < FLASH_PAGE_SIZE; i++)
00055 sum += htons(data[i]);
00056 return htons(0xFFFF - sum);
00057 }
00058
00059 int read_calibration(IpCamList *camera)
00060 {
00061 uint8_t calbuff[2 * FLASH_PAGE_SIZE];
00062
00063 fprintf(stderr, "Reading old calibration...\n");
00064 if(wge100ReliableFlashRead(camera, FLASH_CALIBRATION_PAGENO, (uint8_t *) calbuff, NULL) != 0 ||
00065 wge100ReliableFlashRead(camera, FLASH_CALIBRATION_PAGENO+1, (uint8_t *) calbuff+FLASH_PAGE_SIZE, NULL) != 0)
00066 {
00067 fprintf(stderr, "Flash read error. Aborting.\n");
00068 return -2;
00069 }
00070
00071 uint16_t chk = checksum((uint16_t *) calbuff);
00072 if (chk)
00073 {
00074 fprintf(stderr, "Previous camera calibration had bad checksum.\n");
00075 }
00076 else
00077 {
00078 calbuff[sizeof(calbuff) - 2] = 0;
00079 printf("%s\n", calbuff);
00080 }
00081
00082 return 0;
00083 }
00084
00085 int write_calibration(IpCamList *camera, char *filename)
00086 {
00087 uint8_t calbuff[FLASH_PAGE_SIZE * 2];
00088 bzero(calbuff, sizeof(calbuff));
00089
00090 fprintf(stderr, "\nWriting new calibration...\n");
00091
00092 FILE *f;
00093 if (strcmp(filename, "-"))
00094 f = fopen(filename, "r");
00095 else
00096 {
00097 fprintf(stderr, "Enter new calibration information on standard input.\n");
00098 f = stdin;
00099 }
00100 if (!f)
00101 {
00102 fprintf(stderr, "Unable to open file %s.\n", filename);
00103 return -1;
00104 }
00105 int maxsize = sizeof(calbuff) - sizeof(uint16_t) - 1;
00106 int bytesread = fread(calbuff, 1, maxsize + 1, f);
00107 fclose(f);
00108 if (bytesread > maxsize)
00109 {
00110 fprintf(stderr, "File %s is too long. At most %i bytes can be stored.\n", filename, maxsize);
00111 return -1;
00112 }
00113 calbuff[bytesread] = 0;
00114
00115 ((uint16_t *) calbuff)[FLASH_PAGE_SIZE - 1] = 0;
00116 ((uint16_t *) calbuff)[FLASH_PAGE_SIZE - 1] = checksum((uint16_t *) calbuff);
00117
00118 if (wge100ReliableFlashWrite(camera, FLASH_CALIBRATION_PAGENO, (uint8_t *) calbuff, NULL) != 0 ||
00119 wge100ReliableFlashWrite(camera, FLASH_CALIBRATION_PAGENO+1, (uint8_t *) calbuff+FLASH_PAGE_SIZE, NULL) != 0)
00120 {
00121 fprintf(stderr, "Flash write error. The camera calibration is an undetermined state.\n");
00122 return -2;
00123 }
00124
00125 fprintf(stderr, "Success!\n");
00126 return 0;
00127 }
00128
00129 int clear_calibration(IpCamList *camera, char *filename)
00130 {
00131 uint8_t calbuff[FLASH_PAGE_SIZE * 2];
00132 bzero(calbuff, sizeof(calbuff));
00133
00134 fprintf(stderr, "\nClearing calibration...\n");
00135
00136 if (wge100ReliableFlashWrite(camera, FLASH_CALIBRATION_PAGENO, (uint8_t *) calbuff, NULL) != 0 ||
00137 wge100ReliableFlashWrite(camera, FLASH_CALIBRATION_PAGENO+1, (uint8_t *) calbuff+FLASH_PAGE_SIZE, NULL) != 0)
00138 {
00139 fprintf(stderr, "Flash write error. The camera calibration is an undetermined state.\n");
00140 return -2;
00141 }
00142
00143 fprintf(stderr, "Success!\n");
00144 return 0;
00145 }
00146
00147 int main(int argc, char **argv)
00148 {
00149 if ((argc != 3 && argc != 2) || !strcmp(argv[1], "--help")) {
00150 fprintf(stderr, "Usage: %s <camera_url> <calibration_file> # Sets the camera calibration information\n", argv[0]);
00151 fprintf(stderr, " %s <camera_url> - # Sets the camera calibration information from stdin\n", argv[0]);
00152 fprintf(stderr, " %s <camera_url> --invalidate # Invalidates the camera calibration information\n", argv[0]);
00153 fprintf(stderr, " %s <camera_url> # Reads the camera calibration information\n", argv[0]);
00154 fprintf(stderr, "\nReads or writes the camera calibration information stored on the camera's flash.\n");
00155 return -1;
00156 }
00157
00158 char *camera_url = argv[1];
00159
00160
00161 IpCamList camera;
00162 const char *errmsg;
00163 int outval = wge100FindByUrl(camera_url, &camera, SEC_TO_USEC(0.1), &errmsg);
00164 if (outval)
00165 {
00166 fprintf(stderr, "Matching URL %s : %s\n", camera_url, errmsg);
00167 return -1;
00168 }
00169
00170
00171 outval = wge100Configure(&camera, camera.ip_str, SEC_TO_USEC(0.5));
00172 if (outval != 0) {
00173 if (outval == ERR_CONFIG_ARPFAIL) {
00174 fprintf(stderr, "Unable to create ARP entry (are you root?), continuing anyway\n");
00175 } else {
00176 fprintf(stderr, "IP address configuration failed\n");
00177 return -1;
00178 }
00179 }
00180
00181 outval = read_calibration(&camera);
00182 if (outval)
00183 return outval;
00184
00185 if (argc != 3)
00186 return 0;
00187
00188 char *filename = argv[2];
00189
00190 if (strcmp(filename, "--invalidate"))
00191 return write_calibration(&camera, filename);
00192 else
00193 return clear_calibration(&camera, filename);
00194
00195 }