C++ SensorFusion类代码示例

void OculusRiftService::onInit(){
	// initialization(prepare varibles for data acquisition of OculusRift's sensors)
	OVR::System::Init();
     std::string Service_name ="";
     IPv4 ip;
     std::string str ="";
      str = getMyIP(&ip);
      if( str == "" )
        {
          std::cout <<"Theres been an error !" << std::endl;
      
        }

   Service_name = "SIGORS ";
   Service_name += str;
  // sendMsg("multiuserctr_0",Service_name);


	pFusionResult = new SensorFusion();
	pManager = *DeviceManager::Create();

	pHMD = *pManager->EnumerateDevices<HMDDevice>().CreateDevice();

	if (pHMD){
		InfoLoaded = pHMD->GetDeviceInfo(&Info);
		pSensor = *pHMD->GetSensor();
	}
	else{
	   pSensor = *pManager->EnumerateDevices<SensorDevice>().CreateDevice();
	}

	if (pSensor){
	   pFusionResult->AttachToSensor(pSensor);
	}
}

extern "C" RIFTAPI_API int InitRiftApi()
{
	OVR::System::Init();
	pManager = *DeviceManager::Create();

	pHMD     = *pManager->EnumerateDevices<HMDDevice>().CreateDevice();
	if (!pHMD)
	{
		return 0 ;
	}

	pSensor  = *pHMD->GetSensor();

	// Get DisplayDeviceName, ScreenWidth/Height, etc..

	pHMD->GetDeviceInfo(&hmdInfo);

	if (pSensor)
	{
		SFusion.AttachToSensor(pSensor);
		SFusion.SetPrediction(0.03f);
	}

	if(pSensor)
	{
		return 1;
	}

	return 0;
}

void IN_MotionSensor_Read(float &roll, float &pitch, float &yaw)
{
		if (SFusion.IsAttachedToSensor()) {
            float predictionDelta = in_sensorPrediction.GetFloat() * (1.0f / 1000.0f);
            if (SFusion.GetPredictionDelta() != predictionDelta)
            {
                SFusion.SetPrediction(predictionDelta);
            }
			Quatf hmdOrient = SFusion.GetPredictedOrientation();
			float y = 0.0f, p = 0.0f, r = 0.0f;
			hmdOrient.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&y, &p, &r);
			roll =   -RADIANS_TO_DEGREES(r); // ???
			pitch =  -RADIANS_TO_DEGREES(p); // should be degrees down
			yaw =     RADIANS_TO_DEGREES(y); // should be degrees left
		} else if (hasVR920Tracker && IWRGetTracking) {
			LONG y=0, p=0, r=0;
			if (IWRGetTracking(&y, &p, &r)==ERROR_SUCCESS) {
				yaw = y * 180.0f/32767.0f;
				pitch = p * -180.0f/32767.0f;
				roll = r * 180.0f/32767.0f;
			}
		} else {
			roll  = angles[ROLL];
			pitch = angles[PITCH];
			yaw   = angles[YAW];
		}
}

void InputTestApp::OnIdle()
{
    double curtime = pPlatform->GetAppTime();
 //   float  dt      = float(LastUpdate - curtime);
    LastUpdate     = curtime;
    
    if (pBox)
    {
        Quatf q = SFusion.GetOrientation();
        pBox->SetOrientation(q);

   // Test Euler conversion, alternative to the above:
   //     Vector3f euler;
   //     SFusion.GetOrientation().GetEulerABC<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(&euler.y, &euler.x, &euler.z);
   //     Matrix4f mat = Matrix4f::RotationY(euler.y) * Matrix4f::RotationX(euler.x) * Matrix4f::RotationZ(euler.z);
   //  pBox->SetMatrix(mat);    

        // Update titlebar every 20th of a second.
        if ((curtime - LastTitleUpdate) > 0.05f)
        {
            char                          titleBuffer[512];
            SensorDevice::CoordinateFrame coord = SensorDevice::Coord_Sensor;
            if (pSensor)
                coord = pSensor->GetCoordinateFrame();

            OVR_sprintf(titleBuffer, 512, "OVR SensorBox %s %s  Ang: %0.3f",
                        (SFusion.IsGravityEnabled() ?  "" : "[Grav Off]"),
                        (coord == SensorDevice::Coord_HMD) ? "[HMD Coord]" : "",
                        CalcDownAngleDegrees(q));
            pPlatform->SetWindowTitle(titleBuffer);
            LastTitleUpdate = curtime;
        }
    }

    if (pBox2)
    {
        pBox2->SetOrientation(SFusion2.GetOrientation());
    }

    // Render
    int w, h;
    pPlatform->GetWindowSize(&w, &h);

    pRender->SetViewport(0, 0, w, h);

    pRender->Clear();
    pRender->BeginScene();

    pRender->SetProjection(Proj);
    pRender->SetDepthMode(1,1);
    
    Sc.Render(pRender, View);

    pRender->Present();

}

