Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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