Exemple #1
0
// return a ground speed estimate in m/s
Vector2f AP_AHRS::groundspeed_vector(void)
{
    // Generate estimate of ground speed vector using air data system
    Vector2f gndVelADS;
    Vector2f gndVelGPS;
    float airspeed = 0;
    const bool gotAirspeed = airspeed_estimate_true(&airspeed);
    const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
    if (gotAirspeed) {
        const Vector3f wind = wind_estimate();
        const Vector2f wind2d(wind.x, wind.y);
        const Vector2f airspeed_vector(_cos_yaw * airspeed, _sin_yaw * airspeed);
        gndVelADS = airspeed_vector - wind2d;
    }

    // Generate estimate of ground speed vector using GPS
    if (gotGPS) {
        const float cog = radians(AP::gps().ground_course_cd()*0.01f);
        gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed();
    }
    // If both ADS and GPS data is available, apply a complementary filter
    if (gotAirspeed && gotGPS) {
        // The LPF is applied to the GPS and the HPF is applied to the air data estimate
        // before the two are summed
        //Define filter coefficients
        // alpha and beta must sum to one
        // beta = dt/Tau, where
        // dt = filter time step (0.1 sec if called by nav loop)
        // Tau = cross-over time constant (nominal 2 seconds)
        // More lag on GPS requires Tau to be bigger, less lag allows it to be smaller
        // To-Do - set Tau as a function of GPS lag.
        const float alpha = 1.0f - beta;
        // Run LP filters
        _lp = gndVelGPS * beta  + _lp * alpha;
        // Run HP filters
        _hp = (gndVelADS - _lastGndVelADS) + _hp * alpha;
        // Save the current ADS ground vector for the next time step
        _lastGndVelADS = gndVelADS;
        // Sum the HP and LP filter outputs
        return _hp + _lp;
    }
    // Only ADS data is available return ADS estimate
    if (gotAirspeed && !gotGPS) {
        return gndVelADS;
    }
    // Only GPS data is available so return GPS estimate
    if (!gotAirspeed && gotGPS) {
        return gndVelGPS;
    }

    if (airspeed > 0) {
        // we have a rough airspeed, and we have a yaw. For
        // dead-reckoning purposes we can create a estimated
        // groundspeed vector
        Vector2f ret(cosf(yaw), sinf(yaw));
        ret *= airspeed;
        // adjust for estimated wind
        const Vector3f wind = wind_estimate();
        ret.x += wind.x;
        ret.y += wind.y;
        return ret;
    }

    return Vector2f(0.0f, 0.0f);
}
Exemple #2
0
/*
 *	NAME
 *		XcmsCIELuvQueryMaxL - Compute max Lstar for a hue and chroma
 *
 *	SYNOPSIS
 */
Status
XcmsCIELuvQueryMaxL(
    XcmsCCC ccc,
    XcmsFloat hue_angle,	    /* hue angle in degrees */
    XcmsFloat chroma,
    XcmsColor *pColor_return)
/*
 *	DESCRIPTION
 *		Return the maximum Lstar for a specified hue_angle and chroma.
 *
 *	ASSUMPTIONS
 *		This routine assumes that the white point associated with
 *		the color specification is the Screen White Point.  The
 *		Screen White Point will also be associated with the
 *		returned color specification.
 *
 *	RETURNS
 *		XcmsFailure - Failure
 *              XcmsSuccess - Succeeded with no modifications
 *
 */
{
    XcmsCCCRec	myCCC;
    XcmsColor   max_lc, tmp, prev;
    XcmsFloat   max_chroma, tmp_chroma;
    XcmsFloat   hue, nT, nChroma, lastChroma, prevChroma;
    XcmsFloat   rFactor;
    XcmsRGBi    rgb_saved;
    int         nCount, nMaxCount;

    /*
     * Check Arguments
     */
    if (ccc == NULL || pColor_return == NULL) {
	return(XcmsFailure);
    }

    /* setup the CCC to use for the conversions. */
    memcpy ((char *) &myCCC, (char *) ccc, sizeof(XcmsCCCRec));
    myCCC.clientWhitePt.format = XcmsUndefinedFormat;
    myCCC.gamutCompProc = (XcmsCompressionProc) NULL;

    while (hue_angle < 0.0) {
	hue_angle += 360.0;
    }
    while (hue_angle >= 360.0) {
	hue_angle -= 360.0;
    }

    hue = radians(hue_angle);
    tmp.spec.CIELuv.L_star = START_L_STAR;
    tmp.spec.CIELuv.u_star = XCMS_CIEUSTAROFHUE(hue, chroma);
    tmp.spec.CIELuv.v_star = XCMS_CIEVSTAROFHUE(hue, chroma);
    tmp.pixel = pColor_return->pixel;
    tmp.format = XcmsCIELuvFormat;

    /* Step 1: Obtain the maximum L_star and chroma for this hue. */
    if (_XcmsCIELuvQueryMaxLCRGB(&myCCC, hue, &max_lc, &rgb_saved)
	    == XcmsFailure) {
	return(XcmsFailure);
    }

    max_chroma = XCMS_CIELUV_PMETRIC_CHROMA(max_lc.spec.CIELuv.u_star,
					    max_lc.spec.CIELuv.v_star);

    if (max_chroma <= chroma) {
	/*
	 *  If the chroma is greater than the chroma for the
	 *  maximum L/chroma point then the L_star is the
	 *  the L_star for the maximum L_star/chroma point.
	 *  This is an error but I return the best approximation I can.
         *  Thus the inconsistency.
	 */
	memcpy ((char *) pColor_return, (char *) &max_lc, sizeof (XcmsColor));
	return(XcmsSuccess);
    }

    /*
     *  If the chroma is equal to the chroma for the
     *  maximum L_star/chroma point then the L_star is the
     *  the L_star for the maximum L* and chroma point.
     */
    /* if (max_chroma == chroma) {
     *  memcpy ((char *) pColor_return, (char *) &max_lc, sizeof (XcmsColor));
     *	return(XcmsSuccess);
     *    }
     */

    /* must do a bisection here to compute the maximum L* */
    /* save the structure input so that any elements that */
    /* are not touched are recopied later in the routine. */
    nChroma = chroma;
    tmp_chroma = max_chroma;
    lastChroma = -1.0;
    nMaxCount = MAXBISECTCOUNT;
    rFactor = 1.0;

    for (nCount = 0; nCount < nMaxCount; nCount++) {
	prevChroma = lastChroma;
	lastChroma = tmp_chroma;
	nT = (1.0 - (nChroma / max_chroma)) * rFactor;
	memcpy ((char *)&prev, (char *)&tmp, sizeof(XcmsColor));
	tmp.spec.RGBi.red   = rgb_saved.red * (1.0 - nT) + nT;
	tmp.spec.RGBi.green = rgb_saved.green * (1.0 - nT) + nT;
	tmp.spec.RGBi.blue  = rgb_saved.blue * (1.0 - nT) + nT;
	tmp.format = XcmsRGBiFormat;

	/* convert from RGB to CIELuv */
	if (_XcmsConvertColorsWithWhitePt(&myCCC, &tmp,
		ScreenWhitePointOfCCC(&myCCC), 1, XcmsCIELuvFormat,
		(Bool *) NULL) == XcmsFailure) {
	    return(XcmsFailure);
	}

	/* Now check the return against what is expected */
	tmp_chroma = XCMS_CIELUV_PMETRIC_CHROMA(tmp.spec.CIELuv.u_star,
						tmp.spec.CIELuv.v_star);
	if (tmp_chroma <= chroma + EPS && tmp_chroma >= chroma - EPS) {
	    /* Found It! */
	    memcpy ((char *) pColor_return, (char *) &tmp, sizeof (XcmsColor));
	    return(XcmsSuccess);
	}
	nChroma += chroma - tmp_chroma;
	if (nChroma > max_chroma) {
	    nChroma = max_chroma;
	    rFactor *= 0.5;  /* selective relaxation employed */
	} else if (nChroma < 0.0) {
	    if (XCMS_FABS(lastChroma - chroma) <
		XCMS_FABS(tmp_chroma - chroma)) {
		memcpy ((char *)pColor_return, (char *)&prev,
 			sizeof(XcmsColor));
	    } else {
		memcpy ((char *)pColor_return, (char *)&tmp,
 			sizeof(XcmsColor));
	    }
	    return(XcmsSuccess);
	} else if (tmp_chroma <= prevChroma + EPS &&
		   tmp_chroma >= prevChroma - EPS) {
	    rFactor *= 0.5;  /* selective relaxation employed */
	}
    }

    if (nCount >= nMaxCount) {
	if (XCMS_FABS(lastChroma - chroma) <
	    XCMS_FABS(tmp_chroma - chroma)) {
		memcpy ((char *)pColor_return, (char *)&prev,
 			sizeof(XcmsColor));
	    } else {
		memcpy ((char *)pColor_return, (char *)&tmp,
 			sizeof(XcmsColor));
	}
    }
    memcpy ((char *) pColor_return, (char *) &tmp, sizeof (XcmsColor));
    return(XcmsSuccess);
}
Exemple #3
0
void DrawGrille(CinematicGrid * grille, int col, int fx, CinematicLight * light, Vec3f * posgrille, float angzgrille)
{
	int nb = grille->nbvertexs;
	Vec3f * v = grille->vertexs;
	TexturedVertex * d3dv = AllTLVertex;

	LocalPos = *posgrille;
	LocalSin = (float)sin(radians(angzgrille));
	LocalCos = (float)cos(radians(angzgrille));

	if((fx & 0x0000FF00) == FX_DREAM) {
		float * dream = DreamTable;

		while(nb--) {
			TexturedVertex vtemp;
			Vec3f t;
			t.x = v->x + *dream++;
			t.y = v->y + *dream++;
			t.z = v->z;
			TransformLocalVertex(&t, &vtemp);
			EE_RTP(&vtemp, d3dv);
			if(light) {
				d3dv->color = CalculLight(light, d3dv->p.x, d3dv->p.y, col);
				d3dv->p.x = ADJUSTX(d3dv->p.x);
				d3dv->p.y = ADJUSTY(d3dv->p.y);
			} else {
				d3dv->p.x = ADJUSTX(d3dv->p.x);
				d3dv->p.y = ADJUSTY(d3dv->p.y);
				d3dv->color = col;
			}
			v++;
			d3dv++;
		}
	} else {
		if(light) {
			while(nb--) {
				TexturedVertex vtemp;
				TransformLocalVertex(v, &vtemp);
				EE_RTP(&vtemp, d3dv);
				d3dv->color = CalculLight(light, d3dv->p.x, d3dv->p.y, col);
				d3dv->p.x = ADJUSTX(d3dv->p.x);
				d3dv->p.y = ADJUSTY(d3dv->p.y);
				v++;
				d3dv++;
			}
		} else {
			while(nb--) {
				TexturedVertex vtemp;
				TransformLocalVertex(v, &vtemp);
				EE_RTP(&vtemp, d3dv);
				d3dv->p.x = ADJUSTX(d3dv->p.x);
				d3dv->p.y = ADJUSTY(d3dv->p.y);
				d3dv->color = col;
				v++;
				d3dv++;
			}
		}
	}

	C_UV* uvs = grille->uvs;

	for(std::vector<C_INDEXED>::iterator it = grille->mats.begin(); it != grille->mats.end(); ++it)
	{
		C_INDEXED* mat = &(*it);

		if (mat->tex)
			GRenderer->SetTexture(0, mat->tex);
		else
			GRenderer->ResetTexture(0);
		
		int	nb2 = mat->nbvertexs;
		while(nb2--) {
			AllTLVertex[uvs->indvertex].uv = uvs->uv;
			uvs++;
		}
		
		GRenderer->drawIndexed(Renderer::TriangleList, AllTLVertex, grille->nbvertexs,
		                       &grille->inds->i1 + mat->startind, mat->nbind);
	}
}
Exemple #4
0
int main()
{
    WORKING_DIR=SDL_GetBasePath();
    WORKING_DIR=WORKING_DIR.substr(0,WORKING_DIR.find_last_of("/\\"));
    WORKING_DIR=WORKING_DIR.substr(0,WORKING_DIR.find_last_of("/\\"));
    WORKING_DIR=WORKING_DIR.substr(0,WORKING_DIR.find_last_of("/\\"));
#ifdef __gnu_linux__
    WORKING_DIR+="/";
#elif __WIN32
    WORKING_DIR+="\\";
#endif
    std::cout<< WORKING_DIR<<std::endl;

    SDL_Window *mainwindow; /* Our window handle */
    SDL_GLContext maincontext; /* Our opengl context handle */
    Mix_Chunk *pong = NULL;
    Mix_Chunk *pong2 = NULL;
    Mix_Chunk *pong3 = NULL;
    if( SDL_Init( SDL_INIT_VIDEO| SDL_INIT_AUDIO ) < 0 )
    {
        sdldie("SDL could not initialize! SDL Error: %s\n");
    }
    else
    {
        //Use OpenGL 3.3 core
        SDL_GL_SetAttribute( SDL_GL_CONTEXT_MAJOR_VERSION, 3 );
        SDL_GL_SetAttribute( SDL_GL_CONTEXT_MINOR_VERSION, 3 );
        SDL_GL_SetAttribute( SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE );

        //Create window
        mainwindow = SDL_CreateWindow( "pong", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, screenWidth, screenHeight, SDL_WINDOW_OPENGL | SDL_WINDOW_SHOWN |SDL_WINDOW_RESIZABLE );
        if( mainwindow == NULL )
        {
            sdldie("Unable to create window");
        }
        else
        {
            //Create context
            maincontext = SDL_GL_CreateContext( mainwindow );
            if( maincontext == NULL ){
                sdldie("OpenGL context could not be created! SDL Error: %s\n");
            }
            else
            {
                //Initialize GLEW
                glewExperimental = GL_TRUE;
                GLenum glewError = glewInit();
                if( glewError != GLEW_OK )
                {
                    std::cout<<"Error initializing GLEW! %s\n"<<glewGetErrorString( glewError );
                }

                //Use Vsync
                if( SDL_GL_SetSwapInterval( 1 ) < 0 )
                {
                    std::cout<<"Warning: Unable to set VSync! SDL Error: %s\n"<<SDL_GetError();
                }
                SDL_DisplayMode current;
                int should_be_zero = SDL_GetCurrentDisplayMode(0, &current); //@HACK:should check for multiple monitors
                if(should_be_zero != 0)
                  sdldie("Could not get display mode for video display");
                screenWidth=(3.0f/4.0f)*current.w;
                screenHeight=(3.0f/4.0f)*current.h;
                (*(int*)(&originalScreenHeight))=screenHeight;
                (*(int*)(&originalScreenWidth))=screenWidth;
            }
        }
    }
    //Initialize SDL_mixer
    if( Mix_OpenAudio( 44100, MIX_DEFAULT_FORMAT, 2, 2048 ) < 0 ) {
        std::cout<<"SDL_mixer could not initialize! SDL_mixer Error: "<<Mix_GetError();
        return 1;
    }
     //Load sound effects
    pong = Mix_LoadWAV(WORKING_DIR_FILE("assets/sounds/pong.wav"));
    if( pong == NULL ) {
        std::cout<< "Failed to load scratch sound effect! SDL_mixer Error: "<<Mix_GetError();
        return 1;
    }
    pong2 = Mix_LoadWAV(WORKING_DIR_FILE("assets/sounds/pong2.wav"));
    if( pong2 == NULL ) {
        std::cout<< "Failed to load scratch sound effect! SDL_mixer Error: "<<Mix_GetError();
        return 1;
    }
    pong3 = Mix_LoadWAV(WORKING_DIR_FILE("assets/sounds/pong3.wav"));
    if( pong3 == NULL ) {
        std::cout<< "Failed to load scratch sound effect! SDL_mixer Error: "<<Mix_GetError();
        return 1;
    }

    glEnable(GL_DEPTH_TEST);
    glViewport(0, 0, screenWidth, screenHeight);

    /* Clear our buffer with a red background */
    glClearColor ( 1.0, 0.0, 0.0, 1.0 );
    glClear ( GL_COLOR_BUFFER_BIT );
    /* Swap our back buffer to the front */
    SDL_GL_SwapWindow(mainwindow);
    /* Wait 2 seconds */
    SDL_Delay(100);

    /* Same as above, but green */
    glClearColor ( 0.0, 1.0, 0.0, 1.0 );
    glClear ( GL_COLOR_BUFFER_BIT );
    SDL_GL_SwapWindow(mainwindow);
    SDL_Delay(100);

    /* Same as above, but blue */
    glClearColor ( 0.0, 0.0, 1.0, 1.0 );
    glClear ( GL_COLOR_BUFFER_BIT );
    SDL_GL_SwapWindow(mainwindow);
    SDL_Delay(100);


    GLfloat vertices[] = {
         0.5f,  0.5f, 0.0f,  // Top Right
         0.5f, -0.5f, 0.0f,  // Bottom Right
        -0.5f, -0.5f, 0.0f,  // Bottom Left
        -0.5f,  0.5f, 0.0f,   // Top Left
    };
    GLuint indices[] = {  // Note that we start from 0!
        0, 1, 3,  // First Triangle
        1, 2, 3   // Second Triangle
    };

    std::vector<Vertex> vertices2;
    std::vector<GLuint> indices2;
    {
        for(unsigned int i=0;i<(sizeof(vertices)/sizeof(vertices[0]));i+=3){
            Vertex aux;
            aux.position={vertices[i+0],vertices[i+1],vertices[i+2]};
            vertices2.push_back(aux);
        }
        for(unsigned int i=0;i<(sizeof(indices)/sizeof(indices[0]));i++){
            indices2.push_back(indices[i]);
        }

    }
    Sprite sprite(vertices2,indices2);

    GLfloat quadVertices[] = {   // Vertex attributes for a quad that fills the entire screen in Normalized Device Coordinates.
        // Positions   // TexCoords
        -1.0f,  1.0f,  0.0f, 1.0f,
        -1.0f, -1.0f,  0.0f, 0.0f,
         1.0f, -1.0f,  1.0f, 0.0f,

        -1.0f,  1.0f,  0.0f, 1.0f,
         1.0f, -1.0f,  1.0f, 0.0f,
         1.0f,  1.0f,  1.0f, 1.0f
    };

    // Setup cube VAO
    GLuint quadVAO, quadVBO;
    glGenVertexArrays(1, &quadVAO);
    glGenBuffers(1, &quadVBO);
    glBindVertexArray(quadVAO);
        glBindBuffer(GL_ARRAY_BUFFER, quadVBO);
            glBufferData(GL_ARRAY_BUFFER, sizeof(quadVertices), quadVertices, GL_STATIC_DRAW);
            glEnableVertexAttribArray(0);
            glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(GLfloat), (GLvoid*)0);
            glEnableVertexAttribArray(1);
            glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(GLfloat), (GLvoid*)(2 * sizeof(GLfloat)));
    glBindVertexArray(0);


    unsigned int points_p1=0,points_p2=0;
    bool first_point_p1=true,first_point_p2=true;

    GameObject player1(&sprite,{0.0f,2.0f});
    GameObject player2(&sprite,{0.0f,2.0f});
    GameObject ball(&sprite,{0.0f,0.0f});
    Entity roof(&sprite);
    Entity floor(&sprite);
    GameObject leftwall(&sprite,{0.0f,0.0f});
    GameObject rightwall(&sprite,{0.0f,0.0f});
    {
        player1.entity.setPos({-0.95f,0.0f});
        player2.entity.setPos({0.95f,0.0f});
        ball.entity.setPos({0.0f,8.0f});

        roof.setPos({-1.0f,1.10f});
        floor.setPos({-1.0f,-1.10f});
        roof.scale({4.0f,1.0f});
        floor.scale({4.0f,1.0f});

        leftwall.entity.setPos({-1.0f,0.0f});
        rightwall.entity.setPos({1.0f,0.0f});
        leftwall.entity.scale({0.025f,2.0f});
        rightwall.entity.scale({0.025f,2.0f});

        player1.entity.scale({0.025f, 0.49f});
        player2.entity.scale({0.025f, 0.49f});
        ball.entity.scale({0.0625f,0.0625f});
        ball.entity.order(SCALE,ROTATE,TRANSLATE);


    }

    std::vector<CollisionChecker> collisions;
    {
        collisions.push_back(CollisionChecker(&player1.entity,&roof));
        collisions.push_back(CollisionChecker(&player1.entity,&floor));
        collisions.push_back(CollisionChecker(&player2.entity,&roof));
        collisions.push_back(CollisionChecker(&player2.entity,&floor));
    }


    CollisionChecker ball_floor(&ball.entity,&floor);
    CollisionChecker ball_roof(&ball.entity,&roof);
    CollisionChecker ball_p1(&ball.entity,&player1.entity);
    CollisionChecker ball_p2(&ball.entity,&player2.entity);
    CollisionChecker ball_leftwall(&ball.entity,&leftwall.entity);
    CollisionChecker ball_rightwall(&ball.entity,&rightwall.entity);

    SDL_StartTextInput();
    bool quit = false;
    bool started=false;
    glm::vec2 p1_speed_gain(0.0f,0.0f);
    glm::vec2 p2_speed_gain(0.0f,0.0f);
    unsigned int i=0;
    Uint32 lastFrame=0;
    Uint32 deltaTime=0;
    float framerate=0.0f;
    float dt;


    framebuffer fb(originalScreenWidth,originalScreenHeight);
    Shader shader(WORKING_DIR_FILE("assets/shaders/shader.vert"),WORKING_DIR_FILE("assets/shaders/shader.frag"));
    Shader fb_shader(WORKING_DIR_FILE("assets/shaders/framebuffer_shader.vert"),WORKING_DIR_FILE("assets/shaders/framebuffer_shader.frag"));
    while(!quit)
    {
        SDL_PumpEvents();
        Uint32 currentFrame =            SDL_GetTicks();//miliseconds
        deltaTime           =  currentFrame - lastFrame;
        lastFrame           =              currentFrame;
        dt                  =         deltaTime/1000.0f;
        framerate          +=         1000.0f/deltaTime;

        p1_speed_gain={0.0f,0.0f};
        p2_speed_gain={0.0f,0.0f};
        {
            const Uint8 *keystates =                                     SDL_GetKeyboardState( NULL );
            quit                   = keystates[SDL_GetScancodeFromKey(SDLK_q)] || SDL_QuitRequested();
            started                =         started || keystates[SDL_GetScancodeFromKey(SDLK_SPACE)];
            if(started){
                if(keystates[SDL_GetScancodeFromKey(SDLK_w   )])
                {
                    player1.move( dt );
                    p1_speed_gain={ 0.0f , 1.0f };
                }
                if(keystates[SDL_GetScancodeFromKey(SDLK_s   )])
                {
                    player1.move(-dt );
                    p1_speed_gain={ 0.0f ,-1.0f };
                }
                if(keystates[SDL_GetScancodeFromKey(SDLK_UP  )])
                {
                    player2.move( dt );
                    p2_speed_gain={ 0.0f , 1.0f };
                }
                if(keystates[SDL_GetScancodeFromKey(SDLK_DOWN)])
                {
                    player2.move(-dt );
                    p2_speed_gain={ 0.0f ,-1.0f };
                }
                if(keystates[SDL_GetScancodeFromKey(SDLK_j)])
                {
                    player1.entity.rotate(15.0f );
                }

                do_ball_movement(ball,dt);
            }

        }
        {
            unsigned int size=collisions.size();
            for(unsigned int i=0;i<size;i++)
            {
                if(collisions[i].checkCollision())
                    handleCollision(collisions[i]);
            }
        }
        {
            glm::vec3 pos=ball.entity.position;
            glm::vec3 oldpos=ball.entity.oldPosition;
            if(ball_floor.checkCollision()){
                handleCollision(ball_floor);
                ball.speed=glm::normalize(glm::reflect(glm::vec2(pos.x-oldpos.x,pos.y-oldpos.y),glm::vec2(0.0f,1.0f)))*glm::length(ball.speed);
                Mix_PlayChannel( -1, pong2, 0 );
            }
            if(ball_roof.checkCollision()){
                handleCollision(ball_roof);
                ball.speed=glm::normalize(glm::reflect(glm::vec2(pos.x-oldpos.x,pos.y-oldpos.y),glm::vec2(0.0f,-1.0f)))*glm::length(ball.speed);
                Mix_PlayChannel( -1, pong2, 0 );
            }
            if(ball_p1.checkCollision()){
                handleCollision(ball_p1);
                ball.speed=glm::normalize(glm::reflect(glm::vec2(pos.x-oldpos.x,pos.y-oldpos.y),glm::vec2(1.0f,0.0f)) + p1_speed_gain)
                        *
                        (glm::length(ball.speed)+glm::length(p1_speed_gain));
                Mix_PlayChannel( -1, pong, 0 );
            }
            if(ball_p2.checkCollision()){
                handleCollision(ball_p2);
                ball.speed=glm::normalize(glm::reflect(glm::vec2(pos.x-oldpos.x,pos.y-oldpos.y),glm::vec2(-1.0f,0.0f)) + p2_speed_gain)
                        *
                        (glm::length(ball.speed)+glm::length(p2_speed_gain));
                Mix_PlayChannel( -1, pong, 0 );
            }

            if(ball_leftwall.checkCollision()){
                points_p2++;
                ball.entity.setPos({0.0f,8.0f});//scale adjusted due the order it uses...
                ball.speed={0.0f,0.0f};
                Mix_PlayChannel( -1, pong3, 0 );
            }
            if(ball_rightwall.checkCollision()){
                points_p1++;
                ball.entity.setPos({0.0f,8.0f});
                ball.speed={0.0f,0.0f};
                Mix_PlayChannel( -1, pong3, 0 );
            }
            if(((ball.speed.y/ball.speed.x)>3.0f) || ((ball.speed.y/ball.speed.x)<-3.0f)){
                ball.speed.y/=2.0f;
                ball.speed.x*=4.0f;
            }
        }
        if(i==100){
            i=0;
            framerate/=100.0f;
            std::cout<<framerate<<std::endl;
            std::cout<<points_p1<<'-'<<points_p2<<std::endl;
            framerate=0.0f;
        }
        i+=1;
        shader.Use();
            fb.bind();
                glViewport(0,0,originalScreenWidth,originalScreenHeight);
                glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
                glEnable(GL_DEPTH_TEST);
                glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);

                glm::mat4 projection;//= glm::ortho(-1.0f,1.0f,-1.0f,1.0f,0.0f,1.0f);
                GLint projection_uniform=glGetUniformLocation(shader.Program, "projection");
                glUniformMatrix4fv(projection_uniform, 1, GL_FALSE, glm::value_ptr(projection));


                glm::vec2 position;
                position=glm::vec2(-3.0f*1.06255f*0.125f,0.0f);
                if(points_p1>=5 || points_p2>=5){
                    drawdigit(-1,&shader,position,{0.125f,0.125f},DIGIT_TOP|DIGIT_MIDDLE|DIGIT_TOPLEFT|DIGIT_TOPRIGHT|DIGIT_BOTTOMLEFT);
                    position+=glm::vec2(1.0625f*0.125f,0.0f);
                    drawdigit((points_p1>=5)?1:2,&shader,position,{0.125f,0.125f});
                    position+=glm::vec2(1.0625f*0.125f,0.0f);
                    position+=glm::vec2(1.0625f*0.125f,0.0f);
                    drawdigit(-1,&shader,position,{0.125f,0.125f},DIGIT_TOPLEFT |DIGIT_BOTTOMLEFT |DIGIT_BOTTOMLEFT_MIDDLE|
                                                                  DIGIT_TOPRIGHT|DIGIT_BOTTOMRIGHT|DIGIT_BOTTOMRIGHT_MIDDLE);
                    position+=glm::vec2(1.0625f*0.125f,0.0f);
                    drawdigit(-1,&shader,position,{0.125f,0.125f},DIGIT_TOPMIDDLE|DIGIT_BOTTOMMIDDLE);
                    position+=glm::vec2(1.0625f*0.125f,0.0f);
                    drawdigit(-1,&shader,position,{0.125f,0.125f},DIGIT_BOTTOMLEFT |DIGIT_TOPLEFT|DIGIT_TOPLEFT_BOTTOMRIGHT|
                                                                  DIGIT_BOTTOMRIGHT|DIGIT_TOPRIGHT);
                    position+=glm::vec2(1.0625f*0.125f,0.0f);
                    drawdigit(5,&shader,position,{0.125f,0.125f});
                    started=false;
                }

                player1.entity.draw(&shader);
                player2.entity.draw(&shader);
                if(started) ball.entity.draw(&shader);
                roof.draw(&shader);
                floor.draw(&shader);
                leftwall.entity.draw(&shader);
                rightwall.entity.draw(&shader);




                unsigned int aux=points_p2;
                position=glm::vec2(0.25f,0.5f);
                //NOTE: when one of the points hits 20 I should put a you win screen
                first_point_p2=points_p2? false:true;
                while((aux/10) || (aux%10) || first_point_p2){
                    drawdigit(aux%10,&shader,position,{0.125f,0.125f});
                    position.x-=1.5f*0.125f;
                    aux=aux/10;
                    first_point_p2=false;//endless loop if I dont
                }

                aux=points_p1;
                position={-0.25f,0.5f};
                first_point_p1=points_p1? false:true;
                while((aux/10) || (aux%10) || first_point_p1){
                    drawdigit(aux%10,&shader,position,{0.125f,0.125f});
                    position.x-=1.5f*0.125f;
                    aux=aux/10;
                    first_point_p1=false;
                }


            fb.unbind();
        SDL_GetWindowSize(mainwindow,&screenWidth,&screenHeight);
        glViewport(0,0,screenWidth,screenHeight);
        glClearColor(0.0f, 0.0f, 0.1f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT);
        glDisable(GL_DEPTH_TEST);
        fb_shader.Use();
            glm::mat4 screenscaler;
            float aspectRatio=(float)screenWidth/(float)screenHeight;
            float inverseAspectRatio=(float)screenHeight/(float)screenWidth;
            if(aspectRatio>1.0f)
            screenscaler = glm::perspective(radians(59.2f),aspectRatio,0.1f,1.0f);
            else
            screenscaler = glm::perspective(radians(59.2f),inverseAspectRatio,0.1f,1.0f);
            GLint model_uniform=glGetUniformLocation(fb_shader.Program, "model");
            glUniformMatrix4fv(model_uniform, 1, GL_FALSE, glm::value_ptr(screenscaler));

            glBindVertexArray(quadVAO);//should scale the scale to the % of resolution
            glActiveTexture(GL_TEXTURE0);
            glBindTexture(GL_TEXTURE_2D, fb.texture);
                glDrawArrays(GL_TRIANGLES, 0, 6);
            glBindTexture(GL_TEXTURE_2D,0);
            glBindVertexArray(0);


        SDL_Delay(1);
        SDL_GL_SwapWindow(mainwindow);
        if(points_p1>=5 || points_p2>=5){
            points_p1=0;
            points_p2=0;
            ball.speed={0.0f,0.0f};
            SDL_Delay(3000);
        }
    }
    DESTRUCTOR(fb);
    DESTRUCTOR(shader);
    DESTRUCTOR(fb_shader);
    Mix_FreeChunk(pong);
    Mix_FreeChunk(pong2);
    Mix_FreeChunk(pong3);
    SDL_GL_DeleteContext(maincontext);
    SDL_DestroyWindow(mainwindow);
    SDL_Quit();

    return 0;
}
Exemple #5
0
/*
  update state of tracker
 */
