Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
nonvolatile.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 | // transfers to Flash seem to be ~35KByte/Sec |
gke | 0:62a1c91a859a | 24 | |
gke | 0:62a1c91a859a | 25 | void CheckSDCardValid(void); |
gke | 0:62a1c91a859a | 26 | |
gke | 0:62a1c91a859a | 27 | void CreateLogfile(void); |
gke | 0:62a1c91a859a | 28 | void CloseLogfile(void); |
gke | 0:62a1c91a859a | 29 | void TxLogChar(uint8); |
gke | 0:62a1c91a859a | 30 | |
gke | 0:62a1c91a859a | 31 | void WritePXImagefile(void); |
gke | 0:62a1c91a859a | 32 | boolean ReadPXImagefile(void); |
gke | 0:62a1c91a859a | 33 | |
gke | 0:62a1c91a859a | 34 | int8 ReadPX(uint16); |
gke | 0:62a1c91a859a | 35 | int16 Read16PX(uint16); |
gke | 0:62a1c91a859a | 36 | int32 Read32PX(uint16); |
gke | 0:62a1c91a859a | 37 | void WritePX(uint16, int8); |
gke | 0:62a1c91a859a | 38 | void Write16PX(uint16, int16); |
gke | 0:62a1c91a859a | 39 | void Write32PX(uint16, int32); |
gke | 0:62a1c91a859a | 40 | |
gke | 0:62a1c91a859a | 41 | FILE *pxfile = NULL; |
gke | 0:62a1c91a859a | 42 | FILE *newpxfile = NULL; |
gke | 0:62a1c91a859a | 43 | |
gke | 0:62a1c91a859a | 44 | const int PX_LENGTH = 2048; |
gke | 0:62a1c91a859a | 45 | int8 PX[PX_LENGTH], PXNew[PX_LENGTH]; |
gke | 0:62a1c91a859a | 46 | |
gke | 0:62a1c91a859a | 47 | void CheckSDCardValid(void) { |
gke | 0:62a1c91a859a | 48 | |
gke | 0:62a1c91a859a | 49 | F.SDCardValid = SDCard.initialise_card() != 0; |
gke | 0:62a1c91a859a | 50 | |
gke | 0:62a1c91a859a | 51 | } // CheckSDCardValid |
gke | 0:62a1c91a859a | 52 | |
gke | 0:62a1c91a859a | 53 | void WritePXImagefile(void) { |
gke | 0:62a1c91a859a | 54 | static uint16 a; |
gke | 0:62a1c91a859a | 55 | static int32 CheckSum; |
gke | 0:62a1c91a859a | 56 | static int8 v, i; |
gke | 0:62a1c91a859a | 57 | |
gke | 0:62a1c91a859a | 58 | UpdateRTC(); |
gke | 0:62a1c91a859a | 59 | |
gke | 0:62a1c91a859a | 60 | if ( F.PXImageStale ) { |
gke | 0:62a1c91a859a | 61 | if ( F.SDCardValid ) |
gke | 0:62a1c91a859a | 62 | newpxfile = fopen("/SDCard/Params.txt", "w"); |
gke | 0:62a1c91a859a | 63 | else |
gke | 0:62a1c91a859a | 64 | newpxfile = fopen("/local/Params.txt", "w"); |
gke | 0:62a1c91a859a | 65 | |
gke | 0:62a1c91a859a | 66 | if ( newpxfile != NULL ) { |
gke | 0:62a1c91a859a | 67 | CheckSum = 0; |
gke | 0:62a1c91a859a | 68 | for ( a = 0; a < PX_LENGTH; a++ ) { |
gke | 0:62a1c91a859a | 69 | v = PX[a]; |
gke | 0:62a1c91a859a | 70 | CheckSum += (int32)v; |
gke | 0:62a1c91a859a | 71 | fprintf(newpxfile, "%i \r\n", v); |
gke | 0:62a1c91a859a | 72 | } |
gke | 0:62a1c91a859a | 73 | fprintf(newpxfile, "%li \r\n ", -CheckSum ); |
gke | 0:62a1c91a859a | 74 | |
gke | 0:62a1c91a859a | 75 | i=0; |
gke | 0:62a1c91a859a | 76 | while ( RTCString[i] != 0 ) |
gke | 0:62a1c91a859a | 77 | fprintf(newpxfile, "%c", RTCString[i++]); |
gke | 0:62a1c91a859a | 78 | fprintf(newpxfile, "\r\n"); |
gke | 0:62a1c91a859a | 79 | |
gke | 0:62a1c91a859a | 80 | F.PXImageStale = false; |
gke | 0:62a1c91a859a | 81 | fclose(newpxfile); |
gke | 0:62a1c91a859a | 82 | } |
gke | 0:62a1c91a859a | 83 | } |
gke | 0:62a1c91a859a | 84 | |
gke | 0:62a1c91a859a | 85 | } // WritePXIMagefile |
gke | 0:62a1c91a859a | 86 | |
gke | 0:62a1c91a859a | 87 | boolean ReadPXImagefile(void) { |
gke | 0:62a1c91a859a | 88 | static uint16 a; |
gke | 0:62a1c91a859a | 89 | static int32 v, CheckSum; |
gke | 0:62a1c91a859a | 90 | static boolean OK; |
gke | 0:62a1c91a859a | 91 | |
gke | 0:62a1c91a859a | 92 | if ( F.SDCardValid ) |
gke | 0:62a1c91a859a | 93 | pxfile = fopen("/SDCard/Params.txt", "r"); |
gke | 0:62a1c91a859a | 94 | else |
gke | 0:62a1c91a859a | 95 | pxfile = fopen("/local/Params.txt", "r"); |
gke | 0:62a1c91a859a | 96 | |
gke | 0:62a1c91a859a | 97 | OK = false; |
gke | 0:62a1c91a859a | 98 | if ( pxfile != NULL ) { |
gke | 0:62a1c91a859a | 99 | CheckSum = 0; |
gke | 0:62a1c91a859a | 100 | a = 0; |
gke | 0:62a1c91a859a | 101 | for ( a = 0; a < PX_LENGTH ; a++ ) { |
gke | 0:62a1c91a859a | 102 | r = fscanf(pxfile, "%i ", &v); |
gke | 0:62a1c91a859a | 103 | CheckSum += v; |
gke | 0:62a1c91a859a | 104 | PXNew[a] = (int8)v; |
gke | 0:62a1c91a859a | 105 | } |
gke | 0:62a1c91a859a | 106 | r = fscanf(pxfile, "%li ", &v); |
gke | 0:62a1c91a859a | 107 | CheckSum += (int32)v; |
gke | 0:62a1c91a859a | 108 | OK = CheckSum == 0; |
gke | 0:62a1c91a859a | 109 | |
gke | 0:62a1c91a859a | 110 | fclose(pxfile); |
gke | 0:62a1c91a859a | 111 | |
gke | 0:62a1c91a859a | 112 | if ( OK ) { |
gke | 0:62a1c91a859a | 113 | for ( a = 0; a < PX_LENGTH; a++ ) |
gke | 0:62a1c91a859a | 114 | PX[a] = PXNew[a]; |
gke | 0:62a1c91a859a | 115 | F.PXImageStale = false; |
gke | 0:62a1c91a859a | 116 | } |
gke | 0:62a1c91a859a | 117 | } |
gke | 0:62a1c91a859a | 118 | |
gke | 0:62a1c91a859a | 119 | return( OK ); |
gke | 0:62a1c91a859a | 120 | |
gke | 0:62a1c91a859a | 121 | } // ReadPXImagefile |
gke | 0:62a1c91a859a | 122 | |
gke | 2:90292f8bd179 | 123 | void CreateLogfile(void) { |
gke | 0:62a1c91a859a | 124 | |
gke | 2:90292f8bd179 | 125 | #ifndef SUPPRESS_SDCARD |
gke | 2:90292f8bd179 | 126 | |
gke | 2:90292f8bd179 | 127 | static int16 i; |
gke | 0:62a1c91a859a | 128 | |
gke | 0:62a1c91a859a | 129 | UpdateRTC(); |
gke | 2:90292f8bd179 | 130 | |
gke | 0:62a1c91a859a | 131 | if ( F.SDCardValid ) |
gke | 0:62a1c91a859a | 132 | strftime(RTCLogfile, 32, "/SDCard/L%H-%M.log", RTCTime ); |
gke | 0:62a1c91a859a | 133 | else |
gke | 0:62a1c91a859a | 134 | strftime(RTCLogfile, 32, "/local/L%H-%M.log", RTCTime ); |
gke | 0:62a1c91a859a | 135 | |
gke | 0:62a1c91a859a | 136 | logfile = fopen(RTCLogfile, "w"); |
gke | 0:62a1c91a859a | 137 | |
gke | 0:62a1c91a859a | 138 | LogfileIsOpen = logfile != NULL; |
gke | 0:62a1c91a859a | 139 | if ( LogfileIsOpen ) { |
gke | 2:90292f8bd179 | 140 | i = 0; |
gke | 0:62a1c91a859a | 141 | while ( RTCString[i] != 0 ) |
gke | 0:62a1c91a859a | 142 | TxLogChar(RTCString[i++]); |
gke | 0:62a1c91a859a | 143 | TxLogChar(CR); |
gke | 0:62a1c91a859a | 144 | TxLogChar(LF); |
gke | 0:62a1c91a859a | 145 | } |
gke | 0:62a1c91a859a | 146 | LogChars = 0; |
gke | 0:62a1c91a859a | 147 | |
gke | 2:90292f8bd179 | 148 | #endif // !SUPPRESS_SDCARD |
gke | 2:90292f8bd179 | 149 | |
gke | 0:62a1c91a859a | 150 | } // CreateLogfile |
gke | 0:62a1c91a859a | 151 | |
gke | 0:62a1c91a859a | 152 | void CloseLogfile(void) { |
gke | 0:62a1c91a859a | 153 | fclose(logfile); |
gke | 0:62a1c91a859a | 154 | } // CloseLog |
gke | 0:62a1c91a859a | 155 | |
gke | 0:62a1c91a859a | 156 | void TxLogChar(uint8 ch) { |
gke | 0:62a1c91a859a | 157 | |
gke | 0:62a1c91a859a | 158 | #ifndef SUPPRESS_SDCARD |
gke | 2:90292f8bd179 | 159 | if ( LogfileIsOpen ) { |
gke | 2:90292f8bd179 | 160 | LogfileIsOpen = fprintf(logfile, "%c", ch) > 0; |
gke | 2:90292f8bd179 | 161 | if ( !LogfileIsOpen ) |
gke | 2:90292f8bd179 | 162 | CloseLogfile(); |
gke | 2:90292f8bd179 | 163 | } |
gke | 0:62a1c91a859a | 164 | #endif // !SUPPRESS_SDCARD |
gke | 0:62a1c91a859a | 165 | } // TxLogChar |
gke | 0:62a1c91a859a | 166 | |
gke | 0:62a1c91a859a | 167 | |
gke | 0:62a1c91a859a | 168 | int8 ReadPX(uint16 a) { |
gke | 0:62a1c91a859a | 169 | static int8 b; |
gke | 0:62a1c91a859a | 170 | b = PX[a]; |
gke | 0:62a1c91a859a | 171 | return(b); |
gke | 0:62a1c91a859a | 172 | } // ReadPX |
gke | 0:62a1c91a859a | 173 | |
gke | 0:62a1c91a859a | 174 | int16 Read16PX(uint16 a) { |
gke | 0:62a1c91a859a | 175 | static i16u Temp16; |
gke | 0:62a1c91a859a | 176 | |
gke | 0:62a1c91a859a | 177 | Temp16.b0 = ReadPX(a); |
gke | 0:62a1c91a859a | 178 | Temp16.b1 = ReadPX(a + 1); |
gke | 0:62a1c91a859a | 179 | |
gke | 0:62a1c91a859a | 180 | return ( Temp16.i16 ); |
gke | 0:62a1c91a859a | 181 | } // Read16P |
gke | 0:62a1c91a859a | 182 | |
gke | 0:62a1c91a859a | 183 | int32 Read32PX(uint16 a) { |
gke | 0:62a1c91a859a | 184 | static i32u Temp32; |
gke | 0:62a1c91a859a | 185 | |
gke | 0:62a1c91a859a | 186 | Temp32.b0 = ReadPX(a); |
gke | 0:62a1c91a859a | 187 | Temp32.b1 = ReadPX(a + 1); |
gke | 0:62a1c91a859a | 188 | Temp32.b2 = ReadPX(a + 2); |
gke | 0:62a1c91a859a | 189 | Temp32.b3 = ReadPX(a + 3); |
gke | 0:62a1c91a859a | 190 | |
gke | 0:62a1c91a859a | 191 | return ( Temp32.i32 ); |
gke | 0:62a1c91a859a | 192 | } // Read32P |
gke | 0:62a1c91a859a | 193 | |
gke | 0:62a1c91a859a | 194 | void WritePX(uint16 a, int8 d) { |
gke | 0:62a1c91a859a | 195 | if ( PX[a] != d ) { |
gke | 0:62a1c91a859a | 196 | PX[a] = d; |
gke | 0:62a1c91a859a | 197 | F.PXImageStale = true; |
gke | 0:62a1c91a859a | 198 | } |
gke | 0:62a1c91a859a | 199 | } // WritePX |
gke | 0:62a1c91a859a | 200 | |
gke | 0:62a1c91a859a | 201 | void Write16PX(uint16 a, int16 d) { |
gke | 0:62a1c91a859a | 202 | static i16u Temp16; |
gke | 0:62a1c91a859a | 203 | |
gke | 0:62a1c91a859a | 204 | Temp16.i16 = d; |
gke | 0:62a1c91a859a | 205 | WritePX(a, Temp16.b0); |
gke | 0:62a1c91a859a | 206 | WritePX(a + 1, Temp16.b1); |
gke | 0:62a1c91a859a | 207 | |
gke | 0:62a1c91a859a | 208 | } // Write16P |
gke | 0:62a1c91a859a | 209 | |
gke | 0:62a1c91a859a | 210 | void Write32PX(uint16 a, int32 d) { |
gke | 0:62a1c91a859a | 211 | static i32u Temp32; |
gke | 0:62a1c91a859a | 212 | |
gke | 0:62a1c91a859a | 213 | Temp32.i32 = d; |
gke | 0:62a1c91a859a | 214 | WritePX(a, Temp32.b0); |
gke | 0:62a1c91a859a | 215 | WritePX(a + 1, Temp32.b1); |
gke | 0:62a1c91a859a | 216 | WritePX(a + 2, Temp32.b2); |
gke | 0:62a1c91a859a | 217 | WritePX(a + 3, Temp32.b3); |
gke | 0:62a1c91a859a | 218 | |
gke | 0:62a1c91a859a | 219 | } // Write16PX |
gke | 0:62a1c91a859a | 220 |