void land_dynamicshadow(land_t *land,float *light,thing_t *thing) {
int i,j,k,x0,x1,y0,y1;
float dir[3],up[3],dx[3],dy[3],blight[3],bpos[3],point[3],min[3],max[3];
VectorSub(thing->center,light,dir);
VectorNormalize(dir,dir);
VectorSet(0,0,1,up);
VectorCrossProduct(up,dir,dx);
VectorCrossProduct(dir,dx,dy);
VectorNormalize(dx,dx);
VectorNormalize(dy,dy);
VectorScale(dx,thing->radius,dx);
VectorScale(dy,thing->radius,dy);
VectorSet(1000000,1000000,1000000,min);
VectorSet(-1000000,-1000000,-1000000,max);
for(i = 0; i < 4; i++) {
if(i == 0 || i == 1) {
VectorAdd(light,dx,blight);
VectorAdd(thing->center,dx,bpos);
} else {
VectorSub(light,dx,blight);
VectorSub(thing->center,dx,bpos);
}
if(i == 0 || i == 2) {
VectorSub(blight,dy,blight);
VectorSub(bpos,dy,bpos);
} else {
VectorAdd(blight,dy,blight);
VectorAdd(bpos,dy,bpos);
}
land_crossline(land,blight,bpos,point);
for(j = 0; j < 2; j++) {
if(point[j] < min[j]) min[j] = point[j];
if(point[j] > max[j]) max[j] = point[j];
}
}
x0 = (int)(min[0] / land->step);
y0 = (int)(min[1] / land->step);
x1 = (int)(max[0] / land->step) + 1;
y1 = (int)(max[1] / land->step) + 1;
if(x0 < 0) x0 = 0;
if(x0 > land->width - 1) x0 = land->width - 1;
if(y0 < 0) y0 = 0;
if(y0 > land->height - 1) y0 = land->height - 1;
if(x1 < 0) x1 = 0;
if(x1 > land->width - 1) x1 = land->width - 1;
if(y1 < 0) y1 = 0;
if(y1 > land->height - 1) y1 = land->height - 1;
glBegin(GL_TRIANGLES);
for(j = y0; j < y1; j++)
for(i = x0; i < x1; i++) {
k = land->width * j + i;
glVertex3fv(land->vertex[k].v);
glVertex3fv(land->vertex[k + 1].v);
glVertex3fv(land->vertex[k + land->width].v);
glVertex3fv(land->vertex[k + land->width + 1].v);
glVertex3fv(land->vertex[k + land->width].v);
glVertex3fv(land->vertex[k + 1].v);
}
glEnd();
}
land_node_vertex_t *land_create_mesh(land_t *land,land_config_t *config) {
unsigned char *heightmap;
int i,j,k,l,width,height;
land_node_vertex_t *vertex;
float p00[3],p10[3],p01[3],v10[3],v01[3],n[3];
heightmap = NULL;
if(strstr(config->heightmap,".tga") || strstr(config->heightmap,".TGA"))
heightmap = LoadTGA(config->heightmap,&width,&height);
else if(strstr(config->heightmap,".jpg") || strstr(config->heightmap,".JPG"))
heightmap = LoadJPEG(config->heightmap,&width,&height);
if(!heightmap) return NULL;
vertex = (land_node_vertex_t*)malloc(sizeof(land_node_vertex_t) * width * height);
if(!vertex) return NULL;
land->vertex = (land_vertex_t*)malloc(sizeof(land_vertex_t) * width * height);
if(!land->vertex) return NULL;
for(j = 0, k = 0, l = 0; j < height; j++) // create mesh
for(i = 0; i < width; i++, k++, l += 4) {
VectorSet((float)i * config->step,(float)j * config->step,(float)heightmap[l] / 255.0 * config->altitude,vertex[k].v);
VectorSet(0,0,0,vertex[k].n);
vertex[k].t0[0] = vertex[k].t1[0] = (float)i / (float)(width - 1);
vertex[k].t0[1] = vertex[k].t1[1] = (float)j / (float)(height - 1);
vertex[k].t0[0] *= (float)config->num_base;
vertex[k].t0[1] *= (float)config->num_base;
vertex[k].t1[0] *= (float)config->num_detail;
vertex[k].t1[1] *= (float)config->num_detail;
VectorCopy(vertex[k].v,land->vertex[k].v);
}
for(j = 0, k = 0; j < height - 1; j++, k++) // calculate normals
for(i = 0; i < width - 1; i++, k++) {
VectorCopy(vertex[k].v,p00);
VectorCopy(vertex[k + 1].v,p10);
VectorCopy(vertex[k + width].v,p01);
VectorSub(p10,p00,v10);
VectorSub(p01,p00,v01);
VectorCrossProduct(v10,v01,n);
VectorNormalize(n,n);
VectorAdd(vertex[k].n,n,vertex[k].n);
VectorAdd(vertex[k + 1].n,n,vertex[k + 1].n);
VectorAdd(vertex[k + width].n,n,vertex[k + width].n);
VectorCopy(vertex[k + width + 1].v,p00);
VectorCopy(vertex[k + width].v,p10);
VectorCopy(vertex[k + 1].v,p01);
VectorSub(p10,p00,v10);
VectorSub(p01,p00,v01);
VectorCrossProduct(v10,v01,n);
VectorNormalize(n,n);
VectorAdd(vertex[k + width + 1].n,n,vertex[k + width + 1].n);
VectorAdd(vertex[k + width].n,n,vertex[k + width].n);
VectorAdd(vertex[k + 1].n,n,vertex[k + 1].n);
}
for(i = 0; i < width * height; i++) // normalize normals
VectorNormalize(vertex[i].n,vertex[i].n);
land->width = width;
land->height = height;
land->step = config->step;
land->lod = config->lod;
free(heightmap);
return vertex;
}
float land_height(land_t *land,float *point) {
int i,j,k;
float x,y,dot0,dot1,p00[3],p10[3],p01[3],v10[3],v01[3],point0[3],point1[3],plane[4];
x = point[0] / land->step;
y = point[1] / land->step;
i = (int)x;
j = (int)y;
if(i < 0) i = 0;
else if(i > land->width - 2) i = land->width - 2;
if(j < 0) j = 0;
else if(j > land->height - 2) j = land->height - 2;
k = land->width * j + i;
if(x + y >= 1 + (int)x + (int)y) { // 1st or 2nd triangle
VectorCopy(land->vertex[k + land->width + 1].v,p00);
VectorCopy(land->vertex[k + land->width].v,p10);
VectorCopy(land->vertex[k + 1].v,p01);
} else {
VectorCopy(land->vertex[k].v,p00);
VectorCopy(land->vertex[k + 1].v,p10);
VectorCopy(land->vertex[k + land->width].v,p01);
}
VectorSub(p10,p00,v10);
VectorSub(p01,p00,v01);
VectorCrossProduct(v10,v01,plane);
plane[3] = -VectorDotProduct(plane,p00);
VectorCopy(point,point0);
VectorCopy(point,point1);
point0[2] = 0;
point1[2] = 1;
dot0 = -VectorDotProduct(plane,point0);
dot1 = -VectorDotProduct(plane,point1);
return (point0[2] + (point1[2] - point0[2]) *
(plane[3] - dot0) / (dot1 - dot0));
}
// LordHavoc: like AngleVectors, but taking a forward vector instead of angles, useful!
void VectorVectors(const vec3_t forward, vec3_t right, vec3_t up) {
right[0] = forward[2];
right[1] = -forward[0];
right[2] = forward[1];
float d = DotProduct(forward, right);
right[0] -= d * forward[0];
right[1] -= d * forward[1];
right[2] -= d * forward[2];
VectorNormalize(right);
VectorCrossProduct(right, forward, up);
}
void BuildQuaternion ( const vec3_t in_base,
const vec3_t in_vector,
vec4_t out_quat )
{
double theta;
double s, c;
const float error = 0.0001f;
if ( (in_vector [0] + 1.0f) < error )
{
out_quat [0] = 0.0f;
out_quat [1] = 1.0f;
out_quat [2] = 0.0f;
out_quat [3] = 0.0f;
}
else if ( (1.0f - in_vector [0]) < error )
{
out_quat [0] = 0.0f;
out_quat [1] = 0.0f;
out_quat [2] = 0.0f;
out_quat [3] = 1.0f;
}
else
{
VectorCrossProduct ( g_nVector, in_base, in_vector );
D3DXVec3Normalize ( (D3DXVECTOR3 *)g_nVector, (D3DXVECTOR3 *)g_nVector );
theta = acos ( VectorDotProduct ( in_base, in_vector ) / VectorLength ( in_vector ) );
s = sin ( theta / 2 );
c = cos ( theta / 2 );
out_quat [0] = (float)(s * g_nVector [0]);
out_quat [1] = (float)(s * g_nVector [1]);
out_quat [2] = (float)(s * g_nVector [2]);
out_quat [3] = (float)c;
D3DXQuaternionNormalize ( (D3DXQUATERNION *)out_quat, (D3DXQUATERNION *)out_quat );
}
return;
}
extern bool IntersectionPointBettwenTwoSegments( const Point* beginSegmentA, const Point* endSegmentA,
const Point* beginSegmentB, const Point* endSegementB,
Point* intersectionPoint )
{
//如果有交点，则求交点并将交点插入正确的位置
//两相交线段求交点
//从而可以求出p的坐标 具体算法可以参考图形学算法
Vector segementA = {endSegmentA->x - beginSegmentA->x, 0.0f, endSegmentA->z - beginSegmentA->z};
Vector normalLineofSegementA;
VectorCrossProduct(&normalLineofSegementA, &segementA, &StandardVector);
Vector aBegin2BBegin = {beginSegmentB->x - beginSegmentA->x, 0.0f, beginSegmentB->z - beginSegmentA->z};
Vector aBegin2BEnd = {endSegementB->x - beginSegmentA->x, 0.0f, endSegementB->z - beginSegmentA->z};
float cosof2BeginPointB = VectorDotProduct(&aBegin2BBegin, &normalLineofSegementA);
float cosof2EndPointB = VectorDotProduct(&aBegin2BEnd, &normalLineofSegementA);
float tValue =cosof2BeginPointB;
tValue /= (cosof2BeginPointB - cosof2EndPointB);
if (tValue < 0.0 || tValue > 1.0)
{
return false;
}
float insertionX = (float)(beginSegmentB->x + tValue * (endSegementB->x - beginSegmentB->x));
float insertionZ = (float)(beginSegmentB->z + tValue * (endSegementB->z - beginSegmentB->z));
//检查点是否在线段的延长线上
Rect checkRect;
const Point point = {insertionX, insertionZ};
MakeRectByPoint(&checkRect, beginSegmentA, endSegmentA);
if (true != InRect(&checkRect, &point))
{
return false;
}
if(NULL != intersectionPoint)
{
*intersectionPoint = point;
}
return true;
}
//==================================================================================
uint DCMUpdate( ulong GyroTS, // Gyro timestamp
Vector* Gyro, // Gyro measurement
//--------------------------------------
ulong AccTS, // Acc timestamp
Vector* Acc, // Acc measurement
//--------------------------------------
ulong MagTS, // Mag timestamp
Vector* Mag, // Mag measurement
//--------------------------------------
DCMData* IMUResult)
{
//==========================================================================
if (_DCMInit != 1)
return DCM_NOTINIT;
//==========================================================================
// Calculate integration time interval for respective sensors
//--------------------------------------------------------------------------
float GyroDT = TMRGetTSRate() * (GyroTS - _DCM_GyroTS);
float AccDT = TMRGetTSRate() * (AccTS - _DCM_AccTS);
float MagDT = TMRGetTSRate() * (MagTS - _DCM_MagTS);
//--------------------------------------------------------------------------
// Update last sample timestamps for respective sensors
//--------------------------------------------------------------------------
_DCM_GyroTS = GyroTS;
_DCM_AccTS = AccTS;
_DCM_MagTS = MagTS;
//==========================================================================
//==========================================================================
// Accelerometer-based and Magnetometer-based rotation correction vectors
//--------------------------------------------------------------------------
Vector DriftComp; // Current step total drift compensation
VectorSet(0.0, 0.0, 0.0, &DriftComp);
//--------------------------------------------------------------------------
if (TRUE == _DCM_UseAcc)
{
// <editor-fold defaultstate="collapsed" desc="Calculate Acc-based correction">
//--------------------------------------------------------------------------
// Local variables used in calculations of the Accelerometer-based correction
//--------------------------------------------------------------------------
Vector EarthZ; // Earth vertical in the Body frame
Vector AccNorm; // Normalized Accelerometer measurement
// in the Body frame
Vector AccError; // Rotational "error" between the Earth
// vertical (in Body frame) and
// acceleration vector.
Vector AccPterm; // Proportional term
Vector AccIterm; // Current step component of Integral term
//--------------------------------------------------------------------------
// Calculating Accelerometer-based correction
//--------------------------------------------------------------------------
// Extract Z-axis in the Earth frame as seen from the Body frame
// from the DCM
DCMEarthZ(&EarthZ);
//---------------------------------------------------------
// We have to "normalize" gravity vector so that calculated
// error proportional to rotation error and independent of
// the level of current acceleration
//---------------------------------------------------------
VectorNormalize(Acc, &AccNorm);
//---------------------------------------------------------
// Cross-product of the axis Z in the Earth frame (as seen
// from the Body frame) of the DCM with the normalized
// Accelerometer measurement (true Z-axis of Earth frame
// in the Body frame ignoring linear accelerations) is the
// ERROR accumulated in DCM and defines how much the Rotation
// Matrix need to be rotated so that these vectors match.
//---------------------------------------------------------
VectorCrossProduct(&EarthZ, &AccNorm, &AccError);
//---------------------------------------------------------
// Accelerometer correction P-Term
//---------------------------------------------------------
VectorScale(&AccError, _DCMAccKp, &AccPterm);
//---------------------------------------------------------
// Accelerometer correction I-Term
//---------------------------------------------------------
VectorScale(&AccError, (_DCMAccKi * AccDT), &AccIterm);
VectorAdd(&_DCMAccIterm, &AccIterm, &_DCMAccIterm);
//---------------------------------------------------------
// Adjust drift compensation vector with Accelerometer-
// based correction factors
//---------------------------------------------------------
VectorAdd(&DriftComp, &AccPterm, &DriftComp);
VectorAdd(&DriftComp, &_DCMAccIterm, &DriftComp);
//--------------------------------------------------------------------------
// </editor-fold>
}
//--------------------------------------------------------------------------
if (TRUE == _DCM_UseMag)
{
// <editor-fold defaultstate="collapsed" desc="Calculate Mag-based correction">
//--------------------------------------------------------------------------
// Local variables used in calculations of the Magnetometer-based correction
//--------------------------------------------------------------------------
Vector EarthMag; // Magnetic vector measurement
// transformed to Earth frame using
// current rotation matrix
Vector MagNorm; // Normalized Magnetometer measurement
// in the Earth frame
Vector MagError; // Rotational "error" between rotated
// magnetic vector (in Body frame) and
// measured magnetic vector.
Vector MagPterm; // Proportional term
Vector MagIterm; // Current step component of Integral term
//--------------------------------------------------------------------------
// Calculating Magnetometer-based correction
//--------------------------------------------------------------------------
// Transform magnetic vector measurement to Earth frame
DCMToEarth(Mag, &EarthMag);
//---------------------------------------------------------
// We have to "normalize" magnetic vector so that calculated
// error proportional to rotation error and independent of
// the strength of magnetic field
//---------------------------------------------------------
VectorNormalize(&EarthMag, &MagNorm);
//------------------------------------------------------------------
// If magnetic vector is used ONLY for Yaw drift compensation we
// should nullify Z-axis component (Z-axis component of the reference
// vector _DCM_BaseMAG was nullified in DCMReset(...) )
//------------------------------------------------------------------
if (TRUE == _DCM_UseMagYawOnly)
MagNorm.Z = 0.0;
//---------------------------------------------------------
// Cross-product of the original magnetic vector transformed
// to Body frame using the DCM with the normalized
// magnetometer measurement in the Body frame is the ERROR
// accumulated in DCM and defines how much the Rotation
// Matrix need to be rotated so that these vectors match.
//---------------------------------------------------------
VectorCrossProduct(&MagNorm, &_DCM_BaseMAG, &MagError);
//---------------------------------------------------------
// Magnetometer correction P-Term
//---------------------------------------------------------
VectorScale(&MagError, _DCMMagKp, &MagPterm);
//---------------------------------------------------------
// Magnetometer correction I-Term
//---------------------------------------------------------
VectorScale(&MagError, (_DCMMagKi * MagDT), &MagIterm);
VectorAdd(&_DCMMagIterm, &MagIterm, &_DCMMagIterm);
//---------------------------------------------------------
// Adjust drift compensation vector with Magnetometer-
// based correction factors
//---------------------------------------------------------
VectorAdd(&DriftComp, &MagPterm, &DriftComp);
VectorAdd(&DriftComp, &_DCMMagIterm, &DriftComp);
//--------------------------------------------------------------------------
// </editor-fold>
}
//==========================================================================
//==========================================================================
// Local variables used in "rotating" Rotational Matrix
//--------------------------------------------------------------------------
Vector RotationDelta;
Matrix SmallRotation;
Matrix Rotated;
//--------------------------------------------------------------------------
// Calculating total adjustment and "rotate" Rotational Matrix
//--------------------------------------------------------------------------
// Calculate rotation delta in body frame in radians through "integration"
// of the gyro rotation rates
VectorScale( Gyro, GyroDT, &RotationDelta);
//--------------------------------------------------------------------------
VectorAdd(&RotationDelta, &DriftComp, &RotationDelta);
// Construct "infinitesimal" rotation matrix
MatrixSetSmallRotation(&RotationDelta, &SmallRotation);
// Rotate DCM by "small rotation"
MatrixMult(&_DCMRM, &SmallRotation, &Rotated);
// Normalize and store current DCM Rotation Matrix
_DCMNormalize(&Rotated, &_DCMRM);
//==========================================================================
//--------------------------------------------------------------------------
// Load Current orientation parameters into DCMData
//--------------------------------------------------------------------------
IMUResult->Roll = _DCMRoll(&_DCMRM);
IMUResult->Pitch = _DCMPitch(&_DCMRM);
IMUResult->Yaw = _DCMYaw(&_DCMRM);
//------------------------------------------------------------
IMUResult->Incl = _DCMIncl(&_DCMRM);
//--------------------------------------------------------------------------
IMUResult->Azimuth = DCMRangeTo2Pi(_DCM_BaseAzimuth + IMUResult->Yaw);
//--------------------------------------------------------------------------
IMUResult->TS = TMRGetTS();
//--------------------------------------------------------------------------
return DCM_OK;
}
Vector Cross(const Vector &vOther) {
Vector res;
VectorCrossProduct(*this, vOther, res);
return res;
}
/**********************************************************************************************************
*函 数 名: AttitudeEstimateUpdate
*功能说明: 姿态更新
*形 参: 姿态角指针 角速度 加速度测量值 磁力计测量值 时间间隔
*返 回 值: 无
**********************************************************************************************************/
static void AttitudeEstimateUpdate(Vector3f_t* angle, Vector3f_t gyro, Vector3f_t acc, Vector3f_t mag, float deltaT)
{
Vector3f_t deltaAngle;
static Vector3f_t gError;
static Vector3f_t gErrorIntRate = {0.2f, 0.2f, 0.05f};
static Vector3f_t gyro_bias = {0, 0, 0}; //陀螺仪零偏
static Vector3f_t input = {0, 0, 0};
float dcMat[9];
Vector3f_t mVectorEf;
static uint32_t count = 0;
//修正陀螺仪零偏
gyro.x -= gyro_bias.x;
gyro.y -= gyro_bias.y;
gyro.z -= gyro_bias.z;
//一阶积分计算角度变化量，单位为弧度
deltaAngle.x = Radians(gyro.x * deltaT);
deltaAngle.y = Radians(gyro.y * deltaT);
deltaAngle.z = Radians(gyro.z * deltaT);
//角度变化量转换为方向余弦矩阵
EulerAngleToDCM(deltaAngle, dcMat);
//更新卡尔曼状态转移矩阵
KalmanStateTransMatSet(&kalmanRollPitch, dcMat);
KalmanStateTransMatSet(&kalmanYaw, dcMat);
//卡尔曼滤波器更新
//磁强数据更新频率要低于陀螺仪，因此磁强数据未更新时只进行状态预估计
KalmanUpdate(&kalmanRollPitch, input, acc, true);
KalmanUpdate(&kalmanYaw, input, mag, count++ % 10 == 0?true:false);
//计算俯仰与横滚角
AccVectorToRollPitchAngle(angle, kalmanRollPitch.state);
angle->x = Degrees(angle->x);
angle->y = Degrees(angle->y);
//计算偏航角，并修正磁偏角误差
//磁偏角东偏为正，西偏为负，中国除新疆外大部分地区为西偏，比如深圳地区为-2°左右
BodyFrameToEarthFrame(*angle, kalmanYaw.state, &mVectorEf);
MagVectorToYawAngle(angle, mVectorEf);
angle->z = WrapDegree360(Degrees(angle->z) + GetMagDeclination());
//向量观测值与估计值进行叉积运算得到旋转误差矢量
gError = VectorCrossProduct(acc, kalmanRollPitch.state);
BodyFrameToEarthFrame(*angle, gError, &gError);
//计算偏航误差
float magAngle;
BodyFrameToEarthFrame(*angle, mag, &mVectorEf);
magAngle = WrapDegree360(Degrees(atan2f(-mVectorEf.y, mVectorEf.x)) + GetMagDeclination());
if(abs(angle->z - magAngle) < 10 && abs(angle->x) < 10 && abs(angle->y)< 10)
{
gError.z = Radians(angle->z - magAngle);
}
else
{
gError.z = 0;
}
/********************************************陀螺仪零偏估计******************************************/
//系统初始化结束后的一段时间内，加快零偏估计速度
if(GetInitStatus() == INIT_FINISH && (GetSysTimeMs() - GetInitFinishTime()) < 15000)
{
gErrorIntRate.x = 1.0f;
gErrorIntRate.y = 1.0f;
gErrorIntRate.z = 0.2f;
}
else
{
gErrorIntRate.x = 0.2f;
gErrorIntRate.y = 0.2f;
gErrorIntRate.z = 0.05f;
}
//加热完成前传感器零偏变化较大，因此不进行零偏估计
if(GetInitStatus() >= HEAT_FINISH)
{
gyro_bias.x += (gError.x * deltaT) * gErrorIntRate.x;
gyro_bias.y += (gError.y * deltaT) * gErrorIntRate.y;
gyro_bias.z += (gError.z * deltaT) * gErrorIntRate.z;
}
//陀螺仪零偏限幅
gyro_bias.x = ConstrainFloat(gyro_bias.x, -1.0f, 1.0f);
gyro_bias.y = ConstrainFloat(gyro_bias.y, -1.0f, 1.0f);
gyro_bias.z = ConstrainFloat(gyro_bias.z, -1.0f, 1.0f);
/************************************近似计算姿态角误差，用于观察和调试**********************************/
AccVectorToRollPitchAngle(&ahrs.angleMeasure, acc);
ahrs.angleMeasure.x = Degrees(ahrs.angleMeasure.x);
ahrs.angleMeasure.y = Degrees(ahrs.angleMeasure.y);
ahrs.angleError.x = ahrs.angleError.x * 0.999f + (angle->x - ahrs.angleMeasure.x) * 0.001f;
ahrs.angleError.y = ahrs.angleError.y * 0.999f + (angle->y - ahrs.angleMeasure.y) * 0.001f;
Vector3f_t magEf;
BodyFrameToEarthFrame(*angle, mag, &magEf);
ahrs.angleMeasure.z = WrapDegree360(Degrees(atan2f(-magEf.y, magEf.x)) + GetMagDeclination());
ahrs.angleError.z = ahrs.angleError.z * 0.999f + (angle->z - ahrs.angleMeasure.z) * 0.001f;
/********************************************************************************************************/
}
void init(void) {
float tmp,xmin,xmax,ymin,ymax,zmin,zmax,a[3],b[3];
int i,j,k,width,height;
unsigned char *data;
float plane_S[] = { 1.0, 0.0, 0.0, 0.0 };
float plane_T[] = { 0.0, 1.0, 0.0, 0.0 };
float plane_R[] = { 0.0, 0.0, 1.0, 0.0 };
float plane_Q[] = { 0.0, 0.0, 0.0, 1.0 };
glClearDepth(1.0);
glShadeModel(GL_SMOOTH);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glDisable(GL_CULL_FACE);
glEnable(GL_LIGHT0);
modelsize = 0;
vertexmodel = Load3DS("data/model.3ds",&num_vertexmodel);
printf("./data/model.3ds %d face\n",num_vertexmodel / 3);
xmin = xmax = ymin = ymax = zmin = zmax = 0;
for(i = 0; i < num_vertexmodel; i++) {
if(vertexmodel[(i << 3) + 0] < xmin) xmin = vertexmodel[(i << 3) + 0];
if(vertexmodel[(i << 3) + 0] > xmax) xmax = vertexmodel[(i << 3) + 0];
if(vertexmodel[(i << 3) + 1] < ymin) ymin = vertexmodel[(i << 3) + 1];
if(vertexmodel[(i << 3) + 1] > ymax) ymax = vertexmodel[(i << 3) + 1];
if(vertexmodel[(i << 3) + 2] < zmin) zmin = vertexmodel[(i << 3) + 2];
if(vertexmodel[(i << 3) + 2] > zmax) zmax = vertexmodel[(i << 3) + 2];
}
for(i = 0; i < num_vertexmodel; i++) {
vertexmodel[(i << 3) + 0] -= (xmax + xmin) / 2.0;
vertexmodel[(i << 3) + 1] -= (ymax + ymin) / 2.0;
vertexmodel[(i << 3) + 2] -= (zmax + zmin) / 2.0;
}
for(i = 0; i < num_vertexmodel; i++) {
tmp = sqrt(vertexmodel[(i << 3) + 0] * vertexmodel[(i << 3) + 0] +
vertexmodel[(i << 3) + 1] * vertexmodel[(i << 3) + 1] +
vertexmodel[(i << 3) + 2] * vertexmodel[(i << 3) + 2]);
if(tmp > modelsize) modelsize = tmp;
}
tmp = MODELSIZE / modelsize;
modelsize = MODELSIZE;
for(i = 0; i < num_vertexmodel; i++) {
vertexmodel[(i << 3) + 0] *= tmp;
vertexmodel[(i << 3) + 1] *= tmp;
vertexmodel[(i << 3) + 2] *= tmp;
}
vertexground = Load3DS("data/ground.3ds",&num_vertexground);
printf("./data/ground.3ds %d face\n",num_vertexground / 3);
planeground = (float*)malloc(sizeof(float) * 4 * num_vertexground / 3);
for(i = 0, j = 0, k = 0; i < num_vertexground; i += 3, j += 24, k += 4) {
VectorSub(&vertexground[j + 8],&vertexground[j],a);
VectorSub(&vertexground[j + 16],&vertexground[j],b);
VectorCrossProduct(a,b,&planeground[k]);
VectorNormalize(&planeground[k],&planeground[k]);
planeground[k + 3] = -VectorDotProduct(&planeground[k],&vertexground[j]);
}
glGenTextures(1,&textureground);
glBindTexture(GL_TEXTURE_2D,textureground);
if((data = LoadJPEG("data/ground.jpg",&width,&height))) {
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
gluBuild2DMipmaps(GL_TEXTURE_2D,4,width,height,GL_RGBA,GL_UNSIGNED_BYTE,data);
free(data);
}
glGenTextures(1,&texturemodel);
glBindTexture(GL_TEXTURE_2D,texturemodel);
if((data = LoadJPEG("data/model.jpg",&width,&height))) {
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
gluBuild2DMipmaps(GL_TEXTURE_2D,4,width,height,GL_RGBA,GL_UNSIGNED_BYTE,data);
free(data);
}
glGenTextures(1,&textureshadow);
glBindTexture(GL_TEXTURE_2D,textureshadow);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_CLAMP);
glTexImage2D(GL_TEXTURE_2D,0,4,SHADOWSIZE,SHADOWSIZE,0,GL_RGBA,GL_UNSIGNED_BYTE,NULL);
glTexGenfv(GL_S,GL_EYE_PLANE,plane_S);
glTexGenfv(GL_T,GL_EYE_PLANE,plane_T);
glTexGenfv(GL_R,GL_EYE_PLANE,plane_R);
glTexGenfv(GL_Q,GL_EYE_PLANE,plane_Q);
modellist = glGenLists(1);
glNewList(modellist,GL_COMPILE);
glPushMatrix();
glBegin(GL_TRIANGLES);
for(i = 0; i < num_vertexmodel; i++) {
glNormal3fv((float*)&vertexmodel[i << 3] + 3);
glTexCoord2fv((float*)&vertexmodel[i << 3] + 6);
glVertex3fv((float*)&vertexmodel[i << 3]);
}
glEnd();
glPopMatrix();
glEndList();
groundlist = glGenLists(1);
glNewList(groundlist,GL_COMPILE);
glBegin(GL_TRIANGLES);
for(i = 0; i < num_vertexground; i++) {
glNormal3fv((float*)&vertexground[i << 3] + 3);
glTexCoord2fv((float*)&vertexground[i << 3] + 6);
glVertex3fv((float*)&vertexground[i << 3]);
}
glEnd();
glEndList();
lightlist = glGenLists(1);
glNewList(lightlist,GL_COMPILE);
glutSolidSphere(0.6,16,16);
glEndList();
}
CVector CVector::CrossProduct(const CVector& otherVec) const
{
CVector result;
VectorCrossProduct(*this, otherVec, result);
return result;
}
void CVector::CrossProduct(const CVector& otherVec, CVector& result) const
{
VectorCrossProduct(*this, otherVec, result);
}
int main(void)
{
// <editor-fold defaultstate="collapsed" desc="Initialization of HW components/modules">
//*******************************************************************
// Initialization of HW components/modules
//===================================================================
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//===================================================================
BLIAsyncStart(50, 50); // Fast blinking - initialization
//===================================================================
MCMInitT(2.5, 2500); // Initialize Motor Control for PPM with setting
// Throttle to HIGH for delay interval to let ESC
// capture Throttle range
//--------------------------
ADCInit(3); // Initialize ADC to control battery
//--------------------------
RCInit(4); // Initialize Receiver interface with Priority=4
//--------------------------
I2CInit(5, 1); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz
//--------------------------
UARTInitTX(6, 350); // Initialize UART1 for TX on IPL=6 at
// BaudRate = 48 => 115,200 bps - ZigBEE
//--------------------------------------
// BaudRate = 100 => 250,000 bps
// BaudRate = 200 => 500,000 bps
// BaudRate = 250 => 625,000 bps
// BaudRate = 350 => 833,333 bps - SD Logger, FTDI
// BaudRate = 500 => 1,250,000 bps
// BaudRate = 1000 => 2,500,000 bps
//*******************************************************************
// Initialize Magnetometer
//--------------------------------------------------------------
if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor
// ODR = 6 (max, 75 Hz),
// Gain = 1 (1.3 Gs)
// DLPF = 0 (no averaging)
BLIDeadStop("EM", 2);
//==================================================================
// Initialize motion sensor - No calibration at this time!!!
//------------------------------------------------------------------
if ( MPUInit(0, 3) ) // Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
BLIDeadStop("EA", 2);
//------------------------------------------------------------------
//==================================================================
AltimeterInit(4); // Initialize Altimeter with IPL=4 (if needed)
// NOTE: All sensors areinitialized, so no more synchronous I2C
// operations are needed. Thus we may start Altimeter to give
// it more time to "warm-up"
AltimeterReset(); // NOTE: will be reset again later
//==================================================================
BLIAsyncStop(); // Initialization complete
//===================================================================
// </editor-fold>
//*******************************************************************
// Quadrocopter control variables
//-------------------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="Timing control variables">
ulong StartTS = 0; // Flight start time
ulong StepTS; // Control Loop step start time
ulong Alarm = 0; // "alarm" variable used to set
// the duration of the Control Loop
// </editor-fold>
// <editor-fold defaultstate="collapsed" desc="Variable required to process RC Feed ">
//-------------------------------------------------------------------
RCData RCFeed; // Control input from Receiver adjusted
// by control rates. It may also be
// corrected (if necessary) to account
// for "+" or "X" configuration as
// specified by MODE_CB_to_MF flag
//---------------------------------------
RCData RCSmthd; // Smoothed control input used in all
// subsequent control calculations
//---------------------------------------
RCData RCFinal; // Smoothed RC input adjusted for the
// Yaw angle to provide "Course Lock" (if
// required). Final RC input provided to
// Quarocopter Control Module
//-------------------------------------------------------------------
// We need battery Nominal voltage to adjust RC Throttle as battery
// charge depletes.
float BatNomV = ADCGetBatteryNomVoltage ();
//-------------------------------------------------------------------
Matrix Mtr_CB_To_MF; // Rotation matrix used to adjust Control
// Board orientation to Model front
//---------------------------------------
Matrix Mtr_CrsLck; // Rotation matrix used to adjust RC input
// to account for current Yaw to implement
// "Course Lock"
//---------------------------------------
if (MODE_CB_to_MF)
// Initialize Rotation Matrix for pre-set angle
MatrixYawRotation(__CB_To_MF_Angle__, &Mtr_CB_To_MF);
else
// Reset Rotation matrix to Identity
MatrixSetIdentity(&Mtr_CB_To_MF);
//-------------------------------------------------------------------
// </editor-fold>
DCMData IMU; // Orientation data from DCM
MCMData MC; // Motor control Variables
//-------------------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="ARMing RC and initializing IMU">
//==================================================================
// Wait for the Receiver ARMing: Throttle should go up and then down
//------------------------------------------------------------------
BLIAsyncStart(200, 200); // Really slow
RCArm();
BLIAsyncStop();
//==================================================================
// </editor-fold>
Re_Start:
//==================================================================
// <editor-fold defaultstate="collapsed" desc="Intialize Re-Start">
//------------------------------------------------------------------
StartTS = 0; // Reset timing of control Loop
//------------------------------------------------------------------
BLIAsyncMorse( "I", 1); // dit - dit
RCReadWhenReady (&RCFeed); // Get reading from receiver
//------------------------------------------------------------------
// Safety check: CONTROL should be UP and THROTTLE - LOW
//------------------------------------------------------------------
while (
0 == RCFeed.Control
||
RCFeed.Throttle > 0.1
)
{
RCReadWhenReady (&RCFeed); // Get next reading from receiver
}
//------------------------------------------------------------------
BLIAsyncStop ();
//==================================================================
//==================================================================
// Normalize last read RC values as they will represent the first
// input into the control algorithm, so should be adjusted accordingly
//------------------------------------------------------------------
NormalizeRCFeed(&RCFeed);
//------------------------------------------------------------------
// Reset Smothed RC data (initialize filter)
//------------------------------------------------------------------
RCDataReset(&RCSmthd);
//==================================================================
// Initialize sensors... Performed after ARMing to ensure that model
// is stable
//--------------------------------------------------------------
BLIAsyncStart(100, 50);
//------------------------------------------------------------------
// Start IMU and wait until orientation estimate stabilizes
//------------------------------------------------------------------
IMUReset();
//--------------------------------------------------------------
BLIAsyncStart(50, 100);
//------------------------------------------------------------------
// Reset Altimeter
//------------------------------------------------------------------
AltimeterReset();
//------------------------------------------------------------------
QCMReset (); // Initialize (reset) QCM variables
//------------------------------------------------------------------
// Force update of DCMData as inside the control Loop this
// call is non-blocking!
//------------------------------------------------------------------
if (IMU_OK != IMUGetUpdateWhenReady (&IMU))
// Failure...
BLIDeadStop ("A", 1);
//--------------------------------------------------------------
BLIAsyncStop();
//--------------------------------------------------------------
// </editor-fold>
//==================================================================
//*******************************************************************
// Quadrocopter Control Loop
//-------------------------------------------------------------------
BLISignalON();
while (1)
{
//============================================================
// Timing of control Loop
//============================================================
// <editor-fold defaultstate="collapsed" desc="Control Loop timing management">
// Sets the "frquency" of Control Loop
Alarm = TMRSetAlarm (10); // Set Alarm 10 msec in the future
//------------------------------------------------------------
StepTS = TMRGetTS (); // Capture TS of this step
if ( 0 == StartTS ) // First iteration of Control?
StartTS = StepTS; // Yes, capture timestamp
// </editor-fold>
//============================================================
//============================================================
// Read commands from receiver - non-blocking call!
// (we will get out of this call even if connection to the
// receiver is lost!)
// At the end processed Receiver data is stored in RCSmthd
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process Receiver feed">
if ( RCRead(&RCFeed) )
{
//------------------------------------------------------------
// Every NEW sample need to be "normalized"
//------------------------------------------------------------
NormalizeRCFeed(&RCFeed);
//------------------------------------------------------------
// Adjust orientation from "+" to "X" if needed
//------------------------------------------------------------
if (MODE_CB_to_MF)
{
// Control board front does not coincide with the model front
Vector RCInput;
Vector RCRotated;
VectorSet(RCFeed.Roll, RCFeed.Pitch, RCFeed.Yaw, &RCInput);
MatrixTimesVector(&Mtr_CB_To_MF, &RCInput, &RCRotated);
RCFeed.Roll = RCRotated.X;
RCFeed.Pitch = RCRotated.Y;
RCFeed.Yaw = RCRotated.Z; // NOTE: Yaw is not affected!
}
}
//============================================================
// Smooth RC data
//------------------------------------------------------------
// Roll, Pitch, and Yaw are smoothed with the IIR(4)
//------------------------------------------------------------
RCSmthd.Roll = (RCSmthd.Roll * 3.0 + RCFeed.Roll ) * 0.25; // 1/4 = 0.25
RCSmthd.Pitch = (RCSmthd.Pitch* 3.0 + RCFeed.Pitch) * 0.25;
if (MODE_Course_Lock)
{
// In this mode Yaw is integrated over the Control Loop
// duration (0.01 sec) and normalized to +/-Pi range
RCSmthd.Yaw = QCMRangeToPi(RCSmthd.Yaw + RCFeed.Yaw * 0.01);
}
else
{
// Without Course-Lock Yaw is treated as "direct input" and
// just smothed similar to other control variables.
RCSmthd.Yaw = (RCSmthd.Yaw * 3.0 + RCFeed.Yaw ) * 0.25;
}
//------------------------------------------------------------
// Throttle is smoothed with the IIR(4) and adjusted to
// account for actual battery voltage. This is done to
// improve "hovering" when throttle stick is not moving.
//------------------------------------------------------------
// Adjust Native (from RC) throttle to a value corresponding
float BatV = ADCGetBatteryVoltage();
float BatAdjTh = RCFeed.Throttle;
if (BatV > 2.0) // Sanity check :)
BatAdjTh = BatAdjTh * BatNomV / BatV;
//-----------------------------------------
RCSmthd.Throttle = (RCSmthd.Throttle*3.0 + BatAdjTh) * 0.25; // 1/4 = 0.25
//-----------------------------------------
RCSmthd.Control = RCFeed.Control;
// </editor-fold>
//============================================================
//============================================================
// Implement Motor Cut-Off if RC Control is LOW
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process RC Control">
if ( 0 == RCSmthd.Control )
{
// Yes, Control is reliably low!
//--------------------------------------------
// Override motor control
//--------------------------------------------
MC.F = MC.B = MC.L = MC.R = 0;
MCMSet(&MC);
//--------------------------------------------
// Flight terminated, post EOF to Data Logger
//--------------------------------------------
TMRDelay(10); // Wait for pipe to clear
UARTPostIfReady( NULL, 0);
// ... and again - to be sure!
TMRDelay(10); // Wait for pipe to clear
UARTPostIfReady( NULL, 0);
//----------------------------------
goto Re_Start;
}
// </editor-fold>
//============================================================
//============================================================
// Obtain and process battery charge status
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process Battery level">
float BatteryCharge = QCMBatteryMgmt();
if (BatteryCharge < 0.35) // BC < 35%
{
float MaxLevel = 2.0 * BatteryCharge;
if (RCSmthd.Throttle > MaxLevel)
RCSmthd.Throttle = MaxLevel;
}
// </editor-fold>
//============================================================
//============================================================
// Read current orientation from the IMU with sensors' raw
// measurements (non-blocking call) and, if necessary, perform
// adjustment to RCSmthd to obtain RCFinal
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process IMU data and generate Motor Control">
// First, unconditionally promote RCSmthd to RCFinal
RCDataCopy(&RCSmthd, &RCFinal);
//------------------------------------------------------------
if (IMU_OK == IMUGetUpdate(&IMU))
{
if (MODE_Course_Lock)
{
// We need to rotate RC input to adjust for current Yaw
// to implement "Course Lock" - RC Roll and Pitch commands
// are interpreted as being applied to the original (pre-
// takeoff) orientation of the model
//-------------------------------------------------------
Vector RCInput;
Vector RCRotated;
// Build rotation matrix to adjust for orientation discrepancy
MatrixYawRotation(-IMU.Yaw, &Mtr_CrsLck);
// Load RC input into RCInput vector
VectorSet(RCSmthd.Roll, RCSmthd.Pitch, RCSmthd.Yaw, &RCInput);
// Rotate RCInput vector
MatrixTimesVector(&Mtr_CrsLck, &RCInput, &RCRotated);
// Save rotated values
RCFinal.Roll = RCRotated.X;
RCFinal.Pitch = RCRotated.Y;
RCFinal.Yaw = RCRotated.Z;
}
// Calculate motor control based
// upon IMU data
QCMPerformStep(&RCFinal, &IMU, &MC);
}
else
{
// IMU Failed to provide update -
// set Motor Control to RC throttle
MC.F = MC.B = MC.L = MC.R = RCFinal.Throttle;
}
// </editor-fold>
//============================================================
//************************************************************
// Implement Motor Cut-Off to protect props if model is
// dangerously tilted (> 60 degrees) while RC Throttle is low
//------------------------------------------------------------
if (IMU.Incl <= 0.5 && RCFinal.Throttle <= 0.40)
{
// Override motor control
MC.F = MC.B = MC.L = MC.R = 0.0;
}
//------------------------------------------------------------
//************************************************************
// Update motor control
//************************************************************
MCMSet(&MC);
//------------------------------------------------------------
//===========================================================
// Load and post telemetry data (non-blocking call)
//-----------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="Populating Telemetry">
//-----------------------------------------
TM.TS = StepTS - StartTS;
//----------------------
TM.Roll = IMU.Roll;
TM.Pitch = IMU.Pitch;
TM.Yaw = IMU.Yaw;
TM.Inclination = IMU.Incl;
TM.Azimuth = IMU.Azimuth;
//----------------------
#ifdef _TMReport_GYRO_
#ifdef _TMReport_GYRO_Rotated
//------------------------------------------------------
// We rotate Gyro vector using "partial" DCM built using
// only Roll and Pitch angles as the actual Yaw does not
// affect Angualr Velocity by axis
//------------------------------------------------------
Vector Earth; // One of the Earth axis in Body frame
Vector Cross; // Temporary vector to hold the cross
// product
//------------------------------------------------------
// Calculate Roll and Pitch rates
//------------------------------------------------------
// Retrieve Earth Z-axis and calculate cross-product of
// Z-axis with Gyro vector
VectorCrossProduct(DCMEarthZ(&Earth), &IMU.GyroRate, &Cross);
TM.Gyro.X = Cross.X;
TM.Gyro.Y = Cross.Y;
//------------------------------------------------------
// Calculate Yaw rate
//------------------------------------------------------
// Retrieve Earth X-axis and calculate cross-product of
// X-axis with Gyro vector
VectorCrossProduct(DCMEarthX(&Earth), &IMU.GyroRate, &Cross);
TM.Gyro.Z = Cross.Z;
#else
// Just report native Gyro data
VectorCopy(&IMU.GyroRate, &TM.Gyro);
#endif
#endif
//----------------------
#ifdef _TMReport_ACC_
// Just report native Gyro data
VectorCopy(&IMU.Gravity, &TM.Acc);
#endif
//----------------------
TM.RollDer = QSD.RollDer;
TM.PitchDer = QSD.PitchDer;
TM.YawDer = QSD.YawDer;
//----------------------
TM.RC_Throttle = RCFinal.Throttle;
TM.RC_Roll = RCFinal.Roll;
TM.RC_Pitch = RCFinal.Pitch;
TM.RC_Yaw = RCFinal.Yaw;
//----------------------
#ifdef _TMReport_RCSmthd_
TM.RCS_Throttle = RCSmthd.Throttle;
TM.RCS_Roll = RCSmthd.Roll;
TM.RCS_Pitch = RCSmthd.Pitch;
TM.RCS_Yaw = RCSmthd.Yaw;
#endif
//----------------------
#ifdef _TMReport_RCFeed_
TM.RCN_Throttle = RCFeed.Throttle;
TM.RCN_Roll = RCFeed.Roll;
TM.RCN_Pitch = RCFeed.Pitch;
TM.RCN_Yaw = RCFeed.Yaw;
#endif
//----------------------
#ifdef _TMReport_Altimetry_
if ( 0 == AltimeterGetAltData( IMU.TS,
&IMU.Gravity,
&TM.AltResult) )
goto EndOfCycle; // Skip reporting on this step
#endif
//----------------------
#ifdef _TMReport_PID_
#ifdef _TMReport_PID_Details
TM.DRProp = QSD.DeltaRollProp;
TM.DRDiff = QSD.DeltaRollDiff;
TM.DRInt = QSD.DeltaRollInt;
#endif
TM.DRTot = QSD.DeltaRoll;
//-------------
#ifdef _TMReport_PID_Details
TM.DPProp = QSD.DeltaPitchProp;
TM.DPDiff = QSD.DeltaPitchDiff;
TM.DPInt = QSD.DeltaPitchInt;
#endif
TM.DPTot = QSD.DeltaPitch;
//-------------
#ifdef _TMReport_PID_Details
TM.DYProp = QSD.DeltaYawProp;
TM.DYDiff = QSD.DeltaYawDiff;
TM.DYInt = QSD.DeltaYawInt;
#endif
TM.DYTot = QSD.DeltaYaw;
#endif
//----------------------
TM.Throttle = QSD.Throttle; // Real Throttle
//----------------------
TM.Voltage = ADCGetBatteryVoltage();
// </editor-fold>
//===========================================================
UARTPostIfReady( (unsigned char *) &TM, sizeof(TM) );
//===========================================================
// Insert controlled "delay" to slow down the Control Loop
// to pre-set frequency
EndOfCycle:
TMRWaitAlarm(Alarm);
//===========================================================
}
//*******************************************************************
return 1;
}