void Tracker::update(const struct sitl_input &input)
{
    // how much time has passed?
    float delta_time = frame_time_us * 1.0e-6f;

    float yaw_rate, pitch_rate;

    yaw_input = (input.servos[0]-1500)/500.0f;
    pitch_input = (input.servos[1]-1500)/500.0f;

    // implement yaw and pitch limits
    float r, p, y;
    dcm.to_euler(&r, &p, &y);

    pitch_current_relative = degrees(p) - zero_pitch;
    yaw_current_relative = degrees(y) - zero_yaw;
    float roll_current = degrees(r);
    if (yaw_current_relative > 180) {
        yaw_current_relative -= 360;
    }
    if (yaw_current_relative < -180) {
        yaw_current_relative += 360;
    }
    if (yaw_rate > 0 && yaw_current_relative >= yaw_range) {
        yaw_rate = 0;
    }
    if (yaw_rate < 0 && yaw_current_relative <= -yaw_range) {
        yaw_rate = 0;
    }
    if (pitch_rate > 0 && pitch_current_relative >= pitch_range) {
        pitch_rate = 0;
    }
    if (pitch_rate < 0 && pitch_current_relative <= -pitch_range) {
        pitch_rate = 0;
    }

    if (onoff) {
        update_onoff_servos(yaw_rate, pitch_rate);
    } else {
        update_position_servos(delta_time, yaw_rate, pitch_rate);
    }


    // keep it level
    float roll_rate = 0 - roll_current;

    if (time_now_us - last_debug_us > 2e6f && !onoff) {
        last_debug_us = time_now_us;
        printf("roll=%.1f pitch=%.1f yaw=%.1f rates=%.1f/%.1f/%.1f in=%.3f,%.3f\n",
               roll_current,
               pitch_current_relative,
               yaw_current_relative,
               roll_rate, pitch_rate, yaw_rate,
               yaw_input, pitch_input);
    }

    gyro = Vector3f(radians(roll_rate),radians(pitch_rate),radians(yaw_rate));

    // update attitude
    dcm.rotate(gyro * delta_time);
    dcm.normalize();

    Vector3f accel_earth = Vector3f(0, 0, -GRAVITY_MSS);
    accel_body = dcm.transposed() * accel_earth;

    // new velocity vector
    velocity_ef.zero();
    update_position();
}
Exemple #6
0
/// returns the angle (radians) that the RC_Channel input is receiving
float
AP_Mount::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
{
    return radians(angle_input(rc, angle_min, angle_max)*0.01f);
}
Exemple #7
0
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsTri::output_armed_stabilizing()
{
    float   roll_thrust;                // roll thrust input value, +/- 1.0
    float   pitch_thrust;               // pitch thrust input value, +/- 1.0
    float   yaw_thrust;                 // yaw thrust input value, +/- 1.0
    float   throttle_thrust;            // throttle thrust input value, 0.0 - 1.0
    float   throttle_thrust_best_rpy;   // throttle providing maximum roll, pitch and yaw range without climbing
    float   rpy_scale = 1.0f;           // this is used to scale the roll, pitch and yaw to fit within the motor limits
    float   rpy_low = 0.0f;             // lowest motor value
    float   rpy_high = 0.0f;            // highest motor value
    float   thr_adj;                    // the difference between the pilot's desired throttle and throttle_thrust_best_rpy

    // sanity check YAW_SV_ANGLE parameter value to avoid divide by zero
    _yaw_servo_angle_max_deg = constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX);

    // apply voltage and air pressure compensation
    roll_thrust = _roll_in * get_compensation_gain();
    pitch_thrust = _pitch_in * get_compensation_gain();
    yaw_thrust = _yaw_in * get_compensation_gain()*sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
    throttle_thrust = get_throttle() * get_compensation_gain();

    // calculate angle of yaw pivot
    _pivot_angle = safe_asin(yaw_thrust);
    if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) {
        limit.yaw = true;
        _pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg));
    }

    float pivot_thrust_max = cosf(_pivot_angle);
    float thrust_max = 1.0f;

    // sanity check throttle is above zero and below current limited throttle
    if (throttle_thrust <= 0.0f) {
        throttle_thrust = 0.0f;
        limit.throttle_lower = true;
    }
    if (throttle_thrust >= _throttle_thrust_max) {
        throttle_thrust = _throttle_thrust_max;
        limit.throttle_upper = true;
    }

    _throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);

    // The following mix may be offer less coupling between axis but needs testing
    //_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
    //_thrust_left = roll_thrust * 0.5f + pitch_thrust * 1.0f;
    //_thrust_rear = 0;

    _thrust_right = roll_thrust * -0.5f + pitch_thrust * 0.5f;
    _thrust_left = roll_thrust * 0.5f + pitch_thrust * 0.5f;
    _thrust_rear = pitch_thrust * -0.5f;

    // calculate roll and pitch for each motor
    // set rpy_low and rpy_high to the lowest and highest values of the motors

    // record lowest roll pitch command
    rpy_low = MIN(_thrust_right,_thrust_left);
    rpy_high = MAX(_thrust_right,_thrust_left);
    if (rpy_low > _thrust_rear){
        rpy_low = _thrust_rear;
    }
    // check to see if the rear motor will reach maximum thrust before the front two motors
    if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)){
        thrust_max = pivot_thrust_max;
        rpy_high = _thrust_rear;
    }

    // calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
    //      1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible room margin above the highest motor and below the lowest
    //      2. the higher of:
    //            a) the pilot's throttle input
    //            b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
    //      Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
    //      Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
    //      We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
    //      We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low.  We favor reducing throttle instead of better yaw control because the pilot has commanded it

    // check everything fits
    throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, _throttle_avg_max);
    if(is_zero(rpy_low)){
        rpy_scale = 1.0f;
    } else {
        rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
    }

    // calculate how close the motors can come to the desired throttle
    thr_adj = throttle_thrust - throttle_thrust_best_rpy;
    if(rpy_scale < 1.0f){
        // Full range is being used by roll, pitch, and yaw.
        limit.roll_pitch = true;
        if (thr_adj > 0.0f){
            limit.throttle_upper = true;
        }
        thr_adj = 0.0f;
    }else{
        if(thr_adj < -(throttle_thrust_best_rpy+rpy_low)){
            // Throttle can't be reduced to desired value
            thr_adj = -(throttle_thrust_best_rpy+rpy_low);
        }else if(thr_adj > thrust_max - (throttle_thrust_best_rpy+rpy_high)){
            // Throttle can't be increased to desired value
            thr_adj = thrust_max - (throttle_thrust_best_rpy+rpy_high);
            limit.throttle_upper = true;
        }
    }

    // add scaled roll, pitch, constrained yaw and throttle for each motor
    _thrust_right = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_right;
    _thrust_left = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_left;
    _thrust_rear = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_rear;

    // scale pivot thrust to account for pivot angle
    // we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range
    _thrust_rear = _thrust_rear/cosf(_pivot_angle);

    // constrain all outputs to 0.0f to 1.0f
    // test code should be run with these lines commented out as they should not do anything
    _thrust_right = constrain_float(_thrust_right, 0.0f, 1.0f);
    _thrust_left = constrain_float(_thrust_left, 0.0f, 1.0f);
    _thrust_rear = constrain_float(_thrust_rear, 0.0f, 1.0f);
}
// Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
// A positive demand is up
// Inputs are: 
// 1) demanded pitch angle in centi-degrees
// 2) control gain scaler = scaling_speed / aspeed
// 3) boolean which is true when stabilise mode is active
// 4) minimum FBW airspeed (metres/sec)
// 5) maximum FBW airspeed (metres/sec)
//
int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stabilize, int16_t aspd_min, int16_t aspd_max)
{
	uint32_t tnow = hal.scheduler->millis();
	uint32_t dt = tnow - _last_t;
	
	if (_last_t == 0 || dt > 1000) {
		dt = 0;
	}
	_last_t = tnow;
	
	if(_ahrs == NULL) return 0;
	float delta_time    = (float)dt * 0.001f;
	
	// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
    // No conversion is required for K_D
	float kp_ff = max((_K_P - _K_I * _tau) * _tau  - _K_D , 0);
	float ki_rate = _K_I * _tau;

	// Calculate offset to pitch rate demand required to maintain pitch angle whilst banking
	// Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn
	// Pitch rate offset is the component of turn rate about the pitch axis
	float aspeed;
	float rate_offset;
	float bank_angle = _ahrs->roll;
	bool inverted = false;

    if (_tau < 0.1) {
        _tau = 0.1;
    }

	// limit bank angle between +- 80 deg if right way up
	if (fabsf(bank_angle) < radians(90))	{
	    bank_angle = constrain_float(bank_angle,-radians(80),radians(80));
	} else {
		inverted = true;
		if (bank_angle > 0.0f) {
			bank_angle = constrain_float(bank_angle,radians(100),radians(180));
		} else {
			bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
		}
	}
	if (!_ahrs->airspeed_estimate(&aspeed)) {
	    // If no airspeed available use average of min and max
        aspeed = 0.5f*(float(aspd_min) + float(aspd_max));
	}
    rate_offset = fabsf(ToDeg((GRAVITY_MSS / max(aspeed , float(aspd_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
	if (inverted) {
		rate_offset = -rate_offset;
	}
	
	//Calculate pitch angle error in centi-degrees
	int32_t angle_err = angle - _ahrs->pitch_sensor;

	// Calculate the desired pitch rate (deg/sec) from the angle error
	float desired_rate = angle_err * 0.01f / _tau;
	
	// limit the maximum pitch rate demand. Don't apply when inverted
	// as the rates will be tuned when upright, and it is common that
	// much higher rates are needed inverted	
	if (!inverted) {
		if (_max_rate_neg && desired_rate < -_max_rate_neg) {
			desired_rate = -_max_rate_neg;
		} else if (_max_rate_pos && desired_rate > _max_rate_pos) {
			desired_rate = _max_rate_pos;
		}
	}
	
	if (inverted) {
		desired_rate = -desired_rate;
	}

	// Apply the turn correction offset
	desired_rate = desired_rate + rate_offset;

	// Get body rate vector (radians/sec)
	float omega_y = _ahrs->get_gyro().y;
	
	// Calculate the pitch rate error (deg/sec) and scale
	float rate_error = (desired_rate - ToDeg(omega_y)) * scaler;
	
	// Multiply pitch rate error by _ki_rate and integrate
	// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
	if (!stabilize && ki_rate > 0) {
		//only integrate if gain and time step are positive and airspeed above min value.
		if (dt > 0 && aspeed > float(aspd_min)) {
		    float integrator_delta = rate_error * ki_rate * delta_time;
			if (_last_out < -45) {
				// prevent the integrator from increasing if surface defln demand is above the upper limit
				integrator_delta = max(integrator_delta , 0);
			} else if (_last_out > 45) {
				// prevent the integrator from decreasing if surface defln demand  is below the lower limit
				integrator_delta = min(integrator_delta , 0);
			}
			_integrator += integrator_delta;
		}
	} else {
		_integrator = 0;
	}
	
	// Calculate the demanded control surface deflection
	// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
	// path, but want a 1/speed^2 scaler applied to the rate error path. 
	// This is because acceleration scales with speed^2, but rate scales with speed.
	_last_out = ( (rate_error * _K_D) + _integrator + (desired_rate * kp_ff) ) * scaler;
	
	// Convert to centi-degrees and constrain
	return constrain_float(_last_out * 100, -4500, 4500);
}
Exemple #9
0
bool AP_Arming::ins_checks(bool report) 
{
    if ((checks_to_perform & ARMING_CHECK_ALL) ||
        (checks_to_perform & ARMING_CHECK_INS)) {
        const AP_InertialSensor &ins = ahrs.get_ins();
        if (! ins.get_gyro_health_all()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: gyros not healthy!"));
            }
            return false;
        }
        if (!skip_gyro_cal && ! ins.gyro_calibrated_ok_all()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: gyros not calibrated!"));
            }
            return false;
        }
        if (! ins.get_accel_health_all()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: accels not healthy!"));
            }
            return false;
        }
        if (!ahrs.healthy()) {
            if (report) {
                const char *reason = ahrs.prearm_failure_reason();
                if (reason) {
                    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: %s"), reason);
                } else {
                    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: AHRS not healthy!"));
                }
            }
            return false;
        }
        if (!ins.accel_calibrated_ok_all()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: 3D accel cal needed"));
            }
            return false;
        }
#if INS_MAX_INSTANCES > 1
        // check all accelerometers point in roughly same direction
        if (ins.get_accel_count() > 1) {
            const Vector3f &prime_accel_vec = ins.get_accel();
            for(uint8_t i=0; i<ins.get_accel_count(); i++) {
                // get next accel vector
                const Vector3f &accel_vec = ins.get_accel(i);
                Vector3f vec_diff = accel_vec - prime_accel_vec;
                // allow for up to 0.75 m/s/s difference. Has to pass
                // in last 10 seconds
                float threshold = 0.75f;
                if (i >= 2) {
                    /*
                      we allow for a higher threshold for IMU3 as it
                      runs at a different temperature to IMU1/IMU2,
                      and is not used for accel data in the EKF
                     */
                    threshold *= 3;
                }
                if (vec_diff.length() <= threshold) {
                    last_accel_pass_ms[i] = hal.scheduler->millis();
                }
                if (hal.scheduler->millis() - last_accel_pass_ms[i] > 10000) {
                    if (report) {
                        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: inconsistent Accelerometers"));
                    }
                    return false;
                }
            }
        }

        // check all gyros are giving consistent readings
        if (ins.get_gyro_count() > 1) {
            const Vector3f &prime_gyro_vec = ins.get_gyro();
            for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
                // get next gyro vector
                const Vector3f &gyro_vec = ins.get_gyro(i);
                Vector3f vec_diff = gyro_vec - prime_gyro_vec;
                // allow for up to 5 degrees/s difference. Pass if its
                // been OK in last 10 seconds
                if (vec_diff.length() <= radians(5)) {
                    last_gyro_pass_ms[i] = hal.scheduler->millis();
                }
                if (hal.scheduler->millis() - last_gyro_pass_ms[i] > 10000) {
                    if (report) {
                        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: inconsistent gyros"));
                    }
                    return false;
                }
            }
        }
#endif
    }

    return true;
}
// CCPM Mixers - calculate mixing scale factors by swashplate type
void AP_MotorsHeli_Swash::calculate_roll_pitch_collective_factors()
{
    if (_swash_type == SWASHPLATE_TYPE_H1) {
        // CCPM mixing not used
        _collectiveFactor[CH_1] = 0;
        _collectiveFactor[CH_2] = 0;
        _collectiveFactor[CH_3] = 1;
    } else if ((_swash_type == SWASHPLATE_TYPE_H4_90) || (_swash_type == SWASHPLATE_TYPE_H4_45)) {
        // collective mixer for four-servo CCPM
        _collectiveFactor[CH_1] = 1;
        _collectiveFactor[CH_2] = 1;
        _collectiveFactor[CH_3] = 1;
        _collectiveFactor[CH_4] = 1;
    } else {
        // collective mixer for three-servo CCPM
        _collectiveFactor[CH_1] = 1;
        _collectiveFactor[CH_2] = 1;
        _collectiveFactor[CH_3] = 1;
    }

    if (_swash_type == SWASHPLATE_TYPE_H3) {
        // Three-servo roll/pitch mixer for adjustable servo position
        // can be any style swashplate, phase angle is adjustable
        _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle));
        _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle));
        _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle));
        _pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle));
        _pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle));
        _pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle));
        
        // defined swashplates, servo1 is always left, servo2 is right,
        // servo3 is elevator
    } else if (_swash_type == SWASHPLATE_TYPE_H3_140) {    //
        // Three-servo roll/pitch mixer for H3-140
        // HR3-140 uses reversed servo and collective direction in heli setup
        // 1:1 pure input style, phase angle not adjustable
        _rollFactor[CH_1] = 1;
        _rollFactor[CH_2] = -1;
        _rollFactor[CH_3] = 0;
        _pitchFactor[CH_1] = 1;
        _pitchFactor[CH_2] = 1;
        _pitchFactor[CH_3] = -1;
    } else if (_swash_type == SWASHPLATE_TYPE_H3_120) {
        // three-servo roll/pitch mixer for H3-120
        // HR3-120 uses reversed servo and collective direction in heli setup
        // not a pure mixing swashplate, phase angle is adjustable
        _rollFactor[CH_1] = 0.866025f;
        _rollFactor[CH_2] = -0.866025f;
        _rollFactor[CH_3] = 0;
        _pitchFactor[CH_1] = 0.5f;
        _pitchFactor[CH_2] = 0.5f;
        _pitchFactor[CH_3] = -1;
    } else if (_swash_type == SWASHPLATE_TYPE_H4_90) {
        // four-servo roll/pitch mixer for H4-90
        // 1:1 pure input style, phase angle not adjustable
        // servos 3 & 7 are elevator
        // can also be used for all versions of 90 deg three-servo swashplates
        _rollFactor[CH_1] = 1; 
        _rollFactor[CH_2] = -1;
        _rollFactor[CH_3] = 0;
        _rollFactor[CH_4] = 0;
        _pitchFactor[CH_1] = 0;
        _pitchFactor[CH_2] = 0;
        _pitchFactor[CH_3] = -1;
        _pitchFactor[CH_4] = 1;
    } else if (_swash_type == SWASHPLATE_TYPE_H4_45) {
        // four-servo roll/pitch mixer for H4-45
        // 1:1 pure input style, phase angle not adjustable
        // for 45 deg plates servos 1&2 are LF&RF, 3&7 are LR&RR.
        _rollFactor[CH_1] = 0.707107f; 
        _rollFactor[CH_2] = -0.707107f;
        _rollFactor[CH_3] = 0.707107f;
        _rollFactor[CH_4] = -0.707107f;
        _pitchFactor[CH_1] = 0.707107f;
        _pitchFactor[CH_2] = 0.707107f;
        _pitchFactor[CH_3] = -0.707f;
        _pitchFactor[CH_4] = -0.707f;
    } else {
        // CCPM mixing not being used, so H1 straight outputs
        _rollFactor[CH_1] = 1;
        _rollFactor[CH_2] = 0;
        _rollFactor[CH_3] = 0;
        _pitchFactor[CH_1] = 0;
        _pitchFactor[CH_2] = 1;
        _pitchFactor[CH_3] = 0;
    }
}
Exemple #11
0
/*
  receive data from X-Plane via UDP
*/
bool XPlane::receive_data(void)
{
    uint8_t pkt[10000];
    uint8_t *p = &pkt[5];
    const uint8_t pkt_len = 36;
    uint64_t data_mask = 0;
    const uint64_t one = 1U;
    const uint64_t required_mask = (one<<Times | one<<LatLonAlt | one<<Speed | one<<PitchRollHeading |
                                    one<<LocVelDistTraveled | one<<AngularVelocities | one<<Gload |
                                    one << Joystick1 | one << ThrottleCommand | one << Trim |
                                    one << PropPitch | one << EngineRPM | one << PropRPM | one << Generator |
                                    one << Mixture);
    Location loc {};
    Vector3f pos;
    uint32_t wait_time_ms = 1;
    uint32_t now = AP_HAL::millis();

    // if we are about to get another frame from X-Plane then wait longer
    if (xplane_frame_time > wait_time_ms &&
        now+1 >= last_data_time_ms + xplane_frame_time) {
        wait_time_ms = 10;
    }
    ssize_t len = socket_in.recv(pkt, sizeof(pkt), wait_time_ms);
    
    if (len < pkt_len+5 || memcmp(pkt, "[email protected]", 5) != 0) {
        // not a data packet we understand
        goto failed;
    }
    len -= 5;

    if (!connected) {
        // we now know the IP X-Plane is using
        uint16_t port;
        socket_in.last_recv_address(xplane_ip, port);
        socket_out.connect(xplane_ip, xplane_port);
        connected = true;
        printf("Connected to %s:%u\n", xplane_ip, (unsigned)xplane_port);
    }
    
    while (len >= pkt_len) {
        const float *data = (const float *)p;
        uint8_t code = p[0];
        // keep a mask of what codes we have received
        if (code < 64) {
            data_mask |= (((uint64_t)1) << code);
        }
        switch (code) {
        case Times: {
            uint64_t tus = data[3] * 1.0e6f;
            if (tus + time_base_us <= time_now_us) {
                uint64_t tdiff = time_now_us - (tus + time_base_us);
                if (tdiff > 1e6f) {
                    printf("X-Plane time reset %lu\n", (unsigned long)tdiff);
                }
                time_base_us = time_now_us - tus;
            }
            uint64_t tnew = time_base_us + tus;
            //uint64_t dt = tnew - time_now_us;
            //printf("dt %u\n", (unsigned)dt);
            time_now_us = tnew;
            break;
        }
            
        case LatLonAlt: {
            loc.lat = data[1] * 1e7;
            loc.lng = data[2] * 1e7;
            loc.alt = data[3] * FEET_TO_METERS * 100.0f;
            float hagl = data[4] * FEET_TO_METERS;
            ground_level = loc.alt * 0.01f - hagl;
            break;
        }

        case Speed:
            airspeed = data[2] * KNOTS_TO_METERS_PER_SECOND;
            airspeed_pitot = airspeed;
            break;

        case AoA:
            // ignored
            break;

        case Trim:
            if (heli_frame) {
                // use flaps for collective as no direct collective data input
                rcin[2] = data[4];
            }
            break;
            
        case PitchRollHeading: {
            float roll, pitch, yaw;
            pitch = radians(data[1]);
            roll = radians(data[2]);
            yaw = radians(data[3]);
            dcm.from_euler(roll, pitch, yaw);
            break;
        }

        case AtmosphereWeather:
            // ignored
            break;

        case LocVelDistTraveled:
            pos.y = data[1];
            pos.z = -data[2];
            pos.x = -data[3];
            velocity_ef.y = data[4];
            velocity_ef.z = -data[5];
            velocity_ef.x = -data[6];
            break;

        case AngularVelocities:
            gyro.y = data[1];
            gyro.x = data[2];
            gyro.z = data[3];
            break;

        case Gload:
            accel_body.z = -data[5] * GRAVITY_MSS;
            accel_body.x = data[6] * GRAVITY_MSS;
            accel_body.y = data[7] * GRAVITY_MSS;
            break;

        case Joystick1:
            rcin_chan_count = 4;
            rcin[0] = (data[2] + 1)*0.5f;
            rcin[1] = (data[1] + 1)*0.5f;
            rcin[3] = (data[3] + 1)*0.5f;
            break;

        case ThrottleCommand: {
            if (!heli_frame) {
                /* getting joystick throttle input is very weird. The
                 * problem is that XPlane sends the ThrottleCommand packet
                 * both for joystick throttle input and for throttle that
                 * we have provided over the link. So we need some way to
                 * detect when we get our own values back. The trick used
                 * is to add throttle_magic to the values we send, then
                 * detect this offset in the data coming back. Very ugly,
                 * but I can't find a better way of allowing joystick
                 * input from XPlane10
                 */
                bool has_magic = ((uint32_t)(data[1] * throttle_magic_scale) % 1000U) == (uint32_t)(throttle_magic * throttle_magic_scale);
                if (data[1] < 0 ||
                    data[1] == throttle_sent ||
                    has_magic) {
                    break;
                }
                rcin[2] = data[1];
            }
            break;
        }

        case PropPitch: {
            break;
        }

        case EngineRPM:
            rpm1 = data[1];
            break;

        case PropRPM:
            rpm2 = data[1];
            break;
            
        case Joystick2:
            break;

        case Generator:
            /*
              in order to get interlock switch on helis we map the
              "generator1 on/off" function of XPlane 10 to channel 8.
             */
            rcin_chan_count = 8;
            rcin[7] = data[1];
            break;

        case Mixture:
            // map channel 6 and 7 from Mixture3 and Mixture4 for extra channels
            rcin_chan_count = MAX(7, rcin_chan_count);
            rcin[5] = data[3];
            rcin[6] = data[4];
            break;
        }
        len -= pkt_len;
        p += pkt_len;
    }

    if (data_mask != required_mask) {
        // ask XPlane to change what data it sends
        select_data(data_mask & ~required_mask, required_mask & ~data_mask);
        goto failed;
    }
    position = pos + position_zero;
    update_position();

    accel_earth = dcm * accel_body;
    accel_earth.z += GRAVITY_MSS;
    
    // the position may slowly deviate due to float accuracy and longitude scaling
    if (get_distance(loc, location) > 4 || fabsf(loc.alt - location.alt)*0.01 > 2) {
        printf("X-Plane home reset dist=%f alt=%.1f/%.1f\n",
               get_distance(loc, location), loc.alt*0.01f, location.alt*0.01f);
        // reset home location
        position_zero(-pos.x, -pos.y, -pos.z);
        home.lat = loc.lat;
        home.lng = loc.lng;
        home.alt = loc.alt;
        position.x = 0;
        position.y = 0;
        position.z = 0;
        update_position();
    }

    update_mag_field_bf();

    if (now > last_data_time_ms && now - last_data_time_ms < 100) {
        xplane_frame_time = now - last_data_time_ms;
    }
    last_data_time_ms = AP_HAL::millis();

    report.data_count++;
    report.frame_count++;
    return true;
        
failed:
    if (AP_HAL::millis() - last_data_time_ms > 200) {
        // don't extrapolate beyond 0.2s
        return false;
    }

    // advance time by 1ms
    Vector3f rot_accel;
    frame_time_us = 1000;
    float delta_time = frame_time_us * 1e-6f;

    time_now_us += frame_time_us;

    // extrapolate sensors
    dcm.rotate(gyro * delta_time);
    dcm.normalize();

    // work out acceleration as seen by the accelerometers. It sees the kinematic
    // acceleration (ie. real movement), plus gravity
    accel_body = dcm.transposed() * (accel_earth + Vector3f(0,0,-GRAVITY_MSS));

    // new velocity and position vectors
    velocity_ef += accel_earth * delta_time;
    position += velocity_ef * delta_time;
    velocity_air_ef = velocity_ef - wind_ef;
    velocity_air_bf = dcm.transposed() * velocity_air_ef;

    update_position();
    update_mag_field_bf();
    report.frame_count++;
    return false;
}
Exemple #12
0
// init_swash - initialise the swash plate
void AP_MotorsHeli::init_swash()
{

    // swash servo initialisation
    _servo_1->set_range(0,1000);
    _servo_2->set_range(0,1000);
    _servo_3->set_range(0,1000);
    _servo_4->set_angle(4500);

    // ensure _coll values are reasonable
    if( collective_min >= collective_max ) {
        collective_min = 1000;
        collective_max = 2000;
    }

    collective_mid = constrain_int16(collective_mid, collective_min, collective_max);

    // calculate throttle mid point
    throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0f;

    // determine roll, pitch and throttle scaling
    _roll_scaler = (float)roll_max/4500.0f;
    _pitch_scaler = (float)pitch_max/4500.0f;
    _collective_scalar = ((float)(collective_max-collective_min))/1000.0f;
    _stab_throttle_scalar = ((float)(stab_col_max - stab_col_min))/100.0f;

    if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) {                     //CCPM Swashplate, perform control mixing

        // roll factors
        _rollFactor[CH_1] = cosf(radians(servo1_pos + 90 - phase_angle));
        _rollFactor[CH_2] = cosf(radians(servo2_pos + 90 - phase_angle));
        _rollFactor[CH_3] = cosf(radians(servo3_pos + 90 - phase_angle));

        // pitch factors
        _pitchFactor[CH_1] = cosf(radians(servo1_pos - phase_angle));
        _pitchFactor[CH_2] = cosf(radians(servo2_pos - phase_angle));
        _pitchFactor[CH_3] = cosf(radians(servo3_pos - phase_angle));

        // collective factors
        _collectiveFactor[CH_1] = 1;
        _collectiveFactor[CH_2] = 1;
        _collectiveFactor[CH_3] = 1;

    } else {             //H1 Swashplate, keep servo outputs seperated

        // roll factors
        _rollFactor[CH_1] = 1;
        _rollFactor[CH_2] = 0;
        _rollFactor[CH_3] = 0;

        // pitch factors
        _pitchFactor[CH_1] = 0;
        _pitchFactor[CH_2] = 1;
        _pitchFactor[CH_3] = 0;

        // collective factors
        _collectiveFactor[CH_1] = 0;
        _collectiveFactor[CH_2] = 0;
        _collectiveFactor[CH_3] = 1;
    }

    // servo min/max values
    _servo_1->radio_min = 1000;
    _servo_1->radio_max = 2000;
    _servo_2->radio_min = 1000;
    _servo_2->radio_max = 2000;
    _servo_3->radio_min = 1000;
    _servo_3->radio_max = 2000;

    // mark swash as initialised
    _swash_initialised = true;
}
Exemple #13
0
inline const Angle operator""_rad(long double r) noexcept {
	return radians(static_cast<float>(r));
}
void Copter::update_ground_effect_detector(void)
{
    if(!motors.armed()) {
        // disarmed - disable ground effect and return
        gndeffect_state.takeoff_expected = false;
        gndeffect_state.touchdown_expected = false;
        ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
        ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
        return;
    }

    // variable initialization
    uint32_t tnow_ms = millis();
    float xy_des_speed_cms = 0.0f;
    float xy_speed_cms = 0.0f;
    float des_climb_rate_cms = pos_control.get_desired_velocity().z;

    if (pos_control.is_active_xy()) {
        Vector3f vel_target = pos_control.get_vel_target();
        vel_target.z = 0.0f;
        xy_des_speed_cms = vel_target.length();
    }

    if (position_ok() || optflow_position_ok()) {
        Vector3f vel = inertial_nav.get_velocity();
        vel.z = 0.0f;
        xy_speed_cms = vel.length();
    }

    // takeoff logic

    // if we are armed and haven't yet taken off
    if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
        gndeffect_state.takeoff_expected = true;
    }

    // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
    bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.control_in > 0;
    if (!throttle_up && ap.land_complete) {
        gndeffect_state.takeoff_time_ms = tnow_ms;
        gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
    }

    // if we are in takeoff_expected and we meet the conditions for having taken off
    // end the takeoff_expected state
    if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
        gndeffect_state.takeoff_expected = false;
    }

    // landing logic
    Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f);
    bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
    bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
    bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f;
    bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);

    bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f;
    bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
    bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f;
    bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));

    gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;

    // Prepare the EKF for ground effect if either takeoff or touchdown is expected.
    ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
    ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
}
Exemple #15
0
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
									int32_t EAS_dem_cm, 
									enum FlightStage flight_stage,
									int32_t ptchMinCO_cd, 
									int16_t throttle_nudge,
									float hgt_afe,
                                    float load_factor) 
{
    // Calculate time in seconds since last update
    uint32_t now = hal.scheduler->micros();
	_DT = max((now - _update_pitch_throttle_last_usec),0)*1.0e-6f;
	_update_pitch_throttle_last_usec = now;	

    // Update the speed estimate using a 2nd order complementary filter
    _update_speed(load_factor);

	// Convert inputs
    _hgt_dem = hgt_dem_cm * 0.01f;
	_EAS_dem = EAS_dem_cm * 0.01f;
    _THRmaxf  = aparm.throttle_max * 0.01f;
    _THRminf  = aparm.throttle_min * 0.01f;

	// work out the maximum and minimum pitch
	// if TECS_PITCH_{MAX,MIN} isn't set then use
	// LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
	// larger than LIM_PITCH_{MAX,MIN}
	if (_pitch_max <= 0) {
		_PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f;
	} else {
		_PITCHmaxf = min(_pitch_max, aparm.pitch_limit_max_cd * 0.01f);
	}
	if (_pitch_min >= 0) {
		_PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
	} else {
		_PITCHminf = max(_pitch_min, aparm.pitch_limit_min_cd * 0.01f);
	}
    if (flight_stage == FLIGHT_LAND_FINAL) {
        // in flare use min pitch from LAND_PITCH_CD
        _PITCHminf = max(_PITCHminf, aparm.land_pitch_cd * 0.01f);

        // and use max pitch from TECS_LAND_PMAX
        if (_land_pitch_max > 0) {
            _PITCHmaxf = min(_PITCHmaxf, _land_pitch_max);
        }
        
        // and allow zero throttle
        _THRminf = 0;
    } else if (flight_stage == FLIGHT_LAND_APPROACH && (-_climb_rate) > _land_sink) {
        // constrain the pitch in landing as we get close to the flare
        // point. Use a simple linear limit from 15 meters after the
        // landing point
        float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec;
        if (time_to_flare < 0) {
            // we should be flaring already
            _PITCHminf = max(_PITCHminf, aparm.land_pitch_cd * 0.01f);
        } else if (time_to_flare < timeConstant()*2) {
            // smoothly move the min pitch to the flare min pitch over
            // twice the time constant
            float p = time_to_flare/(2*timeConstant());
            float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd;
#if 0
            ::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n",
                     time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
#endif
            _PITCHminf = max(_PITCHminf, pitch_limit_cd*0.01f);
        }
    }

	// convert to radians
	_PITCHmaxf = radians(_PITCHmaxf);
	_PITCHminf = radians(_PITCHminf);
	_flight_stage = flight_stage;

	// initialise selected states and variables if DT > 1 second or in climbout
	_initialise_states(ptchMinCO_cd, hgt_afe);

    // Calculate Specific Total Energy Rate Limits
	_update_STE_rate_lim();

    // Calculate the speed demand
    _update_speed_demand();

	// Calculate the height demand
	_update_height_demand();

    // Detect underspeed condition
    _detect_underspeed();

    // Calculate specific energy quantitiues
    _update_energies();

    // Calculate throttle demand - use simple pitch to throttle if no airspeed sensor
	if (_ahrs.airspeed_sensor_enabled()) {
        _update_throttle();
	} else {
        _update_throttle_option(throttle_nudge);
	}

    // Detect bad descent due to demanded airspeed being too high
	_detect_bad_descent();

	// Calculate pitch demand
	_update_pitch();

    // Write internal variables to the log_tuning structure. This
    // structure will be logged in dataflash at 10Hz
	log_tuning.hgt_dem  = _hgt_dem_adj;
	log_tuning.hgt      = _integ3_state;
	log_tuning.dhgt_dem = _hgt_rate_dem;
	log_tuning.dhgt     = _climb_rate;
	log_tuning.spd_dem  = _TAS_dem_adj;
	log_tuning.spd      = _integ5_state;
	log_tuning.dspd     = _vel_dot;
	log_tuning.ithr     = _integ6_state;
	log_tuning.iptch    = _integ7_state;
	log_tuning.thr      = _throttle_dem;
	log_tuning.ptch     = _pitch_dem;
	log_tuning.dspd_dem = _TAS_rate_dem;
	log_tuning.time_ms  = hal.scheduler->millis();
}
Exemple #16
0
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
                                    int32_t EAS_dem_cm,
                                    enum FlightStage flight_stage,
                                    bool is_doing_auto_land,
                                    float distance_beyond_land_wp,
                                    int32_t ptchMinCO_cd,
                                    int16_t throttle_nudge,
                                    float hgt_afe,
                                    float load_factor)
{
    // Calculate time in seconds since last update
    uint64_t now = AP_HAL::micros64();
    _DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
    _update_pitch_throttle_last_usec = now;

    _flags.is_doing_auto_land = is_doing_auto_land;
    _distance_beyond_land_wp = distance_beyond_land_wp;
    _flight_stage = flight_stage;

    // Convert inputs
    _hgt_dem = hgt_dem_cm * 0.01f;
    _EAS_dem = EAS_dem_cm * 0.01f;

    // Update the speed estimate using a 2nd order complementary filter
    _update_speed(load_factor);

    if (aparm.takeoff_throttle_max != 0 &&
            (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) {
        _THRmaxf  = aparm.takeoff_throttle_max * 0.01f;
    } else {
        _THRmaxf  = aparm.throttle_max * 0.01f;
    }
    _THRminf  = aparm.throttle_min * 0.01f;

    // work out the maximum and minimum pitch
    // if TECS_PITCH_{MAX,MIN} isn't set then use
    // LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
    // larger than LIM_PITCH_{MAX,MIN}
    if (_pitch_max <= 0) {
        _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f;
    } else {
        _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f);
    }

    if (_pitch_min >= 0) {
        _PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
    } else {
        _PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f);
    }

    // apply temporary pitch limit and clear
    if (_pitch_max_limit < 90) {
        _PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit);
        _PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf);
        _pitch_max_limit = 90;
    }
        
    if (flight_stage == FLIGHT_LAND_FINAL) {
        // in flare use min pitch from LAND_PITCH_CD
        _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f);

        // and use max pitch from TECS_LAND_PMAX
        if (_land_pitch_max != 0) {
            _PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max);
        }

        // and allow zero throttle
        _THRminf = 0;
    } else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) {
        // constrain the pitch in landing as we get close to the flare
        // point. Use a simple linear limit from 15 meters after the
        // landing point
        float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec;
        if (time_to_flare < 0) {
            // we should be flaring already
            _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f);
        } else if (time_to_flare < timeConstant()*2) {
            // smoothly move the min pitch to the flare min pitch over
            // twice the time constant
            float p = time_to_flare/(2*timeConstant());
            float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd;
#if 0
            ::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n",
                     time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
#endif
            _PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f);
        }
    }

    if (flight_stage == FLIGHT_TAKEOFF || flight_stage == FLIGHT_LAND_ABORT) {
        if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
            // we have reached our target speed in takeoff, allow for
            // normal throttle control
            _flags.reached_speed_takeoff = true;
        }
    }
    
    // convert to radians
    _PITCHmaxf = radians(_PITCHmaxf);
    _PITCHminf = radians(_PITCHminf);

    // initialise selected states and variables if DT > 1 second or in climbout
    _initialise_states(ptchMinCO_cd, hgt_afe);

    // Calculate Specific Total Energy Rate Limits
    _update_STE_rate_lim();

    // Calculate the speed demand
    _update_speed_demand();

    // Calculate the height demand
    _update_height_demand();

    // Detect underspeed condition
    _detect_underspeed();

    // Calculate specific energy quantitiues
    _update_energies();

    // Calculate throttle demand - use simple pitch to throttle if no airspeed sensor
    if (_ahrs.airspeed_sensor_enabled()) {
        _update_throttle_with_airspeed();
    } else {
        _update_throttle_without_airspeed(throttle_nudge);
    }

    // Detect bad descent due to demanded airspeed being too high
    _detect_bad_descent();

    // Calculate pitch demand
    _update_pitch();

    // log to DataFlash
    DataFlash_Class::instance()->Log_Write("TECS", "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f", "QfffffffffffffB",
                                           now,
                                           (double)_height,
                                           (double)_climb_rate,
                                           (double)_hgt_dem_adj,
                                           (double)_hgt_rate_dem,
                                           (double)_TAS_dem_adj,
                                           (double)_TAS_state,
                                           (double)_vel_dot,
                                           (double)_integTHR_state,
                                           (double)_integSEB_state,
                                           (double)_throttle_dem,
                                           (double)_pitch_dem,
                                           (double)_TAS_rate_dem,
                                           (double)logging.SKE_weighting,
                                           _flags_byte);
    DataFlash_Class::instance()->Log_Write("TEC2", "TimeUS,KErr,PErr,EDelta,LF", "Qffff",
                                           now,
                                           (double)logging.SKE_error,
                                           (double)logging.SPE_error,
                                           (double)logging.SEB_delta,
                                           (double)load_factor);
}
Exemple #17
0
/// This one should be called periodically
void AP_Mount::update_mount_position()
{
#if MNT_RETRACT_OPTION == ENABLED
    static bool mount_open = 0;     // 0 is closed
#endif

    switch((enum MAV_MOUNT_MODE)_mount_mode.get())
    {
#if MNT_RETRACT_OPTION == ENABLED
    // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
    case MAV_MOUNT_MODE_RETRACT:
    {
        Vector3f vec = _retract_angles.get();
        _roll_angle  = vec.x;
        _tilt_angle  = vec.y;
        _pan_angle   = vec.z;
        break;
    }
#endif

    // move mount to a neutral position, typically pointing forward
    case MAV_MOUNT_MODE_NEUTRAL:
    {
        Vector3f vec = _neutral_angles.get();
        _roll_angle  = vec.x;
        _tilt_angle  = vec.y;
        _pan_angle   = vec.z;
        break;
    }

    // point to the angles given by a mavlink message
    case MAV_MOUNT_MODE_MAVLINK_TARGETING:
    {
        Vector3f vec = _control_angles.get();
        _roll_control_angle  = radians(vec.x);
        _tilt_control_angle  = radians(vec.y);
        _pan_control_angle   = radians(vec.z);
        stabilize();
        break;
    }

    // RC radio manual angle control, but with stabilization from the AHRS
    case MAV_MOUNT_MODE_RC_TARGETING:
    {
#if MNT_JSTICK_SPD_OPTION == ENABLED
        if (_joystick_speed) {                  // for spring loaded joysticks
            // allow pilot speed position input to come directly from an RC_Channel
            if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) {
                _roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.0001f * _joystick_speed;
                if (_roll_control_angle < radians(_roll_angle_min*0.01f)) _roll_control_angle = radians(_roll_angle_min*0.01f);
                if (_roll_control_angle > radians(_roll_angle_max*0.01f)) _roll_control_angle = radians(_roll_angle_max*0.01f);
            }
            if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
                _tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.0001f * _joystick_speed;
                if (_tilt_control_angle < radians(_tilt_angle_min*0.01f)) _tilt_control_angle = radians(_tilt_angle_min*0.01f);
                if (_tilt_control_angle > radians(_tilt_angle_max*0.01f)) _tilt_control_angle = radians(_tilt_angle_max*0.01f);
            }
            if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
                _pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.0001f * _joystick_speed;
                if (_pan_control_angle < radians(_pan_angle_min*0.01f)) _pan_control_angle = radians(_pan_angle_min*0.01f);
                if (_pan_control_angle > radians(_pan_angle_max*0.01f)) _pan_control_angle = radians(_pan_angle_max*0.01f);
            }
        } else {
#endif
            // allow pilot position input to come directly from an RC_Channel
            if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) {
                _roll_control_angle = angle_input_rad(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max);
            }
            if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
                _tilt_control_angle = angle_input_rad(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max);
            }
            if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
                _pan_control_angle = angle_input_rad(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max);
            }
