/********************************************************************************************************** *函 数 名: AttitudeEstimate *功能说明: 姿态估计 *形 参: 角速度测量值 加速度测量值 磁场强度测量值 *返 回 值: 无 **********************************************************************************************************/ void AttitudeEstimate(Vector3f_t gyro, Vector3f_t acc, Vector3f_t mag) { Vector3f_t accCompensate; static uint64_t previousT; float deltaT = (GetSysTimeUs() - previousT) * 1e-6; deltaT = ConstrainFloat(deltaT, 0.0005, 0.002); previousT = GetSysTimeUs(); //姿态初始对准 if(!AttitudeInitAlignment(&kalmanRollPitch, &kalmanYaw, acc, mag)) return; //运动加速度补偿 accCompensate = AccSportCompensate(acc, GetSportAccEf(), ahrs.angle, ahrs.accBfOffset); //向心加速度误差补偿 accCompensate = Vector3f_Sub(accCompensate, ahrs.centripetalAccBf); //加速度零偏补偿 accCompensate = Vector3f_Sub(accCompensate, ahrs.accBfOffset); //姿态更新 AttitudeEstimateUpdate(&ahrs.angle, gyro, accCompensate, mag, deltaT); //计算导航系下的运动加速度 TransAccToEarthFrame(ahrs.angle, acc, &ahrs.accEf, &ahrs.accEfLpf, &ahrs.accBfOffset); //转换角速度至导航系 GyroEfUpdate(gyro, ahrs.angle, &ahrs.gyroEf); //计算导航系下的向心加速度 CentripetalAccUpdate(&ahrs.centripetalAcc, GetCopterVelocity(), ahrs.gyroEf.z); //转换向心加速度至机体系,用于姿态更新补偿 EarthFrameToBodyFrame(ahrs.angle, ahrs.centripetalAcc, &ahrs.centripetalAccBf); }
/********************************************************************************************************** *函 数 名: AccSportCompensate *功能说明: 运动加速度补偿 *形 参: 加速度 地理系运动加速度 姿态角 机体系加速度零偏 *返 回 值: 经过补偿的加速度 **********************************************************************************************************/ static Vector3f_t AccSportCompensate(Vector3f_t acc, Vector3f_t sportAccEf, Vector3f_t angle, Vector3f_t accBfOffset) { Vector3f_t sportAccBf; //转换运动加速度到机体坐标系 EarthFrameToBodyFrame(angle, sportAccEf, &sportAccBf); //减去加速度零偏 sportAccBf = Vector3f_Sub(sportAccBf, accBfOffset); //应用死区 sportAccBf.x = ApplyDeadbandFloat(sportAccBf.x, 0.03f); sportAccBf.y = ApplyDeadbandFloat(sportAccBf.y, 0.03f); sportAccBf.z = ApplyDeadbandFloat(sportAccBf.z, 0.03f); //补偿到姿态估计主回路中的加速度 acc.x = acc.x - sportAccBf.x * 0.95f; acc.y = acc.y - sportAccBf.y * 0.95f; acc.z = acc.z - sportAccBf.z * 0.95f; return acc; }
/// Draw the shadow for a shape static void pie_DrawShadow(iIMDShape *shape, int flag, int flag_data, Vector3f* light) { unsigned int i, j, n; Vector3f *pVertices; iIMDPoly *pPolys; unsigned int edge_count = 0; static EDGE *edgelist = NULL; static unsigned int edgelistsize = 256; EDGE *drawlist = NULL; if(!edgelist) { edgelist = (EDGE*)malloc(sizeof(EDGE)*edgelistsize); } pVertices = shape->points; if( flag & pie_STATIC_SHADOW && shape->shadowEdgeList ) { drawlist = shape->shadowEdgeList; edge_count = shape->nShadowEdges; } else { for (i = 0, pPolys = shape->polys; i < shape->npolys; ++i, ++pPolys) { Vector3f p[3], v[2], normal = {0.0f, 0.0f, 0.0f}; VERTEXID current, first; for(j = 0; j < 3; j++) { current = pPolys->pindex[j]; p[j] = Vector3f_Init(pVertices[current].x, scale_y(pVertices[current].y, flag, flag_data), pVertices[current].z); } v[0] = Vector3f_Sub(p[2], p[0]); v[1] = Vector3f_Sub(p[1], p[0]); normal = Vector3f_CrossP(v[0], v[1]); if (Vector3f_ScalarP(normal, *light) > 0) { first = pPolys->pindex[0]; for (n = 1; n < pPolys->npnts; n++) { // link to the previous vertex addToEdgeList(pPolys->pindex[n-1], pPolys->pindex[n], edgelist, &edge_count, pVertices); // check if the edgelist is still large enough if(edge_count >= edgelistsize-1) { // enlarge EDGE* newstack; edgelistsize *= 2; newstack = realloc(edgelist, sizeof(EDGE) * edgelistsize); if (newstack == NULL) { debug(LOG_FATAL, "pie_DrawShadow: Out of memory!"); abort(); return; } edgelist = newstack; debug(LOG_WARNING, "new edge list size: %u", edgelistsize); } } // back to the first addToEdgeList(pPolys->pindex[pPolys->npnts-1], first, edgelist, &edge_count, pVertices); } } //debug(LOG_WARNING, "we have %i edges", edge_count); drawlist = edgelist; if(flag & pie_STATIC_SHADOW) { // first compact the current edgelist for(i = 0, j = 0; i < edge_count; i++) { if(edgelist[i].from < 0) { continue; } edgelist[j] = edgelist[i]; j++; } edge_count = j; // then store it in the imd shape->nShadowEdges = edge_count; shape->shadowEdgeList = realloc(shape->shadowEdgeList, sizeof(EDGE) * shape->nShadowEdges); memcpy(shape->shadowEdgeList, edgelist, sizeof(EDGE) * shape->nShadowEdges); } } // draw the shadow volume glBegin(GL_QUADS); for(i=0;i<edge_count;i++) { int a = drawlist[i].from, b = drawlist[i].to; if(a < 0) { continue; } glVertex3f(pVertices[b].x, scale_y(pVertices[b].y, flag, flag_data), pVertices[b].z); glVertex3f(pVertices[b].x+light->x, scale_y(pVertices[b].y, flag, flag_data)+light->y, pVertices[b].z+light->z); glVertex3f(pVertices[a].x+light->x, scale_y(pVertices[a].y, flag, flag_data)+light->y, pVertices[a].z+light->z); glVertex3f(pVertices[a].x, scale_y(pVertices[a].y, flag, flag_data), pVertices[a].z); } glEnd(); #ifdef SHOW_SHADOW_EDGES glDisable(GL_DEPTH_TEST); glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); glColor4ub(0xFF, 0, 0, 0xFF); glBegin(GL_LINES); for(i = 0; i < edge_count; i++) { int a = drawlist[i].from, b = drawlist[i].to; if(a < 0) { continue; } glVertex3f(pVertices[b].x, scale_y(pVertices[b].y, flag, flag_data), pVertices[b].z); glVertex3f(pVertices[a].x, scale_y(pVertices[a].y, flag, flag_data), pVertices[a].z); } glEnd(); glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE); glEnable(GL_DEPTH_TEST); #endif }