rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Force some floating point units to be safe

Browse Source
This commit is contained in:
James Cotton 2011-11-26 15:52:25 -06:00
parent 04f4afce46
commit 07e3ad10af
2 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/Libraries/CoordinateConversions.c
View File

@ -240,7 +240,7 @@ void R2Quaternion(float R[3][3], float q[4])
index = i;
}
}
mag = 2*sqrt(mag);
mag = 2*sqrtf(mag);
if (index == 0) {
q[0] = mag/4;
@ -374,7 +374,7 @@ void CrossProduct(const float v1[3], const float v2[3], float result[3])
// ****** Vector Magnitude ********
float VectorMagnitude(const float v[3])
{
return(sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
return(sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
}
/**

26
flight/Modules/Stabilization/stabilization.c
View File

@ -373,24 +373,24 @@ float ApplyPid(pid_type * pid, const float err)
pid->lastErr = err;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000);
if(pid->iAccumulator > (pid->iLim * 1000)) {
pid->iAccumulator = pid->iLim * 1000;
} else if (pid->iAccumulator < -(pid->iLim * 1000)) {
pid->iAccumulator = -pid->iLim * 1000;
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
if(pid->iAccumulator > (pid->iLim * 1000.0f)) {
pid->iAccumulator = pid->iLim * 1000.0f;
} else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) {
pid->iAccumulator = -pid->iLim * 1000.0f;
}
return ((err * pid->p) + pid->iAccumulator / 1000 + (diff * pid->d / dT));
return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
}
static void ZeroPids(void)
{
for(int8_t ct = 0; ct < PID_MAX; ct++) {
pids[ct].iAccumulator = 0;
pids[ct].lastErr = 0;
pids[ct].iAccumulator = 0.0f;
pids[ct].lastErr = 0.0f;
}
for(uint8_t i = 0; i < 3; i++)
axis_lock_accum[i] = 0;
axis_lock_accum[i] = 0.0f;
}
@ -399,10 +399,10 @@ static void ZeroPids(void)
*/
static float bound(float val)
{
if(val < -1) {
val = -1;
} else if(val > 1) {
val = 1;
if(val < -1.0f) {
val = -1.0f;
} else if(val > 1.0f) {
val = 1.0f;
}
return val;
}