Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

analog.c@0:62a1c91a859a, 2011-02-18 (annotated)

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
First release

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 0:62a1c91a859a 5 // = http://code.google.com/p/uavp-mods/ http://uavp.ch =
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 GetBattery(void);
gke 0:62a1c91a859a 24 void BatteryTest(void);
gke 0:62a1c91a859a 25 void InitBattery(void);
gke 0:62a1c91a859a 26
gke 0:62a1c91a859a 27 real32 BatteryVolts, BatteryCurrentADCEstimated, BatteryChargeUsedAH;
gke 0:62a1c91a859a 28 real32 BatteryCharge, BatteryCurrent;
gke 0:62a1c91a859a 29 real32 BatteryVoltsScale;
gke 0:62a1c91a859a 30
gke 0:62a1c91a859a 31 void GetBattery(void) {
gke 0:62a1c91a859a 32 // AttoPilot Voltage and Current Sense Breakout SparkFun Part: SEN-09028
gke 0:62a1c91a859a 33
gke 0:62a1c91a859a 34 const real32 BatteryCurrentScale = 90.15296; // Amps FS
gke 0:62a1c91a859a 35
gke 0:62a1c91a859a 36 if ( F.HaveBatterySensor ) {
gke 0:62a1c91a859a 37 BatteryCurrent = BatteryCurrentADC.read() * BatteryCurrentScale;
gke 0:62a1c91a859a 38 BatteryChargeUsedAH += BatteryCurrent * (real32)(mSClock() - mS[LastBattery]) * 2.777777e-7;
gke 0:62a1c91a859a 39 mS[LastBattery] = mSClock();
gke 0:62a1c91a859a 40 } else
gke 0:62a1c91a859a 41 BatteryCurrent = BatteryChargeUsedAH = 0;
gke 0:62a1c91a859a 42
gke 0:62a1c91a859a 43 BatteryVolts = BatteryVoltsADC.read() * BatteryVoltsScale;
gke 0:62a1c91a859a 44 F.LowBatt = BatteryVolts < K[LowVoltThres];
gke 0:62a1c91a859a 45
gke 0:62a1c91a859a 46 } // GetBattery
gke 0:62a1c91a859a 47
gke 0:62a1c91a859a 48 void BatteryTest(void) {
gke 0:62a1c91a859a 49
gke 0:62a1c91a859a 50 TxString("\r\nBattery test\r\n");
gke 0:62a1c91a859a 51
gke 0:62a1c91a859a 52 GetBattery();
gke 0:62a1c91a859a 53
gke 0:62a1c91a859a 54 // Battery
gke 0:62a1c91a859a 55 TxString("Volts :\t");
gke 0:62a1c91a859a 56 TxVal32(BatteryVolts * 10.0, 1, 'V');
gke 0:62a1c91a859a 57 TxString(" Limit > ");
gke 0:62a1c91a859a 58 TxVal32( K[LowVoltThres] * 10.0, 1, 'V');
gke 0:62a1c91a859a 59
gke 0:62a1c91a859a 60 if ( F.HaveBatterySensor ) {
gke 0:62a1c91a859a 61 TxString("\r\nCurrent:\t");
gke 0:62a1c91a859a 62 TxVal32(BatteryCurrent * 10.0, 1, 'A');
gke 0:62a1c91a859a 63 TxString(" ( ");
gke 0:62a1c91a859a 64 TxVal32(BatteryChargeUsedAH * 1000.0, 0, 0 );
gke 0:62a1c91a859a 65 TxString(" mAH )\r\n");
gke 0:62a1c91a859a 66 } else
gke 0:62a1c91a859a 67 TxString("\r\nCurrent:\tnot available - no battery sensor\r\n");
gke 0:62a1c91a859a 68
gke 0:62a1c91a859a 69 } // BatteryTest
gke 0:62a1c91a859a 70
gke 0:62a1c91a859a 71 void InitBattery() {
gke 0:62a1c91a859a 72
gke 0:62a1c91a859a 73 F.HaveBatterySensor = true;
gke 0:62a1c91a859a 74 GetBattery();
gke 0:62a1c91a859a 75 F.HaveBatterySensor = BatteryCurrent < 2.0;
gke 0:62a1c91a859a 76 if ( F.HaveBatterySensor )
gke 0:62a1c91a859a 77 BatteryVoltsScale = 51.8144; // Volts FS
gke 0:62a1c91a859a 78 else
gke 0:62a1c91a859a 79 BatteryVoltsScale = BATTERY_VOLTS_SCALE;
gke 0:62a1c91a859a 80
gke 0:62a1c91a859a 81 } // InitBattery
gke 0:62a1c91a859a 82
gke 0:62a1c91a859a 83 //_____________________________________________________________________
gke 0:62a1c91a859a 84
gke 0:62a1c91a859a 85 void GetRangefinderAltitude(void);
gke 0:62a1c91a859a 86 void InitRangefinder(void);
gke 0:62a1c91a859a 87
gke 0:62a1c91a859a 88 real32 RangefinderAltitude;
gke 0:62a1c91a859a 89
gke 0:62a1c91a859a 90 const real32 RangefinderScale = 10.24; // Metres FS
gke 0:62a1c91a859a 91
gke 0:62a1c91a859a 92 void GetRangefinderAltitude(void) {
gke 0:62a1c91a859a 93
gke 0:62a1c91a859a 94 if ( F.RangefinderAltitudeValid ) {
gke 0:62a1c91a859a 95 RangefinderAltitude = RangefinderADC.read() * RangefinderScale;
gke 0:62a1c91a859a 96 if ( F.RFInInches )
gke 0:62a1c91a859a 97 RangefinderAltitude *= 2.54;
gke 0:62a1c91a859a 98
gke 0:62a1c91a859a 99 if (( RangefinderAltitude < ALT_RF_ENABLE_M ) && !F.UsingRangefinderAlt)
gke 0:62a1c91a859a 100 F.UsingRangefinderAlt = true;
gke 0:62a1c91a859a 101 else
gke 0:62a1c91a859a 102 if (( RangefinderAltitude > ALT_RF_DISABLE_M ) && F.UsingRangefinderAlt)
gke 0:62a1c91a859a 103 F.UsingRangefinderAlt = false;
gke 0:62a1c91a859a 104 } else {
gke 0:62a1c91a859a 105 RangefinderAltitude = 0.0;
gke 0:62a1c91a859a 106 F.UsingRangefinderAlt = false;
gke 0:62a1c91a859a 107 }
gke 0:62a1c91a859a 108 } // GetRangefinderAltitude
gke 0:62a1c91a859a 109
gke 0:62a1c91a859a 110 void InitRangefinder(void) {
gke 0:62a1c91a859a 111
gke 0:62a1c91a859a 112 F.RangefinderAltitudeValid = true;
gke 0:62a1c91a859a 113 GetRangefinderAltitude();
gke 0:62a1c91a859a 114 F.RangefinderAltitudeValid = RangefinderAltitude < 1.0; // => supply not RF
gke 0:62a1c91a859a 115 GetRangefinderAltitude();
gke 0:62a1c91a859a 116
gke 0:62a1c91a859a 117 } // InitRangefinder