JNIEXPORT void JNICALL Java_de_fruitfly_ovr_OculusRift_pollSubsystem(JNIEnv *, jobject) {
	if (!Initialized) return;
	if (!pSensor) return;

	bool isPredictionEnabled = FusionResult.IsPredictionEnabled();
	if (isPredictionEnabled)
		quaternion = FusionResult.GetPredictedOrientation();
	else
		quaternion = FusionResult.GetOrientation();

	quaternion.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);
}

JNIEXPORT void JNICALL Java_jrift_OculusRift_pollSubsystem(JNIEnv *, jobject) {
	if (!Initialized) return;
	if (!pSensor) return;

	quaternion = FusionResult.GetOrientation();
	quaternion.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);
}

 __declspec(dllexport) void initialize(void) {
     System::Init(Log::ConfigureDefaultLog(LogMask_None));    
     pManager = *DeviceManager::Create();
     pHMD = *pManager->EnumerateDevices<HMDDevice>().CreateDevice(); 
     pSensor = *pHMD->GetSensor();
     SFusion.AttachToSensor(pSensor);
     MagCal.BeginAutoCalibration(SFusion);
 }

extern "C" RIFTAPI_API int GetSensorSample()
{
   
	SFusion.GetOrientation().GetEulerAngles<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(&Heading, &Pitch, &Roll);
	

	return 1;
}

 __declspec(dllexport) void update_calibration(void) {
     if (MagCal.IsAutoCalibrating()) 
     {
         MagCal.UpdateAutoCalibration(SFusion);
         if (MagCal.IsCalibrated())
         {
             SFusion.SetYawCorrectionEnabled(true);
         }
     }
 }

JNIEXPORT jboolean JNICALL Java_de_fruitfly_ovr_OculusRift_initSubsystem(JNIEnv *env, jobject jobj) 
{
	Initialized = false;

	System::Init();

	pManager = *DeviceManager::Create();

	_ipd = 0.0635f; // Default
	Info.InterpupillaryDistance = _ipd;

	pHMD = *pManager->EnumerateDevices<HMDDevice>().CreateDevice();
	if (pHMD) {
		printf("Oculus Rift Device Interface created.\n");
		InfoLoaded = pHMD->GetDeviceInfo(&Info);
		pSensor = *pHMD->GetSensor();
		FusionResult.AttachToSensor(pSensor);
		Initialized = InfoLoaded && pSensor;
		printf("Oculus Rift Device Interface initialized.\n");
	}
	else {
		printf("Unable to create Oculus Rift device interface.\n");
	}

	if (InfoLoaded) {

		_ipd = Info.InterpupillaryDistance;

		printf(" DisplayDeviceName: %s\n", Info.DisplayDeviceName);
		printf(" ProductName: %s\n", Info.ProductName);
		printf(" Manufacturer: %s\n", Info.Manufacturer);
		printf(" Version: %s\n", Info.Version);
		printf(" HResolution: %i\n", Info.HResolution);
		printf(" VResolution: %i\n", Info.VResolution);
		printf(" HScreenSize: %f\n", Info.HScreenSize);
		printf(" VScreenSize: %f\n", Info.VScreenSize);
		printf(" VScreenCenter: %f\n", Info.VScreenCenter);
		printf(" EyeToScreenDistance: %f\n", Info.EyeToScreenDistance);
		printf(" LensSeparationDistance: %f\n", Info.LensSeparationDistance);
        printf(" InterpupillaryDistance: %f\n", Info.InterpupillaryDistance);
        printf(" DistortionK[0]: %f\n", Info.DistortionK[0]);
        printf(" DistortionK[1]: %f\n", Info.DistortionK[1]);
		printf(" DistortionK[2]: %f\n", Info.DistortionK[2]);
		printf(" DistortionK[3]: %f\n", Info.DistortionK[3]);
		printf(" ChromaticAb[0]: %f\n", Info.ChromaAbCorrection[0]);
        printf(" ChromaticAb[1]: %f\n", Info.ChromaAbCorrection[1]);
		printf(" ChromaticAb[2]: %f\n", Info.ChromaAbCorrection[2]);
		printf(" ChromaticAb[3]: %f\n", Info.ChromaAbCorrection[3]);
	}

	return Initialized;
}

