/**********************************************************************************************************
*函 数 名: 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
}
