Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

utils.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 void InitMisc(void);
gke 0:62a1c91a859a 24 void Delay1mS(int16);
gke 0:62a1c91a859a 25 void Delay100mS(int16);
gke 0:62a1c91a859a 26 void DoBeep100mS(uint8, uint8);
gke 0:62a1c91a859a 27 void DoStartingBeeps(uint8);
gke 2:90292f8bd179 28 void DoBeeperPulse1mS(uint16);
gke 0:62a1c91a859a 29 void CheckAlarms(void);
gke 0:62a1c91a859a 30 real32 SlewLimit(real32, real32, real32);
gke 0:62a1c91a859a 31 real32 DecayX(real32, real32);
gke 2:90292f8bd179 32 real32 LPFilter(real32, real32, real32);
gke 0:62a1c91a859a 33 void Timing(uint8, uint32);
gke 0:62a1c91a859a 34
gke 0:62a1c91a859a 35 TimingRec Times[(UnknownT+1)];
gke 0:62a1c91a859a 36
gke 2:90292f8bd179 37 uint32 BeeperOnTime, BeeperOffTime;
gke 2:90292f8bd179 38
gke 0:62a1c91a859a 39 void InitMisc(void) {
gke 0:62a1c91a859a 40 int8 i;
gke 0:62a1c91a859a 41
gke 0:62a1c91a859a 42 State = Starting; // For trace preconditions
gke 0:62a1c91a859a 43
gke 0:62a1c91a859a 44 for ( i = 0; i <= UnknownT; i++ )
gke 0:62a1c91a859a 45 Times[i].T = Times[i].Count = 0;
gke 0:62a1c91a859a 46
gke 0:62a1c91a859a 47 for ( i = 0; i < FLAG_BYTES ; i++ )
gke 0:62a1c91a859a 48 F.AllFlags[i] = false;
gke 0:62a1c91a859a 49
gke 0:62a1c91a859a 50 F.ParametersValid = F.AcquireNewPosition = F.AllowNavAltitudeHold = true;
gke 0:62a1c91a859a 51
gke 0:62a1c91a859a 52 #ifdef SIMULATE
gke 0:62a1c91a859a 53 F.Simulation = true;
gke 0:62a1c91a859a 54 #endif // SIMULATE
gke 0:62a1c91a859a 55
gke 0:62a1c91a859a 56 BatteryCharge = 0;
gke 0:62a1c91a859a 57
gke 0:62a1c91a859a 58 IdleThrottle = ((10L*OUT_MAXIMUM)/100);
gke 0:62a1c91a859a 59 InitialThrottle = RC_MAXIMUM;
gke 0:62a1c91a859a 60 ESCMin = OUT_MINIMUM;
gke 0:62a1c91a859a 61 ESCMax = OUT_MAXIMUM;
gke 0:62a1c91a859a 62
gke 0:62a1c91a859a 63 ALL_LEDS_OFF;
gke 0:62a1c91a859a 64 LEDRed_ON;
gke 0:62a1c91a859a 65 Beeper_OFF;
gke 0:62a1c91a859a 66 } // InitMisc
gke 0:62a1c91a859a 67
gke 0:62a1c91a859a 68 void Delay1mS(int16 d) {
gke 0:62a1c91a859a 69 static int32 Timeout;
gke 0:62a1c91a859a 70
gke 0:62a1c91a859a 71 Timeout = timer.read_us() + ((int32)d * 1000 );
gke 0:62a1c91a859a 72 while ( timer.read_us() < Timeout ) {};
gke 0:62a1c91a859a 73
gke 0:62a1c91a859a 74 } // Delay1mS
gke 0:62a1c91a859a 75
gke 0:62a1c91a859a 76 void Delay100mS(int16 d) {
gke 0:62a1c91a859a 77 Delay1mS( 100 * d );
gke 0:62a1c91a859a 78 } // Delay100mS
gke 0:62a1c91a859a 79
gke 0:62a1c91a859a 80 void DoBeep100mS(uint8 t, uint8 d) {
gke 0:62a1c91a859a 81 Beeper_ON;
gke 0:62a1c91a859a 82 Delay100mS(t);
gke 0:62a1c91a859a 83 Beeper_OFF;
gke 0:62a1c91a859a 84 Delay100mS(d);
gke 0:62a1c91a859a 85 } // DoBeep100mS
gke 0:62a1c91a859a 86
gke 0:62a1c91a859a 87 void DoStartingBeeps(uint8 b) {
gke 0:62a1c91a859a 88 uint8 i;
gke 0:62a1c91a859a 89
gke 0:62a1c91a859a 90 for ( i = 0; i < b; i++ )
gke 0:62a1c91a859a 91 DoBeep100mS(2, 8);
gke 0:62a1c91a859a 92
gke 0:62a1c91a859a 93 DoBeep100mS(8,0);
gke 2:90292f8bd179 94
gke 0:62a1c91a859a 95 } // DoStartingBeeps
gke 0:62a1c91a859a 96
gke 2:90292f8bd179 97 void DoBeeperPulse1mS(int16 d) {
gke 2:90292f8bd179 98
gke 2:90292f8bd179 99 if ( !F.BeeperInUse ) {
gke 2:90292f8bd179 100 mS[BeeperTimeout] = mSClock() + 500L;
gke 2:90292f8bd179 101 Beeper_ON;
gke 2:90292f8bd179 102 }
gke 2:90292f8bd179 103
gke 2:90292f8bd179 104 // BeeperOnTime = d;
gke 2:90292f8bd179 105 // BeeperOffTime = 0x7ffffff;
gke 2:90292f8bd179 106
gke 2:90292f8bd179 107 } // DoBeeperPulse1mS
gke 2:90292f8bd179 108
gke 0:62a1c91a859a 109 void CheckAlarms(void) {
gke 0:62a1c91a859a 110
gke 0:62a1c91a859a 111 F.BeeperInUse = F.LowBatt || F.LostModel || (State == Shutdown);
gke 0:62a1c91a859a 112
gke 0:62a1c91a859a 113 if ( F.BeeperInUse ) {
gke 0:62a1c91a859a 114 if ( F.LowBatt ) {
gke 2:90292f8bd179 115 BeeperOffTime = 500;
gke 2:90292f8bd179 116 BeeperOnTime = 500;
gke 0:62a1c91a859a 117 } else
gke 0:62a1c91a859a 118 if ( State == Shutdown ) {
gke 0:62a1c91a859a 119 BeeperOffTime = 4750;
gke 0:62a1c91a859a 120 BeeperOnTime = 250;
gke 0:62a1c91a859a 121 } else
gke 0:62a1c91a859a 122 if ( F.LostModel ) {
gke 0:62a1c91a859a 123 BeeperOffTime = 125;
gke 0:62a1c91a859a 124 BeeperOnTime = 125;
gke 0:62a1c91a859a 125 }
gke 0:62a1c91a859a 126
gke 0:62a1c91a859a 127 if ( (mSClock() > mS[BeeperUpdate]) && BEEPER_IS_ON ) {
gke 0:62a1c91a859a 128 mS[BeeperUpdate] = mSClock() + BeeperOffTime;
gke 0:62a1c91a859a 129 Beeper_OFF;
gke 0:62a1c91a859a 130 LEDRed_OFF;
gke 0:62a1c91a859a 131 } else
gke 0:62a1c91a859a 132 if ( (mSClock() > mS[BeeperUpdate]) && BEEPER_IS_OFF ) {
gke 0:62a1c91a859a 133 mS[BeeperUpdate] = mSClock() + BeeperOnTime;
gke 0:62a1c91a859a 134 Beeper_ON;
gke 0:62a1c91a859a 135 LEDRed_ON;
gke 0:62a1c91a859a 136 }
gke 0:62a1c91a859a 137 }
gke 0:62a1c91a859a 138 #ifdef NAV_ACQUIRE_BEEPER
gke 0:62a1c91a859a 139 else
gke 0:62a1c91a859a 140 if ( (State == InFlight) && (!F.AcquireNewPosition) && (mSClock() > mS[BeeperTimeout]) )
gke 0:62a1c91a859a 141 Beeper_OFF;
gke 0:62a1c91a859a 142 #endif // NAV_ACQUIRE_BEEPER
gke 0:62a1c91a859a 143
gke 0:62a1c91a859a 144 } // CheckAlarms
gke 0:62a1c91a859a 145
gke 0:62a1c91a859a 146 real32 DecayX(real32 i, real32 d) {
gke 0:62a1c91a859a 147 if ( i < 0 ) {
gke 0:62a1c91a859a 148 i += d;
gke 0:62a1c91a859a 149 if ( i >0 )
gke 0:62a1c91a859a 150 i = 0;
gke 0:62a1c91a859a 151 } else
gke 0:62a1c91a859a 152 if ( i > 0 ) {
gke 0:62a1c91a859a 153 i -= d;
gke 0:62a1c91a859a 154 if ( i < 0 )
gke 0:62a1c91a859a 155 i = 0;
gke 0:62a1c91a859a 156 }
gke 0:62a1c91a859a 157 return (i);
gke 0:62a1c91a859a 158 } // DecayX
gke 0:62a1c91a859a 159
gke 2:90292f8bd179 160 real32 LPFilter(real32 i, real32 ip, real32 A) {
gke 0:62a1c91a859a 161
gke 2:90292f8bd179 162 return ( ip + (i - ip) * A );
gke 0:62a1c91a859a 163
gke 0:62a1c91a859a 164 } // LPFilter
gke 0:62a1c91a859a 165
gke 0:62a1c91a859a 166 real32 SlewLimit(real32 Old, real32 New, real32 Slew) {
gke 0:62a1c91a859a 167 real32 Low, High;
gke 0:62a1c91a859a 168
gke 0:62a1c91a859a 169 Low = Old - Slew;
gke 0:62a1c91a859a 170 High = Old + Slew;
gke 0:62a1c91a859a 171 return(( New < Low ) ? Low : (( New > High ) ? High : New));
gke 0:62a1c91a859a 172 } // SlewLimit
gke 0:62a1c91a859a 173
gke 0:62a1c91a859a 174 void Timing(uint8 w, uint32 T) {
gke 0:62a1c91a859a 175
gke 0:62a1c91a859a 176 Times[w].T += timer.read_us() - T;
gke 0:62a1c91a859a 177 Times[w].Count++;
gke 0:62a1c91a859a 178
gke 0:62a1c91a859a 179 } // Timing
gke 0:62a1c91a859a 180