//periodic procedure for sending messages to the controller
double OculusRiftService::onAction(){
  
     IPv4 ip;
     std::string str ="";
    str = getMyIP(&ip);
    if( str == "" )
        {
          std::cout <<"Theres been an error !" << std::endl;
      
        }
    // else
    //    std::cout << "IPv4 address stored in struct: ";
    //    std::cout << ip.b1<<"."<<ip.b2<<"."<<ip.b3<<"."<<ip.b4<< std::endl;
    
	float r_yaw, r_pitch, r_roll;

	Quatf q = pFusionResult->GetOrientation();
	Matrix4f bodyFrameMatrix(q);
	q.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&r_yaw, &r_pitch, &r_roll);

	std::vector<std::string> names;
	names = this->getAllConnectedEntitiesName();
	int entSize = names.size();

   // std::cout << "IPv4 Address:  " << str << std::endl;
   // std::cout << " entity " <<  entSize << std::endl;


	for(int i = 0; i < entSize; i++) {
    std::string msg = "ORS_DATA ";	
	    msg += str;
		msg += DoubleToString(r_yaw);
		msg += DoubleToString(r_pitch);
		msg += DoubleToString(r_roll);

		this->sendMsgToCtr(names[i], msg);
    std::cout << msg << std::endl;
	}


	return 0.1;  //time period
}

/** Oculus init **/
void Init()
{
	System::Init();

	pManager = *DeviceManager::Create();

	pHMD = *pManager->EnumerateDevices<HMDDevice>().CreateDevice();

	if (pHMD)
        {
           InfoLoaded = pHMD->GetDeviceInfo(&Info);

	   pSensor = *pHMD->GetSensor();
	}
	else
	{
	   pSensor = *pManager->EnumerateDevices<SensorDevice>().CreateDevice();
	}

	if (pSensor)
	{
	   FusionResult.AttachToSensor(pSensor);
	}
}

