static void calcTileIllum(UDWORD tileX, UDWORD tileY)
{
/* The number or normals that we got is in numNormals*/
Vector3f finalVector = {0.0f, 0.0f, 0.0f};
unsigned int i, val;
int dotProduct;
unsigned int numNormals = 0; // How many normals have we got?
Vector3f normals[8]; // Maximum 8 possible normals
/* Quadrants look like:-
*
*
0 * 1
*
*
**********V**********
*
*
3 * 2
*
*
*/
/* Do quadrant 0 - tile that's above and left*/
normalsOnTile(tileX-1, tileY-1, 0, &numNormals, normals);
/* Do quadrant 1 - tile that's above and right*/
normalsOnTile(tileX, tileY-1, 1, &numNormals, normals);
/* Do quadrant 2 - tile that's down and right*/
normalsOnTile(tileX, tileY, 2, &numNormals, normals);
/* Do quadrant 3 - tile that's down and left*/
normalsOnTile(tileX-1, tileY, 3, &numNormals, normals);
for(i = 0; i < numNormals; i++)
{
finalVector = Vector3f_Add(finalVector, normals[i]);
}
dotProduct = Vector3f_ScalarP(Vector3f_Normalise(finalVector), theSun);
val = abs(dotProduct) / 16;
if (val == 0) val = 1;
if (val > 254) val = 254;
mapTile(tileX, tileY)->illumination = val;
}
/**********************************************************************************************************
*函 数 名: AttitudeInitAlignment
*功能说明: 姿态初始对准
*形 参: 横滚俯仰的卡尔曼结构体指针 航向的卡尔曼结构体指针 加速度测量值 磁场强度测量值
*返 回 值: 对准完成状态
**********************************************************************************************************/
int8_t AttitudeInitAlignment(Kalman_t* rollPitch, Kalman_t* yaw, Vector3f_t acc, Vector3f_t mag)
{
static int16_t alignCnt = 200;
static Vector3f_t accSum, magSum;
static uint8_t alignFinishFlag = 0;
if(alignFinishFlag)
{
return 1;
}
if(alignCnt > 0)
{
//传感器采样值累加
accSum = Vector3f_Add(accSum, acc);
magSum = Vector3f_Add(magSum, mag);
alignCnt--;
return 0;
}
else
{
//求平均值
rollPitch->state.x = accSum.x / 200;
rollPitch->state.y = accSum.y / 200;
rollPitch->state.z = accSum.z / 200;
yaw->state.x = magSum.x / 200;
yaw->state.y = magSum.y / 200;
yaw->state.z = magSum.z / 200;
alignFinishFlag = 1;
return 1;
}
}