#if MNT_JSTICK_SPD_OPTION == ENABLED
        }
#endif
        stabilize();
        break;
    }

#if MNT_GPSPOINT_OPTION == ENABLED
    // point mount to a GPS point given by the mission planner
    case MAV_MOUNT_MODE_GPS_POINT:
    {
        if(_gps->fix) {
            calc_GPS_target_angle(&_target_GPS_location);
            stabilize();
        }
        break;
    }
#endif

    default:
        //do nothing
        break;
    }

#if MNT_RETRACT_OPTION == ENABLED
    // move mount to a "retracted position" into the fuselage with a fourth servo
	bool mount_open_new = (enum MAV_MOUNT_MODE)_mount_mode.get()==MAV_MOUNT_MODE_RETRACT ? 0 : 1;
	if (mount_open != mount_open_new) {
		mount_open = mount_open_new;
		move_servo(_open_idx, mount_open_new, 0, 1);
    }
#endif

    // write the results to the servos
    move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1f, _roll_angle_max*0.1f);
    move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1f, _tilt_angle_max*0.1f);
    move_servo(_pan_idx,  _pan_angle*10,  _pan_angle_min*0.1f,  _pan_angle_max*0.1f);
}
Exemple #18
0
void SpotLight::setInnerCutoff(const Real &_cutoff) noexcept
{
  m_innerCutoffAngle=cos(radians(_cutoff));
}
Exemple #19
0
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second
void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{
    float rate_limit;
    Vector3f rate_ef_level, rate_bf_level, rate_bf_request;

    // apply circular limit to pitch and roll inputs
    float total_in = norm(pitch_in, roll_in);

    if (total_in > ROLL_PITCH_YAW_INPUT_MAX) {
        float ratio = (float)ROLL_PITCH_YAW_INPUT_MAX / total_in;
        roll_in *= ratio;
        pitch_in *= ratio;
    }
    
    // calculate roll, pitch rate requests
    if (g.acro_rp_expo <= 0) {
        rate_bf_request.x = roll_in * g.acro_rp_p;
        rate_bf_request.y = pitch_in * g.acro_rp_p;
    } else {
        // expo variables
        float rp_in, rp_in3, rp_out;

        // range check expo
        if (g.acro_rp_expo > 1.0f) {
            g.acro_rp_expo = 1.0f;
        }

        // roll expo
        rp_in = float(roll_in)/ROLL_PITCH_YAW_INPUT_MAX;
        rp_in3 = rp_in*rp_in*rp_in;
        rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in);
        rate_bf_request.x = ROLL_PITCH_YAW_INPUT_MAX * rp_out * g.acro_rp_p;

        // pitch expo
        rp_in = float(pitch_in)/ROLL_PITCH_YAW_INPUT_MAX;
        rp_in3 = rp_in*rp_in*rp_in;
        rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in);
        rate_bf_request.y = ROLL_PITCH_YAW_INPUT_MAX * rp_out * g.acro_rp_p;
    }

    // calculate yaw rate request
    rate_bf_request.z = get_pilot_desired_yaw_rate(yaw_in);

    // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode

    if (g.acro_trainer != ACRO_TRAINER_DISABLED) {

        // get attitude targets
        const Vector3f att_target = attitude_control->get_att_target_euler_cd();

        // Calculate trainer mode earth frame rate command for roll
        int32_t roll_angle = wrap_180_cd(att_target.x);
        rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;

        // Calculate trainer mode earth frame rate command for pitch
        int32_t pitch_angle = wrap_180_cd(att_target.y);
        rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;

        // Calculate trainer mode earth frame rate command for yaw
        rate_ef_level.z = 0;

        // Calculate angle limiting earth frame rate commands
        if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
            if (roll_angle > aparm.angle_max){
                rate_ef_level.x -=  g.acro_balance_roll*(roll_angle-aparm.angle_max);
            }else if (roll_angle < -aparm.angle_max) {
                rate_ef_level.x -=  g.acro_balance_roll*(roll_angle+aparm.angle_max);
            }

            if (pitch_angle > aparm.angle_max){
                rate_ef_level.y -=  g.acro_balance_pitch*(pitch_angle-aparm.angle_max);
            }else if (pitch_angle < -aparm.angle_max) {
                rate_ef_level.y -=  g.acro_balance_pitch*(pitch_angle+aparm.angle_max);
            }
        }

        // convert earth-frame level rates to body-frame level rates
        attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);

        // combine earth frame rate corrections with rate requests
        if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
            rate_bf_request.x += rate_bf_level.x;
            rate_bf_request.y += rate_bf_level.y;
            rate_bf_request.z += rate_bf_level.z;
        }else{
            float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();

            // Scale leveling rates by stick input
            rate_bf_level = rate_bf_level*acro_level_mix;

            // Calculate rate limit to prevent change of rate through inverted
            rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x));
            rate_bf_request.x += rate_bf_level.x;
            rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);

            // Calculate rate limit to prevent change of rate through inverted
            rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y));
            rate_bf_request.y += rate_bf_level.y;
            rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);

            // Calculate rate limit to prevent change of rate through inverted
            rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z));
            rate_bf_request.z += rate_bf_level.z;
            rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
        }
    }

    // hand back rate request
    roll_out = rate_bf_request.x;
    pitch_out = rate_bf_request.y;
    yaw_out = rate_bf_request.z;
}
bool Plane::verify_takeoff()
{
    if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
        const float min_gps_speed = 5;
        if (auto_state.takeoff_speed_time_ms == 0 && 
            gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
            gps.ground_speed() > min_gps_speed) {
            auto_state.takeoff_speed_time_ms = millis();
        }
        if (auto_state.takeoff_speed_time_ms != 0 &&
            millis() - auto_state.takeoff_speed_time_ms >= 2000) {
            // once we reach sufficient speed for good GPS course
            // estimation we save our current GPS ground course
            // corrected for summed yaw to set the take off
            // course. This keeps wings level until we are ready to
            // rotate, and also allows us to cope with arbitary
            // compass errors for auto takeoff
            float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err;
            takeoff_course = wrap_PI(takeoff_course);
            steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
            gcs_send_text_fmt(PSTR("Holding course %ld at %.1fm/s (%.1f)"), 
                              steer_state.hold_course_cd,
                              (double)gps.ground_speed(),
                              (double)degrees(steer_state.locked_course_err));
        }
    }

    if (steer_state.hold_course_cd != -1) {
        // call navigation controller for heading hold
        nav_controller->update_heading_hold(steer_state.hold_course_cd);
    } else {
        nav_controller->update_level_flight();        
    }

    // see if we have reached takeoff altitude
    int32_t relative_alt_cm = adjusted_relative_altitude_cm();
    if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
        gcs_send_text_fmt(PSTR("Takeoff complete at %.2fm"), 
                          (double)(relative_alt_cm*0.01f));
        steer_state.hold_course_cd = -1;
        auto_state.takeoff_complete = true;
        next_WP_loc = prev_WP_loc = current_loc;

