static void event_handler(switch_event_t *event)
{
const char *dest_proto = switch_event_get_header(event, "dest_proto");
const char *check_failure = switch_event_get_header(event, "Delivery-Failure");
const char *check_nonblocking = switch_event_get_header(event, "Nonblocking-Delivery");
switch_event_add_header(event, SWITCH_STACK_BOTTOM, "skip_global_process", "true");
if (switch_true(check_failure)) {
switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_ERROR, "Delivery Failure\n");
DUMP_EVENT(event);
send_report(event, "Failure");
return;
} else if ( check_failure && switch_false(check_failure) ) {
switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_DEBUG, "SMS Delivery Success\n");
send_report(event, "Success");
return;
} else if ( check_nonblocking && switch_true(check_nonblocking) ) {
switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_DEBUG, "SMS Delivery assumed successful due to being sent in non-blocking manner\n");
send_report(event, "Accepted");
return;
}
switch_core_chat_send(dest_proto, event);
}
static int switch_logitech(struct device_info *devinfo)
{
char devname[PATH_MAX + 1];
int i, fd, err = -1;
for (i = 0; i < 16; i++) {
struct hiddev_devinfo dinfo;
char rep1[] = { 0xff, 0x80, 0x80, 0x01, 0x00, 0x00 };
char rep2[] = { 0xff, 0x80, 0x00, 0x00, 0x30, 0x00 };
char rep3[] = { 0xff, 0x81, 0x80, 0x00, 0x00, 0x00 };
sprintf(devname, "%s/hiddev%d", devpath, i);
fd = open(devname, O_RDWR);
if (fd < 0) {
sprintf(devname, "%s/usb/hiddev%d", devpath, i);
fd = open(devname, O_RDWR);
if (fd < 0) {
sprintf(devname, "%s/usb/hid/hiddev%d", devpath, i);
fd = open(devname, O_RDWR);
if (fd < 0)
continue;
}
}
memset(&dinfo, 0, sizeof(dinfo));
err = ioctl(fd, HIDIOCGDEVINFO, &dinfo);
if (err < 0 || dinfo.busnum != atoi(devinfo->dev->bus->dirname) ||
dinfo.devnum != atoi(devinfo->dev->filename)) {
close(fd);
continue;
}
err = ioctl(fd, HIDIOCINITREPORT, 0);
if (err < 0) {
close(fd);
break;
}
err = send_report(fd, rep1, sizeof(rep1));
if (err < 0) {
close(fd);
break;
}
err = send_report(fd, rep2, sizeof(rep2));
if (err < 0) {
close(fd);
break;
}
err = send_report(fd, rep3, sizeof(rep3));
close(fd);
break;
}
return err;
}
void vrpn_Tracker_MotionNode::get_report()
{
if (NULL != m_handle) {
sampler_type *sampler = reinterpret_cast<sampler_type *>(m_handle);
sampler_type::data_type data;
if (sampler->get_data_block(data, m_num_sensor) && !data.empty()) {
for (std::size_t i=0; i<m_num_sensor; i++) {
const std::size_t index = i * MNCAPI_PREVIEW_SIZE;
if (index + 4 < data.size()) {
vrpn_Tracker::d_sensor = i;
vrpn_Tracker::d_quat[0] = data[index + 1];
vrpn_Tracker::d_quat[1] = data[index + 2];
vrpn_Tracker::d_quat[2] = data[index + 3];
vrpn_Tracker::d_quat[3] = data[index + 0];
send_report();
}
}
}
}
}
void vrpn_Tracker_3DMouse::mainloop()
{
server_mainloop();
switch(status)
{
case vrpn_TRACKER_AWAITING_STATION:
case vrpn_TRACKER_PARTIAL:
case vrpn_TRACKER_SYNCING:
if (get_report()) send_report();
break;
case vrpn_TRACKER_RESETTING:
reset();
break;
case vrpn_TRACKER_FAIL:
fprintf(stderr, "3DMouse failed, trying to reset (Try power cycle if more than 4 attempts made)\n");
if (serial_fd >= 0)
{
vrpn_close_commport(serial_fd);
serial_fd = -1;
}
if ( (serial_fd=vrpn_open_commport(portname, baudrate)) == -1)
{
fprintf(stderr,"vrpn_Tracker_3DMouse::mainloop(): Cannot Open serial port\n");
status = vrpn_TRACKER_FAIL;
return;
}
status = vrpn_TRACKER_RESETTING;
break;
default:
break;
}
}
/*
update the ADSB peripheral state
*/
void ADSB::update(void)
{
if (_sitl == nullptr) {
_sitl = (SITL *)AP_Param::find_object("SIM_");
return;
} else if (_sitl->adsb_plane_count <= 0) {
return;
} else if (_sitl->adsb_plane_count >= num_vehicles_MAX) {
_sitl->adsb_plane_count.set_and_save(0);
num_vehicles = 0;
return;
} else if (num_vehicles != _sitl->adsb_plane_count) {
num_vehicles = _sitl->adsb_plane_count;
for (uint8_t i=0; i<num_vehicles_MAX; i++) {
vehicles[i].initialised = false;
}
}
// calculate delta time in seconds
uint32_t now_us = AP_HAL::micros();
float delta_t = (now_us - last_update_us) * 1.0e-6f;
last_update_us = now_us;
for (uint8_t i=0; i<num_vehicles; i++) {
vehicles[i].update(delta_t);
}
// see if we should do a report
send_report();
}
static int mx_query(int fd, u8 b1, u8 *res)
{
u8 buf[6] = { first_byte, 0x81, b1, 0, 0, 0 };
int i;
send_report(fd, 0x10, buf, 6);
query_report(fd, 0x10, res, 6);
for (int i = 0; i < 6; i++)
res[i] = res[i+1];
if ((
(res[0] != 0x02 && res[0] != 0x01 && res[0] != 0x00) ||
res[1] != 0x81 ||
(res[2] != 0xb1 && res[2] != 0x08)
) && (
(res[0] != 0x02 && res[0] != 0x01 && res[0] != 0x00) ||
res[1] != 0x81 ||
(res[2] != 0x0d && res[2] != 0x08)
))
{
printf("bad answer:");
for (i = 0; i < 6; ++i)
{
printf("%02X ", res[i]);
}
printf("\n");
return 0;
}
return 1;
}
static int do_voodoo(int fd) {
int i;
char recv_buf[256];
static struct {
char buf[64];
size_t len;
} reports[] = {
{{18, 0x01}, 2},
{{18, 0x05}, 2},
{{23, 0x05}, 2},
{{23, 0x01}, 2},
{{23, 0x11}, 2},
{{40, 0x00, 0x02}, 3},
{{40, 0x10, 0x00}, 3},
{{0x22, 0x14, 0x01, 0x00, 0x06, 0x00, 0x00, 0x03, 0xe8, 0x00, 0x01, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 27},
{{0x24, 0xfe, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x5e, 0x04, 0x73, 0x07, 0xf0, 0xe0, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}, 32},
};
for (i=0; i < sizeof(reports)/sizeof(reports[0]); i++) {
send_report(fd, reports[i].buf, reports[i].len);
get_report(fd, reports[i].buf[0], recv_buf, reports[i].len);
}
return 0;
}
void vrpn_Tracker_MotionNode::get_report()
{
if (NULL != m_handle) {
sampler_type * sampler = reinterpret_cast<sampler_type *>(m_handle);
sampler_type::Data data;
if (sampler->get_data_block(data, 1) && !data.empty()) {
// Copy the MotionNode SDK preview data map into the
// local quaternion storage.
std::size_t index = 0;
for (sampler_type::Data::iterator itr=data.begin(); itr!=data.end(); ++itr) {
int i = std::distance(data.begin(), itr);
if ((i >= 0) && (i < static_cast<int>(m_num_sensor))) {
std::vector<float> q = itr->second.getQuaternion(false);
vrpn_Tracker::d_sensor = i;
vrpn_Tracker::d_quat[0] = q[1];
vrpn_Tracker::d_quat[1] = q[2];
vrpn_Tracker::d_quat[2] = q[3];
vrpn_Tracker::d_quat[3] = q[0];
send_report();
}
}
}
}
}
static void mousing_can_send_now(void){
send_report(buttons, dx, dy);
// reset
dx = 0;
dy = 0;
if (buttons){
buttons = 0;
hid_device_request_can_send_now_event(hid_cid);
}
}
int8_t usb_keyboard_send_report(report_keyboard_t *report)
{
int8_t result = 0;
#ifdef USB_NKRO_ENABLE
if (keyboard_nkro)
result = send_report(report, KBD2_ENDPOINT, 0, KBD2_REPORT_KEYS);
else
#endif
{
if (usb_keyboard_protocol)
result = send_report(report, KBD_ENDPOINT, 0, KBD_REPORT_KEYS);
else
result = send_report(report, KBD_ENDPOINT, 0, 6);
}
if (result) return result;
usb_keyboard_idle_count = 0;
usb_keyboard_print_report(report);
return 0;
}
void vrpn_Tracker_ViewPoint::get_tracker()
{
// Get information for each eye
for (int i = 0; i < 2; i++) {
// The sensor
d_sensor = i;
// Which eye?
VPX_EyeType eye;
if (d_sensor == 0) eye = EYE_A;
else eye = EYE_B;
// Get tracker data from the DLL
VPX_RealPoint gp, cv, ga;
if (useSmoothedData) {
// Use smoothed data, when available
VPX_GetGazePointSmoothed2(eye, &gp);
VPX_GetComponentVelocity2(eye, &cv); // Always smoothed
VPX_GetGazeAngleSmoothed2(eye, &ga);
}
else {
// Use raw data
VPX_GetGazePoint2(eye, &gp);
VPX_GetComponentVelocity2(eye, &cv); // Always smoothed
VPX_GetGazeAngle2(eye, &ga);
}
// Set the tracker position from the gaze point
pos[0] = gp.x;
pos[1] = gp.y;
pos[2] = 0.0;
// Set the tracker velocity from the eye velocity
vel[0] = cv.x;
vel[1] = cv.y;
vel[2] = 0.0;
// Convert the gaze angle to a quaternion
q_from_euler(d_quat, 0.0, Q_DEG_TO_RAD(ga.y), Q_DEG_TO_RAD(ga.x));
// Send the data for this eye
send_report();
}
}
void vrpn_Tracker_PhaseSpace::report_rigid(vrpn_int32 sensor, const OWL::Rigid& r, bool is_stylus)
{
d_sensor = sensor;
if(debug)
{
printf("[debug] sensor=%d type=rigid tracker=%d x=%f y=%f z=%f w=%f a=%f b=%f c=%f cond=%f\n", d_sensor, r.id, r.pose[0], r.pose[1], r.pose[2], r.pose[3], r.pose[4], r.pose[5], r.pose[6], r.cond);
}
if(r.cond <= 0) return;
// set the position/orientation
set_pose(r);
send_report();
}
static int do_simple_voodoo(int fd) {
char buffer[27];
get_report(fd, 0x22, buffer, sizeof(buffer));
if (buffer[0] != 0x22 || buffer[1] != 0x14)
return 1;
buffer[2] = 1;
if (!buffer[3] && buffer[4] != 0x6) {
buffer[4] = 0x6;
send_report(fd, buffer, sizeof(buffer));
}
return 0;
}
/*
update the ADSB peripheral state
*/
void ADSB::update(void)
{
// calculate delta time in seconds
uint32_t now_us = AP_HAL::micros();
float delta_t = (now_us - last_update_us) * 1.0e-6f;
last_update_us = now_us;
for (uint8_t i=0; i<num_vehicles; i++) {
vehicles[i].update(delta_t);
}
// see if we should do a report
send_report();
}
static int mx_query(int fd, int b1, int *res)
{
int buf[6] = { first_byte, 0x81, b1, 0, 0, 0 };
send_report(fd, 0x10, buf, 6);
res[0] = -1;
query_report(fd, 0x10, res, 6);
if ((res[0] != 0x01 || res[1] != 0x81 || res[2] != 0xb1) &&
(res[0] != 0x02 || res[1] != 0x81 || (res[2] != 0x0d && res[2] != 0x08)))
{
printf("bad answer (%02x %02x %02x...)\n", res[0], res[1], res[2]);
return 0;
}
return 1;
}
void read_callback(int read_success)
{
if(read_success) {
//nunchuk_print_data(&nun);
int x = nun.X-128;
int y = nun.Y-128;
if(abs(x) < 15) x = 0;
if(abs(y) < 15) y = 0;
//x = (int)((float)x/7.5f);
//y = (int)((float)y/7.5f);
send_rel_mouse(x, -y);
if(nun.C && !last_nun.C) {
send_key_press(BTN_LEFT);
}
if(!nun.C && last_nun.C) {
send_key_release(BTN_LEFT);
}
if(nun.Z && !last_nun.Z) {
send_key_press(BTN_RIGHT);
}
if(!nun.Z && last_nun.Z) {
send_key_release(BTN_RIGHT);
}
send_report();
last_nun = nun;
if (y != 0 || x != 0) {
double r = x*x + y*y, p = atan2(y, x) * 4 / M_PI - 0.5;
const char *d = "RF";
if (p > -4) d = "XR";
if (p > -3) d = "RR";
if (p > -2) d = "RX";
if (p > -1) d = "FR";
if (p > 0) d = "FX";
if (p > 1) d = "FF";
if (p > 2) d = "XF";
if (p > 3) d = "RF";
if (r > 6400) {
lirc_send(d);
return;
}
}
lirc_send(brake);
}
else
lirc_send(NULL);
}
int vrpn_Tracker_NDI_Polaris::get_report(void)
// returns 0 on fail
{
vrpn_gettimeofday(×tamp, NULL);
sendCommand("TX ");
readResponse();
//printf("DEBUG: TX response: >%s %i<\n",latestResponseStr);
//int numOfHandles=parse2CharIntFromNDIResponse(latestResponseStr);
int TXResponseStrIndex=2;
bool gotAtLeastOneReport=false;
for (int t=0; t<numOfRigidBodies; t++) {
//int handleNum=parse2CharIntFromNDIResponse(latestResponseStr,&TXResponseStrIndex);
// check if the tool is present. Parse and print out the transform data if and only if
// it's not missing
if (strncmp("MISSING",(char *) &latestResponseStr[TXResponseStrIndex],7)!=0) {
gotAtLeastOneReport=true;
d_sensor = t;
// NDI gives qw first(qw,qx,qy,qz), whereas VRPN wants it last (qx,qy,qz,qw)
d_quat[3]=parse6CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex); //qw
d_quat[0]=parse6CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex); //qx
d_quat[1]=parse6CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex); //qy
d_quat[2]=parse6CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex); //qz
pos[0]=parse7CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex);
pos[1]=parse7CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex);
pos[2]=parse7CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex);
send_report(); //call this once per sensor
}
//seek to the new line character (since each rigid body is on its own line)
while (latestResponseStr[TXResponseStrIndex]!='\n') {
TXResponseStrIndex++;
}
TXResponseStrIndex++; // advance one more char as to be on the next line of text
}// for
return(gotAtLeastOneReport); //return 1 if something was sent, 0 otherwise
}
void vrpn_Tracker_Flock_Parallel_Slave::mainloop()
{
// We don't call the generic server mainloop code, because the master unit
// will have done that for us.
switch (status) {
case vrpn_TRACKER_SYNCING:
case vrpn_TRACKER_PARTIAL:
{
// It turns out to be important to get the report before checking
// to see if it has been too long since the last report. This is
// because there is the possibility that some other device running
// in the same server may have taken a long time on its last pass
// through mainloop(). Trackers that are resetting do this. When
// this happens, you can get an infinite loop -- where one tracker
// resets and causes the other to timeout, and then it returns the
// favor. By checking for the report here, we reset the timestamp
// if there is a report ready (ie, if THIS device is still operating).
while (get_report()) {
send_report();
}
struct timeval current_time;
vrpn_gettimeofday(¤t_time, NULL);
if ( duration(current_time,timestamp) > MAX_TIME_INTERVAL) {
fprintf(stderr,"Tracker failed to read... current_time=%ld:%ld, timestamp=%ld:%ld\n",
current_time.tv_sec, static_cast<long>(current_time.tv_usec),
timestamp.tv_sec, static_cast<long>(timestamp.tv_usec));
send_text_message("Too long since last report, resetting", current_time, vrpn_TEXT_ERROR);
status = vrpn_TRACKER_FAIL;
}
}
break;
// master resets us
case vrpn_TRACKER_RESETTING:
break;
case vrpn_TRACKER_FAIL:
// here we just fail and let the master figure out that we have
// failed and need to be reset
fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel_Slave %d: tracker "
"failed, trying to reset ...", d_sensor);
break;
}
}
void vrpn_Tracker_NovintFalcon::mainloop()
{
struct timeval current_time;
server_mainloop();
// no need to report more often than we can poll the device
vrpn_gettimeofday(¤t_time, NULL);
if ( timediff(current_time, m_timestamp) >= 1000000.0/m_update_rate) {
// Update the time
m_timestamp.tv_sec = current_time.tv_sec;
m_timestamp.tv_usec = current_time.tv_usec;
switch(status)
{
case vrpn_TRACKER_AWAITING_STATION:
case vrpn_TRACKER_PARTIAL:
case vrpn_TRACKER_SYNCING:
if (get_report()) {
send_report();
vrpn_Button::report_changes();
handle_forces();
}
break;
case vrpn_TRACKER_RESETTING:
reset();
break;
case vrpn_TRACKER_FAIL:
fprintf(stderr, "NovintFalcon #%d failed, trying to reset (Try power cycle if more than 4 attempts made)\n",
vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags);
status = vrpn_TRACKER_RESETTING;
break;
default:
fprintf(stderr, "NovintFalcon #%d , unknown status message: %d)\n",
vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags, status);
break;
}
}
}
void vrpn_Tracker_PhaseSpace::report_marker(vrpn_int32 sensor, const OWL::Marker &m)
{
d_sensor = sensor;
if(debug)
{
int tr = context.markerInfo(m.id).tracker_id;
printf("[debug] sensor=%d type=point tracker=%d led=%d x=%f y=%f z=%f cond=%f\n", d_sensor, tr, m.id, m.x, m.y, m.z, m.cond);
}
if(m.cond <= 0) return;
pos[0] = m.x;
pos[1] = m.y;
pos[2] = m.z;
//raw positions have no rotation
d_quat[0] = 0;
d_quat[1] = 0;
d_quat[2] = 0;
d_quat[3] = 1;
send_report();
}
int main(int argc, char **argv)
{
WSADATA wsd;
SOCKET sock;
int msg_size;
char path[SIZE_BUF];
char name[SIZE_STR];
char buffer[SIZE_BUF];
char author[SIZE_STR];
char command[SIZE_CMD];
char content[SIZE_CONTENT];
ValidateArgs(argc, argv);
if (WSAStartup(MAKEWORD(2,2), &wsd) != 0)
{
printf("Failed to load Winsock library!\n");
return 1;
}
if (binterface)
{
client.sin_addr.s_addr = inet_addr(szAddress);
if (client.sin_addr.s_addr == INADDR_NONE)
usage();
}
else
client.sin_addr.s_addr = htonl(INADDR_ANY);
client.sin_family = AF_INET;
client.sin_port = htons(port);
int len = sizeof(client);
if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
{
printf("Socket is not created: %d\n", WSAGetLastError());
exit(1);
}
memset(buffer, 0, sizeof(buffer));
while(strcmp(buffer, ":start"))
{
fgets(buffer, sizeof(buffer), stdin);
if (buffer[strlen(buffer) - 1] == '\n')
buffer[strlen(buffer) - 1] = '\0';
if ((msg_size = sendto(sock, buffer, strlen(buffer), 0, (struct sockaddr *)&client, sizeof(client))) == SOCKET_ERROR)
{
printf("SEND start message failed: %d\n", WSAGetLastError());
exit(1);
}
}
//printf("SEND [%d bytes]: start message '%s'\n", msg_size, buffer);
memset(buffer, 0, sizeof(buffer));
if ((msg_size = recvfrom(sock, buffer, sizeof(buffer), 0, (struct sockaddr *)&client, &len)) == SOCKET_ERROR)
{
printf("RECV directory content failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("RECV [%d bytes]: directory content\n", msg_size);
output(buffer);
send_report(sock, SUCCESS);
while(1)
{
memset(path, 0, sizeof(path));
if ((msg_size = recvfrom(sock, path, sizeof(path), 0, (struct sockaddr *)&client, &len)) == SOCKET_ERROR)
{
printf("RECV current path failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("RECV [%d bytes]: current path '%s'\n", msg_size, path);
send_report(sock, SUCCESS);
memset(buffer, 0, sizeof(buffer));
if ((msg_size = recvfrom(sock, buffer, sizeof(buffer), 0, (struct sockaddr *)&client, &len)) == SOCKET_ERROR)
{
printf("RECV invitation message failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("RECV [%d bytes]: invitation message\n", msg_size);
output(buffer);
char space;
memset(name, 0, sizeof(name));
memset(buffer, 0, sizeof(buffer));
memset(author, 0, sizeof(author));
memset(command, 0, sizeof(command));
memset(content, 0, sizeof(content));
scanf("%5s%1c", command, &space);
if ((msg_size = sendto(sock, command, strlen(command), 0, (struct sockaddr *)&client, sizeof(client))) == SOCKET_ERROR)
{
printf("SEND command failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("SEND [%d bytes]: command '%s'\n", msg_size, path);
if (!strcmp(command, ":exit"))
{
memset(buffer, 0, sizeof(buffer));
if ((msg_size = recvfrom(sock, buffer, sizeof(buffer), 0, (struct sockaddr *)&client, &len)) == SOCKET_ERROR) // Receive the content of file
{
printf("RECV file or directory content failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("RECV [%d bytes]: file or directory content\n", msg_size);
output(buffer);
break;
}
if (recv_report(sock) < 0)
{
puts("!No such command");
send_report(sock, SUCCESS);
}
if (!strcmp(command, "add"))
{
char str[SIZE_ARG];
fgets(name, sizeof(name), stdin);
if (name[strlen(name) - 1] == '\n')
name[strlen(name) - 1] = '\0';
if ((msg_size = sendto(sock, name, strlen(name), 0, (struct sockaddr *)&client, sizeof(client))) == SOCKET_ERROR)
{
printf("SEND command failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("SEND [%d bytes]: title of article '%s'\n", msg_size, name);
if (recv_report(sock) < 0)
{
puts("!Such file already exist");
send_report(sock, SUCCESS);
}
else
{
int length = sizeof(content) - sizeof(author) - sizeof(name);
printf("Input author: ");
fgets(author, sizeof(author), stdin);
if (author[strlen(author) - 1] == '\n')
author[strlen(author) - 1] = '\0';
printf("name's read: %s [%d bytes]\n", name, msg_size);
printf("author's read: %s [%d bytes]\n", author, msg_size);
puts("Put content:");
printf("[%d of %d] ", (strlen(content)+strlen(str)), length);
while (fgets(str, sizeof(str), stdin) != NULL)
{
if (!strncmp(":end", str, strlen(":end")))
break;
if ((strlen(content)+strlen(str)) > length)
{
puts("!Text size will not allow");
memset(str, 0, strlen(str));
printf("[%d of %d] ",(strlen(content)+strlen(str)), length );
}
strcat(content, str);
memset(str, 0, strlen(str));
}
if ((msg_size = sendto(sock, author, strlen(author), 0, (struct sockaddr *)&client, sizeof(client))) == SOCKET_ERROR)
{
printf("SEND author of article failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("SEND [%d bytes]: author of article '%s'\n", msg_size, author);
recv_report(sock);
if ((msg_size = sendto(sock, content, strlen(content), 0, (struct sockaddr *)&client, sizeof(client))) == SOCKET_ERROR)
{
printf("SEND file content failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("SEND [%d bytes]: file content '%s'\n", msg_size, content);
if (recv_report(sock) < 0)
puts("!Such file already exist");
send_report(sock, SUCCESS);
}
}
gets(buffer);
if (!strcmp(command, "open"))
{
strcat(path, buffer);
if ((msg_size = sendto(sock, path, strlen(path), 0, (struct sockaddr *)&client, sizeof(client))) == SOCKET_ERROR)
{
printf("SEND full path to file failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("SEND [%d bytes]: full path to file '%s'\n", msg_size, path);
}
else if (!strcmp(command, "find"))
{
if ((msg_size = sendto(sock, buffer, strlen(buffer), 0, (struct sockaddr *)&client, sizeof(client))) == SOCKET_ERROR)
{
printf("SEND author to find failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("SEND [%d bytes]: author to find '%s'\n", msg_size, buffer);
}
memset(content, 0, sizeof(content));
if ((msg_size = recvfrom(sock, content, sizeof(content), 0, (struct sockaddr *)&client, &len)) == SOCKET_ERROR) // Receive the content of file
{
printf("RECV file or directory content failed: %d\n", WSAGetLastError());
exit(1);
}
//printf("RECV [%d bytes]: file or directory content\n", msg_size);
output(content);
send_report(sock, SUCCESS);
}
closesocket(sock);
WSACleanup();
return 0;
}
/*
update the gimbal state
*/
void Gimbal::update(void)
{
// calculate delta time in seconds
uint32_t now_us = hal.scheduler->micros();
float delta_t = (now_us - last_update_us) * 1.0e-6f;
last_update_us = now_us;
Matrix3f vehicle_dcm;
vehicle_dcm.from_euler(radians(fdm.rollDeg), radians(fdm.pitchDeg), radians(fdm.yawDeg));
Vector3f vehicle_gyro = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
Vector3f vehicle_accel_body = Vector3f(fdm.xAccel, fdm.yAccel, fdm.zAccel);
// take a copy of the demanded rates to bypass the limiter function for testing
Vector3f demRateRaw = demanded_angular_rate;
// 1) Rotate the copters rotation rates into the gimbals frame of reference
// copterAngRate_G = transpose(DCMgimbal)*DCMcopter*copterAngRate
Vector3f copterAngRate_G = dcm.transposed()*vehicle_dcm*vehicle_gyro;
// 2) Subtract the copters body rates to obtain a copter relative rotational
// rate vector (X,Y,Z) in gimbal sensor frame
// relativeGimbalRate(X,Y,Z) = gimbalRateDemand - copterAngRate_G
Vector3f relativeGimbalRate = demanded_angular_rate - copterAngRate_G;
// calculate joint angles (euler312 order)
// calculate copter -> gimbal rotation matrix
Matrix3f rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm;
joint_angles = rotmat_copter_gimbal.transposed().to_euler312();
/* 4) For each of the three joints, calculate upper and lower rate limits
from the corresponding angle limits and current joint angles
upperRatelimit = (jointAngle - lowerAngleLimit) * travelLimitGain
lowerRatelimit = (jointAngle - upperAngleLimit) * travelLimitGain
travelLimitGain is equal to the inverse of the bump stop time constant and
should be set to something like 20 initially. If set too high it can cause
the rates to 'ring' when they the limiter is in force, particularly given
we are using a first order numerical integration.
*/
Vector3f upperRatelimit = -(joint_angles - upper_joint_limits) * travelLimitGain;
Vector3f lowerRatelimit = -(joint_angles - lower_joint_limits) * travelLimitGain;
/*
5) Calculate the gimbal joint rates (roll, elevation, azimuth)
gimbalJointRates(roll, elev, azimuth) = Matrix*relativeGimbalRate(X,Y,Z)
where matrix =
+- -+
| cos(elevAngle), 0, sin(elevAngle) |
| |
| sin(elevAngle) tan(rollAngle), 1, -cos(elevAngle) tan(rollAngle) |
| |
| sin(elevAngle) cos(elevAngle) |
| - --------------, 0, -------------- |
| cos(rollAngle) cos(rollAngle) |
+- -+
*/
float rollAngle = joint_angles.x;
float elevAngle = joint_angles.y;
Matrix3f matrix = Matrix3f(Vector3f(cosf(elevAngle), 0, sinf(elevAngle)),
Vector3f(sinf(elevAngle)*tanf(rollAngle), 1, -cosf(elevAngle)*tanf(rollAngle)),
Vector3f(-sinf(elevAngle)/cosf(rollAngle), 0, cosf(elevAngle)/cosf(rollAngle)));
Vector3f gimbalJointRates = matrix * relativeGimbalRate;
// 6) Apply the rate limits from 4)
gimbalJointRates.x = constrain_float(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x);
gimbalJointRates.y = constrain_float(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y);
gimbalJointRates.z = constrain_float(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z);
/*
7) Convert the modified gimbal joint rates to body rates (still copter
relative)
relativeGimbalRate(X,Y,Z) = Matrix * gimbalJointRates(roll, elev, azimuth)
where Matrix =
+- -+
| cos(elevAngle), 0, -cos(rollAngle) sin(elevAngle) |
| |
| 0, 1, sin(rollAngle) |
| |
| sin(elevAngle), 0, cos(elevAngle) cos(rollAngle) |
+- -+
*/
matrix = Matrix3f(Vector3f(cosf(elevAngle), 0, -cosf(rollAngle)*sinf(elevAngle)),
Vector3f(0, 1, sinf(rollAngle)),
Vector3f(sinf(elevAngle), 0, cosf(elevAngle)*cosf(rollAngle)));
relativeGimbalRate = matrix * gimbalJointRates;
// 8) Add to the result from step 1) to obtain the demanded gimbal body rates
// in an inertial frame of reference
// demandedGimbalRatesInertial(X,Y,Z) = relativeGimbalRate(X,Y,Z) + copterAngRate_G
// Vector3f demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G;
// for the moment we will set gyros equal to demanded_angular_rate
gimbal_angular_rate = demRateRaw; // demandedGimbalRatesInertial + true_gyro_bias - supplied_gyro_bias
// update rotation of the gimbal
dcm.rotate(gimbal_angular_rate*delta_t);
dcm.normalize();
// calculate copter -> gimbal rotation matrix
rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm;
// calculate joint angles (euler312 order)
joint_angles = rotmat_copter_gimbal.transposed().to_euler312();
// update observed gyro
gyro = gimbal_angular_rate + true_gyro_bias;
// update delta_angle (integrate)
delta_angle += gyro * delta_t;
// calculate accel in gimbal body frame
Vector3f copter_accel_earth = vehicle_dcm * vehicle_accel_body;
Vector3f accel = dcm.transposed() * copter_accel_earth;
// integrate velocity
delta_velocity += accel * delta_t;
// see if we should do a report
send_report();
}
bool AdminRequestHandler::handleStatsRequest(const std::string &cmd,
Transport *transport) {
if (cmd == "stats-on") {
RuntimeOption::EnableStats = true;
transport->sendString("OK\n");
return true;
}
if (cmd == "stats-off") {
RuntimeOption::EnableStats = false;
transport->sendString("OK\n");
return true;
}
if (cmd == "stats-clear") {
ServerStats::Clear();
transport->sendString("OK\n");
return true;
}
if (cmd == "stats-web") {
return toggle_switch(transport, RuntimeOption::EnableWebStats);
}
if (cmd == "stats-mem") {
toggle_switch(transport, RuntimeOption::EnableMemoryStats);
return true;
}
if (cmd == "stats-malloc") {
toggle_switch(transport, RuntimeOption::EnableMallocStats);
LeakDetectable::EnableMallocStats(RuntimeOption::EnableMallocStats);
return true;
}
if (cmd == "stats-apc") {
return toggle_switch(transport, RuntimeOption::EnableAPCStats);
}
if (cmd == "stats-apc-key") {
return toggle_switch(transport, RuntimeOption::EnableAPCKeyStats);
}
if (cmd == "stats-mcc") {
return toggle_switch(transport, RuntimeOption::EnableMemcacheStats);
}
if (cmd == "stats-sql") {
return toggle_switch(transport, RuntimeOption::EnableSQLStats);
}
if (cmd == "stats-mutex") {
int sampling = transport->getIntParam("sampling");
if (sampling > 0) {
LockProfiler::s_profile_sampling = sampling;
}
return toggle_switch(transport, LockProfiler::s_profile);
}
if (cmd == "stats.keys") {
int64 from = transport->getInt64Param("from");
int64 to = transport->getInt64Param("to");
string out;
ServerStats::GetKeys(out, from, to);
transport->sendString(out);
return true;
}
if (cmd == "stats.xml") {
return send_report(transport, ServerStats::XML, "application/xml");
}
if (cmd == "stats.json") {
return send_report(transport, ServerStats::JSON, "application/json");
}
if (cmd == "stats.kvp") {
return send_report(transport, ServerStats::KVP, "text/plain");
}
if (cmd == "stats.html" || cmd == "stats.htm") {
return send_report(transport, ServerStats::HTML, "text/html");
}
if (cmd == "stats.xsl") {
string xsl;
if (!RuntimeOption::StatsXSLProxy.empty()) {
StringBuffer response;
if (HttpClient().get(RuntimeOption::StatsXSLProxy.c_str(), response) ==
200) {
xsl = response.data();
if (!xsl.empty()) {
transport->addHeader("Content-Type", "application/xml");
transport->sendString(xsl);
return true;
}
}
}
transport->sendString("Not Found\n", 404);
return true;
}
return false;
}
int vrpn_Tracker_PhaseSpace::get_report(void)
{
if(!owlRunning) return 0;
int maxiter = 1;
if(readMostRecent) maxiter = 1024; // not technically most recent, but if the client is slow, avoids infinite loop.
int ret = 1;
int oldframe = frame;
for(int i = 0; i < maxiter && ret; i++) {
int cframe = frame;
while(ret && cframe == frame) {
ret = read_frame();
}
}
// no new data? abort.
if(oldframe == frame)
return 0;
#ifdef DEBUG
char buffer[1024];
owlGetString(OWL_FRAME_BUFFER_SIZE, buffer);
printf("%s\n", buffer);
#endif
for(int i = 0; i < markers.size(); i++)
{
if(markers[i].cond <= 0) continue;
//set the sensor
d_sensor = markers[i].id;
pos[0] = markers[i].x;
pos[1] = markers[i].y;
pos[2] = markers[i].z;
//raw positions have no rotation
d_quat[0] = 0;
d_quat[1] = 0;
d_quat[2] = 0;
d_quat[3] = 1;
// send time out in OWL time
if(frequency) frame_to_time(frame, frequency, timestamp);
else memset(×tamp, 0, sizeof(timestamp));
//send the report
send_report();
}
for(int j = 0; j < rigids.size(); j++)
{
if(rigids[j].cond <= 0) continue;
//set the sensor
d_sensor = r2s_map[rigids[j].id];
if(slave && d_sensor == 0) {
// rigid bodies aren't allowed to be sensor zero in slave mode
r2s_map[rigids[j].id] = rigids[j].id;
}
//set the position
pos[0] = rigids[j].pose[0];
pos[1] = rigids[j].pose[1];
pos[2] = rigids[j].pose[2];
//set the orientation quaternion
//OWL has the scale factor first, whereas VRPN has it last.
d_quat[0] = rigids[j].pose[4];
d_quat[1] = rigids[j].pose[5];;
d_quat[2] = rigids[j].pose[6];;
d_quat[3] = rigids[j].pose[3];;
// send time out in OWL time
if(frequency) frame_to_time(frame, frequency, timestamp);
else memset(×tamp, 0, sizeof(timestamp));
//send the report
send_report();
}
return markers.size() || rigids.size() > 0 ? 1 : 0;
}
static void configure(int handle, int argc, char **argv)
{
int i, arg1, arg2;
for (i = 1; i < argc; ++i)
{
int perm = 0x80;
char *cmd = argv[i];
if (strneq(cmd, "temp-", 5))
perm = 0, cmd += 5;
if (streq(cmd, "free"))
{
mx_cmd(handle, perm + 1, 0, 0);
}
else if (streq(cmd, "click"))
{
mx_cmd(handle, perm + 2, 0, 0);
}
else if (strneq(cmd, "manual", 6))
{
twoargs(cmd + 6, &arg1, &arg2, 0, 0, 15);
if (arg1 != arg2)
mx_cmd(handle, perm + 7, arg1 * 16 + arg2, 0);
else
mx_cmd(handle, perm + 8, arg1, 0);
}
else if (strneq(cmd, "auto", 4))
{
twoargs(cmd + 4, &arg1, &arg2, 0, 0, 50);
mx_cmd(handle, perm + 5, arg1, arg2);
}
else if (strneq(argv[i], "soft-free", 9))
{
twoargs(argv[i] + 9, &arg1, &arg2, 0, 0, 255);
mx_cmd(handle, 3, arg1, arg2);
}
else if (strneq(argv[i], "soft-click", 10))
{
twoargs(argv[i] + 10, &arg1, &arg2, 0, 0, 255);
mx_cmd(handle, 4, arg1, arg2);
}
else if (strneq(argv[i], "reconnect", 9))
{
static const int cmd[] = { 0xff, 0x80, 0xb2, 1, 0, 0 };
twoargs(argv[i] + 9, &arg1, &arg2, 0, 0, 255);
send_report(handle, 0x10, cmd, 6);
printf("Reconnection initiated\n");
printf(" - Turn off the mouse\n");
printf(" - Press and hold the left mouse button\n");
printf(" - Turn on the mouse\n");
printf(" - Press the right button 5 times\n");
printf(" - Release the left mouse button\n");
wait_report(handle, 60000);
}
else if (strneq(argv[i], "mode", 4))
{
int buf[6];
if (mx_query(handle, 0x08, buf))
{
if (buf[5] & 1)
printf("click-by-click\n");
else
printf("free spinning\n");
}
}
else if (strneq(argv[i], "battery", 7))
{
int buf[6];
if (mx_query(handle, 0x0d, buf))
{
char str[32], *st;
switch (buf[5])
{
case 0x30: st = "running on battery"; break;
case 0x50: st = "charging"; break;
case 0x90: st = "fully charged"; break;
default: sprintf(st = str, "status %02x", buf[5]);
}
printf("battery level %d%%, %s\n", buf[3], st);
}
}
/*** debug commands ***/
else if (strneq(argv[i], "raw", 3))
{
int buf[256], n;
n = nargs(argv[i] + 3, buf, 256, 0, 0, 255);
send_report(handle, buf[0], buf+1, n-1);
}
else if (strneq(argv[i], "query", 5))
{
int buf[256], j;
twoargs(argv[i] + 5, &arg1, &arg2, -1, 0, 255);
if (arg1 == -1)
arg1 = 0x10, arg2 = 6;
query_report(handle, arg1, buf, arg2);
printf("report %02x:", arg1);
for (j = 0; j < arg2; ++j)
printf(" %02x", buf[j]);
printf("\n");
}
else if (strneq(argv[i], "dump", 4))
{
twoargs(cmd + 4, &arg1, &arg2, 3, -1, 24*60*60);
if (arg1 > 0)
arg1 *= 1000;
while (wait_for_input(handle, arg1) > 0)
{
struct hiddev_usage_ref uref;
if (read(handle, &uref, sizeof(uref)) == sizeof(uref))
printf("read: type=%u, id=%u, field=%08x, usage=%08x,"
" code=%08x, value=%u\n",
uref.report_type, uref.report_id, uref.field_index,
uref.usage_index, uref.usage_code, uref.value);
}
}
else if (strneq(argv[i], "sleep", 5))
{
twoargs(argv[i] + 5, &arg1, &arg2, 1, 0, 255);
sleep(arg1);
}
else
fatal("unknown option `%s'", argv[i]);
}
}
static void mx_cmd(int fd, int b1, int b2, int b3)
{
int buf[6] = { first_byte, 0x80, 0x56, b1, b2, b3 };
send_report(fd, 0x10, buf, 6);
}
// Parse the given buffer as a SNMPv3-Message.
int v3MP::snmp_parse(Snmp *snmp_session,
struct snmp_pdu *pdu,
unsigned char *inBuf,
int inBufLength,
OctetStr &securityEngineID,
OctetStr &securityName,
OctetStr &contextEngineID,
OctetStr &contextName,
long &securityLevel,
long &msgSecurityModel,
snmp_version &spp_version,
UdpAddress from_address)
{
debugprintf(3, "mp is parsing incoming message:");
debughexprintf(25, inBuf, inBufLength);
if (inBufLength > MAX_SNMP_PACKET)
return SNMPv3_MP_ERROR;
unsigned char type;
long version;
int origLength = inBufLength;
unsigned char *inBufPtr = inBuf;
long msgID, msgMaxSize;
unsigned char msgFlags;
Buffer<unsigned char> msgSecurityParameters(MAX_SNMP_PACKET);
Buffer<unsigned char> msgData(MAX_SNMP_PACKET);
int msgSecurityParametersLength = inBufLength, msgDataLength = inBufLength;
Buffer<unsigned char> scopedPDU(MAX_SNMP_PACKET);
int scopedPDULength = MAX_SNMP_PACKET;
long maxSizeResponseScopedPDU = 0;
struct SecurityStateReference *securityStateReference = NULL;
int securityParametersPosition;
int rc;
int errorCode = 0;
// get the type
inBuf = asn_parse_header( inBuf, &inBufLength, &type);
if (inBuf == NULL){
debugprintf(0, "snmp_parse: bad header");
return SNMPv3_MP_PARSE_ERROR;
}
if (type != (ASN_SEQ_CON)){
debugprintf(0, "snmp_parse: wrong auth header type");
return SNMPv3_MP_PARSE_ERROR;
}
if (origLength != inBufLength + (inBuf - inBufPtr)) {
debugprintf(0, "snmp_parse: wrong length of received packet");
return SNMPv3_MP_PARSE_ERROR;
}
// get the version
inBuf = asn_parse_int(inBuf, &inBufLength, &type, &version);
if (inBuf == NULL){
debugprintf(0, "snmp_parse: bad parse of version");
return SNMPv3_MP_PARSE_ERROR;
}
debugprintf(3, "Parsed length(%x), version(0x%lx)", inBufLength, version);
if ( version != SNMP_VERSION_3 )
return SNMPv3_MP_PARSE_ERROR;
spp_version = (snmp_version) version;
inBuf = asn1_parse_header_data(inBuf, &inBufLength,
&msgID, &msgMaxSize,
&msgFlags, &msgSecurityModel);
if (inBuf == NULL){
debugprintf(0, "snmp_parse: bad parse of msgHeaderData");
return SNMPv3_MP_PARSE_ERROR;
}
pdu->msgid = msgID;
if ((msgMaxSize < 484) || (msgMaxSize > 0x7FFFFFFF)) {
debugprintf(0, "snmp_parse: bad parse of msgMaxSize");
return SNMPv3_MP_PARSE_ERROR;
}
// do not allow larger messages than this entity can handle
if (msgMaxSize > MAX_SNMP_PACKET) msgMaxSize = MAX_SNMP_PACKET;
pdu->maxsize_scopedpdu = msgMaxSize;
inBuf = asn_parse_string( inBuf, &inBufLength, &type,
msgSecurityParameters.get_ptr(),
&msgSecurityParametersLength);
if (inBuf == NULL){
debugprintf(0, "snmp_parse: bad parse of msgSecurityParameters");
return SNMPv3_MP_PARSE_ERROR;
}
securityParametersPosition= SAFE_INT_CAST(inBuf - inBufPtr) - msgSecurityParametersLength;
// the rest of the message is passed directly to the security module
msgDataLength = origLength - SAFE_INT_CAST(inBuf - inBufPtr);
memcpy(msgData.get_ptr(), inBuf, msgDataLength);
debugprintf(3, "Parsed msgdata length(0x%x), "
"msgSecurityParameters length(0x%x)", msgDataLength,
msgSecurityParametersLength);
switch (msgFlags & 0x03) {
case 3: { securityLevel = SNMP_SECURITY_LEVEL_AUTH_PRIV; break;}
case 0: { securityLevel = SNMP_SECURITY_LEVEL_NOAUTH_NOPRIV; break;}
case 1: { securityLevel = SNMP_SECURITY_LEVEL_AUTH_NOPRIV; break;}
default: { securityLevel = SNMP_SECURITY_LEVEL_NOAUTH_NOPRIV;
snmpInvalidMsgs++;
// do not send back report
return SNMPv3_MP_INVALID_MESSAGE;
break;
}
}
bool reportableFlag;
if (msgFlags & 0x04) reportableFlag = true;
else reportableFlag = false;
securityStateReference = usm->get_new_sec_state_reference();
if (!securityStateReference)
return SNMPv3_MP_ERROR;
switch (msgSecurityModel) {
case SNMP_SECURITY_MODEL_USM:
{
rc = usm->process_msg(
msgMaxSize,
msgSecurityParameters.get_ptr(),
msgSecurityParametersLength,
securityParametersPosition,
securityLevel,
inBufPtr, origLength, //wholeMsg
msgData.get_ptr(), msgDataLength,
securityEngineID,
securityName,
scopedPDU.get_ptr(), &scopedPDULength,
&maxSizeResponseScopedPDU,
securityStateReference,
from_address);
pdu->maxsize_scopedpdu = maxSizeResponseScopedPDU;
if (rc != SNMPv3_USM_OK) {
if (rc == SNMPv3_USM_NOT_IN_TIME_WINDOW) {
errorCode = SNMPv3_MP_NOT_IN_TIME_WINDOW;
}
else {
// error handling! rfc2262 page 31
debugprintf(0, "mp: error while executing USM::process_msg");
errorCode = rc;
}
}
if (errorCode != SNMPv3_USM_PARSE_ERROR)
if (securityEngineID.len() == 0)
errorCode = SNMPv3_MP_INVALID_ENGINEID;
break;
}
default: {
snmpUnknownSecurityModels++;
usm->delete_sec_state_reference(securityStateReference);
debugprintf(0, "SecurityModel of incomming Message not supported!");
// Message should be dropped without a report
return SNMPv3_MP_UNSUPPORTED_SECURITY_MODEL;
}
}
// process scopedPDU
debughexcprintf(21, "scoped PDU", scopedPDU.get_ptr(), scopedPDULength);
unsigned char *scopedPDUPtr= scopedPDU.get_ptr();
unsigned char tmp_contextEngineID[MAXLENGTH_ENGINEID];
unsigned char tmp_contextName[MAXLENGTH_CONTEXT_NAME];
int tmp_contextEngineIDLength = MAXLENGTH_ENGINEID;
int tmp_contextNameLength = MAXLENGTH_CONTEXT_NAME;
unsigned char *data;
int dataLength;
debugprintf(1,"ErrorCode is %i",errorCode);
if (!errorCode) {
data = asn1_parse_scoped_pdu(scopedPDUPtr, &scopedPDULength,
tmp_contextEngineID,
&tmp_contextEngineIDLength,
tmp_contextName, &tmp_contextNameLength);
if (data == NULL) {
debugprintf(0, "mp: Error Parsing scopedPDU!");
usm->delete_sec_state_reference(securityStateReference);
return SNMPv3_MP_PARSE_ERROR;
}
dataLength = scopedPDULength;
contextEngineID.set_data(tmp_contextEngineID, tmp_contextEngineIDLength);
contextName.set_data(tmp_contextName, tmp_contextNameLength);
// parse data of scopedPDU
if (snmp_parse_data_pdu(pdu, data, dataLength) != SNMP_CLASS_SUCCESS) {
debugprintf(0, "mp: Error parsing PDU!");
usm->delete_sec_state_reference(securityStateReference);
return SNMPv3_MP_PARSE_ERROR;
}
if (SNMP_CLASS_SUCCESS != snmp_parse_vb(pdu, data, dataLength)) {
debugprintf(0, "mp: Error parsing Vb");
usm->delete_sec_state_reference(securityStateReference);
return SNMPv3_MP_PARSE_ERROR;
}
if ((tmp_contextEngineIDLength == 0) &&
((pdu->command == GET_REQ_MSG) || (pdu->command == GETNEXT_REQ_MSG) ||
(pdu->command == SET_REQ_MSG) || (pdu->command == GETBULK_REQ_MSG) ||
(pdu->command == TRP_REQ_MSG) || (pdu->command == INFORM_REQ_MSG) ||
(pdu->command == TRP2_REQ_MSG)))
{
// RFC 2572 � 4.2.2.1 (2a)
debugprintf(2, "mp: received request message with zero length"
" contextEngineID -> unknownPduHandlers.");
inc_stats_unknown_pdu_handlers();
errorCode = SNMPv3_MP_UNKNOWN_PDU_HANDLERS;
}
}
if (errorCode) {
if ((reportableFlag) && (errorCode != SNMPv3_USM_PARSE_ERROR)) {
// error occured: prepare reportpdu in agent
cache.add_entry(msgID, pdu->reqid, securityEngineID,
msgSecurityModel,
securityName, securityLevel, "", "",
securityStateReference, errorCode, CACHE_REMOTE_REQ);
send_report(scopedPDUPtr, scopedPDULength, pdu, errorCode,
securityLevel, msgSecurityModel, securityName,
from_address, snmp_session);
clear_pdu(pdu, true); // Clear pdu and free all content AND IDs!
}
else {
usm->delete_sec_state_reference(securityStateReference);
}
return errorCode;
}
struct Cache::Entry_T centry;
if ((pdu->command == GET_RSP_MSG) || (pdu->command == REPORT_MSG)) {
rc = cache.get_entry(msgID, true, ¢ry);
if (rc != SNMPv3_MP_OK) {
// RFC 2572 � 4
debugprintf(2, "Received rspMsg without outstanding request."
" -> SnmpUnknownPduHandler");
usm->delete_sec_state_reference(securityStateReference);
inc_stats_unknown_pdu_handlers();
return SNMPv3_MP_UNKNOWN_PDU_HANDLERS;
}
if (((pdu->reqid == 0) || (pdu->reqid == 0x7fffffff))
&& (pdu->command == REPORT_MSG))
pdu->reqid = centry.req_id;
#ifdef BUGGY_REPORT_REQID
if ((pdu->reqid != centry.req_id) && (pdu->command == REPORT_MSG))
{
debugprintf(0, "WARNING: setting reqid of REPORT PDU (from) (to): (%ld) (%ld)", pdu->reqid, centry.req_id);
pdu->reqid = centry.req_id;
}
#endif
}
if (pdu->command == REPORT_MSG) {
// !! rfc2262 page 33
debugprintf(2, "*** Receiving a ReportPDU ***");
if (/*((securityEngineID != centry.sec_engine_id)
&& (centry.sec_engine_id.len() != 0)) ||*/
((msgSecurityModel != centry.sec_model)
&& (msgSecurityModel != SNMP_SECURITY_MODEL_USM)) ||
((securityName != centry.sec_name)
&& (securityName.len() != 0)))
{
debugprintf(0, "Received report message doesn't match sent message!");
usm->delete_sec_state_reference(securityStateReference);
return SNMPv3_MP_MATCH_ERROR;
}
usm->delete_sec_state_reference(securityStateReference);
cache.delete_content(centry);
debugprintf(1, "mp finished (OK)");
return SNMPv3_MP_OK;
}
if (pdu->command == GET_RSP_MSG) {
if (((securityEngineID != centry.sec_engine_id)
&& (centry.sec_engine_id.len() != 0)) ||
(msgSecurityModel != centry.sec_model) ||
(securityName != centry.sec_name) ||
(securityLevel != centry.sec_level) ||
((contextEngineID != centry.context_engine_id)
&& (centry.context_engine_id.len() != 0))||
((contextName != centry.context_name)
&& (centry.context_name.len() != 0))) {
debugprintf(0, "Received response message doesn't match sent message!");
usm->delete_sec_state_reference(securityStateReference);
cache.delete_content(centry);
return SNMPv3_MP_MATCH_ERROR;
}
usm->delete_sec_state_reference(securityStateReference);
cache.delete_content(centry);
debugprintf(1, "mp finished (OK)");
return SNMPv3_MP_OK;
}
if ((pdu->command == GET_REQ_MSG) || (pdu->command == GETNEXT_REQ_MSG) ||
(pdu->command == SET_REQ_MSG) || (pdu->command == GETBULK_REQ_MSG) ||
(pdu->command == INFORM_REQ_MSG)) {
if (securityEngineID.len() == 0) {
debugprintf(2, "Received Message with engineID = 0.");
}
else {
if (!(unsignedCharCompare(securityEngineID.data(), securityEngineID.len(),
own_engine_id, own_engine_id_len))) {
debugprintf(0, "snmp_parse: securityEngineID doesn't match own_engine_id.");
/* we are authoritative but engine id of message is wrong
if discovery in USM is enabled:
- remove automatically added illegal engine id from USM tables
- send a report
*/
if (usm->is_discovery_enabled())
{
// TODO: try to remove engine id from USM
if (reportableFlag)
{
cache.add_entry(msgID, pdu->reqid, securityEngineID,
msgSecurityModel,
securityName, securityLevel, "", "",
securityStateReference,
SNMPv3_MP_INVALID_ENGINEID,
CACHE_REMOTE_REQ);
send_report(0, MAX_SNMP_PACKET, pdu, SNMPv3_MP_INVALID_ENGINEID,
SNMP_SECURITY_LEVEL_NOAUTH_NOPRIV, msgSecurityModel,
securityName, from_address, snmp_session);
clear_pdu(pdu, true); // Clear pdu and free all content AND IDs!
}
else
{
usm->delete_sec_state_reference(securityStateReference);
}
return SNMPv3_MP_INVALID_ENGINEID;
}
usm->delete_sec_state_reference(securityStateReference);
return SNMPv3_MP_MATCH_ERROR;
}
}
int ret = cache.add_entry(msgID, pdu->reqid, securityEngineID,
msgSecurityModel, securityName,
securityLevel, contextEngineID,
contextName, securityStateReference,
SNMPv3_MP_OK, CACHE_REMOTE_REQ);
if (ret == SNMPv3_MP_DOUBLED_MESSAGE) {
debugprintf(0, "*** received doubled message ***");
// message will be ignored so return OK
usm->delete_sec_state_reference(securityStateReference);
}
debugprintf(1, "mp: parsing finished (ok).");
return SNMPv3_MP_OK;
}
if ((pdu->command == TRP_REQ_MSG) || (pdu->command == TRP2_REQ_MSG))
{
usm->delete_sec_state_reference(securityStateReference);
return SNMPv3_MP_OK;
}
debugprintf(0, "mp error: This line should not be executed.");
usm->delete_sec_state_reference(securityStateReference);
return SNMPv3_MP_ERROR;
}
// This function will get the reports from the intersense dll, then
// put that report into the time, sensor, pos and quat fields, and
// finally call send_report to send it.
void vrpn_Tracker_InterSense::get_report(void)
{
#ifdef VRPN_INCLUDE_INTERSENSE
q_vec_type angles;
ISD_TRACKING_DATA_TYPE data;
int i;
if(ISD_GetTrackingData(m_Handle,&data)) {
for(int station=0;station<ISD_MAX_STATIONS;station++) {
if(data.Station[station].NewData == TRUE) {
d_sensor = station;
//--------------------------------------------------------------------
// If we are doing IS900 timestamps, decode the time, add it to the
// time we zeroed the tracker, and update the report time. Remember
// to convert the MILLIseconds from the report into MICROseconds and
// seconds.
//--------------------------------------------------------------------
if (do_is900_timestamps) {
vrpn_float32 read_time = data.Station[station].TimeStamp;
struct timeval delta_time; // Time since the clock was reset
// Convert from the float in MILLIseconds to the struct timeval
delta_time.tv_sec = (long)(read_time / 1000); // Integer trunction to seconds
read_time -= delta_time.tv_sec * 1000; // Subtract out what we just counted
delta_time.tv_usec = (long)(read_time * 1000); // Convert remainder to MICROseconds
// Store the current time
timestamp = vrpn_TimevalSum(is900_zerotime, delta_time);
} else {
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
}
//--------------------------------------------------------------------
// If this sensor has an IS900 button on it, decode
// the button values into the button device and mainloop the button
// device so that it will report any changes. Each button is stored
// in one bit of the byte, with the lowest-numbered button in the
// lowest bit.
//--------------------------------------------------------------------
if (is900_buttons[station]) {
for (i = 0; i < is900_buttons[station]->number_of_buttons(); i++) {
is900_buttons[station]->set_button(i, data.Station[station].ButtonState[i]);
}
is900_buttons[station]->mainloop();
}
//--------------------------------------------------------------------
// If this sensor has an IS900 analog on it, decode the analog values
// into the analog device and mainloop the analog device so that it
// will report any changes. The first byte holds the unsigned char
// representation of left/right. The second holds up/down. For each,
// 0 means min (left or rear), 127 means center and 255 means max.
//--------------------------------------------------------------------
if (is900_analogs[station]) {
// Normalize the values to the range -1 to 1
is900_analogs[station]->setChannelValue(0, (data.Station[station].AnalogData[0] - 127) / 128.0);
is900_analogs[station]->setChannelValue(1, (data.Station[station].AnalogData[1] - 127) / 128.0);
// Report the new values
is900_analogs[station]->report_changes();
is900_analogs[station]->mainloop();
}
// Copy the tracker data into our internal storage before sending
// (no unit problem as the Position vector is already in meters, see ISD_STATION_STATE_TYPE)
// Watch: For some reason, to get consistent rotation and translation axis permutations,
// we need non direct mapping.
// RMT: Based on a report from Christian Odom, it seems like the Quaternions in the
// Isense are QXYZ, whereas in Quatlib (and VRPN) they are XYZQ. Once these
// are switched correctly, the positions can be read without strange swapping.
pos[0] = data.Station[station].Position[0];
pos[1] = data.Station[station].Position[1];
pos[2] = data.Station[station].Position[2];
if(m_StationInfo[station].AngleFormat == ISD_QUATERNION) {
d_quat[0] = data.Station[station].Quaternion[1];
d_quat[1] = data.Station[station].Quaternion[2];
d_quat[2] = data.Station[station].Quaternion[3];
d_quat[3] = data.Station[station].Quaternion[0];
} else {
// Just return Euler for now...
// nahon@virtools needs to convert to radians
angles[0] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[0];
angles[1] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[1];
angles[2] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[2];
q_from_euler(d_quat, angles[0], angles[1], angles[2]);
}
// have to just send it now
status = vrpn_TRACKER_REPORT_READY;
// fprintf(stderr, "sending message len %d\n", len);
send_report();
//printf("Isense %f, %f, %f\n",pos[0],pos[1],pos[2]);
//printf("Isense a:%f, %f, %f : ",angles[0],angles[1],angles[2]); //if the tracker reports a quat, these will be garbage
//printf("q: %f, %f, %f, %f\n",d_quat[0],d_quat[1],d_quat[2],d_quat[3]);
}
}
}
#endif
}
void *pthread_handler(void *ptr)
{
P_socket *data;
data = (P_socket *) ptr;
struct sockaddr_in addr = *(data->ptr_addr);
int sock = data->socket_fd;
int len = sizeof(addr);
int msg_size;
const char *invite_msg = "> ";
const char *exit_msg = "Bye-bye!!!|";
char path[SIZE_BUF];
char name[SIZE_STR];
char buffer[SIZE_BUF];
char author[SIZE_STR];
char content[SIZE_CONTENT];
while(strcmp(buffer, ":start"))
{
bzero(buffer, sizeof(buffer));
if ((msg_size = recvfrom(sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&addr, (socklen_t *)&len)) < 0)
{
perror("RECV start message error");
exit(1);
}
}
printf("RECV [%d bytes]: start message '%s'\n", msg_size, buffer);
send_content(sock, START_PATH, &addr);
strcpy(path, START_PATH);
while(1)
{
if ((msg_size = sendto(sock, invite_msg, strlen(invite_msg), 0, (struct sockaddr*)&addr, sizeof(addr))) < 0)
{
perror("SEND invitation message error");
exit(1);
}
printf("SEND [%d bytes]: invitation message '%s'\n", msg_size, invite_msg);
bzero(buffer, sizeof(buffer));
if ((msg_size = recvfrom(sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&addr, (socklen_t *)&len)) < 0)
{
perror("RECV command error");
exit(1);
}
printf("RECV [%d bytes]: command '%s'\n", msg_size, buffer);
if (!strcmp(buffer, ":exit"))
{
if ((msg_size = sendto(sock, exit_msg, strlen(exit_msg), 0, (struct sockaddr*)&addr, sizeof(addr))) < 0)
{
perror("SEND directory content error");
exit(1);
}
printf("SEND [%d bytes]: directory content '%s'\n", msg_size, exit_msg);
break;
}
if (strcmp(buffer, "find") && strcmp(buffer, "open") && strcmp(buffer, "add"))
{
send_input_error(sock, &addr);
send_content(sock, path, &addr);
continue;
}
send_report(sock, SUCCESS, &addr);
if (!strcmp(buffer, "open"))
{
bzero(path, sizeof(path));
if ((msg_size = recvfrom(sock, path, sizeof(path), 0, (struct sockaddr*)&addr, (socklen_t *)&len)) < 0)
{
perror("RECV path to file error");
exit(1);
}
printf("RECV [%d bytes]: path to file message '%s'\n", msg_size, path);
open_file(sock, path, &addr);
}
if (!strcmp(buffer, "find"))
{
bzero(author, sizeof(author));
if ((msg_size = recvfrom(sock, author, sizeof(author), 0, (struct sockaddr*)&addr, (socklen_t *)&len)) < 0)
{
perror("RECV author to find error");
exit(1);
}
printf("RECV [%d bytes]: author to find '%s'\n", msg_size, author);
find_for_author(sock, path, author, &addr);
sendPath_recvReport(sock, path, &addr);
}
if (!strcmp(buffer, "add"))
{
bzero(name, sizeof(name));
if ((msg_size = recvfrom(sock, name, sizeof(name), 0, (struct sockaddr*)&addr, (socklen_t *)&len)) < 0)
{
perror("RECV name error");
exit(1);
}
printf("RECV [%d bytes]: name'%s'\n", msg_size, name);
char *dir = strdup(path);
strcat(path, name);
strcat(path, ".txt");
if (check_file_existence(path) < 0)
{
send_report(sock, UNSUCCESS, &addr);
recv_report(sock, &addr);
}
else
{
send_report(sock, SUCCESS, &addr);
bzero(author, sizeof(author));
if ((msg_size = recvfrom(sock, author, sizeof(author), 0, (struct sockaddr*)&addr, (socklen_t *)&len)) < 0)
{
perror("RECV author error");
exit(1);
}
printf("RECV [%d bytes]: author '%s'\n", msg_size, author);
send_report(sock, SUCCESS, &addr);
bzero(content, sizeof(content));
if ((msg_size = recvfrom(sock, content, sizeof(content), 0, (struct sockaddr*)&addr, (socklen_t *)&len)) < 0)
{
perror("RECV content of file error");
exit(1);
}
printf("RECV [%d bytes]: content of file '%s'\n", msg_size, content);
strcat(name, "\n");
strcat(author, "\n\n");
if (add_article(path, name, author, content) < 0)
send_report(sock, UNSUCCESS, &addr);
else
send_report(sock, SUCCESS, &addr);
recv_report(sock, &addr);
}
send_content(sock, dir, &addr);
dirname(path);
if (path[strlen(path) - 1] != '/')
strcat(path, "/");
}
}
/* int DAGRM_SIZE =16;
int NUM_SIZE = 3;
char _msg[1024] = "1234567890123456789012345678901234567890123456789012345678901234567890";
char dagrm[DAGRM_SIZE];
char *_ptr = _msg;
int num, size = 0;
int length = DAGRM_SIZE - NUM_SIZE - sizeof(char);
for (num = 0;_ptr <= &_msg[strlen(_msg) - 1];){
memset(dagrm, 0, sizeof(dagrm));
sprintf(dagrm, "%3x", num);
if (num == ((strlen(_msg) - 1)/(DAGRM_SIZE - NUM_SIZE - sizeof(char))))
dagrm[0] = '1';
strncat(dagrm, _ptr, length);
if ((msg_size = sendto(sock, dagrm, strlen(dagrm), 0, (struct sockaddr*)&addr, sizeof(addr))) < 0)
{
perror("SEND directory content error");
exit(1);
}
printf("SEND [%d bytes]: directory content '%s'\n", msg_size, dagrm);
if (recv_report(sock,&addr) == 0)
continue;
_ptr = _ptr + length;
size = size + length;
num++;
}*/
send_msg(sock, "Hello,world!Hello,World!!!", strlen("Hello,world!Hello,World!!!"), 0, (struct sockaddr*)&addr, sizeof(addr));
pthread_exit(0);
return 0;
}
void send_input_error(int sock, struct sockaddr_in *ptr_addr)
{
send_report(sock, UNSUCCESS, ptr_addr);
recv_report(sock, ptr_addr);
}