void IN_MotionSensor_Init(void)
{
	//Carl: Don't initialize has* here to false, because they can also be set by command line parameters

	// *** Oculus Sensor Initialization
	OVR::System::Init(OVR::Log::ConfigureDefaultLog(OVR::LogMask_All));

    // Create DeviceManager and first available HMDDevice from it.
    // Sensor object is created from the HMD, to ensure that it is on the
    // correct device.

    pManager = *DeviceManager::Create();

	// We'll handle it's messages in this case.
	//pManager->SetMessageHandler(this);

    // Release Sensor/HMD in case this is a retry.
    pSensor.Clear();
    pHMD.Clear();
    pHMD  = *pManager->EnumerateDevices<HMDDevice>().CreateDevice();
    if (pHMD)
    {
        pSensor = *pHMD->GetSensor();
    }
    else
    {            
        // If we didn't detect an HMD, try to create the sensor directly.
        // This is useful for debugging sensor interaction; it is not needed in
        // a shipping app.
        pSensor = *pManager->EnumerateDevices<SensorDevice>().CreateDevice();
    }
    if (!pHMD && !pSensor)
        common->Warning("Oculus Rift not detected.\n");
    else if (!pHMD)
        common->Warning("Oculus Sensor detected; HMD Display not detected.\n");
    else if (!pSensor)
        common->Warning("Oculus HMD Display detected; Sensor not detected.\n");
    //else if (HMDInfo.DisplayDeviceName[0] == '\0')
    //    common->Printf("Oculus Sensor detected; HMD display EDID not detected.\n");

	if (pSensor)
    {
		SFusion.AttachToSensor(pSensor);
        SFusion.SetPredictionEnabled(true);
		hasOculusRift = true;
		hasHMD = true;
    }
	
	if (!pSensor)
		LoadVR920();
	hasHMD = hasHMD || hasVR920Tracker;

	//Hillcrest libfreespace stuff
	LPDWORD dwThreadId=0;
	struct freespace_message message;
	int numIds;
	int rc;
	int i;

	// Initialize the freespace library
	rc = freespace_init();
	if (rc != FREESPACE_SUCCESS) {
		common->Warning("Hillcrest Initialization error. rc=%d\n", rc);
		return;
	}

	/** --- START EXAMPLE INITIALIZATION OF DEVICE -- **/
	rc = freespace_getDeviceList(&device, 1, &numIds);
	if (numIds == 0) {
		common->Warning("MotionSensor: Didn't find any devices.\n");
		return;
	}

	rc = freespace_openDevice(device);
	if (rc != FREESPACE_SUCCESS) {
		common->Warning("MotionSensor: Error opening device: %d\n", rc);
		return;
	}

	rc = freespace_flush(device);
	if (rc != FREESPACE_SUCCESS) {
		common->Warning("MotionSensor: Error flushing device: %d\n", rc);
		return;
	}

	memset(&message, 0, sizeof(message));
	message.messageType = FREESPACE_MESSAGE_DATAMODECONTROLV2REQUEST;
	message.dataModeControlV2Request.packetSelect = 2;
	message.dataModeControlV2Request.modeAndStatus |= 0 << 1;

	rc = freespace_sendMessage(device, &message);
	if (rc != FREESPACE_SUCCESS) {
		common->Warning("freespaceInputThread: Could not send message: %d.\n", rc);
	}
	CreateThread(NULL, //Choose default security
		0, //Default stack size
		(LPTHREAD_START_ROUTINE)&IN_MotionSensor_Thread,
		//Routine to execute
		(LPVOID) &i, //Thread parameter
		0, //Immediately run the thread
//.........这里部分代码省略.........

void InputTestApp::OnKey(KeyCode key, int chr, bool down, int modifiers)
{
    OVR_UNUSED2(chr, modifiers);

    switch (key)
    {
    case Key_Q:
        if (!down)
            pPlatform->Exit(0);
        break;

    case Key_F1:
        CurrentView = View_Perspective;
        SetView(CurrentView);
        //UpdateWindowTitle();
        break;
    case Key_F2:
        CurrentView = View_XY_DownZ;
        SetView(CurrentView);
        break;
    case Key_F3:
        CurrentView = View_XZ_UpY;
        SetView(CurrentView);
        break;

    case Key_R:
        if (down)
        {
            SFusion.Reset();
            SFusion2.Reset();
        }
        break;

    case Key_H:
        if (down && pSensor)
        {
            SensorDevice::CoordinateFrame coord = pSensor->GetCoordinateFrame();
            pSensor->SetCoordinateFrame(
                (coord == SensorDevice::Coord_Sensor) ?
                SensorDevice::Coord_HMD : SensorDevice::Coord_Sensor);
            SFusion.Reset();
            SFusion2.Reset();
        }
        break;

    case Key_G:
        if (down)
        {
            SFusion.SetGravityEnabled(!SFusion.IsGravityEnabled());
            SFusion2.SetGravityEnabled(SFusion.IsGravityEnabled());
        }
        break;

    case Key_A:

        if (down)
        {
            if (!pSensor2)
            {
                LogText("Angle: %2.3f\n", CalcDownAngleDegrees(SFusion.GetOrientation()));
            }
            else
            {
                LogText("Angle: %2.3f Secondary Sensor Angle: %2.3f\n",
                        CalcDownAngleDegrees(SFusion.GetOrientation()),
                        CalcDownAngleDegrees(SFusion2.GetOrientation()));
            }                        
        }
        break;

        /*
    case Key_End:
        if (!down)
        {
            OriAdjust = OriSensor.Conj();
            Sc.ViewPoint.SetOrientation(Quatf());
        }
        break; */
        default:
            break;
    }
}

int InputTestApp::OnStartup(int argc, const char** argv)
{
    if (!pPlatform->SetupWindow(1200,800))
        return 1;
    

    pManager = *DeviceManager::Create();
    
    // This initialization logic supports running two sensors at the same time.
   
    DeviceEnumerator<SensorDevice> isensor = pManager->EnumerateDevices<SensorDevice>();
    DeviceEnumerator<SensorDevice> oculusSensor;
    DeviceEnumerator<SensorDevice> oculusSensor2;
    
    while(isensor)
    {
        DeviceInfo di;
        if (isensor.GetDeviceInfo(&di))
        {
            if (strstr(di.ProductName, "Tracker"))
            {
                if (!oculusSensor)
                    oculusSensor = isensor;
                else if (!oculusSensor2)
                    oculusSensor2 = isensor;
            }
        }

        isensor.Next();
    }

    if (oculusSensor)
    {
        pSensor = *oculusSensor.CreateDevice();

        if (pSensor)
            pSensor->SetRange(SensorRange(4 * 9.81f, 8 * Math<float>::Pi, 1.0f), true);

        if (oculusSensor2)
        {
            // Second Oculus sensor, useful for comparing firmware behavior & settings.
            pSensor2 = *oculusSensor2.CreateDevice();

            if (pSensor2)
                pSensor2->SetRange(SensorRange(4 * 9.81f, 8 * Math<float>::Pi, 1.0f), true);
        }
    }

    oculusSensor.Clear();
    oculusSensor2.Clear();
       
    
    /*
    DeviceHandle hHMD = pManager->EnumerateDevices<HMDDevice>();
    HMDInfo      hmdInfo;
    if (hHMD)
    {        
        hHMD.GetDeviceInfo(&hmdInfo);
    }
    */

    if (pSensor)
        SFusion.AttachToSensor(pSensor);
    if (pSensor2)
        SFusion2.AttachToSensor(pSensor2);

    /*
    // Test rotation: This give rotations clockwise (CW) while looking from
    // origin in the direction of the axis.

    Vector3f xV(1,0,0);
    Vector3f zV(0,0,1);

    Vector3f rxV = Matrix4f::RotationZ(DegreeToRad(10.0f)).Transform(xV);
    Vector3f ryV = Matrix4f::RotationY(DegreeToRad(10.0f)).Transform(xV);
    Vector3f rzV = Matrix4f::RotationX(DegreeToRad(10.0f)).Transform(zV);
    */

    // Report relative mouse motion (not absolute position)
   // pPlatform->SetMouseMode(Mouse_Relative);

    const char* graphics = "d3d10";
    for (int i = 1; i < argc; i++)
        if (!strcmp(argv[i], "-r") && i < argc-1)
            graphics = argv[i+1];

    pRender = pPlatform->SetupGraphics(OVR_DEFAULT_RENDER_DEVICE_SET, graphics,
                                       RendererParams());
  
    //WireframeFill = pRender->CreateSimpleFill(Fill::F_Wireframe);


    
    // *** Rotating Box
    
    pBox = *new Container;
    pBox->Add(Ptr<Model>(        
       *Model::CreateAxisFaceColorBox(-2.0f, 2.0f, Color(0,   0xAA, 0),        // x = green
                                      -1.0f, 1.0f, Color(0xAA,0,    0),        // y = red
                                      -1.0f, 1.0f, Color(0,   0,    0xAA)) )); // z = blue 
//.........这里部分代码省略.........