math.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 int16 SRS16(int16, uint8);
00024 int32 SRS32(int32, uint8);
00025 real32 Make2Pi(real32);
00026 real32 MakePi(real32);
00027 int16 Table16(int16, const int16 *);
00028 
00029 real32 VDot(real32 v1[3], real32 v2[3]);
00030 void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]);
00031 void VScale(real32 VOut[3], real32 v[3], real32 s);
00032 void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]);
00033 void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]);
00034 
00035 int16 SRS16(int16 x, uint8 s) {
00036     static i16u Temp;
00037 
00038     if ( s == (uint8)8 ) {
00039         Temp.i16 = x;
00040         return( (int16) Temp.i1 );
00041     } else
00042         return((x<0) ? -((-x)>>s) : (x>>s));
00043 } // SRS16
00044 
00045 int32 SRS32(int32 x, uint8 s) {
00046     static i32u Temp;
00047 
00048     if ( s == (uint8)8 ) {
00049         Temp.i32 = x;
00050         return( (int32)Temp.i3_1 );
00051     } else
00052         return((x<0) ? -((-x)>>s) : (x>>s));
00053 } // SRS32
00054 
00055 real32 Make2Pi(real32 A) {
00056     while ( A < 0 ) A += TWOPI;
00057     while ( A >= TWOPI ) A -= TWOPI;
00058     return( A );
00059 } // Make2Pi
00060 
00061 real32 MakePi(real32 A) {
00062     while ( A < -PI ) A += TWOPI;
00063     while ( A >= PI ) A -= TWOPI;
00064     return( A );
00065 } // MakePi
00066 
00067 int16 Table16(int16 Val, const int16 *T) {
00068     static uint8 Index,Offset;
00069     static int16 Temp, Low, High;
00070 
00071     Index = (uint8) (Val >> 4);
00072     Offset = (uint8) (Val & 0x0f);
00073     Low = T[Index];
00074     High = T[++Index];
00075     Temp = (High-Low) * Offset;
00076 
00077     return( Low + SRS16(Temp, 4) );
00078 } // Table16
00079 
00080 
00081 real32 VDot(real32 v1[3], real32 v2[3]) {
00082     static real32 op;
00083     static uint8 i;
00084 
00085     op = 0.0;
00086     for ( i = 0; i < (uint8)3; i++ )
00087         op += v1[i] * v2[i];
00088 
00089     return op;
00090 } // VDot
00091 
00092 void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]) {
00093     VOut[0]= (v1[1] * v2[2]) - (v1[2] * v2[1]);
00094     VOut[1]= (v1[2] * v2[0]) - (v1[0] * v2[2]);
00095     VOut[2]= (v1[0] * v2[1]) - (v1[1] * v2[0]);
00096 } // VCross
00097 
00098 void VScale(real32 VOut[3], real32 v[3], real32 s) {
00099     static uint8 i;
00100 
00101     for ( i = 0; i < (uint8)3; i++ )
00102         VOut[i] = v[i] * s;
00103 } // VScale
00104 
00105 void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]) {
00106     static uint8 i;
00107 
00108     for ( i = 0; i < (uint8)3; i++ )
00109         VOut[i] = v1[i] + v2[i];
00110 } // VAdd
00111 
00112 void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]) {
00113     static uint8 i;
00114 
00115     for ( i = 0; i < (uint8)3; i++ )
00116         VOut[i] = v1[i] - v2[i];
00117 } // VSub
00118