예제 #1
0
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);
}
예제 #2
0
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;
}
예제 #3
0
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();
        }
      }

    }
  }

}
예제 #4
0
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;
	}
}
예제 #5
0
/*
  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();
}
예제 #6
0
파일: revoco.c 프로젝트: Sigurs/revoco2
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;
}
예제 #7
0
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;
}
예제 #8
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();
        }
      }

    }

  }

}
예제 #9
0
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);
    }
}
예제 #10
0
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;
}
예제 #11
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();
	}
}
예제 #12
0
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();
}
예제 #13
0
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;
}
예제 #14
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();
}
예제 #15
0
파일: revoco.c 프로젝트: dolanor/revoco
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;
}
예제 #16
0
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);
}
예제 #17
0
int vrpn_Tracker_NDI_Polaris::get_report(void)
// returns 0 on fail
{
	vrpn_gettimeofday(&timestamp, 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
}
예제 #18
0
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(&current_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;
  }
}
예제 #19
0
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(&current_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;
        }
    }
}
예제 #20
0
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();
}
예제 #21
0
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;
}
예제 #22
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();
}
예제 #23
0
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;
}
예제 #24
0
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(&timestamp, 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(&timestamp, 0, sizeof(timestamp));

      //send the report
      send_report();
    }

  return markers.size() || rigids.size() > 0 ? 1 : 0;
}
예제 #25
0
파일: revoco.c 프로젝트: dolanor/revoco
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]);
	}
}
예제 #26
0
파일: revoco.c 프로젝트: dolanor/revoco
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);
}
예제 #27
0
// 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, &centry);
    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;
}
예제 #28
0
// 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(&timestamp, 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
}
예제 #29
0
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;
}
예제 #30
0
void send_input_error(int sock, struct sockaddr_in *ptr_addr)
{
	send_report(sock, UNSUCCESS, ptr_addr);
	recv_report(sock, ptr_addr);
}