// Look-at code
Matrix lookatMatrix(Vector eye, Vector center, Vector up) {
Vector f = VectorSub(center, eye);
Vector fn = VectorNorm(f);
Vector upn = VectorNorm(up);
Vector s = VectorCross(fn, upn);
Vector u = VectorCross(s, fn);
Matrix lookat = MakeMatrix(
s.x, s.y, s.z, 0.0f,
u.x, u.y, u.z, 0.0f,
-f.x, -f.y, -f.z, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
);
return lookat;
}
void CCameraFreeFly::vectorsFromAngles()
{
if (m_phi > 89.0f)
{
m_phi = 89.0f;
}
else if (m_phi < -89.0f)
{
m_phi = -89.0f;
}
// Pour éviter un dépassement de valeur ou une perte de précision
if (m_theta < -180.0f)
{
m_theta += 360.0f;
}
else if (m_theta > 180.0f)
{
m_theta -= 360.0f;
}
double r_temp = cos(m_phi * PiOver180);
m_forward.Z = sin(m_phi * PiOver180);
m_forward.X = r_temp * cos(m_theta * PiOver180);
m_forward.Y = r_temp * sin(m_theta * PiOver180);
m_left = VectorCross(TVector3F(0.0f, 0.0f, 1.0f), m_forward);
m_left.normalize();
m_direction = m_position + m_forward;
}
void CameraControl(CHARACTERid targetid)
{
FnCamera camera;
camera.ID(cID);
FnCharacter actor;
actor.ID(targetid);
float targetPos[3];
actor.GetPosition(targetPos);
float negativeDistance[3];
negativeDistance[0] = -distance;
negativeDistance[1] = 0;
negativeDistance[2] = 0;
float* rotation = EulerToQuarternion(0, degToRad(rot_y), degToRad(rot_x));
float* forwardDir = QuaternionMultiVector(rotation, negativeDistance);
float camPos[3], GlobalRight[3];
GlobalRight[0] = 0;
GlobalRight[1] = 1;
GlobalRight[2] = 0;
camPos[0] = targetPos[0] - forwardDir[0];
camPos[1] = targetPos[1] - forwardDir[1];
camPos[2] = targetPos[2] - forwardDir[2];
float* upDir = VectorCross(GlobalRight, forwardDir);
//camera.Quaternion(rotation[0], rotation[1], rotation[2], rotation[3], GLOBAL);
camera.SetPosition(camPos);
camera.SetDirection(forwardDir, upDir);
}
/**
* Class constructor with initialize parameteres.
* @param face1 is first face.
* @param face2 is second face.
*/
Line::Line(Face * face1, Face * face2)
{
Vector normalFace1 = face1->getNormal();
Vector normalFace2 = face2->getNormal();
direction = VectorCross(normalFace1, normalFace2);
if (!(direction.Magnitude()<TOL))
{
float d1 = -(normalFace1.x*face1->v1->x + normalFace1.y*face1->v1->y + normalFace1.z*face1->v1->z);
float d2 = -(normalFace2.x*face2->v1->x + normalFace2.y*face2->v1->y + normalFace2.z*face2->v1->z);
if(fabs(direction.x)>TOL)
{
point.x = 0;
point.y = (d2*normalFace1.z - d1*normalFace2.z)/direction.x;
point.z = (d1*normalFace2.y - d2*normalFace1.y)/direction.x;
}
else if(fabs(direction.y)>TOL)
{
point.x = (d1*normalFace2.z - d2*normalFace1.z)/direction.y;
point.y = 0;
point.z = (d2*normalFace1.x - d1*normalFace2.x)/direction.y;
}
else
{
point.x = (d2*normalFace1.y - d1*normalFace2.y)/direction.z;
point.y = (d1*normalFace2.x - d2*normalFace1.x)/direction.z;
point.z = 0;
}
}
direction.Normalise();
}
void update() {
// Redraw screen now, please, and call again in 15ms.
glutPostRedisplay();
glutTimerFunc(15, updateI, 0);
// Input is win32 only
HWND window = GetActiveWindow();
if(window == GetForegroundWindow()) {
POINT p;
GetCursorPos(&p);
ScreenToClient(window, &p);
glutWarpPointer(WINDOW_WIDTH / 2, WINDOW_HEIGHT / 2);
float angleX = (p.x - (WINDOW_WIDTH / 2)) * CAM_ROTSPEED;
Quaternion rotX = RotationQuaternion(-angleX, MakeVector(0, 1, 0));
camera.front = TransformVector(RotationMatrixFromQuaternion(rotX), camera.front);
camera.elevation += (p.y - (WINDOW_HEIGHT / 2)) * CAM_ROTSPEED;
camera.elevation = max(-0.9f, min(camera.elevation, 0.9f));
if(GetAsyncKeyState('W') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(camera.front, CAM_MOVESPEED));
}
if(GetAsyncKeyState('S') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(camera.front, -CAM_MOVESPEED));
}
if(GetAsyncKeyState('E') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(camera.up, CAM_MOVESPEED));
}
if(GetAsyncKeyState('Q') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(camera.up, -CAM_MOVESPEED));
}
if(GetAsyncKeyState('D') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(VectorCross(camera.front, camera.up), CAM_MOVESPEED));
}
if(GetAsyncKeyState('A') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(VectorCross(camera.front, camera.up), -CAM_MOVESPEED));
}
}
}
CVector3 VectorTriangleNormal(CVector3 vTriangle[])
{
CVector3 vVector1 = VectorSubtract(vTriangle[2], vTriangle[0]);
CVector3 vVector2 = VectorSubtract(vTriangle[1], vTriangle[0]);
CVector3 vNormal = VectorCross(vVector1, vVector2);
vNormal = VectorNormalize(vNormal);
return vNormal;
}
void VectorNormal(vector_t *out, vector_t *arg1,
vector_t *arg2, vector_t *arg3){
vector_t v1minusv2;
vector_t v2minusv3;
VectorSubtract(&v1minusv2, arg1, arg2);
VectorSubtract(&v2minusv3, arg2, arg3);
VectorCross(out, &v1minusv2, &v2minusv3);
VectorNormalize(out);
}
void CameraSetViewPoint(camera_t *cam, float x, float y, float z) {
cam->view.x = x;
cam->view.y = y;
cam->view.z = z;
/* update the forward pointing vector and the right pointing vector */
VectorSubtract(&cam->forward, &cam->view, &cam->position);
VectorNormalize(&cam->forward);
VectorCross(&cam->right, &cam->forward, &cam->up);
VectorNormalize(&cam->right);
return;
}
void Bullet::moveto(QPoint pos){
QPoint a = getSpeedVector(), b = pos - getCurPostion();
double alength = VectorLength(a), blength = VectorLength(b);
double rad;
if (VectorCross(a, b) <= 0){
rad = VectorAngle(a, b) / acos(-1.0) * 180.0;
}
else{
rad = -VectorAngle(a, b) / acos(-1.0) * 180.0;
}
a = b * alength / blength;
setSpeedVector(a);
lastPos = lastPos + getSpeedVector();
}
/**
* Method is used to get face normal.
* @return face normal.
*/
Vector Face::getNormal()
{
Vector p1 = v1->getPosition();
Vector p2 = v2->getPosition();
Vector p3 = v3->getPosition();
Vector xy = p2 - p1;
Vector xz = p3 - p1;
Vector normal = VectorCross(xy, xz);
normal.Normalise();
return normal;
}
void Bullet::Paint(QPainter * painter, QRect){
QPoint a(0, -10), b = this->speed;
double rad;
if (VectorCross(a, b) <= 0){
rad = VectorAngle(a, b) / acos(-1.0) * 180.0;
}
else{
rad = -VectorAngle(a, b) / acos(-1.0) * 180.0;
}
QPixmap tmpImg = img;
QTransform transformed;
transformed.rotate(-rad);
painter->drawPixmap(getCurPostion() - QPoint(width() / 2, height() / 2), img.transformed(transformed));
}
Line::Line(Face * face1, Face * face2)
{
Vector normalFace1 = face1->getNormal();
Vector normalFace2 = face2->getNormal();
//direction: cross product of the faces normals
direction = VectorCross(normalFace1, normalFace2);
//double TOL = 0.00001f;
//if direction lenght is not zero (the planes aren't parallel )...
if (!(direction.Magnitude()<TOL))
{
//getting a line point, zero is set to a coordinate whose direction
//component isn't zero (line intersecting its origin plan)
double d1 = -(normalFace1.x*face1->v1->x + normalFace1.y*face1->v1->y + normalFace1.z*face1->v1->z);
double d2 = -(normalFace2.x*face2->v1->x + normalFace2.y*face2->v1->y + normalFace2.z*face2->v1->z);
if(fabs(direction.x)>TOL)
{
point.x = 0;
point.y = (d2*normalFace1.z - d1*normalFace2.z)/direction.x;
point.z = (d1*normalFace2.y - d2*normalFace1.y)/direction.x;
}
else if(fabs(direction.y)>TOL)
{
point.x = (d1*normalFace2.z - d2*normalFace1.z)/direction.y;
point.y = 0;
point.z = (d2*normalFace1.x - d1*normalFace2.x)/direction.y;
}
else
{
point.x = (d2*normalFace1.y - d1*normalFace2.y)/direction.z;
point.y = (d1*normalFace2.x - d2*normalFace1.x)/direction.z;
point.z = 0;
}
}
direction.Normalise();
}
static float * CorrectQuaternionWithAccelerometer(float quat[4])
{
// Assume that the accelerometer measures ONLY the resistance to gravity
// (opposite the gravity vector). The direction of rotation that takes the
// body from predicted to estimated gravity is (-accelerometer x g_b_ x). This
// is equivalent to (g_b_ x accelerometer). Form a corrective quaternion from
// this rotation.
float quat_c[4] = { 1.0, 0.0, 0.0, 0.0 };
VectorCross(g_b_, AccelerationVector(), &quat_c[1]);
quat_c[1] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
quat_c[2] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
quat_c[3] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
// Apply the correction to the attitude quaternion.
float result[4];
QuaternionMultiply(quat, quat_c, result);
quat[0] = result[0];
quat[1] = result[1];
quat[2] = result[2];
quat[3] = result[3];
return quat;
}
void CCameraFreeFly::anglesFromVectors()
{
m_forward = (m_direction - m_position).getNormalized();
m_left = VectorCross(TVector3F(0.0f, 0.0f, 1.0f), m_forward);
m_left.normalize();
m_phi = asin(m_forward.Z) / PiOver180;
if (std::abs(m_forward.X) < std::numeric_limits<float>::epsilon())
{
m_theta = 90;
}
else
{
m_theta = atan(m_forward.Y / m_forward.X) / PiOver180;
}
if (m_phi > 89.0f)
{
m_phi = 89.0f;
}
else if (m_phi < -89.0f)
{
m_phi = -89.0f;
}
// Pour éviter un dépassement de valeur ou une perte de précision
if (m_theta < -180.0f)
{
m_theta += 360.0f;
}
else if (m_theta > 180.0f)
{
m_theta -= 360.0f;
}
m_direction = m_position + m_forward;
}
/* define behavior for bots in reposition state */
void AIRepositionEnter(ai_t *brain){
player_t *enemy;
vector_t orthog;
vector_t temp;
float diff;
brain->prevState = brain->curState;
brain->curState = AI_REPOSITION;
brain->timer = 0;
brain->timeout = AI_REPO_TIMEOUT + R_TIME_MOD*rand();
PlayerSetAnimation(brain->body, PLAYER_ANIM_RUN);
PlayerReset(brain->body);
VectorClear(&brain->destination);
/* we're currently fighting, we just want to move slightly
* before attacking again */
if((enemy = AIGetEnemy()) != NULL){
/* get a vector pointing towards the enemy */
VectorSubtract(&brain->enemyDir, &enemy->position, &brain->body->position);
/* get the distance to the enemy */
brain->enemy_dis = VectorMagnitude(&brain->enemyDir);
VectorScale(&brain->enemyDir, &brain->enemyDir, 1.0/brain->enemy_dis);
/* get a vector orthogonal to the direction of the enemy */
VectorCross(&orthog, &brain->enemyDir, &brain->body->up);
/* decide if we need to move closer */
if(brain->enemy_dis > AI_DESIRED_RANGE){
diff = brain->enemy_dis - AI_DESIRED_RANGE + UNIRAND(6.0);
VectorScale(&temp, &brain->enemyDir, diff);
VectorAdd(&brain->destination, &temp, &brain->body->position);
}
/* now move sideways some */
diff = AI_MOVE_SIZE + UNIRAND(6.0);
VectorScale(&temp, &orthog, 0.2*SIGN(UNIRAND(1.0))*diff);
VectorAdd(&brain->destination, &brain->destination, &temp);
}
}
void CBaseModel::AdjustScaleAndComputeNormalsToVerts()
{
if (m_Verts.empty())
return;
m_NormalsToVerts.resize(m_Verts.size(), CPoint3D(0, 0, 0));
CPoint3D center(0, 0, 0);
double sumArea(0);
CPoint3D sumNormal(0, 0, 0);
double deta(0);
for (int i = 0; i < (int)m_Faces.size(); ++i)
{
CPoint3D normal = VectorCross(Vert(Face(i)[0]),
Vert(Face(i)[1]),
Vert(Face(i)[2]));
double area = normal.Len();
CPoint3D gravity3 = Vert(Face(i)[0]) + Vert(Face(i)[1]) + Vert(Face(i)[2]);
center += area * gravity3;
sumArea += area;
sumNormal += normal;
deta += gravity3 ^ normal;
normal.x /= area;
normal.y /= area;
normal.z /= area;
for (int j = 0; j < 3; ++j)
{
m_NormalsToVerts[Face(i)[j]] += normal;
}
}
center /= sumArea * 3;
fprintf(stderr,"center %lf %lf %lf\n" , center.x , center.y , center.z );
deta -= 3 * (center ^ sumNormal);
if (true)//deta > 0)
{
for (int i = 0; i < GetNumOfVerts(); ++i)
{
if (fabs(m_NormalsToVerts[i].x)
+ fabs(m_NormalsToVerts[i].y)
+ fabs(m_NormalsToVerts[i].z) >= FLT_EPSILON)
{
m_NormalsToVerts[i].Normalize();
}
}
}
else
{
for (int i = 0; i < GetNumOfFaces(); ++i)
{
int temp = m_Faces[i][0];
m_Faces[i][0] = m_Faces[i][1];
m_Faces[i][1] = temp;
}
for (int i = 0; i < GetNumOfVerts(); ++i)
{
if (fabs(m_NormalsToVerts[i].x)
+ fabs(m_NormalsToVerts[i].y)
+ fabs(m_NormalsToVerts[i].z) >= FLT_EPSILON)
{
double len = m_NormalsToVerts[i].Len();
m_NormalsToVerts[i].x /= -len;
m_NormalsToVerts[i].y /= -len;
m_NormalsToVerts[i].z /= -len;
}
}
}
CPoint3D ptUp(m_Verts[0]);
CPoint3D ptDown(m_Verts[0]);
for (int i = 1; i < GetNumOfVerts(); ++i)
{
if (m_Verts[i].x > ptUp.x)
ptUp.x = m_Verts[i].x;
else if (m_Verts[i].x < ptDown.x)
ptDown.x = m_Verts[i].x;
if (m_Verts[i].y > ptUp.y)
ptUp.y = m_Verts[i].y;
else if (m_Verts[i].y < ptDown.y)
ptDown.y = m_Verts[i].y;
if (m_Verts[i].z > ptUp.z)
ptUp.z = m_Verts[i].z;
else if (m_Verts[i].z < ptDown.z)
ptDown.z = m_Verts[i].z;
}
double maxEdgeLenOfBoundingBox = -1;
if (ptUp.x - ptDown.x > maxEdgeLenOfBoundingBox)
maxEdgeLenOfBoundingBox = ptUp.x - ptDown.x;
if (ptUp.y - ptDown.y > maxEdgeLenOfBoundingBox)
maxEdgeLenOfBoundingBox = ptUp.y - ptDown.y;
if (ptUp.z - ptDown.z > maxEdgeLenOfBoundingBox)
maxEdgeLenOfBoundingBox = ptUp.z - ptDown.z;
m_scale = 2.0 / maxEdgeLenOfBoundingBox;
m_center = center;
m_ptUp = ptUp;
m_ptDown = ptDown;
m_ptUp = (m_ptUp - center) * m_scale;
m_ptDown = (m_ptUp - m_ptDown) * m_scale;
//for (int i = 0; i < (int)m_Verts.size(); ++i)
//{
// m_Verts[i] = (m_Verts[i] - center) * m_scale;
//}
m_scale = 1;
m_center = CPoint3D(0, 0, 0);
}
void helicalTurnCntrl(void)
{
union longww accum;
int16_t pitchAdjustAngleOfAttack;
int16_t rollErrorVector[3];
int16_t rtlkick;
int16_t desiredPitch;
int16_t steeringInput;
int16_t desiredTiltVector[3];
int16_t desiredRotationRateGyro[3];
uint16_t airSpeed;
union longww desiredTilt;
int16_t desiredPitchVector[2];
int16_t desiredPerpendicularPitchVector[2];
int16_t actualPitchVector[2];
int16_t pitchDot;
int16_t pitchCross;
int16_t pitchError;
int16_t pitchEarthBodyProjection[2];
int16_t angleOfAttack;
#ifdef TestGains
state_flags._.GPS_steering = 0; // turn off navigation
state_flags._.pitch_feedback = 1; // turn on stabilization
airSpeed = 981; // for testing purposes, an airspeed is needed
#else
airSpeed = air_speed_3DIMU;
if (airSpeed < TURN_CALC_MINIMUM_AIRSPEED) airSpeed = TURN_CALC_MINIMUM_AIRSPEED;
#endif
// determine the desired turn rate as the sum of navigation and fly by wire.
// this allows the pilot to override navigation if needed.
steeringInput = 0 ; // just in case no airframe type is specified or radio is off
if (udb_flags._.radio_on == 1)
{
#if ( (AIRFRAME_TYPE == AIRFRAME_STANDARD) || (AIRFRAME_TYPE == AIRFRAME_GLIDER) )
if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this
{
steeringInput = udb_pwIn[ AILERON_INPUT_CHANNEL ] - udb_pwTrim[ AILERON_INPUT_CHANNEL ];
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput);
}
else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
{
steeringInput = udb_pwIn[ RUDDER_INPUT_CHANNEL ] - udb_pwTrim[ RUDDER_INPUT_CHANNEL ];
steeringInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, steeringInput);
}
else
{
steeringInput = 0;
}
#endif // AIRFRAME_STANDARD
#if (AIRFRAME_TYPE == AIRFRAME_VTAIL)
// use aileron channel if it is available, otherwise use rudder
if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this
{
steeringInput = udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL];
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput);
}
else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
{
// unmix the Vtail
int16_t rudderInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, (udb_pwIn[ RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]));
int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
steeringInput = (-rudderInput + elevatorInput);
}
else
{
steeringInput = 0;
}
#endif // AIRFRAME_VTAIL
#if (AIRFRAME_TYPE == AIRFRAME_DELTA)
// delta wing must have an aileron input, so use that
// unmix the elevons
int16_t aileronInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, (udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]));
int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
steeringInput = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, ((elevatorInput - aileronInput)));
#endif // AIRFRAME_DELTA
}
if (steeringInput > MAX_INPUT) steeringInput = MAX_INPUT;
if (steeringInput < - MAX_INPUT) steeringInput = - MAX_INPUT;
// note that total steering is the sum of pilot input and waypoint navigation,
// so that the pilot always has some say in the matter
accum.WW = __builtin_mulsu(steeringInput, turngainfbw) /(2*MAX_INPUT);
if ((settings._.AileronNavigation || settings._.RudderNavigation) && state_flags._.GPS_steering)
{
accum.WW +=(int32_t) navigate_determine_deflection('t');
}
if (accum.WW >(int32_t) 2*(int32_t) RMAX - 1) accum.WW =(int32_t) 2*(int32_t) RMAX - 1;
if (accum.WW < -(int32_t) 2*(int32_t) RMAX + 1) accum.WW = -(int32_t) 2*(int32_t) RMAX + 1;
desiredTurnRateRadians = accum._.W0;
// compute the desired tilt from desired turn rate and air speed
// range for acceleration is plus minus 4 times gravity
// range for turning rate is plus minus 4 radians per second
// desiredTilt is the ratio(-rmat[6]/rmat[8]), times RMAX/2 required for the turn
// desiredTilt = desiredTurnRate * airSpeed / gravity
// desiredTilt = RMAX/2*"real desired tilt"
// desiredTurnRate = RMAX/2*"real desired turn rate", desired turn rate in radians per second
// airSpeed is air speed centimeters per second
// gravity is 981 centimeters per second per second
desiredTilt.WW = - __builtin_mulsu(desiredTurnRateRadians, airSpeed);
desiredTilt.WW /= GRAVITYCMSECSEC;
// limit the lateral acceleration to +- 4 times gravity, total wing loading approximately 4.12 times gravity
if (desiredTilt.WW > (int32_t)2 * (int32_t)RMAX - 1)
{
desiredTilt.WW = (int32_t)2 * (int32_t)RMAX - 1;
accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC);
accum.WW /= airSpeed;
desiredTurnRateRadians = accum._.W0;
}
else if (desiredTilt.WW < -(int32_t)2 * (int32_t)RMAX + 1)
{
desiredTilt.WW = -(int32_t)2 * (int32_t)RMAX + 1;
accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC);
accum.WW /= airSpeed;
desiredTurnRateRadians = accum._.W0;
}
// Compute the amount of lift needed to perform the desired turn
// Tests show that the best estimate of lift is obtained using
// actual values of rmat[6] and rmat[8], and the commanded value of their ratio
estimatedLift = wingLift(rmat[6], rmat[8], desiredTilt._.W0);
// compute angle of attack and elevator trim based on relative wing loading.
// relative wing loading is the ratio of wing loading divided by the stall wing loading, as a function of air speed
// both angle of attack and trim are computed by a linear approximation as a function of relative loading:
// y = (2m)*(x/2) + b, y is either angle of attack or elevator trim.
// x is relative wing loading. (x/2 is computed instead of x)
// 2m and b are determined from values of angle of attack and trim at stall speed, normal and inverted.
// b = (y_normal + y_inverted) / 2.
// 2m = (y_normal - y_inverted).
// If airspeed is greater than stall speed, compute angle of attack and elevator trim,
// otherwise set AoA and trim to zero.
if (air_speed_3DIMU > STALL_SPEED_CM_SEC)
{
// compute "x/2", the relative wing loading
relativeLoading = relativeWingLoading(estimatedLift, air_speed_3DIMU);
// multiply x/2 by 2m for angle of attack
accum.WW = __builtin_mulss(AOA_SLOPE, relativeLoading);
// add mx to b
angleOfAttack = AOA_OFFSET + accum._.W1;
// project angle of attack into the earth frame
accum.WW =(__builtin_mulss(angleOfAttack, rmat[8])) << 2;
pitchAdjustAngleOfAttack = accum._.W1;
// similarly, compute elevator trim
accum.WW = __builtin_mulss(ELEVATOR_TRIM_SLOPE, relativeLoading);
elevatorLoadingTrim = ELEVATOR_TRIM_OFFSET + accum._.W1;
}
else
{
angleOfAttack = 0;
pitchAdjustAngleOfAttack = 0;
elevatorLoadingTrim = 0;
}
// SetAofA(angleOfAttack); // removed by helicalTurns
// convert desired turn rate from radians/second to gyro units
accum.WW = (((int32_t)desiredTurnRateRadians) << 4); // desired turn rate in radians times 16 to provide resolution for the divide to follow
accum.WW = accum.WW / RADSTOGYRO; // at this point accum._.W0 has 2 times the required gyro signal for the turn.
// compute desired rotation rate vector in body frame, scaling is same as gyro signal
VectorScale(3, desiredRotationRateGyro, &rmat[6], accum._.W0); // this operation has side effect of dividing by 2
// compute desired rotation rate vector in body frame, scaling is in RMAX/2*radians/sec
VectorScale(3, desiredRotationRateRadians, &rmat[6], desiredTurnRateRadians); // this produces half of what we want
VectorAdd(3, desiredRotationRateRadians, desiredRotationRateRadians, desiredRotationRateRadians); // double
// incorporate roll into desired tilt vector
desiredTiltVector[0] = desiredTilt._.W0;
desiredTiltVector[1] = 0;
desiredTiltVector[2] = RMAX/2; // the divide by 2 is to account for the RMAX/2 scaling in both tilt and rotation rate
vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX
// incorporate pitch into desired tilt vector
// compute return to launch pitch down kick for unpowered RTL
if (!udb_flags._.radio_on && state_flags._.GPS_steering)
{
rtlkick = RTLKICK;
}
else
{
rtlkick = 0;
}
// Compute Matt's glider pitch adjustment
#if (GLIDE_AIRSPEED_CONTROL == 1)
fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust();
#endif
// Compute total desired pitch
#if (GLIDE_AIRSPEED_CONTROL == 1)
desiredPitch = - rtlkick + aspd_pitch_adj + pitchAltitudeAdjust;
#else
desiredPitch = - rtlkick + pitchAltitudeAdjust;
#endif
// Adjustment for inverted flight
if (!canStabilizeInverted() || !desired_behavior._.inverted)
{
// normal flight
desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack;
}
else
{
// inverted flight
desiredTiltVector[0] = - desiredTiltVector[0];
desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack - INVNPITCH; // only one of the adjustments is not zero
desiredTiltVector[2] = - desiredTiltVector[2];
}
vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX
// compute roll error
VectorCross(rollErrorVector, &rmat[6], desiredTiltVector); // compute tilt orientation error
if (VectorDotProduct(3, &rmat[6], desiredTiltVector) < 0) // more than 90 degree error
{
vector3_normalize(rollErrorVector, rollErrorVector); // for more than 90 degrees, make the tilt error vector parallel to desired axis, with magnitude RMAX
}
tiltError[1] = rollErrorVector[1];
// compute pitch error
// start by computing the projection of earth frame pitch error to body frame
pitchEarthBodyProjection[0] = rmat[6];
pitchEarthBodyProjection[1] = rmat[8];
// normalize the projection vector and compute the cosine of the actual pitch as a side effect
actualPitchVector[1] =(int16_t) vector2_normalize(pitchEarthBodyProjection, pitchEarthBodyProjection);
// complete the actual pitch vector
actualPitchVector[0] = rmat[7];
// compute the desired pitch vector
desiredPitchVector[0] = - desiredPitch;
desiredPitchVector[1] = RMAX;
vector2_normalize(desiredPitchVector, desiredPitchVector);
// rotate desired pitch vector by 90 degrees to be able to compute cross product using VectorDot
desiredPerpendicularPitchVector[0] = desiredPitchVector[1];
desiredPerpendicularPitchVector[1] = - desiredPitchVector[0];
// compute pitchDot, the dot product of actual and desired pitch vector
// (the 2* that appears in several of the following expressions is a result of the Q2.14 format)
pitchDot = 2*VectorDotProduct(2, actualPitchVector, desiredPitchVector);
// compute pitchCross, the cross product of the actual and desired pitch vector
pitchCross = 2*VectorDotProduct(2, actualPitchVector, desiredPerpendicularPitchVector);
if (pitchDot > 0)
{
pitchError = pitchCross;
}
else
{
if (pitchCross > 0)
{
pitchError = RMAX;
}
else
{
pitchError = - RMAX;
}
}
// multiply the normalized rmat[6], rmat[8] vector by the pitch error
VectorScale(2, pitchEarthBodyProjection, pitchEarthBodyProjection, pitchError);
tiltError[0] = 2*pitchEarthBodyProjection[1];
tiltError[2] = - 2*pitchEarthBodyProjection[0];
// compute the rotation rate error vector
VectorSubtract(3, rotationRateError, omegaAccum, desiredRotationRateGyro);
}
