utils.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 void InitMisc(void);
00024 void Delay1mS(int16);
00025 void Delay100mS(int16);
00026 void DoBeep100mS(uint8, uint8);
00027 void DoStartingBeeps(uint8);
00028 void DoBeeperPulse1mS(uint16);
00029 void CheckAlarms(void);
00030 real32 SlewLimit(real32, real32, real32);
00031 real32 DecayX(real32, real32);
00032 real32 LPFilter(real32, real32, real32);
00033 void Timing(uint8, uint32);
00034 
00035 TimingRec Times[(UnknownT+1)];
00036 
00037 uint32 BeeperOnTime, BeeperOffTime;
00038 
00039 void InitMisc(void) {
00040     int8 i;
00041 
00042     State = Starting;                // For trace preconditions
00043 
00044     for ( i = 0; i <= UnknownT; i++ )
00045         Times[i].T = Times[i].Count = 0;
00046 
00047     for ( i = 0; i < FLAG_BYTES ; i++ )
00048         F.AllFlags[i] = false;
00049 
00050     F.ParametersValid = F.AcquireNewPosition = F.AllowNavAltitudeHold = true;
00051 
00052 #ifdef SIMULATE
00053     F.Simulation = true;
00054 #endif // SIMULATE
00055 
00056     BatteryCharge = 0;
00057 
00058     IdleThrottle = ((10L*OUT_MAXIMUM)/100);
00059     InitialThrottle = RC_MAXIMUM;
00060     ESCMin = OUT_MINIMUM;
00061     ESCMax = OUT_MAXIMUM;
00062 
00063     ALL_LEDS_OFF;
00064     LEDRed_ON;
00065     Beeper_OFF;
00066 } // InitMisc
00067 
00068 void Delay1mS(int16 d) {
00069     static int32 Timeout;
00070 
00071     Timeout = timer.read_us() + ((int32)d * 1000 );
00072     while (  timer.read_us() < Timeout ) {};
00073 
00074 } // Delay1mS
00075 
00076 void Delay100mS(int16 d) {
00077     Delay1mS( 100 * d );
00078 } // Delay100mS
00079 
00080 void DoBeep100mS(uint8 t, uint8 d) {
00081     Beeper_ON;
00082     Delay100mS(t);
00083     Beeper_OFF;
00084     Delay100mS(d);
00085 } // DoBeep100mS
00086 
00087 void DoStartingBeeps(uint8 b) {
00088     uint8 i;
00089 
00090     for ( i = 0; i < b; i++ )
00091         DoBeep100mS(2, 8);
00092 
00093     DoBeep100mS(8,0);
00094 
00095 } // DoStartingBeeps
00096 
00097 void DoBeeperPulse1mS(int16 d) {
00098 
00099     if ( !F.BeeperInUse ) {
00100         mS[BeeperTimeout] = mSClock() + 500L;
00101         Beeper_ON;
00102     }
00103 
00104 //   BeeperOnTime = d;
00105 //   BeeperOffTime = 0x7ffffff;
00106 
00107 } // DoBeeperPulse1mS
00108 
00109 void CheckAlarms(void) {
00110 
00111     F.BeeperInUse = F.LowBatt || F.LostModel  || (State == Shutdown);
00112 
00113     if ( F.BeeperInUse ) {
00114         if ( F.LowBatt ) {
00115             BeeperOffTime = 500;
00116             BeeperOnTime = 500;
00117         } else
00118             if ( State == Shutdown ) {
00119                 BeeperOffTime = 4750;
00120                 BeeperOnTime = 250;
00121             } else
00122                 if ( F.LostModel ) {
00123                     BeeperOffTime = 125;
00124                     BeeperOnTime = 125;
00125                 }
00126 
00127         if ( (mSClock() > mS[BeeperUpdate]) && BEEPER_IS_ON ) {
00128             mS[BeeperUpdate] = mSClock() + BeeperOffTime;
00129             Beeper_OFF;
00130             LEDRed_OFF;
00131         } else
00132             if ( (mSClock() > mS[BeeperUpdate]) && BEEPER_IS_OFF ) {
00133                 mS[BeeperUpdate] = mSClock() + BeeperOnTime;
00134                 Beeper_ON;
00135                 LEDRed_ON;
00136             }
00137     }
00138 #ifdef NAV_ACQUIRE_BEEPER
00139     else
00140         if ( (State == InFlight) && (!F.AcquireNewPosition) && (mSClock() > mS[BeeperTimeout]) )
00141             Beeper_OFF;
00142 #endif // NAV_ACQUIRE_BEEPER 
00143 
00144 } // CheckAlarms
00145 
00146 real32 DecayX(real32 i, real32 d) {
00147     if ( i < 0 ) {
00148         i += d;
00149         if ( i >0 )
00150             i = 0;
00151     } else
00152         if ( i > 0 ) {
00153             i -= d;
00154             if ( i < 0 )
00155                 i = 0;
00156         }
00157     return (i);
00158 } // DecayX
00159 
00160 real32 LPFilter(real32 i, real32 ip, real32 A) {
00161 
00162     return ( ip + (i - ip) * A );
00163 
00164 } // LPFilter
00165 
00166 real32 SlewLimit(real32 Old, real32 New, real32 Slew) {
00167     real32 Low, High;
00168 
00169     Low = Old - Slew;
00170     High = Old + Slew;
00171     return(( New < Low ) ? Low : (( New > High ) ? High : New));
00172 } // SlewLimit
00173 
00174 void Timing(uint8 w, uint32 T) {
00175 
00176     Times[w].T += timer.read_us() - T;
00177     Times[w].Count++;
00178 
00179 } // Timing
00180