Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
math.c@2:90292f8bd179, 2011-04-26 (annotated)
- Committer:
- gke
- Date:
- Tue Apr 26 12:12:29 2011 +0000
- Revision:
- 2:90292f8bd179
- Parent:
- 0:62a1c91a859a
Not flightworthy. Posted for others to make use of the I2C SW code.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gke | 0:62a1c91a859a | 1 | // =============================================================================================== |
gke | 0:62a1c91a859a | 2 | // = UAVXArm Quadrocopter Controller = |
gke | 0:62a1c91a859a | 3 | // = Copyright (c) 2008 by Prof. Greg Egan = |
gke | 0:62a1c91a859a | 4 | // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = |
gke | 2:90292f8bd179 | 5 | // = http://code.google.com/p/uavp-mods/ = |
gke | 0:62a1c91a859a | 6 | // =============================================================================================== |
gke | 0:62a1c91a859a | 7 | |
gke | 0:62a1c91a859a | 8 | // This is part of UAVXArm. |
gke | 0:62a1c91a859a | 9 | |
gke | 0:62a1c91a859a | 10 | // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU |
gke | 0:62a1c91a859a | 11 | // General Public License as published by the Free Software Foundation, either version 3 of the |
gke | 0:62a1c91a859a | 12 | // License, or (at your option) any later version. |
gke | 0:62a1c91a859a | 13 | |
gke | 0:62a1c91a859a | 14 | // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without |
gke | 0:62a1c91a859a | 15 | // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
gke | 0:62a1c91a859a | 16 | // See the GNU General Public License for more details. |
gke | 0:62a1c91a859a | 17 | |
gke | 0:62a1c91a859a | 18 | // You should have received a copy of the GNU General Public License along with this program. |
gke | 0:62a1c91a859a | 19 | // If not, see http://www.gnu.org/licenses/ |
gke | 0:62a1c91a859a | 20 | |
gke | 0:62a1c91a859a | 21 | #include "UAVXArm.h" |
gke | 0:62a1c91a859a | 22 | |
gke | 0:62a1c91a859a | 23 | int16 SRS16(int16, uint8); |
gke | 0:62a1c91a859a | 24 | int32 SRS32(int32, uint8); |
gke | 0:62a1c91a859a | 25 | real32 Make2Pi(real32); |
gke | 0:62a1c91a859a | 26 | real32 MakePi(real32); |
gke | 0:62a1c91a859a | 27 | int16 Table16(int16, const int16 *); |
gke | 0:62a1c91a859a | 28 | |
gke | 0:62a1c91a859a | 29 | real32 VDot(real32 v1[3], real32 v2[3]); |
gke | 0:62a1c91a859a | 30 | void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]); |
gke | 0:62a1c91a859a | 31 | void VScale(real32 VOut[3], real32 v[3], real32 s); |
gke | 0:62a1c91a859a | 32 | void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]); |
gke | 0:62a1c91a859a | 33 | void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]); |
gke | 0:62a1c91a859a | 34 | |
gke | 0:62a1c91a859a | 35 | int16 SRS16(int16 x, uint8 s) { |
gke | 0:62a1c91a859a | 36 | static i16u Temp; |
gke | 0:62a1c91a859a | 37 | |
gke | 0:62a1c91a859a | 38 | if ( s == (uint8)8 ) { |
gke | 0:62a1c91a859a | 39 | Temp.i16 = x; |
gke | 0:62a1c91a859a | 40 | return( (int16) Temp.i1 ); |
gke | 0:62a1c91a859a | 41 | } else |
gke | 0:62a1c91a859a | 42 | return((x<0) ? -((-x)>>s) : (x>>s)); |
gke | 0:62a1c91a859a | 43 | } // SRS16 |
gke | 0:62a1c91a859a | 44 | |
gke | 0:62a1c91a859a | 45 | int32 SRS32(int32 x, uint8 s) { |
gke | 0:62a1c91a859a | 46 | static i32u Temp; |
gke | 0:62a1c91a859a | 47 | |
gke | 0:62a1c91a859a | 48 | if ( s == (uint8)8 ) { |
gke | 0:62a1c91a859a | 49 | Temp.i32 = x; |
gke | 0:62a1c91a859a | 50 | return( (int32)Temp.i3_1 ); |
gke | 0:62a1c91a859a | 51 | } else |
gke | 0:62a1c91a859a | 52 | return((x<0) ? -((-x)>>s) : (x>>s)); |
gke | 0:62a1c91a859a | 53 | } // SRS32 |
gke | 0:62a1c91a859a | 54 | |
gke | 0:62a1c91a859a | 55 | real32 Make2Pi(real32 A) { |
gke | 0:62a1c91a859a | 56 | while ( A < 0 ) A += TWOPI; |
gke | 0:62a1c91a859a | 57 | while ( A >= TWOPI ) A -= TWOPI; |
gke | 0:62a1c91a859a | 58 | return( A ); |
gke | 0:62a1c91a859a | 59 | } // Make2Pi |
gke | 0:62a1c91a859a | 60 | |
gke | 0:62a1c91a859a | 61 | real32 MakePi(real32 A) { |
gke | 0:62a1c91a859a | 62 | while ( A < -PI ) A += TWOPI; |
gke | 0:62a1c91a859a | 63 | while ( A >= PI ) A -= TWOPI; |
gke | 0:62a1c91a859a | 64 | return( A ); |
gke | 0:62a1c91a859a | 65 | } // MakePi |
gke | 0:62a1c91a859a | 66 | |
gke | 0:62a1c91a859a | 67 | int16 Table16(int16 Val, const int16 *T) { |
gke | 0:62a1c91a859a | 68 | static uint8 Index,Offset; |
gke | 0:62a1c91a859a | 69 | static int16 Temp, Low, High; |
gke | 0:62a1c91a859a | 70 | |
gke | 0:62a1c91a859a | 71 | Index = (uint8) (Val >> 4); |
gke | 0:62a1c91a859a | 72 | Offset = (uint8) (Val & 0x0f); |
gke | 0:62a1c91a859a | 73 | Low = T[Index]; |
gke | 0:62a1c91a859a | 74 | High = T[++Index]; |
gke | 0:62a1c91a859a | 75 | Temp = (High-Low) * Offset; |
gke | 0:62a1c91a859a | 76 | |
gke | 0:62a1c91a859a | 77 | return( Low + SRS16(Temp, 4) ); |
gke | 0:62a1c91a859a | 78 | } // Table16 |
gke | 0:62a1c91a859a | 79 | |
gke | 0:62a1c91a859a | 80 | |
gke | 0:62a1c91a859a | 81 | real32 VDot(real32 v1[3], real32 v2[3]) { |
gke | 0:62a1c91a859a | 82 | static real32 op; |
gke | 0:62a1c91a859a | 83 | static uint8 i; |
gke | 0:62a1c91a859a | 84 | |
gke | 0:62a1c91a859a | 85 | op = 0.0; |
gke | 0:62a1c91a859a | 86 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 87 | op += v1[i] * v2[i]; |
gke | 0:62a1c91a859a | 88 | |
gke | 0:62a1c91a859a | 89 | return op; |
gke | 0:62a1c91a859a | 90 | } // VDot |
gke | 0:62a1c91a859a | 91 | |
gke | 0:62a1c91a859a | 92 | void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]) { |
gke | 0:62a1c91a859a | 93 | VOut[0]= (v1[1] * v2[2]) - (v1[2] * v2[1]); |
gke | 0:62a1c91a859a | 94 | VOut[1]= (v1[2] * v2[0]) - (v1[0] * v2[2]); |
gke | 0:62a1c91a859a | 95 | VOut[2]= (v1[0] * v2[1]) - (v1[1] * v2[0]); |
gke | 0:62a1c91a859a | 96 | } // VCross |
gke | 0:62a1c91a859a | 97 | |
gke | 0:62a1c91a859a | 98 | void VScale(real32 VOut[3], real32 v[3], real32 s) { |
gke | 0:62a1c91a859a | 99 | static uint8 i; |
gke | 0:62a1c91a859a | 100 | |
gke | 0:62a1c91a859a | 101 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 102 | VOut[i] = v[i] * s; |
gke | 0:62a1c91a859a | 103 | } // VScale |
gke | 0:62a1c91a859a | 104 | |
gke | 0:62a1c91a859a | 105 | void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]) { |
gke | 0:62a1c91a859a | 106 | static uint8 i; |
gke | 0:62a1c91a859a | 107 | |
gke | 0:62a1c91a859a | 108 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 109 | VOut[i] = v1[i] + v2[i]; |
gke | 0:62a1c91a859a | 110 | } // VAdd |
gke | 0:62a1c91a859a | 111 | |
gke | 0:62a1c91a859a | 112 | void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]) { |
gke | 0:62a1c91a859a | 113 | static uint8 i; |
gke | 0:62a1c91a859a | 114 | |
gke | 0:62a1c91a859a | 115 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 116 | VOut[i] = v1[i] - v2[i]; |
gke | 0:62a1c91a859a | 117 | } // VSub |
gke | 0:62a1c91a859a | 118 |