#if GEOFENCE_ENABLED == ENABLED
        if (g.fence_autoenable > 0) {
            if (! geofence_set_enabled(true, AUTO_TOGGLED)) {
                gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Enable fence failed (cannot autoenable"));
            } else {
                gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Fence enabled. (autoenabled)"));
            }
        }
#endif

        // don't cross-track on completion of takeoff, as otherwise we
        // can end up doing too sharp a turn
        auto_state.next_wp_no_crosstrack = true;
        return true;
    } else {
        return false;
    }
}
Exemple #21
0
int main(int argc, char **argv)
{
    /*  START # 1  */
    struct application ap;	/*  Structure passed between functions.  */

    static struct rt_i *rtip;	/*  *rtip pointer to structure of  */
				/*  type rt_i  */
    char idbuf[132];	/*  first ID record info, used in  */
			/*  rt_dirbuild  */
    int index;		/*  index for rt_dirbuild & rt_gettree  */

    FILE *fp;		/*  used in opening file for second pass  */
    char spfile[16];	/*  second pass file name  */
    FILE *fp1=NULL;		/*  conductivity file  */
    char confile[16];	/*  conductivity file  */
    FILE *fp2;		/*  conductivity table file  */
    char tblfile[16];	/*  conductivity table file  */

    int i, j;		/*  integers used in loops  */
    int numreg;		/*  number of regions  */
    int nmged;		/*  number of regions in mged file  */
    double gridspace;	/*  spacing between fired rays  */
    int iwrite;		/*  0 => write to standard out, 1 => write  */
			/*  to file  */
    int typeout;		/*  Type of file to be written, 0 => PRISM file,  */
    /*  1 => generic file.  */
    FILE *fp6;		/*  Used in writing generic file.  */
    char genfile[16];	/*  Generic file name.  */
    FILE *fp3=NULL;		/*  used for writing output to file  */
    char filename[16];	/*  output file name  */

    FILE *fp5;		/*  material file  */
    char filemat[16];	/*  material file  */
    char line[150];	/*  used for reading a line from a file  */
    double k[41];	/*  thermal conductivity  */
    int itype;		/*  type of length measurement to use for  */
			/*  rk calculations  */
    double rki, rkj;	/*  used in finding rk  */
    double ki, kj;	/*  thermal conductivity of region  */
    double leni, lenj;	/*  lengths used in finding rk  */
    double areai;	/*  areas used in finding rk  */
    double a1;		/*  area used in writing conductivity table  */
    double l1, l2, l3, l4;	/*  lengths used in writing conductivity table  */
    FILE *fp4;		/*  error file  */
    char fileerr[16];	/*  error file  */

    double angle[3];	/*  Angles of rotation.  angle[0]-rotation about  */
			/*  x-axis, angle[1]-rotation about y-axis, &  */
			/*  angle[2]-rotation about z-axis.  */
    double strtpt[3];	/*  Starting point of fired ray.  */
    double strtdir[3];	/*  Starting direction of fired ray.  */
    double r[3], t[3];	/*  Used in computing rotations.  */
    double center[3];	/*  Center point of bounding rpp.  */
    double diagonal;	/*  Length of diagonal of bounding rpp.  */
    double xmin, xmax;	/*  Maximum & minimum x of grid.  */
    double ymin, ymax;	/*  Maximum & minimum y of grid.  */
    double zmin, zmax;	/*  Maximum & minimum z of grid.  */
    int nadjreg;		/*  Number of adjacent regions.  */
    int prmrel;		/*  PRISM release number, 2=>2.0, 3=>3.0.  */
    int ifire;		/*  Number of sets of rays to be fired, 0=>  */
			/*  fire from 3 orthogonal postions, 1=>fire  */
			/*  from 1 position.  */

    /*  Check to see if arguments implimented correctly.  */
    if (argv[1] == NULL || argv[2] == NULL)
    {
	(void)fprintf(stderr, "\nusage:  secpass file.g objects\n\n");
    }

    else
    {
    	/*  START # 2  */

	/*  Ask if output goes to standard out or to a file.  */
	(void)fprintf(stdout, "Write output to standard out (0) or a file(1) ");
	(void)fprintf(stdout, "not at all (2)?  ");
	(void)fflush(stdout);
	(void)scanf("%d", &iwrite);
	if ((iwrite != 0) && (iwrite != 1)) iwrite=2;
	if (iwrite == 1)
	{
	    (void)fprintf(stdout, "Enter name of output file (15 char max).  ");
	    (void)fflush(stdout);
	    (void)scanf("%15s", filename);
	    fp3 = fopen(filename, "wb");
	}

	/*  Which file that has second pass information in it?  */
	(void)fprintf(stdout, "Enter name of file that has second pass ");
	(void)fprintf(stdout, "information\nin it (15 char max).  ");
	(void)fflush(stdout);
	(void)scanf("%15s", spfile);

	/*  Ask for type of output file to be generated.  */
	(void)printf("Enter type of output file to be generated.\n");
	(void)printf("\t 0 - PRISM File\n");
	(void)printf("\t 1 - Generic File\n");
	(void)fflush(stdout);
	(void)scanf("%d", &typeout);

	/*  Read name of file to write conductivity information  */
	/*  to for use in PRISM.  */
	if (typeout == 0)
	{
	    (void)fprintf(stdout, "Enter name of file to be created for PRISM ");
	    (void)fprintf(stdout, "conductivity\ninformation (15 char max).  ");
	    (void)fflush(stdout);
	    (void)scanf("%15s", confile);

	    /*  Find which release of PRISM is being used.  The format  */
	    /*  for writing region numbers is I3 for PRISM 2.0 & I6 for  */
	    /*  PRISM 3.0.  */
	    prmrel = 2;
	    (void)printf("Which release of PRISM is being used, 2.0 (2) ");
	    (void)printf("or 3.0 (3)?  ");
	    (void)fflush(stdout);
	    (void)scanf("%d", &prmrel);
	    if (prmrel != 3) prmrel = 2;
	}

	/*  Read generic file name if necessary.  */
	if (typeout == 1)
	{
	    (void)printf("Enter name of generic file to be created (15 char ");
	    (void)printf("max).  ");
	    (void)fflush(stdout);
	    (void)scanf("%15s", genfile);
	}

	/*  Which calculated length should be used when writing to  */
	/*  this file:  1 -> average length, 2 -> rms length, 3 ->  */
	/*  minimum length, and 4 -> maximum length.  */
	(void)fprintf(stdout, "Which length calculation should be used when\n");
	(void)fprintf(stdout, "computing conduction\nbetween regions?\n");
	(void)fprintf(stdout, "\t1 - average length\n");
	(void)fprintf(stdout, "\t2 - rms length\n");
	(void)fprintf(stdout, "\t3 - minimum length\n");
	(void)fprintf(stdout, "\t4 - maximum length\n");
	(void)fflush(stdout);
	(void)scanf("%d", &itype);

	/*  Read name of file to write conductivity information to  */
	/*  in table format.  */
	(void)fprintf(stdout, "Enter name of file to be created for ");
	(void)fprintf(stdout, "conductivity\ntable (15 char max).  ");
	(void)fflush(stdout);
	(void)scanf("%15s", tblfile);

	/*  Read name of material file that contains thermal  */
	/*  conductivity information.  */
	(void)fprintf(stdout, "Enter name of material file (15 char max).  ");
	(void)fflush(stdout);
	(void)scanf("%15s", filemat);

	/*  Read name of error file.  */
	(void)fprintf(stdout, "Enter name of error file to be created ");
	(void)fprintf(stdout, "(15 char max).  ");
	(void)fflush(stdout);
	(void)scanf("%15s", fileerr);

	/*  Choose whether 3 orthogonal sets of rays are to be fired  */
	/*  or 1 set of rays is to be fired.  */
	(void)printf("Should there be 3 sets of orhogonal rays fired ");
	(void)printf("(0) or 1 set (1)?\n\t");
	(void)fflush(stdout);
	(void)scanf("%d", &ifire);
	if (ifire != 0) ifire = 1;
	if (ifire == 0)
	{
	    (void)printf("3 sets of orthogonal rays will be fired.\n");
	}
	if (ifire == 1)
	{
	    (void)printf("1 set of rays will be fired.\n");
	}
	(void)fflush(stdout);

	/*  Write out file information.  */
	if (iwrite ==1)
	{
	    (void)fprintf(fp3, "\nsecond pass file:  %s\n", spfile);
	    (void)fprintf(fp3, "material file:  %s\n", filemat);
	    if (typeout == 0)
	    {
		(void)fprintf(fp3, "conductivity file for use ");
		(void)fprintf(fp3, "with PRISM:  %s\n", confile);
		(void)fprintf(fp3, "  (format is PRISM %d.0)\n", prmrel);
	    }
	    if (typeout == 1) (void)fprintf(fp3, "generic file:  %s\n", genfile);
	    (void)fflush(fp3);
	    if (itype == 1) (void)fprintf(fp3, "\taverage length being used\n");
	    if (itype == 2) (void)fprintf(fp3, "\trms length being used\n");
	    if (itype == 3) (void)fprintf(fp3, "\tminimum length being used\n");
	    if (itype == 4) (void)fprintf(fp3, "\tmaximum length being used\n");
	    (void)fprintf(fp3, "conductivity table file:  %s\n", tblfile);
	    (void)fprintf(fp3, "error file:  %s\n\n", fileerr);
	    (void)fflush(fp3);
	}

	/*  Read thermal conductivity file.  */
	fp5 = fopen(filemat, "rb");
	for (i=0; i<41; i++)
	{
	    (void)bu_fgets(line, 151, fp5);
	    (void)sscanf(line, "%*d%*f%*f%*f%*f%lf", &k[i]);
	}

	/*  Print out the thermal conductivity.  */
	/*
	 *	for (i=0; i<41; i++)
	 *	{
	 *	   (void)fprintf(stdout, "  thermal conductivity %d = %f\n", i, k[i]);
	 *	   (void)fflush(stdout);
	 *	}
	 */

	/*  Build the directory.  */
	index = 1;	/*  Set index for rt_dirbuild.  */
	rtip=rt_dirbuild(argv[index], idbuf, sizeof(idbuf));
	(void)fprintf(stdout, "Database title:  %s\n", idbuf);
	(void)fflush(stdout);

	/*  Set useair to 1, to show hits of air.  */
	rtip->useair = 1;

	/*  Load desired objects.  */
	index = 2;	/*  Set index for rt_gettree.  */
	while (argv[index] != NULL)
	{
	    rt_gettree(rtip, argv[index]);
	    index += 1;
	}

	/*  Find total number of regions in mged file.  */
	nmged = (int)rtip->nregions;

	(void)fprintf(stdout, "Number of regions in mged file:  %d\n", nmged);
	(void)fflush(stdout);

	if (iwrite == 1)
	{
	    (void)fprintf(fp3, "Number of regions in mged file:  %d\n", nmged);
	    (void)fflush(fp3);
	}

	/*  Number of regions known, everything can be malloced.  */
	(void)fprintf(stdout, "Mallocing arrays.\n");
	(void)fflush(stdout);

	cond = (struct table *)bu_malloc( nmged * sizeof(struct table), "cond" );
	(void)fprintf(stdout, "cond malloced\n");
	(void)fflush(stdout);
	for (i=0; i<nmged; i++)
	{
	    cond[i].shrarea = (double *)bu_malloc( nmged * sizeof(double), "cond[i].shrarea" );
	    cond[i].avglen = (double *)bu_malloc( nmged * sizeof(double), "cond[i].avglen" );
	    cond[i].rmslen = (double *)bu_malloc( nmged * sizeof(double), "cond[i].rmslen" );
	    cond[i].minlen = (double *)bu_malloc( nmged * sizeof(double), "cond[i].minlen" );
	    cond[i].maxlen = (double *)bu_malloc( nmged * sizeof(double), "cond[i].maxlen" );
	    cond[i].numcal = (double *)bu_malloc( nmged * sizeof(double), "cond[i].numcal" );
	    cond[i].rkavg = (double *)bu_malloc( nmged * sizeof(double), "cond[i].rkavg" );
	    cond[i].rkrms = (double *)bu_malloc( nmged * sizeof(double), "cond[i].rkrms" );
	    cond[i].rkmin = (double *)bu_malloc( nmged * sizeof(double), "cond[i].rkmin" );
	    cond[i].rkmax = (double *)bu_malloc( nmged * sizeof(double), "cond[i].rkmax" );
	}
	(void)fprintf(stdout, "loop malloced\n");
	(void)fflush(stdout);

	/*  All variables 'dimensioned', now zero all variables.  */
	for (i=0; i<nmged; i++)
	{
	    cond[i].centroid[0] = (double)0.;
	    cond[i].centroid[1] = (double)0.;
	    cond[i].centroid[2] = (double)0.;
	    cond[i].mat = (int)0;
	    for (j=0; j<nmged; j++)
	    {
		cond[i].shrarea[j] = (double)0.;
		cond[i].avglen[j] = (double)0.;
		cond[i].rmslen[j] = (double)0.;
		cond[i].minlen[j] = (double)0.;
		cond[i].maxlen[j] = (double)0.;
		cond[i].numcal[j] = (double)0.;
		cond[i].rkavg[j] = (double)0.;
		cond[i].rkrms[j] = (double)0.;
		cond[i].rkmin[j] = (double)0.;
		cond[i].rkmax[j] = (double)0.;
	    }
	}
	(void)fprintf(stdout, "All variables zeroed.\n");
	(void)fflush(stdout);

	/*  Now open file with second pass information in it.  */
	fp = fopen(spfile, "rb");
	(void)fprintf(stdout, "second pass file opened\n");
	(void)fflush(stdout);

	/*  Read number of regions in file.  */
	(void)fscanf(fp, "%d\n", &numreg);
	(void)fprintf(stdout, "The number of regions read was %d\n", numreg);
	(void)fflush(stdout);

	if (iwrite == 1)
	{
	    (void)fprintf(fp3, "number of regions read from second ");
	    (void)fprintf(fp3, "pass file:  %d\n", numreg);
	    (void)fflush(fp3);
	}

	/*  Read all information in file.  */
	for (i=0; i<numreg; i++)
	{
	    (void)fscanf(fp, "%*d%le%le%le%d\n", &cond[i].centroid[0],
			 &cond[i].centroid[1],
			 &cond[i].centroid[2], &cond[i].mat);
	    /*
	     *	   (void)fprintf(stdout, "reg=%8d, centroid:  %10.0f, %10.0f, %10.0f\n",
	     *		i, cond[i].centroid[0], cond[i].centroid[1], cond[i].centroid[2]);
	     *	   (void)fflush(stdout);
	     */

	    for (j=0; j<numreg; j++)
	    {
		(void)fscanf(fp, "%*d%le\n", &cond[i].shrarea[j]);
		/*
		 *		(void)fprintf(stdout, "\treg=%8d, area=%10.0f\n",
		 *		   j, cond[i].shrarea[j]);
		 *		(void)fflush(stdout);
		 */
	    }
	}

	(void)fclose(fp);

	/*  Check that the number of regions in the mged file  */
	/*  and the second pass file are equal.  */
	if (nmged != numreg)
	{
	    (void)fprintf(stdout, "ERROR -- The number of regions in the mged\n");
	    (void)fprintf(stdout, "file (%d) does not equal the number of\n",
			  nmged);
	    (void)fprintf(stdout, "regions in the second pass file (%d).\n",
			  numreg);
	    (void)fprintf(stdout, "Watch for unexplained errors.\n");
	    (void)fflush(stdout);

	    if (iwrite == 1)
	    {
		(void)fprintf(fp3, "ERROR -- The number of regions in the mged\n");
		(void)fprintf(fp3, "file (%d) does not equal the number of\n",
			      nmged);
		(void)fprintf(fp3, "regions in the second pass file (%d).\n",
			      numreg);
		(void)fprintf(fp3, "Watch for unexplained errors.\n");
		(void)fflush(fp3);
	    }
	}

	/*  Get database ready by starting preparation.  */
	rt_prep(rtip);

	/*  Find center of bounding rpp.  */
	center[X] = rtip->mdl_min[X] + (rtip->mdl_max[X] -
					rtip->mdl_min[X]) / 2.;
	center[Y] = rtip->mdl_min[Y] + (rtip->mdl_max[Y] -
					rtip->mdl_min[Y]) / 2.;
	center[Z] = rtip->mdl_min[Z] + (rtip->mdl_max[Z] -
					rtip->mdl_min[Z]) / 2.;

	/*  Find length of diagonal.  */
	diagonal = (rtip->mdl_max[X] - rtip->mdl_min[X])
	    * (rtip->mdl_max[X] - rtip->mdl_min[X])
	    + (rtip->mdl_max[Y] - rtip->mdl_min[Y])
	    * (rtip->mdl_max[Y] - rtip->mdl_min[Y])
	    + (rtip->mdl_max[Z] - rtip->mdl_min[Z])
	    * (rtip->mdl_max[Z] - rtip->mdl_min[Z]);
	diagonal = sqrt(diagonal) / 2. + .5;

	/*  Find minimum & maximum of grid.  */
	xmin = center[X] - diagonal;
	xmax = center[X] + diagonal;
	ymin = center[Y] - diagonal;
	ymax = center[Y] + diagonal;
	zmin = center[Z] - diagonal;
	zmax = center[Z] + diagonal;

	/*  Print center of bounding rpp, diagonal, & maximum  */
	/*  & minimum of grid.  */
	(void)fprintf(stdout, "Center of bounding rpp ( %f, %f, %f )\n",
		      center[X], center[Y], center[Z]);
	(void)fprintf(stdout, "Length of diagonal of bounding rpp:  %f\n",
		      diagonal);
	(void)fprintf(stdout, "Minimums & maximums of grid:\n");
	(void)fprintf(stdout, "  %f - %f\n", xmin, xmax);
	(void)fprintf(stdout, "  %f - %f\n", ymin, ymax);
	(void)fprintf(stdout, "  %f - %f\n\n", zmin, zmax);
	(void)fflush(stdout);

	/*  Write model minimum & maximum.  */
	(void)fprintf(stdout, "Model minimum & maximum.\n");
	(void)fprintf(stdout, "\tX:  %f to %f\n\tY:  %f to %f\n\tZ:  %f to %f\n\n",
		      rtip->mdl_min[X], rtip->mdl_max[X],
		      rtip->mdl_min[Y], rtip->mdl_max[Y],
		      rtip->mdl_min[Z], rtip->mdl_max[Z]);
	(void)fflush(stdout);

	if (iwrite == 1)
	{
	    (void)fprintf(fp3, "Model minimum & maximum.\n");
	    (void)fprintf(fp3, "\tX:  %f to %f\n\tY:  %f kto %f\n",
			  rtip->mdl_min[X], rtip->mdl_max[X],
			  rtip->mdl_min[Y], rtip->mdl_max[Y]);
	    (void)fprintf(fp3, "\tZ:  %f to %f\n\n",
			  rtip->mdl_min[Z], rtip->mdl_max[Z]);
	    (void)fflush(fp3);
	}

	/*  User enters grid spacing.  All units are in mm.  */
	(void)fprintf(stdout, "Enter spacing (mm) between fired rays.  ");
	(void)fflush(stdout);
	(void)scanf("%lf", &gridspace);

	(void)fprintf(stdout, "\ngrid spacing:  %f\n", gridspace);
	(void)fflush(stdout);

	if (iwrite == 1)
	{
	    (void)fprintf(fp3, "gridspacing:  %f\n\n", gridspace);
	    (void)fflush(fp3);
	}

	/*  Set up parameters for rt_shootray.  */
	RT_APPLICATION_INIT(&ap);
	ap.a_hit = hit;		/*  User supplied hit function.  */
	ap.a_miss = miss;	/*  User supplied miss function.  */
	ap.a_overlap = ovrlap;	/*  user supplied overlap function.  */
	ap.a_rt_i = rtip;	/*  Pointer from rt_dirbuild.  */
	ap.a_onehit = 0;	/*  Hit flag (returns all hits).  */
	ap.a_level = 0;		/*  Recursion level for diagnostics.  */
	ap.a_resource = 0;	/*  Address of resource structure (NULL).  */

	/*  Put angles for rotation into radians.  */
	angle[X] = radians((double)GAMMA);
	angle[Y] = radians((double)BETA);
	angle[Z] = radians((double)ALPHA);

	/*  Set up and shoot down the 1st axis, positive to negative  */
	/*  (x-axis).  */

	(void)fprintf(stdout, "\nShooting down 1st axis.\n");
	(void)fflush(stdout);

	strtpt[X] = xmax;
	strtpt[Y] = ymin + gridspace / 2.;
	strtpt[Z] = zmin + gridspace / 2.;
	strtdir[X] = (-1);
	strtdir[Y] = 0;
	strtdir[Z] = 0;

	/*  Rotate starting point.  (new pt = C + R[P -C])  */
	t[X] = strtpt[X] - center[X];
	t[Y] = strtpt[Y] - center[Y];
	t[Z] = strtpt[Z] - center[Z];

	(void)rotate(t, angle, r);

	ap.a_ray.r_pt[X] = center[X] + r[X];
	ap.a_ray.r_pt[Y] = center[Y] + r[Y];
	ap.a_ray.r_pt[Z] = center[Z] + r[Z];

	/*  Rotate firing direction.  (new dir = R[D])  */
	(void)rotate(strtdir, angle, r);
	ap.a_ray.r_dir[X] = r[X];
	ap.a_ray.r_dir[Y] = r[Y];
	ap.a_ray.r_dir[Z] = r[Z];

	while (strtpt[Z] <= zmax)
	{
	    /*  START # 3  */

	    iprev = (-1);	/*  No previous shots.  */

	    /*  Call rt_shootray.  */
	    (void)rt_shootray ( &ap );

	    strtpt[Y] += gridspace;
	    if (strtpt[Y] > ymax)
	    {
		strtpt[Y] = ymin + gridspace / 2.;
		strtpt[Z] += gridspace;
	    }

	    t[X] = strtpt[X] - center[X];
	    t[Y] = strtpt[Y] - center[Y];
	    t[Z] = strtpt[Z] - center[Z];

	    (void)rotate(t, angle, r);

	    ap.a_ray.r_pt[X] = center[X] + r[X];
	    ap.a_ray.r_pt[Y] = center[Y] + r[Y];
	    ap.a_ray.r_pt[Z] = center[Z] + r[Z];

	}	/*  END # 3  */

	/*  Shoot down 2nd & 3rd axes if necessary.  */
	if (ifire == 0)
	{
	    /*  START # 1000  */
	    /*  Set up & shoot down the 2nd axis (y-axis).  */
	    (void)printf("\nShooting down the 2nd axis.\n");
	    (void)fflush(stdout);

	    strtpt[X] = xmin + gridspace / 2.;
	    strtpt[Y] = ymax;
	    strtpt[Z] = zmin + gridspace / 2.;
	    strtdir[X] = 0.;
	    strtdir[Y] = (-1.);
	    strtdir[X] = 0.;

	    /*  Rotate starting point (new pt = C + R[P - C]).  */
	    t[X] = strtpt[X] - center [X];
	    t[Y] = strtpt[Y] - center [Y];
	    t[Z] = strtpt[Z] - center [Z];

	    (void)rotate(t, angle, r);

	    ap.a_ray.r_pt[X] = center[X] + r[X];
	    ap.a_ray.r_pt[Y] = center[Y] + r[Y];
	    ap.a_ray.r_pt[Z] = center[Z] + r[Z];

	    /*  Rotate firing direction (new dir = R[D])  */
	    (void)rotate(strtdir, angle, r);

	    ap.a_ray.r_dir[X] = r[X];
	    ap.a_ray.r_dir[Y] = r[Y];
	    ap.a_ray.r_dir[Z] = r[Z];

	    while (strtpt[Z] <= zmax)
	    {
		/*  START # 1010  */
		iprev = (-1);		/*  No previous shots.  */

		/*  Call rt_shootray.  */
		(void)rt_shootray(&ap);

		strtpt[X] += gridspace;
		if (strtpt[X] > xmax)
		{
		    strtpt[X] = xmin + gridspace / 2.;
		    strtpt[Z] += gridspace;
		}

		t[X] = strtpt[X] - center[X];
		t[Y] = strtpt[Y] - center[Y];
		t[Z] = strtpt[Z] - center[Z];

		(void)rotate(t, angle, r);

		ap.a_ray.r_pt[X] = center[X] + r[X];
		ap.a_ray.r_pt[Y] = center[Y] + r[Y];
		ap.a_ray.r_pt[Z] = center[Z] + r[Z];
	    }						/*  END # 1010  */

	    /*  Set up & shoot down the 3rd axis (z-axis).  */
	    (void)printf("\nShooting down the 3rd axis.\n");
	    (void)fflush(stdout);

	    strtpt[X] = xmin + gridspace / 2.;
	    strtpt[Y] = ymin + gridspace / 2.;
	    strtpt[Z] = zmax;
	    strtdir[X] = 0.;
	    strtdir[Y] = 0.;
	    strtdir[Z] = (-1.);

	    /*  Rotate starting points (new pt = C + R[P - C]).  */
	    t[X] = strtpt[X] - center[X];
	    t[Y] = strtpt[Y] - center[Y];
	    t[Z] = strtpt[Z] - center[Z];

	    (void)rotate(t, angle, r);

	    ap.a_ray.r_pt[X] = r[X];
	    ap.a_ray.r_pt[Y] = r[Y];
	    ap.a_ray.r_pt[Z] = r[Z];

	    while (strtpt[Y] <= ymax)
	    {
		/*  START # 1020  */
		iprev = (-1);		/*  No previous shots.  */

		/*  Call rt_shootray.  */
		(void)rt_shootray(&ap);

		strtpt[X] += gridspace;
		if (strtpt[X] > xmax)
		{
		    strtpt[X] = xmin + gridspace / 2.;
		    strtpt[Y] += gridspace;
		}

		t[X] = strtpt[X] - center[X];
		t[Y] = strtpt[Y] - center[Y];
		t[Z] = strtpt[Z] - center[Z];

		(void)rotate(t, angle, r);

		ap.a_ray.r_pt[X] = center[X] + r[X];
		ap.a_ray.r_pt[Y] = center[Y] + r[Y];
		ap.a_ray.r_pt[Z] = center[Z] + r[Z];
	    }						/*  END # 1020  */
	}						/*  END # 1000  */

	/*  Calculate final length between centroid & shared surface area.  */
	if (iwrite == 0)
	{
	    (void)fprintf(stdout, "\n\nFinal numbers.\n");
	    (void)fflush(stdout);
	}

	for (i=0; i<numreg; i++)
	{

	    if (iwrite == 0)
	    {
		(void)fprintf(stdout, "reg#=%d, matid=%d\n", (i+1), cond[i].mat);
		(void)fflush(stdout);
	    }

	    if (iwrite == 1)
	    {
		(void)fprintf(fp3, "reg#=%d, matid=%d\n", (i+1), cond[i].mat);
		(void)fflush(fp3);
	    }

	    for (j=0; j<numreg; j++)
	    {
		if (cond[i].numcal[j] > ZEROTOL)
		{
		    cond[i].avglen[j] /= cond[i].numcal[j];
		    cond[i].rmslen[j] /= cond[i].numcal[j];
		    cond[i].rmslen[j] = sqrt(cond[i].rmslen[j]);

		    if (iwrite == 0)
		    {
			(void)fprintf(stdout, "\tadjreg=%d, numcal=%f, shrarea=%f, ",
				      (j+1), cond[i].numcal[j], cond[i].shrarea[j]);
			(void)fprintf(stdout, "avglen=%f\n", cond[i].avglen[j]);
			(void)fprintf(stdout, "\t\trmslen=%f, ", cond[i].rmslen[j]);
			(void)fprintf(stdout, "minlen=%f, maxlen=%f\n",
				      cond[i].minlen[j], cond[i].maxlen[j]);
			(void)fflush(stdout);
		    }

		    if (iwrite == 1)
		    {
			(void)fprintf(fp3, "\tadjreg=%d, numcal=%f, shrarea=%f, ",
				      (j+1), cond[i].numcal[j], cond[i].shrarea[j]);
			(void)fprintf(fp3, "avglen=%f\n", cond[i].avglen[j]);
			(void)fprintf(fp3, "\t\trmslen=%f, ", cond[i].rmslen[j]);
			(void)fprintf(fp3, "minlen=%f, maxlen=%f\n",
				      cond[i].minlen[j], cond[i].maxlen[j]);
			(void)fflush(fp3);
		    }

		}
		else
		{
		    cond[i].avglen[j] = 0.;
		    cond[i].rmslen[j] = 0.;
		}
	    }
	}

	if (iwrite == 1)
	{
	    /*  Print summary of all files used.  */
	    (void)fprintf(fp3, "\n\nSUMMARY OF FILES USED & CREATED\n");
	    (void)fprintf(fp3, "\t.g file used:  %s\n", argv[1]);
	    (void)fprintf(fp3, "\tregions used:\n");
	    (void)fflush(fp3);
	    i=2;
	    while (argv[i] != NULL)
	    {
		(void)fprintf(fp3, "\t\t%s\n", argv[i]);
		(void)fflush(fp3);
		i++;
	    }
	    (void)fprintf(fp3, "\tfile containing second pass information:  %s\n",
			  spfile);
	    (void)fprintf(fp3, "\tmaterial file used:  %s\n", filemat);
	    (void)fprintf(fp3, "\toutput file created:  %s\n", filename);
	    if (typeout == 0)
	    {
		(void)fprintf(fp3, "\tconductivity file created:  %s\n", confile);
		(void)fprintf(fp3, "\t  (format is PRISM %d.0)\n", prmrel);
	    }
	    if (typeout == 1) (void)fprintf(fp3, "\tgeneric file created:  %s\n"
					    , genfile);
	    (void)fprintf(fp3, "\tconductivity table file created:  %s\n",
			  tblfile);
	    (void)fprintf(fp3, "\terror file created:  %s\n\n\n", fileerr);
	    (void)fflush(fp3);

	    (void)fclose(fp3);
	}

	/*------------------------------------------------------------------*/

	/*  Open conductivity file to be used with PRISM if needed.  */
	if (typeout == 0)
	{
	    fp1=fopen(confile, "wb");
	    (void)fprintf(fp1, "Conductivity file for use with PRISM.\n");
	    (void)fflush(fp1);
	}

	/*  Make calculations & write to conductivity file.  */
	for (i=0; i<numreg; i++)
	{
	    /*  START # 6  */

	    /*  Make conductivity file triangular.  This program still  */
	    /*  computes the ENTIRE matrix.  */

	    for (j=(i+1); j<numreg; j++)
	    {
	    	/*  START # 7  */

		if ( (cond[i].avglen[j] != 0) )
		{
		    /*  START # 8  */
		    /*  Find correct thermal conductivity.  */
		    /*  If ki or kj = 0 => rk = 0.  */
		    ki = k[cond[i].mat];
		    kj = k[cond[j].mat];

		    /*  All calculations will be in meters &  */
		    /*  square meters.  */

		    areai = cond[i].shrarea[j] * 1.e-6;

		    /*  average length  */
		    leni = cond[i].avglen[j] * 1.e-3;
		    lenj = cond[j].avglen[i] * 1.e-3;
		    if (((-ZEROTOL < ki) && (ki < ZEROTOL)) ||
			((-ZEROTOL < kj) && (kj < ZEROTOL)))
			cond[i].rkavg[j] = 0.;
		    else
		    {
			rki = leni / (ki * areai);
			rkj = lenj / (kj * areai);
			cond[i].rkavg[j] = 1. / (rki + rkj);
		    }

		    /*  rms length  */
		    leni = cond[i].rmslen[j] * 1.e-3;
		    lenj = cond[j].rmslen[i] * 1.e-3;
		    if (((-ZEROTOL < ki) && (ki < ZEROTOL)) ||
			((-ZEROTOL < kj) && (kj < ZEROTOL)))
			cond[i].rkrms[j] = 0.;
		    else
		    {
			rki = leni / (ki * areai);
			rkj = lenj / (kj * areai);
			cond[i].rkrms[j] = 1. / (rki + rkj);
		    }

		    /*  minimum length  */
		    leni = cond[i].minlen[j] * 1.e-3;
		    lenj = cond[j].minlen[i] * 1.e-3;
		    if (((-ZEROTOL < ki) && (ki < ZEROTOL)) ||
			((-ZEROTOL < kj) && (kj < ZEROTOL)))
			cond[i].rkmin[j] = 0.;
		    else
		    {
			rki = leni / (ki * areai);
			rkj = lenj / (kj * areai);
			cond[i].rkmin[j] = 1. / (rki + rkj);
		    }

		    /*  maximum length  */
		    leni = cond[i].maxlen[j] * 1.e-3;
		    lenj = cond[j].maxlen[i] * 1.e-3;
		    if (((-ZEROTOL < ki) && (ki < ZEROTOL)) ||
			((-ZEROTOL < kj) && (kj < ZEROTOL)))
			cond[i].rkmax[j] = 0.;
		    else
		    {
			rki = leni / (ki * areai);
			rkj = lenj / (kj * areai);
			cond[i].rkmax[j] = 1. / (rki + rkj);
		    }

		    /*  Print if had adjacent regions, conductivity  */
		    /*  may be zero.  */

		    /*  Print only if PRISM file is to be created.  */
		    if (typeout == 0) {
			/*  START # 8A  */
			if ( (itype == 1) && (cond[i].shrarea[j] > ZEROTOL) )
			{
			    if (prmrel == 2)
				(void)fprintf(fp1, "%3d %3d %7.3f %.3e\n",
					      (j+1), (i+1), cond[i].rkavg[j],
					      (cond[i].shrarea[j] * 1.0e-6));
			    if (prmrel == 3)
				(void)fprintf(fp1, "%6d %6d %7.3f %.3e\n",
					      (j+1), (i+1), cond[i].rkavg[j],
					      (cond[i].shrarea[j] * 1.0e-6));
			}

			if ( (itype == 2) && (cond[i].shrarea[j] > ZEROTOL) )
			{
			    if (prmrel == 2)
				(void)fprintf(fp1, "%3d %3d %7.3f %.3e\n",
					      (j+1), (i+1), cond[i].rkrms[j],
					      (cond[i].shrarea[j] * 1.0e-6));
			    if (prmrel == 3)
				(void)fprintf(fp1, "%6d %6d %7.3f %.3e\n",
					      (j+1), (i+1), cond[i].rkrms[j],
					      (cond[i].shrarea[j] * 1.0e-6));
			}

			if ( (itype == 3) && (cond[i].shrarea[j] > ZEROTOL) )
			{
			    if (prmrel == 2)
				(void)fprintf(fp1, "%3d %3d %7.3f %.3e\n",
					      (j+1), (i+1), cond[i].rkmin[j],
					      (cond[i].shrarea[j] * 1.0e-6));
			    if (prmrel == 3)
				(void)fprintf(fp1, "%6d %6d %7.3f %.3e\n",
					      (j+1), (i+1), cond[i].rkmin[j],
					      (cond[i].shrarea[j] * 1.0e-6));
			}

			if ( (itype == 4) && (cond[i].shrarea[j] > ZEROTOL) )
			{
			    if (prmrel == 2)
				(void)fprintf(fp1, "%3d %3d %7.3f %.3e\n",
					      (j+1), (i+1), cond[i].rkmax[j],
					      (cond[i].shrarea[j] * 1.0e-6));
			    if (prmrel == 3)
				(void)fprintf(fp1, "%6d %6d %7.3f %.3e\n",
					      (j+1), (i+1), cond[i].rkmax[j],
					      (cond[i].shrarea[j] * 1.0e-6));
			}

			(void)fflush(fp1);
		    }				/*  END of # 8A  */
		}	/*  END # 8  */
	    }	/*  END # 7  */
	}	/*  END # 6  */

	if (typeout == 0) (void)fclose(fp1);

	/*------------------------------------------------------------------*/

	/*  Open and write to generic file if necessary. */
	/*  The format follows.  */
	/*  4  region number  number of adjacent regions  */
	/*     adjacent region  shared area  conduction distance  */

	if (typeout == 1)
	{
	    /*  Open file.  */
	    fp6 = fopen(genfile, "wb");
	    (void)printf("Opened generic file.\n");
	    (void)fflush(stdout);

	    for (i=0; i<numreg; i++)
	    {
		/*  Find number of adjacent regions.  */
		/*
		 *		(void)printf("Ready to find number of adjacent areas.\n");
		 *		(void)fflush(stdout);
		 */
		nadjreg = 0;
		/*
		 *		(void)printf("nadjreg = %d\n", nadjreg);
		 *		(void)fflush(stdout);
		 */
		for (j=0; j<numreg; j++)
		{
		    /*
		     *		   (void)printf("%d, %d, %f\n", i, j, cond[i].shrarea[j]);
		     *		   (void)fflush(stdout);
		     */
		    if (cond[i].shrarea[j] > ZEROTOL) nadjreg += 1;
		}
		/*
		 *		(void)printf("Found number of adjacent areas.\n");
		 *		(void)fflush(stdout);
		 */

		(void)fprintf(fp6, "4  %5d  %5d\n", (i+1), nadjreg);
		(void)fflush(fp6);

		for (j=0; j<numreg; j++)
		{
		    if (cond[i].shrarea[j] > ZEROTOL)
		    {
			(void)fprintf(fp6, "   %5d  %.3e  ", (j+1),
				      (cond[i].shrarea[j] * 1.e-6));
			if (itype == 1) (void)fprintf(fp6, "%.3e\n",
						      (cond[i].avglen[j] * 1.e-3));
			if (itype == 2) (void)fprintf(fp6, "%.3e\n",
						      (cond[i].rmslen[j] * 1.e-3));
			if (itype == 3) (void)fprintf(fp6, "%.3e\n",
						      (cond[i].minlen[j] * 1.e-3));
			if (itype == 4) (void)fprintf(fp6, "%.3e\n",
						      (cond[i].maxlen[j] * 1.e-3));
		    }
		    (void)fflush(fp6);
		}
	    }
	    (void)fclose(fp6);
	}

	/*------------------------------------------------------------------*/

	/*  Open conductivity table file and write information to  */
	/*  it.  All units will be in meters or square meters.  */
	fp2=fopen(tblfile, "wb");
	(void)fprintf(fp2, "Conductivity table.  Units are in meters or ");
	(void)fprintf(fp2, "square meters.\n");

	(void)fprintf(fp2, " reg, mat, adj,   shrarea,");
	(void)fprintf(fp2, "    avglen,     rkavg,");
	(void)fprintf(fp2, "    rmslen,     rkrms,");
	(void)fprintf(fp2, "    minlen,     rkmin,");
	(void)fprintf(fp2, "    maxlen,     rkmax\n");

	(void)fflush(fp2);

	for (i=0; i<numreg; i++)
	{
	    /*  START # 9  */
	    for (j=0; j<numreg; j++)
	    {
	    	/*  START # 10  */
		if (cond[i].shrarea[j] != 0)
		{
		    /*  START # 11  */
		    a1 = cond[i].shrarea[j] * 1.e-6;
		    l1 = cond[i].avglen[j] * 1.e-3;
		    l2 = cond[i].rmslen[j] * 1.e-3;
		    l3 = cond[i].minlen[j] * 1.e-3;
		    l4 = cond[i].maxlen[j] * 1.e-3;

		    (void)fprintf(fp2, "%4d,%4d,%4d, %.3e,",
				  (i+1), cond[i].mat, (j+1), a1);
		    if (j > i)
		    {
			(void)fprintf(fp2, " %.3e, %.3e,", l1, cond[i].rkavg[j]);
			(void)fprintf(fp2, " %.3e, %.3e,", l2, cond[i].rkrms[j]);
			(void)fprintf(fp2, " %.3e, %.3e,", l3, cond[i].rkmin[j]);
			(void)fprintf(fp2, " %.3e, %.3e\n", l4, cond[i].rkmax[j]);
		    }
		    else
		    {
			(void)fprintf(fp2, " %.3e, %.3e,", l1, cond[j].rkavg[i]);
			(void)fprintf(fp2, " %.3e, %.3e,", l2, cond[j].rkrms[i]);
			(void)fprintf(fp2, " %.3e, %.3e,", l3, cond[j].rkmin[i]);
			(void)fprintf(fp2, " %.3e, %.3e\n", l4, cond[j].rkmax[i]);
		    }

		    (void)fflush(fp2);
		}	/*  END # 11  */
	    }	/*  END # 10  */
	}	/*  END # 9  */

	(void)fclose(fp2);

	/*------------------------------------------------------------------*/

	/*  Print summary of all files used.  */
	(void)fprintf(stdout, "\n\nSUMMARY OF FILES USED & CREATED\n");
	(void)fprintf(stdout, "\t.g file used:  %s\n", argv[1]);
	(void)fprintf(stdout, "\tregions used:\n");
	(void)fflush(stdout);
	i=2;
	while (argv[i] != NULL)
	{
	    (void)fprintf(stdout, "\t\t%s\n", argv[i]);
	    (void)fflush(stdout);
	    i++;
	}
	(void)fprintf(stdout, "\tfile containing second pass information:  %s\n",
		      spfile);
	(void)fprintf(stdout, "\tmaterial file used:  %s\n", filemat);
	if (iwrite == 1)
	{
	    (void)fprintf(stdout, "\toutput file created:  %s\n", filename);
	}
	if (typeout == 0)
	{
	    (void)fprintf(stdout, "\tconductivity file created:  %s\n", confile);
	    (void)fprintf(stdout, "\t  (format is PRISM %d.0)\n", prmrel);
	}
	if (typeout == 1) (void)printf("\tgeneric file created:  %s\n", genfile);
	(void)fprintf(stdout, "\tconductivity table file created:  %s\n", tblfile);
	(void)fprintf(stdout, "\terror file created:  %s\n\n\n", fileerr);
	(void)fflush(stdout);

	/*------------------------------------------------------------------*/

	/*  Open error file.  */
	fp4 = fopen(fileerr, "wb");

	/*  Write errors to error file.  */
	(void)fprintf(fp4, "\nERRORS from secpass\n\n");
	/*  Write type of file created to error file.  */
	if (typeout == 0)
	{
	    (void)fprintf(fp4, "PRISM %d.0 conductivity file, %s, created.\n\n",
			  prmrel, confile);
	}
	if (typeout == 1) (void)fprintf(fp4, "Generic file, %s, created.\n\n",
					genfile);
	(void)fflush(fp4);
	for (i=0; i<numreg; i++)
	{
	    for (j=0; j<numreg;  j++)
	    {
		if ( (cond[i].numcal[j] > ZEROTOL) &&
		     ( cond[i].numcal[j] < MINCAL ) )
		{
		    (void)fprintf(fp4, "region %d, adjacent region %d:\n",
				  (i+1), (j+1));
		    (void)fprintf(fp4, "\tnumber of length calculations ");
		    (void)fprintf(fp4, "below minimum of %d\n", MINCAL);
		    (void)fflush(fp4);
		}
	    }
	}
	(void)fclose(fp4);

	/*------------------------------------------------------------------*/

	/*  Everything completed, free memory.  */
	(void)fprintf(stdout, "Freeing memory.\n");
	(void)fflush(stdout);
	for (i=0; i<nmged; i++)
	{
	    bu_free(cond[i].shrarea, "cond[i].shrarea");
	    bu_free(cond[i].avglen, "cond[i].avglen");
	    bu_free(cond[i].rmslen, "cond[i].rmslen");
	    bu_free(cond[i].minlen, "cond[i].minlen");
	    bu_free(cond[i].maxlen, "cond[i].maxlen");
	    bu_free(cond[i].numcal, "cond[i].numcal");
	    bu_free(cond[i].rkavg, "cond[i].rkavg");
	    bu_free(cond[i].rkrms, "cond[i].rkrms");
	    bu_free(cond[i].rkmin, "cond[i].rkmin");
	    bu_free(cond[i].rkmax, "cond[i].rkmax");
	}
	bu_free(cond, "cond");

    }	/*  END # 2  */
    return(0);
}	/*  END # 1  */
Exemple #22
0
bool AP_Arming::ins_checks(bool report)
{
    if ((checks_to_perform & ARMING_CHECK_ALL) ||
        (checks_to_perform & ARMING_CHECK_INS)) {
        const AP_InertialSensor &ins = AP::ins();
        if (!ins.get_gyro_health_all()) {
            if (report) {
                gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Gyros not healthy");
            }
            return false;
        }
        if (!ins.gyro_calibrated_ok_all()) {
            if (report) {
                gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Gyros not calibrated");
            }
            return false;
        }
        if (!ins.get_accel_health_all()) {
            if (report) {
                gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels not healthy");
            }
            return false;
        }
        if (!ins.accel_calibrated_ok_all()) {
            if (report) {
                gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: 3D Accel calibration needed");
            }
            return false;
        }
        
        //check if accelerometers have calibrated and require reboot
        if (ins.accel_cal_requires_reboot()) {
            if (report) {
                gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels calibrated requires reboot");
            }
            return false;
        }

        // check all accelerometers point in roughly same direction
        if (ins.get_accel_count() > 1) {
            const Vector3f &prime_accel_vec = ins.get_accel();
            for(uint8_t i=0; i<ins.get_accel_count(); i++) {
                if (!ins.use_accel(i)) {
                    continue;
                }
                // get next accel vector
                const Vector3f &accel_vec = ins.get_accel(i);
                Vector3f vec_diff = accel_vec - prime_accel_vec;
                // allow for user-defined difference, typically 0.75 m/s/s. Has to pass in last 10 seconds
                float threshold = accel_error_threshold;
                if (i >= 2) {
                    /*
                      we allow for a higher threshold for IMU3 as it
                      runs at a different temperature to IMU1/IMU2,
                      and is not used for accel data in the EKF
                     */
                    threshold *= 3;
                }

                // EKF is less sensitive to Z-axis error
                vec_diff.z *= 0.5f;

                if (vec_diff.length() <= threshold) {
                    last_accel_pass_ms[i] = AP_HAL::millis();
                }
                if (AP_HAL::millis() - last_accel_pass_ms[i] > 10000) {
                    if (report) {
                        gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels inconsistent");
                    }
                    return false;
                }
            }
        }

        // check all gyros are giving consistent readings
        if (ins.get_gyro_count() > 1) {
            const Vector3f &prime_gyro_vec = ins.get_gyro();
            for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
                if (!ins.use_gyro(i)) {
                    continue;
                }
                // get next gyro vector
                const Vector3f &gyro_vec = ins.get_gyro(i);
                Vector3f vec_diff = gyro_vec - prime_gyro_vec;
                // allow for up to 5 degrees/s difference. Pass if it has
                // been OK in last 10 seconds
                if (vec_diff.length() <= radians(5)) {
                    last_gyro_pass_ms[i] = AP_HAL::millis();
                }
                if (AP_HAL::millis() - last_gyro_pass_ms[i] > 10000) {
                    if (report) {
                        gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Gyros inconsistent");
                    }
                    return false;
                }
            }
        }
    }

    return true;
}
void AddFlare(const Vec2s & pos, float sm, short typ, Entity * io, bool bookDraw) {

	long i;
	for(i = 0; i < MAX_FLARES; i++) {
		if(!magicFlares[i].exist) {
			break;
		}
	}
	if(i >= MAX_FLARES) {
		return;
	}

	FLARES * fl = &magicFlares[i];
	fl->exist = 1;
	flarenum++;

	if(!bookDraw)
		fl->bDrawBitmap = 0;
	else
		fl->bDrawBitmap = 1;

	fl->io = io;
	if(io) {
		fl->flags = 1;
		io->flarecount++;
	} else {
		fl->flags = 0;
	}

	fl->x = float(pos.x) - rnd() * 4.f;
	fl->y = float(pos.y) - rnd() * 4.f - 50.f;
	fl->tv.rhw = fl->v.rhw = 1.f;
	fl->tv.specular = fl->v.specular = 1;

	if(!bookDraw) {
		EERIE_CAMERA ka = *Kam;
		ka.angle = Anglef(360.f, 360.f, 360.f) - ka.angle;
		EERIE_CAMERA * oldcam = ACTIVECAM;
		SetActiveCamera(&ka);
		PrepareCamera(&ka);
		fl->v.p += ka.orgTrans.pos;
		EE_RTP(&fl->tv, &fl->v);
		fl->v.p += ka.orgTrans.pos;

		float vx = -(fl->x - subj.center.x) * 0.2173913f;
		float vy = (fl->y - subj.center.y) * 0.1515151515151515f;
		if(io) {
			fl->v.p.x = io->pos.x - EEsin(radians(MAKEANGLE(io->angle.getPitch() + vx))) * 100.f;
			fl->v.p.y = io->pos.y + EEsin(radians(MAKEANGLE(io->angle.getYaw() + vy))) * 100.f - 150.f;
			fl->v.p.z = io->pos.z + EEcos(radians(MAKEANGLE(io->angle.getPitch() + vx))) * 100.f;
		} else {
			fl->v.p.x = float(pos.x - (g_size.width() / 2)) * 150.f / float(g_size.width());
			fl->v.p.y = float(pos.y - (g_size.height() / 2)) * 150.f / float(g_size.width());
			fl->v.p.z = 75.f;
			ka = *oldcam;
			SetActiveCamera(&ka);
			PrepareCamera(&ka);
			float temp = (fl->v.p.y * -ka.orgTrans.xsin) + (fl->v.p.z * ka.orgTrans.xcos);
			fl->v.p.y = (fl->v.p.y * ka.orgTrans.xcos) - (-fl->v.p.z * ka.orgTrans.xsin);
			fl->v.p.z = (temp * ka.orgTrans.ycos) - (-fl->v.p.x * ka.orgTrans.ysin);
			fl->v.p.x = (temp * -ka.orgTrans.ysin) + (fl->v.p.x * ka.orgTrans.ycos);
			fl->v.p += oldcam->orgTrans.pos;
		}
		fl->tv.p = fl->v.p;
		SetActiveCamera(oldcam);
		PrepareCamera(oldcam);
	} else {
		fl->tv.p = Vec3f(fl->x, fl->y, 0.001f);
	}

	switch(PIPOrgb) {
		case 0: {
			fl->rgb = Color3f(rnd() * (2.f/3) + .4f, rnd() * (2.f/3), rnd() * (2.f/3) + .4f);
			break;
		}
		case 1: {
			fl->rgb = Color3f(rnd() * .625f + .5f, rnd() * .625f + .5f, rnd() * .55f);
			break;
		}
		case 2: {
			fl->rgb = Color3f(rnd() * (2.f/3) + .4f, rnd() * .55f, rnd() * .55f);
			break;
		}
	}

	if(typ == -1) {
		float zz = (EERIEMouseButton & 1) ? 0.29f : ((sm > 0.5f) ? rnd() : 1.f);
		if(zz < 0.2f) {
			fl->type = 2;
			fl->size = rnd() * 42.f + 42.f;
			fl->tolive = (800.f + rnd() * 800.f) * FLARE_MUL;
		} else if(zz < 0.5f) {
			fl->type = 3;
			fl->size = rnd() * 52.f + 16.f;
			fl->tolive = (800.f + rnd() * 800.f) * FLARE_MUL;
		} else {
			fl->type = 1;
			fl->size = (rnd() * 24.f + 32.f) * sm;
			fl->tolive = (1700.f + rnd() * 500.f) * FLARE_MUL;
		}
	} else {
		fl->type = (rnd() > 0.8f) ? 1 : 4;
		fl->size = (rnd() * 38.f + 64.f) * sm;
		fl->tolive = (1700.f + rnd() * 500.f) * FLARE_MUL;
	}

	fl->dynlight = -1;
	fl->move = OPIPOrgb;

	for(long kk = 0; kk < 3; kk++) {

		if(rnd() < 0.5f) {
			continue;
		}

		PARTICLE_DEF * pd = createParticle();
		if(!pd) {
			break;
		}

		if(!bookDraw) {
			pd->special = FADE_IN_AND_OUT | ROTATING | MODULATE_ROTATION | DISSIPATING;
			if(!io) {
				pd->special |= PARTICLE_NOZBUFFER;
			}
		} else {
			pd->special = FADE_IN_AND_OUT;
		}

		pd->ov = fl->v.p + randomVec(-5.f, 5.f);
		pd->move = Vec3f(0.f, 5.f, 0.f);
		pd->scale = Vec3f(-2.f);
		pd->tolive = 1300 + kk * 100 + Random::get(0, 800);
		pd->tc = fire2;
		if(kk == 1) {
			pd->move.y = 4.f;
			pd->siz = 1.5f;
		} else {
			pd->siz = 1.f + rnd();
		}
		pd->rgb = Color3f(fl->rgb.r * (2.f/3), fl->rgb.g * (2.f/3), fl->rgb.b * (2.f/3));
		pd->fparam = 1.2f;

		if(bookDraw)
			pd->type = PARTICLE_2D;
	}
}
Exemple #24
0
void CHeal::Create() {
	
	SetAngle(MAKEANGLE(player.angle.b));
	
	if(spells[spellinstance].caster == 0) {
		eSrc = player.pos;
	} else {
		eSrc = entities[spells[spellinstance].caster]->pos;
	}
	
	pPS->lLightId = GetFreeDynLight();

	if (pPS->lLightId != -1)
	{
		long id = pPS->lLightId;
		DynLight[id].exist = 1;
		DynLight[id].intensity = 2.3f;
		DynLight[id].fallstart = 200.f;
		DynLight[id].fallend   = 350.f;
		DynLight[id].rgb.r = 0.4f;
		DynLight[id].rgb.g = 0.4f;
		DynLight[id].rgb.b = 1.0f;
		DynLight[id].pos.x = eSrc.x;
		DynLight[id].pos.y = eSrc.y - 50.f;
		DynLight[id].pos.z = eSrc.z;
		DynLight[id].duration = 200;
		DynLight[id].extras = 0;
	}

	pPS->SetPos(eSrc);
	ParticleParams cp;
	memset(&cp, 0, sizeof(cp));
	cp.iNbMax = 350;
	cp.fLife = 800;
	cp.fLifeRandom = 2000;
	cp.p3Pos.x = 100;
	cp.p3Pos.y = 200;
	cp.p3Pos.z = 100;
	cp.p3Direction.x = 0;
	cp.p3Direction.y = -10;
	cp.p3Direction.z = 0;
	cp.fAngle = radians(5);
	cp.fSpeed = 120; 
	cp.fSpeedRandom = 84; 
	cp.p3Gravity.x = 0;
	cp.p3Gravity.y = -10;
	cp.p3Gravity.z = 0;
	cp.fFlash = 0;
	cp.fRotation = 80;

	cp.fStartSize = 8;
	cp.fStartSizeRandom = 8;
	cp.fStartColor[0] = 205;
	cp.fStartColor[1] = 205;
	cp.fStartColor[2] = 255;
	cp.fStartColor[3] = 245;
	cp.fStartColorRandom[0] = 50;
	cp.fStartColorRandom[1] = 50;
	cp.fStartColorRandom[2] = 0;
	cp.fStartColorRandom[3] = 10;

	cp.fEndSize = 6;
	cp.fEndSizeRandom = 4;
	cp.fEndColor[0] = 20;
	cp.fEndColor[1] = 20;
	cp.fEndColor[2] = 30;
	cp.fEndColor[3] = 0;
	cp.fEndColorRandom[0] = 0;
	cp.fEndColorRandom[1] = 0;
	cp.fEndColorRandom[2] = 40;
	cp.fEndColorRandom[3] = 0;
	
	cp.iBlendMode = 0;

	pPS->SetParams(cp);
	pPS->ulParticleSpawn = PARTICLE_CIRCULAR | PARTICLE_BORDER;
	pPS->SetTexture("graph/particles/heal_0005", 0, 100); 

	fSize = 1;
}
Exemple #25
0
int main()
{
	int ii;

	//-----------------------------------------------------------------------------------------
		//unsigned long baudrate = 9600, uint8_t databits = 8, uint8_t parity = 0, uint8_t stopbits = 1
	serial1.init(115200,8,0,1);
	TRACE("Balancing Robot V1.0\n");
	
	
	TRACE((mpu->test()==0) ? "i2c has failed...\n\n" : "MPU Project V1.0\n\n");
	TRACE("\t=>Restarting MPU6050...");
	mpu->reset();
	TRACE((mpu->test()==0) ? "i2c has failed...\n" : "OK\n");
	//-----------------------------------------------------------------------------------------
	
	mpu->init();
	
	for(ii=0;ii<10;ii++)
	{
		io.toggle(LED3);
		delay.ms(100);
	}
	
	//mpu->Get_Accel_Offset();
	mpu->getGyroOffset();	
	mpu->getAccelVal();
	
	imu.HADXL_P[0] = mpu->Accel_In_g[X];		//x;
	imu.HADXL_P[1] = mpu->Accel_In_g[Y];		//y;
	imu.HADXL_P[2] = mpu->Accel_In_g[Z];		//z;
	
	for(ii=0;ii<10;ii++)
	{
		io.toggle(LED3);
		delay.ms(50);
	}
	
	
	
	
	uint32_t routin_100HZ_start = timer.millis();
	uint32_t routin_90ms_start = timer.millis();
	while(1)
	{
		if(timer.deltaTmillis(routin_100HZ_start)>9) 	// --------> execute every 10ms -> 100Hz routin
		{
			routin_100HZ_start = timer.millis();	

			mpu->getAccelVal();
			imu.interval = 10;//timer.deltaTmillis(gyro.getLastTime());
			mpu->getGyroVal();


			imu.calculate(
										radians(mpu->GyroRate_Val[X]), 
										radians(mpu->GyroRate_Val[Y]), 
										radians(mpu->GyroRate_Val[Z]), 
										mpu->Accel_In_g[X], 
										mpu->Accel_In_g[Y],
										mpu->Accel_In_g[Z]
									 );					
			imu.update();
			
		} // End of 100Hz routin				
		/*******************************************************************/
		if(timer.deltaTmillis(routin_90ms_start)>150) 	// --------> execute every 10ms -> 100Hz routin
		{					
			TRACE("Interval: "); 			TRACEln(imu.interval);						
			TRACE("Roll:     ");			TRACEln(imu.roll);
			TRACE("Pitch:    ");			TRACEln(imu.pitch);
			TRACE("Yaw:      ");			TRACEln(imu.yaw);		
			TRACEln();	

			routin_90ms_start = millis();
		}
	}
}
Exemple #26
0
constexpr 
angle_t<long double> operator "" _deg (unsigned long long deg) {
 	return angle_t<long double>{radians(deg)};
}
Exemple #27
0
static void
draw_text_bubble (cairo_t *cr,
                  GtkWidget *widget,
                  gdouble pointx,
                  gdouble pointy)
{
  static const double corner_radius = 9.0;
  static const double margin_top = 12.0;
  static const double margin_bottom = 12.0;
  static const double margin_left = 24.0;
  static const double margin_right = 24.0;

  CcTimezoneMapPrivate *priv = CC_TIMEZONE_MAP (widget)->priv;
  GtkAllocation alloc;
  PangoLayout *layout;
  PangoRectangle text_rect;
  double x;
  double y;
  double width;
  double height;

  if (!priv->bubble_text)
    return;

  gtk_widget_get_allocation (widget, &alloc);
  layout = gtk_widget_create_pango_layout (widget, NULL);

  /* Layout the text */
  pango_layout_set_alignment (layout, PANGO_ALIGN_CENTER);
  pango_layout_set_spacing (layout, 3);
  pango_layout_set_markup (layout, priv->bubble_text, -1);

  pango_layout_get_pixel_extents (layout, NULL, &text_rect);

  /* Calculate the bubble size based on the text layout size */
  width = text_rect.width + margin_left + margin_right;
  height = text_rect.height + margin_top + margin_bottom;

  if (pointx < alloc.width / 2)
    x = pointx + 25;
  else
    x = pointx - width - 25;

  y = pointy - height / 2;

  /* Make sure it fits in the visible area */
  x = CLAMP (x, 0, alloc.width - width);
  y = CLAMP (y, 0, alloc.height - height);

  cairo_save (cr);
  cairo_translate (cr, x, y);

  /* Draw the bubble */
  cairo_new_sub_path (cr);
  cairo_arc (cr, width - corner_radius, corner_radius, corner_radius, radians (-90), radians (0));
  cairo_arc (cr, width - corner_radius, height - corner_radius, corner_radius, radians (0), radians (90));
  cairo_arc (cr, corner_radius, height - corner_radius, corner_radius, radians (90), radians (180));
  cairo_arc (cr, corner_radius, corner_radius, corner_radius, radians (180), radians (270));
  cairo_close_path (cr);

  cairo_set_source_rgba (cr, 0.2, 0.2, 0.2, 0.7);
  cairo_fill (cr);

  /* And finally draw the text */
  cairo_set_source_rgb (cr, 1, 1, 1);
  cairo_move_to (cr, margin_left, margin_top);
  pango_cairo_show_layout (cr, layout);

  g_object_unref (layout);
  cairo_restore (cr);
}
Exemple #28
0
bool LogReader::update(uint8_t &type)
{
    uint8_t hdr[3];
    if (::read(fd, hdr, 3) != 3) {
        return false;
    }
    if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
        printf("bad log header\n");
        return false;
    }

    if (hdr[2] == LOG_FORMAT_MSG) {
        struct log_Format &f = formats[num_formats];
        memcpy(&f, hdr, 3);
        if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
            return false;
        }
        if (num_formats < LOGREADER_MAX_FORMATS-1) {
            num_formats++;
        }
        type = f.type;
        return true;
    }

    uint8_t i;
    for (i=0; i<num_formats; i++) {
        if (formats[i].type == hdr[2]) break;
    }
    if (i == num_formats) {
        return false;
    }
    const struct log_Format &f = formats[i];
    
    uint8_t data[f.length];
    memcpy(data, hdr, 3);
    if (::read(fd, &data[3], f.length-3) != f.length-3) {
        return false;
    }

    switch (f.type) {
    case LOG_MESSAGE_MSG: {
        struct log_Message msg;
        if(sizeof(msg) != f.length) {
            printf("Bad MESSAGE length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
            vehicle = VEHICLE_PLANE;
            ::printf("Detected Plane\n");
            ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
        } else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
            vehicle = VEHICLE_COPTER;
            ::printf("Detected Copter\n");
            ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
        } else if (strncmp(msg.msg, "ArduRover", strlen("ArduRover")) == 0) {
            vehicle = VEHICLE_ROVER;
            ::printf("Detected Rover\n");
            ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
        }
        break;
    }

    case LOG_IMU_MSG: {
        struct log_IMU msg;
        if(sizeof(msg) != f.length) {
            printf("Bad IMU length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.timestamp);
        if (gyro_mask & 1) {
            ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
        }
        if (accel_mask & 1) {
            ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
        }
        break;
    }

    case LOG_IMU2_MSG: {
        struct log_IMU msg;
        if(sizeof(msg) != f.length) {
            printf("Bad IMU2 length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.timestamp);
        if (gyro_mask & 2) {
            ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
        }
        if (accel_mask & 2) {
            ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
        }
        break;
    }

    case LOG_GPS_MSG: {
        struct log_GPS msg;
        if(sizeof(msg) != f.length) {
            printf("Bad GPS length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.apm_time);
        Location loc;
        loc.lat = msg.latitude;
        loc.lng = msg.longitude;
        loc.alt = msg.altitude;
        loc.options = 0;

        Vector3f vel(msg.ground_speed*0.01f*cosf(radians(msg.ground_course*0.01f)),
                     msg.ground_speed*0.01f*sinf(radians(msg.ground_course*0.01f)),
                     msg.vel_z);
        gps.setHIL(0, (AP_GPS::GPS_Status)msg.status,
                   msg.apm_time,
                   loc,
                   vel,
                   msg.num_sats,
                   msg.hdop,
                   msg.vel_z != 0);
        if (msg.status == 3 && ground_alt_cm == 0) {
            ground_alt_cm = msg.altitude;
        }
        rel_altitude = msg.rel_altitude*0.01f;
        break;
    }

    case LOG_GPS2_MSG: {
        struct log_GPS2 msg;
        if(sizeof(msg) != f.length) {
            printf("Bad GPS2 length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.apm_time);
        Location loc;
        loc.lat = msg.latitude;
        loc.lng = msg.longitude;
        loc.alt = msg.altitude;
        loc.options = 0;

        Vector3f vel(msg.ground_speed*0.01f*cosf(radians(msg.ground_course*0.01f)),
                     msg.ground_speed*0.01f*sinf(radians(msg.ground_course*0.01f)),
                     msg.vel_z);
        gps.setHIL(1, (AP_GPS::GPS_Status)msg.status,
                   msg.apm_time,
                   loc,
                   vel,
                   msg.num_sats,
                   msg.hdop,
                   msg.vel_z != 0);
        if (msg.status == 3 && ground_alt_cm == 0) {
            ground_alt_cm = msg.altitude;
        }
        break;
    }

    case LOG_SIMSTATE_MSG: {
        struct log_AHRS msg;
        if(sizeof(msg) != f.length) {
            printf("Bad SIMSTATE length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
        break;
    }

    case LOG_BARO_MSG: {
        struct log_BARO msg;
        if(sizeof(msg) != f.length) {
            printf("Bad LOG_BARO length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.timestamp);
        baro.setHIL(msg.pressure, msg.temperature*0.01f);
        break;
    }

    case LOG_PARAMETER_MSG: {
        struct log_Parameter msg;
        if(sizeof(msg) != f.length) {
            printf("Bad LOG_PARAMETER length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        set_parameter(msg.name, msg.value);
        break;        
    }
        

    default:
        if (vehicle == VEHICLE_PLANE) {
            process_plane(f.type, data, f.length);
        } else if (vehicle == VEHICLE_COPTER) {
            process_copter(f.type, data, f.length);
        } else if (vehicle == VEHICLE_ROVER) {
            process_rover(f.type, data, f.length);
        }
        break;
    }

    type = f.type;

    return true;
}
bool AP_Gimbal::joints_near_limits()
{
    return fabsf(_measurement.joint_angles.x) > radians(40) || _measurement.joint_angles.y > radians(45) || _measurement.joint_angles.y < -radians(135);
}
int main(int argc, char *argv[])
{
	int i, j, k;
	int nh=0;              /*  counter for the total number of hours  */
	int nhd=0;             /*  counter for the number of hours of the actual day  */
	int month=0, day=0, jday=0, jday_hoy=0, last_day=0, last_month=0, last_jday=0;
	int status=6;             /*  indicates EOF of the input weather data file  */
	int fatal=0;              /*  indicates soon exit due to fatal error        */
	int azimuth_class=0;
	int *daylight_status;     /*  0=night hour, 1=sunrise/sunset hour, 2=innerday hour  */

	double time, centrum_time, *times;
	double irrad_glo = 0.0, irrad_beam_nor, irrad_beam_hor, irrad_dif;     /* in W/m² */
	double *irrads_glo, *irrads_beam_nor, *irrads_dif, *indices_glo, *indices_beam, sr_ss_indices_glo[3];
	double *irrads_glo_st, *irrads_glo_clear_st, *irrads_beam_nor_st, *irrads_dif_st, *indices_glo_st;
	double time_t, time_k, mean_glo_st, mean_beam_st, mean_dif_st, sum_beam_nor, sum_beam_hor, sum_dif;
	double sunrise_localtime, sunset_localtime;
	double solar_elevation, solar_azimuth, eccentricity_correction;
	double punk;              /*  indicates nice sound  */
	double previous_ligoh = 0, actual_ligoh;
	/*  ligoh = last index_glo of an hour: introduced to minimize discontinuities between subsequent hours  */

	
	
	if (argc == 1) {
		char *progname = fixargv0(argv[0]);
		fprintf(stdout, "%s: fatal error -  header file missing\n", progname);
		fprintf(stdout, "start program with:  %s  <header file>\n ", progname);
		exit(1);
	}

	if (!strcmp(argv[1], "-version")) {
		puts(VersionID);
		exit(0);
	}

	header=argv[1];
	read_in_genshortterm_header();

	



	/*printf("input_weather_data=%s\n",input_weather_data);
	  printf("input_weather_data_shortterm=%s\n",input_weather_data_shortterm);
	  printf("shortterm_timestep=%d\n",shortterm_timestep);
	  printf("input_units_genshortterm=%d\n",input_units_genshortterm);
	  printf("output_units_genshortterm=%d\n",output_units_genshortterm);
	  printf("solar_time=%d\n",solar_time);
	  printf("latitude=%f \nlongitude=%f \ntime_zone=%f\n",latitude,longitude,time_zone);
	  printf("site_elevation=%f\n",site_elevation);
	  printf("horizon_in=%d horizon_data_in=%s\n",horizon_in,horizon_data_in);
	  printf("horizon_out=%d horizon_data_out=%s\n",horizon_out,horizon_data_out);
	  printf("linke_estimation=%d\n",linke_estimation);*/

	HOURLY_DATA = open_input(input_weather_data);
	/* added by C. Reinhart                   */
	/* test whether input file has a header   */
	/* in case the is a header, it is skipped */
	fscanf(HOURLY_DATA,"%s", keyword);
  	if( !strcmp(keyword,"place") ){
		rewind(HOURLY_DATA);
		fgets(header_line_1,300,HOURLY_DATA);
		fgets(header_line_2,300,HOURLY_DATA);
		fgets(header_line_3,300,HOURLY_DATA);
		fgets(header_line_4,300,HOURLY_DATA);
		fgets(header_line_5,300,HOURLY_DATA);
		fgets(header_line_6,300,HOURLY_DATA);
		// get time step of input file
		fscanf(HOURLY_DATA,"%d %d %lf", &month, &day, &centrum_time);fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
		fscanf(HOURLY_DATA,"%d %d %lf", &month, &day, &time);fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
		test_input_time_step=(int)(60.0*fabs(time-centrum_time));
		//printf("The time step of the input file (%s) is %d minutes\n",input_weather_data, test_input_time_step);
		if(test_input_time_step != shortterm_timestep && test_input_time_step != 60 ){
		 	printf("wea_data_file does not have a 60 minute time step interval! %d\n",test_input_time_step);
		}
		rewind(HOURLY_DATA);
		fgets(header_line_1,300,HOURLY_DATA);
		fgets(header_line_2,300,HOURLY_DATA);
		fgets(header_line_3,300,HOURLY_DATA);
		fgets(header_line_4,300,HOURLY_DATA);
		fgets(header_line_5,300,HOURLY_DATA);
		fgets(header_line_6,300,HOURLY_DATA);
	}else{	// input file has no header
		rewind(HOURLY_DATA);
		// get time step of input file
		fscanf(HOURLY_DATA,"%d %d %lf", &month, &day, &centrum_time);fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
		fscanf(HOURLY_DATA,"%d %d %lf", &month, &day, &time);fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
		test_input_time_step=(int)(60.0*fabs(time-centrum_time));
		fprintf(stderr,"The time step of the inpt file (%s) is %d minutes\n",input_weather_data, test_input_time_step);
		if(test_input_time_step != shortterm_timestep && test_input_time_step != 60 ){
		 	fprintf(stderr,"wea_data_file does not have a 60 minute time step interval! %d\n",test_input_time_step);
		}
		rewind(HOURLY_DATA);
		sprintf(header_line_1,"place %s\n",input_weather_data);
		sprintf(header_line_2,"latitude %f\n",latitude);
		sprintf(header_line_3,"longitude %f\n",longitude);
		sprintf(header_line_4,"time_zone %f\n",time_zone);
		sprintf(header_line_5,"site_elevation %f\n",site_elevation);
		sprintf(header_line_6,"weather_data_file_units %d\n",output_units_genshortterm);

	}


	SHORT_TERM_DATA = open_output(input_weather_data_shortterm);
    //print file header
  	fprintf(SHORT_TERM_DATA,"%s", header_line_1);
  	fprintf(SHORT_TERM_DATA,"%s", header_line_2);
  	fprintf(SHORT_TERM_DATA,"%s", header_line_3);
  	fprintf(SHORT_TERM_DATA,"%s", header_line_4);
  	fprintf(SHORT_TERM_DATA,"%s", header_line_5);
  	fprintf(SHORT_TERM_DATA,"%s", header_line_6);


	if ((times = malloc(24 * sizeof(double))) == NULL) goto memerr;
	if ((irrads_glo = malloc(24 * sizeof(double))) == NULL) goto memerr;
	if ((irrads_beam_nor = malloc(24 * sizeof(double))) == NULL) goto memerr;
	if ((irrads_dif = malloc(24 * sizeof(double))) == NULL) goto memerr;
	if ((indices_glo = malloc(24 * sizeof(double))) == NULL) goto memerr;
	if ((indices_beam = malloc(24 * sizeof(double))) == NULL) goto memerr;
	if ((daylight_status = malloc(24 * sizeof(int))) == NULL) goto memerr;

	if ((irrads_glo_st = malloc(sph*sizeof(double))) == NULL) goto memerr;
	if ((irrads_glo_clear_st = malloc(sph*sizeof(double))) == NULL) goto memerr;
	if ((irrads_beam_nor_st = malloc(sph*sizeof(double))) == NULL) goto memerr;
	if ((irrads_dif_st = malloc(sph*sizeof(double))) == NULL) goto memerr;
	if ((indices_glo_st = malloc(sph*sizeof(double))) == NULL) goto memerr;


	if ( shortterm_timestep == test_input_time_step )      /*  no generation of shortterm data, but conversion of direct-hor to direct-norm irradiance  */
		{
			//fprintf(stderr,"ds_shortterm: message: input time step equals output time step.\n");
			while ( status > 0 )                       /*  as long as EOF is not reached  */
				{
					if ( input_units_genshortterm == 1 )
					{
						status = fscanf(HOURLY_DATA,"%d %d %lf %lf %lf", &month, &day, &time, &irrad_beam_nor, &irrad_dif);
						if(irrad_beam_nor<0|| irrad_dif<0)
							{
								status=-1;
								printf("FATAL ERROR: Negative direct or diffuse irradiance at month: %d, day: %d, time %.1f\n",month,day,time);
								printf("Generating cliamte file stopped.\n");
							}
					}
					if ( input_units_genshortterm == 2 )
					{
						status = fscanf(HOURLY_DATA,"%d %d %lf %lf %lf", &month, &day, &time, &irrad_beam_hor, &irrad_dif);
						if(irrad_beam_hor<0|| irrad_dif<0)
							{
								status=-1;
								printf("FATAL ERROR: Negative direct or diffuse irradiance at month: %d, day: %d, time %.1f\n",month,day,time);
								printf("Generating cliamte file stopped.\n");
							}
					}
					if ( status <= 0 )  goto end;

					if ( input_units_genshortterm == 2 )                             /*  calculate irrad_beam_nor  */
						{
							if ( irrad_beam_hor > 0 )
								{
									jday = jdate(month, day);
									sunrise_sunset_localtime ( latitude, longitude, time_zone, jday, &sunrise_localtime, &sunset_localtime );
									centrum_time=time;
									if ( fabs(time-sunrise_localtime) <= 0.5 )  centrum_time=sunrise_localtime+(time+0.5-sunrise_localtime)/2.0;
									if ( fabs(time-sunset_localtime) <= 0.5 )  centrum_time=time-0.5+(sunset_localtime-(time-0.5))/2.0;
									solar_elev_azi_ecc ( latitude, longitude, time_zone, jday, centrum_time, solar_time, &solar_elevation, &solar_azimuth, &eccentricity_correction);
									irrad_beam_nor = irrad_beam_hor / sin(radians(solar_elevation));
									if ( irrad_beam_nor < 0 )  irrad_beam_nor=0;
								}
							else irrad_beam_nor=0;
						}
					//}

					fprintf(SHORT_TERM_DATA,"%d %d %.3f %.0f %.0f\n", month, day, time, irrad_beam_nor, irrad_dif);
				}
		}


	else                                 /*  generation of 1-min short-term data according to modified Skartveit & Olseth  */
		{                                    /*  by Oliver Walkenhorst, November 2000                                          */

			if ( test_input_time_step < 60 )create60minTempFile();
			if ( horizon_in )
				{
					printf("reads in input horizon data ... \n");
					read_horizon_azimuth_data ( horizon_data_in, &horizon_azimuth_in[0] );
				}
			else  for ( i=0 ; i<36 ; i++ )  horizon_azimuth_in[i]=0;

			if ( horizon_out )
				{
					printf("reads in output horizon data ... \n");
					read_horizon_azimuth_data ( horizon_data_out, &horizon_azimuth_out[0] );
				}
			else  for ( i=0 ; i<36 ; i++ )  horizon_azimuth_out[i]=0;

			if ( linke_estimation )
				{
					//printf("\nEstimating monthly Linke Turbidities from hourly direct irradiances ... ");
					estimate_linke_factor_from_hourly_direct_irradiances();
					//for (i=0;i<12;i++)  printf("%.1f ",linke_turbidity_factor_am2[i]);
					//printf(" ");
				}

			rewind(HOURLY_DATA);
			/* added by C. Reinhart                   */
			/* test whether input file has a header   */
			/* in case the is a header, it is skipped */
			fscanf(HOURLY_DATA,"%s", keyword);
			if( !strcmp(keyword,"place") )
				{
					fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
					fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
					fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
					fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
					fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
					fscanf(HOURLY_DATA,"%*[^\n]");fscanf(HOURLY_DATA,"%*[\n\r]");
				}else{
					rewind(HOURLY_DATA);
				}



			while ( status > 0 )               /*  read data from the input weather file as long as EOF is not reached  */
				{
					if ( input_units_genshortterm == 1 )
						status = fscanf(HOURLY_DATA,"%d %d %lf %lf %lf", &month, &day, &time, &irrad_beam_nor, &irrad_dif);
					if ( input_units_genshortterm == 2 )
						status = fscanf(HOURLY_DATA,"%d %d %lf %lf %lf", &month, &day, &time, &irrad_beam_hor, &irrad_dif);
					if ( status <= 0 )  goto process_last_day;

					nh++;

					jday = jdate(month, day);

					if ( input_units_genshortterm == 1 )         /*  calculation of the global irradiance for the actual hour  */
						{
							if ( irrad_beam_nor > 0 )
								{
									sunrise_sunset_localtime ( latitude, longitude, time_zone, jday, &sunrise_localtime, &sunset_localtime );
									centrum_time=time;
									if ( fabs(time-sunrise_localtime) <= 0.5 )  centrum_time=sunrise_localtime+(time+0.5-sunrise_localtime)/2.0;
									if ( fabs(time-sunset_localtime) <= 0.5 )  centrum_time=time-0.5+(sunset_localtime-(time-0.5))/2.0;
									solar_elev_azi_ecc ( latitude, longitude, time_zone, jday, centrum_time, solar_time, &solar_elevation, &solar_azimuth, &eccentricity_correction);
									irrad_beam_hor = irrad_beam_nor * sin(radians(solar_elevation));
									if ( irrad_beam_hor < 0 )  irrad_beam_hor=0;
								}
							else irrad_beam_hor=0;

							irrad_glo=irrad_beam_hor+irrad_dif;
						}

					if ( input_units_genshortterm == 2 )         /*  calculation of the global irradiance for the actual hour  */
						{
							if ( irrad_beam_hor > 0 )
								{
									sunrise_sunset_localtime ( latitude, longitude, time_zone, jday, &sunrise_localtime, &sunset_localtime );
									centrum_time=time;
									if ( fabs(time-sunrise_localtime) <= 0.5 )  centrum_time=sunrise_localtime+(time+0.5-sunrise_localtime)/2.0;
									if ( fabs(time-sunset_localtime) <= 0.5 )  centrum_time=time-0.5+(sunset_localtime-(time-0.5))/2.0;
									solar_elev_azi_ecc ( latitude, longitude, time_zone, jday, centrum_time, solar_time, &solar_elevation, &solar_azimuth, &eccentricity_correction);
									irrad_beam_nor = irrad_beam_hor / sin(radians(solar_elevation));
									if ( irrad_beam_nor < 0 )  irrad_beam_nor=0;
								}
							else irrad_beam_nor=0;

							irrad_glo=irrad_beam_hor+irrad_dif;
						}


					/*  check irradiances and correct numbers if necessary  */
					if ( irrad_glo < 0 )
						{
							sprintf(errmsg, "irrad_glo=%f at month: %d day: %d time: %.3f has been replaced with 0", irrad_glo, month, day, time);
							error(WARNING, errmsg);
							irrad_glo=0.0;
						}
					if (irrad_glo > SOLAR_CONSTANT_E)
						{
							sprintf(errmsg, "irrad_glo=%f at month: %d day: %d time: %.3f has been replaced with %f\n", irrad_glo, month, day, time, SOLAR_CONSTANT_E);
							error(WARNING, errmsg);
							irrad_glo = SOLAR_CONSTANT_E;
						}

					if ( irrad_beam_nor < 0 )
						{
							sprintf(errmsg, "irrad_beam_nor=%e at month: %d day: %d time: %.3f has been replaced with %f\n", irrad_beam_nor, month, day, time, 0.0);
							error(WARNING, errmsg);
							irrad_beam_nor = 0.0;
						}
					if (irrad_beam_nor > SOLAR_CONSTANT_E)
						{
							sprintf(errmsg, "irrad_beam_nor=%f at month: %d day: %d time: %.3f has been replaced with %f\n", irrad_beam_nor, month, day, time, SOLAR_CONSTANT_E);
							error(WARNING, errmsg);
							irrad_beam_nor = SOLAR_CONSTANT_E;
						}

					if ( irrad_dif < 0 )
						{
							sprintf(errmsg, "irrad_dif=%f at month: %d day: %d time: %.3f has been replaced with %f\n", irrad_beam_nor, month, day, time, 0.0);
							error(WARNING, errmsg);
							irrad_dif = 0.0;
						}
					if (irrad_dif > SOLAR_CONSTANT_E)
						{
							sprintf(errmsg, "irrad_dif=%f at month: %d day: %d time: %.3f has been replaced with %f\n", irrad_beam_nor, month, day, time, SOLAR_CONSTANT_E);
							error(WARNING, errmsg);
							irrad_dif = SOLAR_CONSTANT_E;
						}

					if ( fatal == 1 )  exit(1);


					if ( last_jday == jday || nh == 1 )      /*  store the hourly irradiances of the actual day  */
						{
							times[nhd]=time;
							irrads_glo[nhd] = irrad_glo;
							irrads_beam_nor[nhd] = irrad_beam_nor;
							irrads_dif[nhd] = irrad_dif;
							nhd++;
						}

					else
						{
						process_last_day:                                     /*  process the last day  */
							{
								for ( i=0 ; i<nhd ; i++ )              /*  determine the daylight status of each hour  */
									{
										if ( i == 0 || i == nhd-1 )
											{
												if ( irrads_glo[i] < 0.001 )  daylight_status[i]=0;
												if ( irrads_glo[i] >= 0.001 )  daylight_status[i]=1;
											}
										if ( i > 0 && i<nhd-1 )
											{
												if ( irrads_glo[i-1] < 0.001 && irrads_glo[i] < 0.001 )  daylight_status[i]=0;
												if ( irrads_glo[i] < 0.001 && irrads_glo[i+1] < 0.001 )  daylight_status[i]=0;
												if ( irrads_glo[i-1] >= 0.001 && irrads_glo[i] < 0.001 && irrads_glo[i+1] >= 0.001 )
												{
													irrads_glo[i]=0.5*(irrads_glo[i-1]+irrads_glo[i+1]);
													sprintf(errmsg, "at %d %d %.3f global irradiance = 0 in between two hours with\n non-vanishing global irradiance: check your data and try again", last_month, last_day, times[i]);
													error(WARNING, errmsg);
												}

												if ( irrads_glo[i-1] < 0.001 && irrads_glo[i] >= 0.001 && irrads_glo[i+1] < 0.001 )
												{
													irrads_glo[i]=0.5*(irrads_glo[i-1]+irrads_glo[i+1]);
													sprintf(errmsg, "month=%d day=%d contains only one hour with non-vanishing global irradiance: remove this day from your input data file and try again", last_month, last_day);
													error(WARNING, errmsg);
												}
												if ( irrads_glo[i-1] < 0.001 && irrads_glo[i] >= 0.001 && irrads_glo[i+1] >= 0.001 )  daylight_status[i]=1;
												if ( irrads_glo[i-1] >= 0.001 && irrads_glo[i] >= 0.001 && irrads_glo[i+1] < 0.001 )  daylight_status[i]=1;
												if ( irrads_glo[i-1] >= 0.001 && irrads_glo[i] >= 0.001 && irrads_glo[i+1] >= 0.001 )  daylight_status[i]=2;
											}

										if ( daylight_status[i] > 0 )        /*  calculate the clearness indices  */
											{
												glo_and_beam_indices_hour ( latitude, longitude, time_zone, last_jday, times[i], solar_time, irrads_glo[i], irrads_beam_nor[i], &indices_glo[i], &indices_beam[i] );
												if ( i < nhd-1 && times[i+1]-times[i] > 1.5 )
												{
													sprintf(errmsg, "at %d %d %.3f the time difference to the subsequent hour is greater than 1.5: check your data and try again (innerday time differences should equal 1)", last_month, last_day, times[i]);
													error(USER, errmsg);
												}
											}
									}

								for ( i=0 ; i<nhd ; i++ )                /*  process each hour  */
									{
										if ( daylight_status[i] == 0 )         /*  print zeros for hours without global irradiance  */
											{
												for ( j=1 ; j<=(60/shortterm_timestep) ; j++ )
													{
														time_t = times[i] - 0.5 + ( j - 0.5 ) / (60/shortterm_timestep);
														if ( output_units_genshortterm == 1 )
															fprintf ( SHORT_TERM_DATA,"%d %d %.3f %.0f %.0f\n", last_month, last_day, time_t, 0.0, 0.0 );
														if ( output_units_genshortterm == 2 )
															{
																jday_hoy = jdate(last_month, last_day);
																solar_elev_azi_ecc (latitude, longitude, time_zone, jday_hoy, time_t, solar_time, &solar_elevation, &solar_azimuth, &eccentricity_correction);

																fprintf ( SHORT_TERM_DATA,"%d %d %.3f %.0f %.0f\n", last_month, last_day, time_t, 0.0, 0.0 );
															}
													}
											}

										else                                  /*  generate short-term irradiances for daylight hours  */
											{
												irrads_clear_st ( latitude, longitude, time_zone, last_jday, times[i], solar_time, sph, irrads_glo_clear_st);

												if ( daylight_status[i] == 1 && times[i] < 12 )             /*  sunrise hour  */
													{
														sr_ss_indices_glo[0] = indices_glo[i+1];
														sr_ss_indices_glo[1] = indices_glo[i];
														sr_ss_indices_glo[2] = indices_glo[i+1];
														skartveit ( sr_ss_indices_glo, indices_beam[i], sph, previous_ligoh, indices_glo_st, &actual_ligoh );
													}

												if ( daylight_status[i] == 1 && times[i] >= 12 )            /*  sunset hour  */
													{
														sr_ss_indices_glo[0] = indices_glo[i-1];
														sr_ss_indices_glo[1] = indices_glo[i];
														sr_ss_indices_glo[2] = indices_glo[i-1];
														skartveit ( sr_ss_indices_glo, indices_beam[i], sph, previous_ligoh, indices_glo_st, &actual_ligoh );
													}

												if ( daylight_status[i] == 2 )                             /*  innerday hours  */
													{
														if ( irrads_glo[i] <= 0.001 )
															{
																irrads_glo[i]=0.5*(irrads_glo[i-1]+irrads_glo[i+1]);
																sprintf(errmsg, "at month=%d day=%d time=%.3f should be non-vanishing global irradiance: check your input file and try again", last_month, last_day, times[i]);
																error(USER, errmsg);
															}
														else  skartveit ( &indices_glo[i-1], indices_beam[i], sph, previous_ligoh, indices_glo_st, &actual_ligoh );
													}

												previous_ligoh = actual_ligoh;

												for ( j=1 ; j<=sph ; j++ )  irrads_glo_st[j-1] = indices_glo_st[j-1] * irrads_glo_clear_st[j-1];

												mean_glo_st = mean ( sph, &irrads_glo_st[0] );

												if ( mean_glo_st > 0 )                /*  global renormalization to the given hourly mean value  */
													for ( j=1 ; j<=sph ; j++ )
														irrads_glo_st[j-1] = irrads_glo[i] / mean_glo_st * irrads_glo_st[j-1];

												for (j = 1; j <= sph; j++)  if (irrads_glo_st[j - 1] > SOLAR_CONSTANT_E)  irrads_glo_st[j - 1] = SOLAR_CONSTANT_E;

												for ( j=1 ; j<=sph ; j++ )        /*  Reindl diffuse fraction estimation  */
													{
														solar_elev_azi_ecc (latitude, longitude, time_zone, last_jday, times[i]-0.5+(j-0.5)/sph, solar_time, &solar_elevation, &solar_azimuth, &eccentricity_correction);
														irrads_dif_st[j-1]= diffuse_fraction(irrads_glo_st[j-1],solar_elevation,eccentricity_correction)*irrads_glo_st[j-1];

														if ( solar_azimuth < 0 )  azimuth_class = ((int)solar_azimuth)/10 + 17;
														else   azimuth_class = ((int)solar_azimuth)/10 + 18;

														if ( solar_elevation > horizon_azimuth_out[azimuth_class] )
															irrads_beam_nor_st[j - 1] = (irrads_glo_st[j - 1] - irrads_dif_st[j - 1]) / sin(radians(solar_elevation));
														else
															{
																irrads_beam_nor_st[j-1]=0;
																irrads_dif_st[j-1]=irrads_glo_st[j-1];
															}

														if (irrads_beam_nor_st[j - 1] > SOLAR_CONSTANT_E)  irrads_beam_nor_st[j - 1] = SOLAR_CONSTANT_E;
													}

												mean_beam_st = mean ( sph, &irrads_beam_nor_st[0] );
												mean_dif_st = mean ( sph, &irrads_dif_st[0] );

												if ( mean_beam_st > 0 )        /*  beam renormalization to the given hourly mean value  */
													for ( j=1 ; j<=sph ; j++ )
														irrads_beam_nor_st[j-1] = irrads_beam_nor[i] / mean_beam_st * irrads_beam_nor_st[j-1];


												if ( daylight_status[i] == 1 ) { /*  Tito  */
													k=0;
													for ( j=1 ; j<=sph ; j++ ){
														if( irrads_dif_st[j-1]>0.01){ k++;}
													}
													if( (k+30) < 60 )
														irrads_dif[i]*=(k*1.0/(k+30.0));
												}

												if ( mean_dif_st > 0 )         /*  diffuse renormalization to the given hourly mean value  */
													for ( j=1 ; j<=sph ; j++ )
														irrads_dif_st[j-1] = irrads_dif[i] / mean_dif_st * irrads_dif_st[j-1];

												for ( j=1 ; j<=sph ; j++ )
													if (irrads_beam_nor_st[j - 1] > SOLAR_CONSTANT_E)  irrads_beam_nor_st[j - 1] = SOLAR_CONSTANT_E;

												for ( j=1 ; j<=(60/shortterm_timestep) ; j++ )
													{
														time_t = times[i] - 0.5 + ( j - 0.5 ) / (60/shortterm_timestep);
														if ( shortterm_timestep == 1 )
															{
																if ( output_units_genshortterm == 1 )
																	fprintf ( SHORT_TERM_DATA,"%d %d %.3f %.0f %.0f\n", last_month, last_day, time_t, irrads_beam_nor_st[j-1], irrads_dif_st[j-1] );
																if ( output_units_genshortterm == 2 )
																	{
																		solar_elev_azi_ecc (latitude, longitude, time_zone, last_jday, time_t, solar_time,&solar_elevation, &solar_azimuth, &eccentricity_correction);
																		/*if ( solar_elevation < 0 )  solar_elevation=0;*/
																		punk = solar_elevation;
																		if ( solar_elevation < 0 )  punk=0;
																		fprintf(SHORT_TERM_DATA, "%.3f %.0f %.0f %.3f %.3f\n", (last_jday - 1) * 24 + time_t, irrads_beam_nor_st[j - 1] * sin(radians(punk)), irrads_dif_st[j - 1], solar_elevation, solar_azimuth);
																	}
															}
														else
															{
																if ( output_units_genshortterm == 1 )
																	{
																		sum_beam_nor=0;
																		sum_dif=0;
																		for ( k=(j-1)*shortterm_timestep ; k<j*shortterm_timestep ; k++ )
																			{
																				sum_beam_nor+=irrads_beam_nor_st[k];
																				sum_dif+=irrads_dif_st[k];
																			}
																		fprintf ( SHORT_TERM_DATA,"%d %d %.3f %.0f %.0f\n",last_month, last_day, time_t, sum_beam_nor/shortterm_timestep, sum_dif/shortterm_timestep );
																	}

																if ( output_units_genshortterm == 2 )
																	{
																		sum_beam_hor=0;
																		sum_dif=0;
																		for ( k=(j-1)*shortterm_timestep ; k<j*shortterm_timestep ; k++ )
																			{
																				time_k = times[i] - 0.5 + ( k + 0.5 ) / 60;
																				solar_elev_azi_ecc (latitude, longitude, time_zone, last_jday, time_k, solar_time,&solar_elevation, &solar_azimuth, &eccentricity_correction);
																				if ( solar_elevation < 0 )  solar_elevation=0;
																				sum_beam_hor += irrads_beam_nor_st[k] * sin(radians(solar_elevation));
																				sum_dif+=irrads_dif_st[k];
																			}
																		fprintf ( SHORT_TERM_DATA,"%.3f %.0f %.0f\n", (last_jday-1)*24+time_t, sum_beam_hor/shortterm_timestep, sum_dif/shortterm_timestep );
																	}
															}
													}
											}
									}

								if ( status <= 0 )  goto end;

								times[0]=time;
								irrads_glo[0] = irrad_glo;
								irrads_beam_nor[0] = irrad_beam_nor;
								irrads_dif[0] = irrad_dif;
								nhd=1;
							}
						}

					last_day=day;
					last_month=month;
					last_jday=jday;
				}
		}

 end:
	{
		close_file(HOURLY_DATA);
		close_file(SHORT_TERM_DATA);
	}
	if (day!=31 || month !=12)
	{
		printf("WARNING - Incomplete input climate file (%s)! The file ends on month %d and day %d.\n",input_weather_data,month,day);
		printf("Please review the output file before proceedingto Step3 using SITE>>OPEN CLIMATE FILE IN TEXT EDITOR.\n");
	} else
	{
			printf("DS_SHORTTERM - A climate file with a time step of %d minutes has been generated under %s.\n\n",time_step,input_weather_data_shortterm);

	}
	return 0;
memerr:
	error(SYSTEM, "out of memory in function main");
}