// 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); }
/* * 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); }
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); } }
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, ¤t); //@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; }
/* 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(); }
/// 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); }
// 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); }
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; } }
/* 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, "DATA@", 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; }
// 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; }
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); }
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(); }
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); }
/// 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); }
void SpotLight::setInnerCutoff(const Real &_cutoff) noexcept { m_innerCutoffAngle=cos(radians(_cutoff)); }
// 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; } }
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 */
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; } }
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; }
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(); } } }
constexpr angle_t<long double> operator "" _deg (unsigned long long deg) { return angle_t<long double>{radians(deg)}; }
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); }
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, ¢rum_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, ¢rum_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"); }