Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
utils.c
- Committer:
- gke
- Date:
- 2011-04-26
- Revision:
- 2:90292f8bd179
- Parent:
- 0:62a1c91a859a
File content as of revision 2:90292f8bd179:
// =============================================================================================== // = UAVXArm Quadrocopter Controller = // = Copyright (c) 2008 by Prof. Greg Egan = // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = // = http://code.google.com/p/uavp-mods/ = // =============================================================================================== // This is part of UAVXArm. // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU // General Public License as published by the Free Software Foundation, either version 3 of the // License, or (at your option) any later version. // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. // See the GNU General Public License for more details. // You should have received a copy of the GNU General Public License along with this program. // If not, see http://www.gnu.org/licenses/ #include "UAVXArm.h" void InitMisc(void); void Delay1mS(int16); void Delay100mS(int16); void DoBeep100mS(uint8, uint8); void DoStartingBeeps(uint8); void DoBeeperPulse1mS(uint16); void CheckAlarms(void); real32 SlewLimit(real32, real32, real32); real32 DecayX(real32, real32); real32 LPFilter(real32, real32, real32); void Timing(uint8, uint32); TimingRec Times[(UnknownT+1)]; uint32 BeeperOnTime, BeeperOffTime; void InitMisc(void) { int8 i; State = Starting; // For trace preconditions for ( i = 0; i <= UnknownT; i++ ) Times[i].T = Times[i].Count = 0; for ( i = 0; i < FLAG_BYTES ; i++ ) F.AllFlags[i] = false; F.ParametersValid = F.AcquireNewPosition = F.AllowNavAltitudeHold = true; #ifdef SIMULATE F.Simulation = true; #endif // SIMULATE BatteryCharge = 0; IdleThrottle = ((10L*OUT_MAXIMUM)/100); InitialThrottle = RC_MAXIMUM; ESCMin = OUT_MINIMUM; ESCMax = OUT_MAXIMUM; ALL_LEDS_OFF; LEDRed_ON; Beeper_OFF; } // InitMisc void Delay1mS(int16 d) { static int32 Timeout; Timeout = timer.read_us() + ((int32)d * 1000 ); while ( timer.read_us() < Timeout ) {}; } // Delay1mS void Delay100mS(int16 d) { Delay1mS( 100 * d ); } // Delay100mS void DoBeep100mS(uint8 t, uint8 d) { Beeper_ON; Delay100mS(t); Beeper_OFF; Delay100mS(d); } // DoBeep100mS void DoStartingBeeps(uint8 b) { uint8 i; for ( i = 0; i < b; i++ ) DoBeep100mS(2, 8); DoBeep100mS(8,0); } // DoStartingBeeps void DoBeeperPulse1mS(int16 d) { if ( !F.BeeperInUse ) { mS[BeeperTimeout] = mSClock() + 500L; Beeper_ON; } // BeeperOnTime = d; // BeeperOffTime = 0x7ffffff; } // DoBeeperPulse1mS void CheckAlarms(void) { F.BeeperInUse = F.LowBatt || F.LostModel || (State == Shutdown); if ( F.BeeperInUse ) { if ( F.LowBatt ) { BeeperOffTime = 500; BeeperOnTime = 500; } else if ( State == Shutdown ) { BeeperOffTime = 4750; BeeperOnTime = 250; } else if ( F.LostModel ) { BeeperOffTime = 125; BeeperOnTime = 125; } if ( (mSClock() > mS[BeeperUpdate]) && BEEPER_IS_ON ) { mS[BeeperUpdate] = mSClock() + BeeperOffTime; Beeper_OFF; LEDRed_OFF; } else if ( (mSClock() > mS[BeeperUpdate]) && BEEPER_IS_OFF ) { mS[BeeperUpdate] = mSClock() + BeeperOnTime; Beeper_ON; LEDRed_ON; } } #ifdef NAV_ACQUIRE_BEEPER else if ( (State == InFlight) && (!F.AcquireNewPosition) && (mSClock() > mS[BeeperTimeout]) ) Beeper_OFF; #endif // NAV_ACQUIRE_BEEPER } // CheckAlarms real32 DecayX(real32 i, real32 d) { if ( i < 0 ) { i += d; if ( i >0 ) i = 0; } else if ( i > 0 ) { i -= d; if ( i < 0 ) i = 0; } return (i); } // DecayX real32 LPFilter(real32 i, real32 ip, real32 A) { return ( ip + (i - ip) * A ); } // LPFilter real32 SlewLimit(real32 Old, real32 New, real32 Slew) { real32 Low, High; Low = Old - Slew; High = Old + Slew; return(( New < Low ) ? Low : (( New > High ) ? High : New)); } // SlewLimit void Timing(uint8 w, uint32 T) { Times[w].T += timer.read_us() - T; Times[w].Count++; } // Timing