Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
stats.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?
User | Revision | Line number | New 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 ZeroStats(void); |
gke | 0:62a1c91a859a | 24 | void ReadStatsPX(void); |
gke | 0:62a1c91a859a | 25 | void WriteStatsPX(void); |
gke | 0:62a1c91a859a | 26 | void ShowStats(void); |
gke | 0:62a1c91a859a | 27 | |
gke | 0:62a1c91a859a | 28 | int16 Stats[MAX_STATS]; |
gke | 0:62a1c91a859a | 29 | |
gke | 0:62a1c91a859a | 30 | #define INIT_MIN 1000L |
gke | 0:62a1c91a859a | 31 | |
gke | 0:62a1c91a859a | 32 | void ZeroStats(void) |
gke | 0:62a1c91a859a | 33 | { |
gke | 0:62a1c91a859a | 34 | int8 s; |
gke | 0:62a1c91a859a | 35 | |
gke | 0:62a1c91a859a | 36 | for (s = 0 ; s < MAX_STATS ; s++ ) |
gke | 0:62a1c91a859a | 37 | Stats[s] = 0; |
gke | 0:62a1c91a859a | 38 | |
gke | 0:62a1c91a859a | 39 | Stats[MinHDiluteS] = INIT_MIN; |
gke | 0:62a1c91a859a | 40 | Stats[MaxHDiluteS] = 0; |
gke | 0:62a1c91a859a | 41 | Stats[MinROCS] = INIT_MIN; |
gke | 0:62a1c91a859a | 42 | Stats[MaxROCS] = 0; |
gke | 0:62a1c91a859a | 43 | Stats[GPSMinSatsS] = INIT_MIN; |
gke | 0:62a1c91a859a | 44 | Stats[GPSMaxSatsS] = 0; |
gke | 0:62a1c91a859a | 45 | Stats[MinTempS] = INIT_MIN; |
gke | 0:62a1c91a859a | 46 | Stats[MaxTempS] = 0; |
gke | 0:62a1c91a859a | 47 | |
gke | 0:62a1c91a859a | 48 | } // ZeroStats |
gke | 0:62a1c91a859a | 49 | |
gke | 0:62a1c91a859a | 50 | void ReadStatsPX(void) |
gke | 0:62a1c91a859a | 51 | { |
gke | 0:62a1c91a859a | 52 | int8 s; |
gke | 0:62a1c91a859a | 53 | |
gke | 0:62a1c91a859a | 54 | for (s = 0 ; s < MAX_STATS ; s++ ) |
gke | 0:62a1c91a859a | 55 | Stats[s] = Read16PX(STATS_ADDR_PX + s*2); |
gke | 0:62a1c91a859a | 56 | } // InitStats |
gke | 0:62a1c91a859a | 57 | |
gke | 0:62a1c91a859a | 58 | void WriteStatsPX() |
gke | 0:62a1c91a859a | 59 | { |
gke | 0:62a1c91a859a | 60 | int8 s, i; |
gke | 0:62a1c91a859a | 61 | int16 Temp; |
gke | 0:62a1c91a859a | 62 | |
gke | 0:62a1c91a859a | 63 | if ( P[ESCType] != ESCPPM ) |
gke | 0:62a1c91a859a | 64 | for ( i = 0; i < NoOfI2CESCOutputs; i++ ) |
gke | 0:62a1c91a859a | 65 | Stats[ESCI2CFailS] += ESCI2CFail[i]; |
gke | 0:62a1c91a859a | 66 | |
gke | 0:62a1c91a859a | 67 | for (s = 0 ; s < MAX_STATS ; s++ ) |
gke | 0:62a1c91a859a | 68 | Write16PX(STATS_ADDR_PX + s*2, Stats[s]); |
gke | 0:62a1c91a859a | 69 | |
gke | 0:62a1c91a859a | 70 | Temp = ToPercent(CruiseThrottle, OUT_MAXIMUM); |
gke | 0:62a1c91a859a | 71 | WritePX(PercentCruiseThr, Temp); |
gke | 0:62a1c91a859a | 72 | |
gke | 0:62a1c91a859a | 73 | } // WriteStatsPX |
gke | 0:62a1c91a859a | 74 | |
gke | 0:62a1c91a859a | 75 | void ShowStats(void) |
gke | 0:62a1c91a859a | 76 | { |
gke | 0:62a1c91a859a | 77 | TxString("\r\nFlight Statistics\r\n"); |
gke | 0:62a1c91a859a | 78 | |
gke | 0:62a1c91a859a | 79 | if ( Stats[BadS] != 0 ) |
gke | 0:62a1c91a859a | 80 | { |
gke | 0:62a1c91a859a | 81 | TxString("Misc(gke): \t");TxVal32((int32)Stats[BadS],0,' '); TxVal32((int32)Stats[BadNumS],0,' ');TxNextLine(); |
gke | 0:62a1c91a859a | 82 | } |
gke | 0:62a1c91a859a | 83 | |
gke | 0:62a1c91a859a | 84 | TxString("\r\nSensor/Rx Failures (Count)\r\n"); |
gke | 0:62a1c91a859a | 85 | TxString("I2CBus: \t");TxVal32((int32)Stats[I2CFailS],0,0); TxNextLine(); |
gke | 0:62a1c91a859a | 86 | TxString("GPS: \t");TxVal32((int32)Stats[GPSInvalidS],0,0); TxNextLine(); |
gke | 0:62a1c91a859a | 87 | TxString("Acc: \t");TxVal32((int32)Stats[AccFailS], 0, 0); TxNextLine(); |
gke | 0:62a1c91a859a | 88 | TxString("Gyro: \t");TxVal32((int32)Stats[GyroFailS], 0, 0); TxNextLine(); |
gke | 0:62a1c91a859a | 89 | TxString("Comp: \t");TxVal32((int32)Stats[CompassFailS], 0, 0); TxNextLine(); |
gke | 0:62a1c91a859a | 90 | TxString("Baro: \t");TxVal32((int32)Stats[BaroFailS],0 , 0); TxNextLine(); |
gke | 0:62a1c91a859a | 91 | if ( P[ESCType] != ESCPPM ) |
gke | 0:62a1c91a859a | 92 | { |
gke | 0:62a1c91a859a | 93 | TxString("I2CESC: \t");TxVal32((int32)Stats[ESCI2CFailS],0 , 0); TxNextLine(); |
gke | 0:62a1c91a859a | 94 | } |
gke | 0:62a1c91a859a | 95 | TxString("Rx: \t");TxVal32((int32)Stats[RCGlitchesS],0,' '); TxNextLine(); |
gke | 0:62a1c91a859a | 96 | TxString("Failsafes:\t");TxVal32((int32)Stats[RCFailsafesS],0,' '); TxNextLine(); |
gke | 0:62a1c91a859a | 97 | |
gke | 0:62a1c91a859a | 98 | TxString("\r\nBaro\r\n"); // can only display to 3276M |
gke | 0:62a1c91a859a | 99 | TxString("Alt: \t");TxVal32((int32)Stats[BaroRelAltitudeS], 1, ' '); TxString("M \r\n"); |
gke | 0:62a1c91a859a | 100 | if ( Stats[MinROCS] < INIT_MIN ) |
gke | 0:62a1c91a859a | 101 | { |
gke | 0:62a1c91a859a | 102 | TxString("ROC: \t");TxVal32((int32)Stats[MinROCS], 1, ' '); |
gke | 0:62a1c91a859a | 103 | TxVal32((int32)Stats[MaxROCS], 1, ' '); TxString("M/S\r\n"); |
gke | 0:62a1c91a859a | 104 | } |
gke | 0:62a1c91a859a | 105 | |
gke | 0:62a1c91a859a | 106 | if ( Stats[MinTempS] < INIT_MIN ) |
gke | 0:62a1c91a859a | 107 | { |
gke | 0:62a1c91a859a | 108 | TxString("Ambient: \t");TxVal32((int32)Stats[MinTempS], 1, ' '); |
gke | 0:62a1c91a859a | 109 | TxVal32((int32)Stats[MaxTempS], 1, ' '); TxString("C\r\n"); |
gke | 0:62a1c91a859a | 110 | } |
gke | 0:62a1c91a859a | 111 | |
gke | 0:62a1c91a859a | 112 | TxString("\r\nGPS\r\n"); |
gke | 0:62a1c91a859a | 113 | TxString("Alt: \t");TxVal32((int32)Stats[GPSAltitudeS], 1, ' '); TxString("M\r\n"); |
gke | 0:62a1c91a859a | 114 | #ifdef GPS_INC_GROUNDSPPXD |
gke | 0:62a1c91a859a | 115 | TxString("Vel: \t");TxVal32(ConvertGPSToM((int32)Stats[GPSVelS]), 1, ' '); TxString("M/S\r\n"); |
gke | 0:62a1c91a859a | 116 | #endif // GPS_INC_GROUNDSPPXD |
gke | 0:62a1c91a859a | 117 | |
gke | 0:62a1c91a859a | 118 | if ( Stats[GPSMinSatsS] < INIT_MIN ) |
gke | 0:62a1c91a859a | 119 | { |
gke | 0:62a1c91a859a | 120 | TxString("Sats: \t");TxVal32((int32)Stats[GPSMinSatsS], 0, ' '); |
gke | 0:62a1c91a859a | 121 | TxVal32((int32)Stats[GPSMaxSatsS], 0, 0); TxNextLine(); |
gke | 0:62a1c91a859a | 122 | } |
gke | 0:62a1c91a859a | 123 | |
gke | 0:62a1c91a859a | 124 | if ( Stats[MinHDiluteS] < INIT_MIN ) |
gke | 0:62a1c91a859a | 125 | { |
gke | 0:62a1c91a859a | 126 | TxString("HDilute: \t");TxVal32((int32)Stats[MinHDiluteS], 2, ' '); |
gke | 0:62a1c91a859a | 127 | TxVal32((int32)Stats[MaxHDiluteS], 2, 0); TxNextLine(); |
gke | 0:62a1c91a859a | 128 | } |
gke | 0:62a1c91a859a | 129 | |
gke | 0:62a1c91a859a | 130 | if ( Stats[NavValidS] ) |
gke | 0:62a1c91a859a | 131 | TxString("Navigation ENABLED\r\n"); |
gke | 0:62a1c91a859a | 132 | else |
gke | 0:62a1c91a859a | 133 | TxString("Navigation DISABLED (No fix at launch)\r\n"); |
gke | 0:62a1c91a859a | 134 | |
gke | 0:62a1c91a859a | 135 | } // ShowStats |
gke | 0:62a1c91a859a | 136 | |
gke | 0:62a1c91a859a | 137 |