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 <sys/socket.h>
00038 #include <netinet/in.h>
00039 #include <sys/types.h>
00040 #include <arpa/inet.h>
00041 #include <string.h>
00042 #include <stdio.h>
00043 #include <stdlib.h>
00044 #include <unistd.h>
00045 #include <signal.h>
00046 #include <errno.h>
00047 #include <sched.h>
00048
00049 #include <ros/console.h>
00050 #include <ros/time.h>
00051
00052 #include <wge100_camera/ipcam_packet.h>
00053 #include <wge100_camera/host_netutil.h>
00054 #include <wge100_camera/wge100lib.h>
00055 #include <wge100_camera/mt9v.h>
00056
00057 class WGE100Simulator
00058 {
00059 public:
00060 WGE100Simulator(uint32_t serial) : serial_no_(serial)
00061 {
00062 in_addr in_addr;
00063 in_addr.s_addr = 0;
00064 socket_ = wge100SocketCreate(&in_addr, WG_CAMCMD_PORT);
00065 if (socket_ == -1)
00066 {
00067 ROS_ERROR("Error creating/binding socket: %s", strerror(errno));
00068 return;
00069 }
00070
00071 signal(SIGTERM, WGE100Simulator::setExiting);
00072 reset();
00073 }
00074
00075 int run()
00076 {
00077 if (socket_ == -1)
00078 return -1;
00079
00080 while (!exiting_)
00081 {
00082 ProcessCommands();
00083 SendData();
00084 }
00085
00086 return 0;
00087 }
00088
00089 ~WGE100Simulator()
00090 {
00091 if (socket_ != -1)
00092 close(socket_);
00093 }
00094
00095 private:
00096 uint32_t serial_no_;
00097 int socket_;
00098 int running_;
00099 int width_;
00100 int height_;
00101 int frame_;
00102
00103 ros::Time next_frame_time_;
00104 sockaddr_in data_addr_;
00105 static const int NUM_IMAGER_REGISTERS = 256;
00106 static const int IMAGER_REGISTER_UNDEFINED = 1;
00107 static const int IMAGER_REGISTER_UNSUPPORTED = 2;
00108 uint16_t imager_registers_[NUM_IMAGER_REGISTERS];
00109 uint8_t imager_register_flags_[NUM_IMAGER_REGISTERS];
00110
00111 void reset()
00112 {
00113 running_ = 0;
00114 frame_ = 0;
00115 width_ = 640;
00116 height_ = 480;
00117
00118 next_frame_time_ = ros::Time(0);
00119 for (int i = 0; i < NUM_IMAGER_REGISTERS; i++)
00120 {
00121 imager_registers_[i] = 0;
00122 imager_register_flags_[i] |= IMAGER_REGISTER_UNDEFINED | IMAGER_REGISTER_UNSUPPORTED;
00123 }
00124 uint8_t supported_registers[] = { 0x7F, 0x06, 0x05, 0x04, 0x03, 0 };
00125 uint16_t init_value[] = { 0 , 0x2D, 0x5E, 0x2F0, 0x1E0, 0x1311 };
00126 for (unsigned int i = 0; i < sizeof(supported_registers) / sizeof(*supported_registers); i++)
00127 {
00128 imager_register_flags_[supported_registers[i]] &= ~(IMAGER_REGISTER_UNDEFINED | IMAGER_REGISTER_UNSUPPORTED);
00129 imager_registers_[supported_registers[i]] = init_value[i];
00130 }
00131
00132 uint8_t tolerated_registers[] = { 0x18, 0x15, 0x20, 0x21, 0x01, 0x02, 0x0D, 0x70, 0x71, 0x1C, 0xAF, 0x0B, 0x47, 0x48, 0x4C, 0xA5, 0x35, 0xBD };
00133
00134 for (unsigned int i = 0; i < sizeof(tolerated_registers) / sizeof(*tolerated_registers); i++)
00135 imager_register_flags_[tolerated_registers[i]] &= ~IMAGER_REGISTER_UNSUPPORTED;
00136 }
00137
00138 static inline uint8_t get_diag_test_pattern(int x, int y)
00139 {
00140 if ((x + 1) / 2 + y < 500)
00141 return 14 + x / 4;
00142 else
00143 return 0;
00144 }
00145
00146 void SendData()
00147 {
00148 if (!running_)
00149 {
00150 next_frame_time_ = ros::Time(0);
00151 ros::Duration(0.01).sleep();
00152 return;
00153 }
00154
00155 if (width_ != imager_registers_[0x04])
00156 {
00157 ROS_ERROR("Width mismatch %i reg: %i", width_, imager_registers_[0x04]);
00158 ros::Duration(0.01).sleep();
00159 return;
00160 }
00161
00162 if (height_ != imager_registers_[0x03])
00163 {
00164 ROS_ERROR("Height mismatch %i reg: %i", height_, imager_registers_[0x03]);
00165 ros::Duration(0.01).sleep();
00166 return;
00167 }
00168
00169 ros::Duration sleeptime = (next_frame_time_ - ros::Time::now());
00170 ros::Duration period = ros::Duration(((imager_registers_[0x03] + imager_registers_[0x06]) * (imager_registers_[0x04] + imager_registers_[0x05]) + 4) / 16e6);
00171
00172 if (sleeptime < -period)
00173 {
00174 if (next_frame_time_ != ros::Time(0))
00175 ROS_WARN("Simulator lost lock on frame #%i", frame_);
00176 next_frame_time_ = ros::Time::now();
00177 }
00178 else
00179 sleeptime.sleep();
00180 next_frame_time_ += period;
00181
00182 PacketVideoLine pvl;
00183 PacketEOF peof;
00184
00185 peof.header.frame_number = pvl.header.frame_number = htonl(frame_);
00186 peof.header.horiz_resolution = pvl.header.horiz_resolution = htons(width_);
00187 peof.header.vert_resolution = pvl.header.vert_resolution = htons(height_);
00188
00189 for (int y = 0; y < height_; y++)
00190 {
00191 if ((imager_registers_[0x7F] & 0x3C00) == 0x3800)
00192 {
00193 if (width_ > 320)
00194 for (int x = 0; x < width_; x++)
00195 pvl.data[x] = get_diag_test_pattern(x, y);
00196 else
00197 for (int x = 0; x < width_; x++)
00198 pvl.data[x] = (get_diag_test_pattern(2 * x, 2 * y) + get_diag_test_pattern(2 * x, 2 * y + 1)) / 2;
00199 }
00200 else
00201 {
00202 for (int x = 0; x < width_; x++)
00203 pvl.data[x] = x + y + frame_;
00204 }
00205
00206 pvl.header.line_number = htons(y | (frame_ << 10));
00207 sendto(socket_, &pvl, sizeof(pvl.header) + width_, 0,
00208 (struct sockaddr *) &data_addr_, sizeof(data_addr_));
00209 if (y == 0)
00210 {
00211 sched_yield();
00212 ROS_INFO("Sending frame #%i", frame_);
00213 fflush(stdout);
00214 }
00215 }
00216
00217 peof.header.line_number = htons(IMAGER_LINENO_EOF);
00218 peof.i2c_valid = 0;
00219 peof.i2c[0] = 0;
00220 sendto(socket_, &peof, sizeof(peof), 0,
00221 (struct sockaddr *) &data_addr_, sizeof(data_addr_));
00222
00223 frame_ = (frame_ + 1) & 0xFFFF;
00224 }
00225
00226 void ProcessCommands()
00227 {
00228 unsigned char buff[100];
00229
00230 while (true)
00231 {
00232 ssize_t len = recv(socket_, buff, 100, MSG_DONTWAIT);
00233 if (len == -1)
00234 break;
00235
00236 PacketGeneric *hdr = (PacketGeneric *) buff;
00237
00238 if (len == 1 && buff[0] == 0xFF)
00239 continue;
00240
00241 if (ntohl(hdr->magic_no) != WG_MAGIC_NO ||
00242 ntohl(hdr->type) > PKT_MAX_ID)
00243 {
00244 ROS_INFO("Got a packet with a bad magic number or type."); \
00245 continue;
00246 }
00247
00248 #define CAST_AND_CHK(type) \
00249 if (len != sizeof(type)) \
00250 { \
00251 ROS_INFO("Got a "#type" with incorrect length"); \
00252 continue;\
00253 } \
00254 if (false) ROS_INFO("Got a "#type""); \
00255 type *pkt = (type *) buff;
00256
00257
00258 switch (ntohl(hdr->type))
00259 {
00260 case PKTT_DISCOVER:
00261 {
00262 CAST_AND_CHK(PacketDiscover);
00263 sendAnnounce(&pkt->hdr);
00264 break;
00265 }
00266
00267 case PKTT_CONFIGURE:
00268 {
00269 CAST_AND_CHK(PacketConfigure);
00270 if (pkt->product_id != htonl(CONFIG_PRODUCT_ID) ||
00271 pkt->ser_no != htonl(serial_no_))
00272 break;
00273 running_ = false;
00274 sendAnnounce(&pkt->hdr);
00275 break;
00276 }
00277
00278 case PKTT_TRIGCTRL:
00279 {
00280 CAST_AND_CHK(PacketTrigControl);
00281 unsigned int new_mode = htons(pkt->trig_state);
00282 if (new_mode == 0 && !running_)
00283 sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00284 else
00285 {
00286 ROS_ERROR("Tried to set to trigger mode %i.", new_mode);
00287 sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00288 }
00289 break;
00290 }
00291
00292 case PKTT_IMGRMODE:
00293 {
00294 CAST_AND_CHK(PacketImagerMode);
00295 uint32_t mode = htonl(pkt->mode);
00296 if (mode >= 10 || running_)
00297 {
00298 sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00299 break;
00300 }
00301
00302
00303
00304
00305 width_ = MT9VModes[mode].width;
00306 height_ = MT9VModes[mode].height;
00307 imager_registers_[0x03] = height_;
00308 imager_registers_[0x04] = width_;
00309 imager_registers_[0x05] = MT9VModes[mode].hblank;
00310 imager_registers_[0x06] = MT9VModes[mode].vblank;
00311 sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00312 }
00313 break;
00314
00315 case PKTT_IMGRSETRES:
00316 {
00317 CAST_AND_CHK(PacketImagerSetRes);
00318 unsigned int new_width = htons(pkt->horizontal);
00319 unsigned int new_height = htons(pkt->vertical);
00320 if (new_width > 752 || new_height > 640)
00321 {
00322 ROS_ERROR("Tried to set resolution to out of range %ix%i", new_width, new_height);
00323 sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00324 break;
00325 }
00326
00327 width_ = new_width;
00328 height_ = new_height;
00329 sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00330 }
00331 break;
00332
00333 case PKTT_VIDSTART:
00334 {
00335 CAST_AND_CHK(PacketVidStart);
00336 if (running_)
00337 sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00338 else
00339 {
00340 sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00341 bzero(&data_addr_, sizeof(data_addr_));
00342 data_addr_.sin_family = AF_INET;
00343 data_addr_.sin_addr.s_addr = pkt->receiver.addr;
00344 data_addr_.sin_port = pkt->receiver.port;
00345 }
00346 running_ = true;
00347 }
00348 break;
00349
00350 case PKTT_VIDSTOP:
00351 {
00352 CAST_AND_CHK(PacketVidStop);
00353 if (running_)
00354 sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00355 else
00356 sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00357 running_ = false;
00358 }
00359 break;
00360
00361 case PKTT_RESET:
00362 {
00363 reset();
00364 }
00365 break;
00366
00367 case PKTT_SENSORRD:
00368 {
00369 CAST_AND_CHK(PacketSensorRequest);
00370 if (imager_register_flags_[pkt->address] & IMAGER_REGISTER_UNDEFINED)
00371 ROS_WARN("Reading uninitialized imager register 0x%02X.", pkt->address);
00372 if (imager_register_flags_[pkt->address] & IMAGER_REGISTER_UNSUPPORTED)
00373 ROS_WARN("Reading unsupported imager register 0x%02X.", pkt->address);
00374 sendSensorData(&pkt->hdr, pkt->address, imager_registers_[pkt->address]);
00375 }
00376 break;
00377
00378 case PKTT_SENSORWR:
00379 {
00380 CAST_AND_CHK(PacketSensorData);
00381 if (imager_register_flags_[pkt->address] & IMAGER_REGISTER_UNSUPPORTED)
00382 ROS_ERROR("Writing unsupported imager register 0x%02X.", pkt->address);
00383 imager_register_flags_[pkt->address] &= ~IMAGER_REGISTER_UNDEFINED;
00384 imager_registers_[pkt->address] = ntohs(pkt->data);
00385 sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00386 }
00387 break;
00388
00389 case PKTT_FLASHREAD:
00390 {
00391 CAST_AND_CHK(PacketFlashRequest);
00392 sendFlashData(&pkt->hdr, pkt->address);
00393 }
00394 break;
00395
00396 default:
00397 ROS_ERROR("Got an unknown message type: %i", ntohl(hdr->type));
00398 break;
00399 }
00400 #undef CAST_AND_CHK
00401 }
00402 }
00403
00404 #define FILL_HDR(pkttype, pktid) \
00405 if (false) ROS_INFO("Sending a "#pkttype" packet."); \
00406 Packet##pkttype rsp; \
00407 rsp.hdr.magic_no = htonl(WG_MAGIC_NO); \
00408 strncpy(rsp.hdr.hrt, #pkttype, sizeof(rsp.hdr.hrt)); \
00409 rsp.hdr.type = htonl(pktid);
00410
00411
00412 #define SEND_RSP \
00413 sockaddr_in rsp_addr; \
00414 bzero(&rsp_addr, sizeof(rsp_addr)); \
00415 rsp_addr.sin_family = AF_INET; \
00416 rsp_addr.sin_addr.s_addr = hdr->reply_to.addr; \
00417 rsp_addr.sin_port = hdr->reply_to.port; \
00418 sendto(socket_, &rsp, sizeof(rsp), 0, \
00419 (struct sockaddr *) &rsp_addr, sizeof(rsp_addr));
00420
00421 void sendAnnounce(PacketGeneric *hdr)
00422 {
00423 FILL_HDR(Announce, PKTT_ANNOUNCE);
00424 bzero(rsp.mac, sizeof(rsp.mac));
00425 rsp.product_id = htonl(CONFIG_PRODUCT_ID);
00426 rsp.ser_no = htonl(serial_no_);
00427 strncpy(rsp.product_name, "WGE100_camera_simulator.", sizeof(rsp.product_name));
00428 rsp.hw_version = htonl(0x2041);
00429 rsp.fw_version = htonl(0x0112);
00430 strncpy(rsp.camera_name, "WGE100_camera_simulator.", sizeof(rsp.camera_name));
00431 SEND_RSP;
00432 }
00433
00434 void sendStatus(PacketGeneric *hdr, uint32_t type, uint32_t code)
00435 {
00436 if (type != PKT_STATUST_OK)
00437 ROS_ERROR("Error Condition.");
00438 FILL_HDR(Status, PKTT_STATUS);
00439 rsp.status_type = htonl(type);
00440 rsp.status_code = htonl(code);
00441 SEND_RSP;
00442 }
00443
00444 void sendFlashData(PacketGeneric *hdr, uint32_t address)
00445 {
00446 FILL_HDR(FlashPayload, PKTT_FLASHDATA);
00447 rsp.address = address;
00448 bzero(rsp.data, sizeof(rsp.data));
00449 SEND_RSP;
00450 }
00451
00452 void sendSensorData(PacketGeneric *hdr, uint8_t addr, uint16_t value)
00453 {
00454 FILL_HDR(SensorData, PKTT_SENSORDATA);
00455 rsp.address = addr;
00456 rsp.data = htons(value);
00457 SEND_RSP;
00458 }
00459
00460 #undef FILL_HDR
00461 #undef SEND_RSP
00462 volatile static bool exiting_;
00463
00464 static void setExiting(int i)
00465 {
00466 exiting_ = true;
00467 }
00468 };
00469
00470 volatile bool WGE100Simulator::exiting_ = false;
00471
00472 int main(int argc, char **argv)
00473 {
00474 ros::Time::init();
00475 WGE100Simulator wge100sim(12345);
00476 return wge100sim.run();
00477 }
00478