Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
serial.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 | 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 | // USART routines |
gke | 0:62a1c91a859a | 24 | |
gke | 0:62a1c91a859a | 25 | // Much of this legacy code to support UAVPset, the original GUI for UAVP. |
gke | 0:62a1c91a859a | 26 | // It is to be replaced by packet based comms ASAP. |
gke | 0:62a1c91a859a | 27 | |
gke | 0:62a1c91a859a | 28 | |
gke | 0:62a1c91a859a | 29 | void TxString(const uint8*); |
gke | 0:62a1c91a859a | 30 | void TxChar(uint8); |
gke | 0:62a1c91a859a | 31 | void TxValU(uint8); |
gke | 0:62a1c91a859a | 32 | void TxValS(int8); |
gke | 0:62a1c91a859a | 33 | void TxBin8(uint8); |
gke | 0:62a1c91a859a | 34 | void TxNextLine(uint8); |
gke | 0:62a1c91a859a | 35 | void TxNibble( uint8); |
gke | 2:90292f8bd179 | 36 | void TxValH(uint8); |
gke | 0:62a1c91a859a | 37 | void TxValH16( uint16); |
gke | 0:62a1c91a859a | 38 | uint8 RxChar(void); |
gke | 0:62a1c91a859a | 39 | uint8 PollRxChar(void); |
gke | 0:62a1c91a859a | 40 | uint8 RxNumU(void); |
gke | 0:62a1c91a859a | 41 | int8 RxNumS(void); |
gke | 0:62a1c91a859a | 42 | void TxVal32(int32, int8, uint8); |
gke | 0:62a1c91a859a | 43 | void TxChar( uint8); |
gke | 0:62a1c91a859a | 44 | void TxESCu8( uint8); |
gke | 0:62a1c91a859a | 45 | void Sendi16( int16); |
gke | 0:62a1c91a859a | 46 | void TxESCi8( int8); |
gke | 0:62a1c91a859a | 47 | void TxESCi16( int16); |
gke | 0:62a1c91a859a | 48 | void TxESCi24( int24); |
gke | 0:62a1c91a859a | 49 | void TxESCi32( int32); |
gke | 0:62a1c91a859a | 50 | |
gke | 0:62a1c91a859a | 51 | void TxString(const uint8 *pch) { |
gke | 0:62a1c91a859a | 52 | while ( *pch != '\0' ) |
gke | 0:62a1c91a859a | 53 | TxChar(*pch++); |
gke | 0:62a1c91a859a | 54 | } // TxString |
gke | 0:62a1c91a859a | 55 | |
gke | 0:62a1c91a859a | 56 | void TxChar(uint8 ch) { |
gke | 0:62a1c91a859a | 57 | |
gke | 0:62a1c91a859a | 58 | TxCheckSum ^= ch; |
gke | 0:62a1c91a859a | 59 | |
gke | 2:90292f8bd179 | 60 | while ( !SERIAL_TELEMETRY.writeable() ) {}; |
gke | 2:90292f8bd179 | 61 | SERIAL_TELEMETRY.putc(ch); |
gke | 0:62a1c91a859a | 62 | |
gke | 0:62a1c91a859a | 63 | if ( EchoToLogFile ) |
gke | 0:62a1c91a859a | 64 | TxLogChar(ch); |
gke | 0:62a1c91a859a | 65 | |
gke | 0:62a1c91a859a | 66 | } // TxChar |
gke | 0:62a1c91a859a | 67 | |
gke | 0:62a1c91a859a | 68 | void TxValU(uint8 v) { |
gke | 0:62a1c91a859a | 69 | // UAVPSet requires 3 digits exactly ( 000 to 999 ) |
gke | 0:62a1c91a859a | 70 | TxChar((v / 100) + '0'); |
gke | 0:62a1c91a859a | 71 | v %= 100; |
gke | 0:62a1c91a859a | 72 | |
gke | 0:62a1c91a859a | 73 | TxChar((v / 10) + '0'); |
gke | 0:62a1c91a859a | 74 | v %= 10; |
gke | 0:62a1c91a859a | 75 | |
gke | 0:62a1c91a859a | 76 | TxChar(v + '0'); |
gke | 0:62a1c91a859a | 77 | } // TxValU |
gke | 0:62a1c91a859a | 78 | |
gke | 0:62a1c91a859a | 79 | void TxValS(int8 v) { |
gke | 0:62a1c91a859a | 80 | // UAVPSet requires sign and 3 digits exactly (-999 to 999) |
gke | 0:62a1c91a859a | 81 | if ( v < 0 ) { |
gke | 0:62a1c91a859a | 82 | TxChar('-'); // send sign |
gke | 0:62a1c91a859a | 83 | v = -v; |
gke | 0:62a1c91a859a | 84 | } else |
gke | 0:62a1c91a859a | 85 | TxChar('+'); // send sign |
gke | 0:62a1c91a859a | 86 | |
gke | 0:62a1c91a859a | 87 | TxValU(v); |
gke | 0:62a1c91a859a | 88 | } // TxValS |
gke | 0:62a1c91a859a | 89 | |
gke | 0:62a1c91a859a | 90 | void TxNextLine(void) { |
gke | 0:62a1c91a859a | 91 | TxChar(CR); |
gke | 0:62a1c91a859a | 92 | TxChar(LF); |
gke | 0:62a1c91a859a | 93 | } // TxNextLine |
gke | 0:62a1c91a859a | 94 | |
gke | 0:62a1c91a859a | 95 | void TxBin8(uint8 v) { |
gke | 0:62a1c91a859a | 96 | |
gke | 0:62a1c91a859a | 97 | static uint8 i; |
gke | 0:62a1c91a859a | 98 | |
gke | 0:62a1c91a859a | 99 | for (i = 0; i <8; i++) { |
gke | 0:62a1c91a859a | 100 | if ( v & 0x80 ) |
gke | 0:62a1c91a859a | 101 | TxChar('1'); |
gke | 0:62a1c91a859a | 102 | else TxChar('0'); |
gke | 0:62a1c91a859a | 103 | v <<= 1; |
gke | 0:62a1c91a859a | 104 | } |
gke | 0:62a1c91a859a | 105 | } // TxBin8 |
gke | 0:62a1c91a859a | 106 | |
gke | 0:62a1c91a859a | 107 | void TxNibble(uint8 v) { |
gke | 0:62a1c91a859a | 108 | if ( v > (uint8)9) |
gke | 0:62a1c91a859a | 109 | TxChar('A' + v - 10); |
gke | 0:62a1c91a859a | 110 | else |
gke | 0:62a1c91a859a | 111 | TxChar('0' + v); |
gke | 0:62a1c91a859a | 112 | } // TxNibble |
gke | 0:62a1c91a859a | 113 | |
gke | 0:62a1c91a859a | 114 | void TxValH(uint8 v) { |
gke | 0:62a1c91a859a | 115 | TxNibble(v >> 4); |
gke | 0:62a1c91a859a | 116 | TxNibble(v & 0x0f); |
gke | 0:62a1c91a859a | 117 | } // TxValH |
gke | 0:62a1c91a859a | 118 | |
gke | 0:62a1c91a859a | 119 | void TxValH16(uint16 v) { |
gke | 0:62a1c91a859a | 120 | TxValH(v >> 8); |
gke | 0:62a1c91a859a | 121 | TxValH(v); |
gke | 0:62a1c91a859a | 122 | } // TxValH16 |
gke | 0:62a1c91a859a | 123 | |
gke | 0:62a1c91a859a | 124 | uint8 PollRxChar(void) { |
gke | 0:62a1c91a859a | 125 | uint8 ch; |
gke | 0:62a1c91a859a | 126 | |
gke | 2:90292f8bd179 | 127 | if ( SERIAL_TELEMETRY.readable() ) { // a character is waiting in the buffer |
gke | 2:90292f8bd179 | 128 | ch = SERIAL_TELEMETRY.getc(); // get the character |
gke | 0:62a1c91a859a | 129 | TxChar(ch); // echo it for UAVPSet |
gke | 0:62a1c91a859a | 130 | return(ch); // and return it |
gke | 0:62a1c91a859a | 131 | } |
gke | 0:62a1c91a859a | 132 | return( NUL ); // nothing in buffer |
gke | 0:62a1c91a859a | 133 | |
gke | 0:62a1c91a859a | 134 | } // PollRxChar |
gke | 0:62a1c91a859a | 135 | |
gke | 0:62a1c91a859a | 136 | uint8 RxChar(void) { |
gke | 0:62a1c91a859a | 137 | uint8 ch; |
gke | 0:62a1c91a859a | 138 | |
gke | 2:90292f8bd179 | 139 | while ( !SERIAL_TELEMETRY.readable() ) {}; |
gke | 0:62a1c91a859a | 140 | |
gke | 2:90292f8bd179 | 141 | ch = SERIAL_TELEMETRY.getc(); // get the character |
gke | 0:62a1c91a859a | 142 | |
gke | 0:62a1c91a859a | 143 | return(ch); |
gke | 0:62a1c91a859a | 144 | } // RxChar |
gke | 0:62a1c91a859a | 145 | |
gke | 0:62a1c91a859a | 146 | |
gke | 0:62a1c91a859a | 147 | uint8 RxNumU(void) { |
gke | 0:62a1c91a859a | 148 | // UAVPSet sends 2 digits |
gke | 0:62a1c91a859a | 149 | uint8 ch; |
gke | 0:62a1c91a859a | 150 | uint8 n; |
gke | 0:62a1c91a859a | 151 | |
gke | 0:62a1c91a859a | 152 | n = 0; |
gke | 0:62a1c91a859a | 153 | do |
gke | 0:62a1c91a859a | 154 | ch = PollRxChar(); |
gke | 0:62a1c91a859a | 155 | while ( (ch < '0') || (ch > '9') ); |
gke | 0:62a1c91a859a | 156 | n = (ch - '0') * 10; |
gke | 0:62a1c91a859a | 157 | do |
gke | 0:62a1c91a859a | 158 | ch = PollRxChar(); |
gke | 0:62a1c91a859a | 159 | while ( (ch < '0') || (ch > '9') ); |
gke | 0:62a1c91a859a | 160 | n += ch - '0'; |
gke | 0:62a1c91a859a | 161 | return(n); |
gke | 0:62a1c91a859a | 162 | } // RxNumU |
gke | 0:62a1c91a859a | 163 | |
gke | 0:62a1c91a859a | 164 | |
gke | 0:62a1c91a859a | 165 | int8 RxNumS(void) { |
gke | 0:62a1c91a859a | 166 | // UAVPSet sends sign and 2 digits |
gke | 0:62a1c91a859a | 167 | uint8 ch; |
gke | 0:62a1c91a859a | 168 | int8 n; |
gke | 0:62a1c91a859a | 169 | boolean Neg; |
gke | 0:62a1c91a859a | 170 | n = 0; |
gke | 0:62a1c91a859a | 171 | |
gke | 0:62a1c91a859a | 172 | Neg = false; |
gke | 0:62a1c91a859a | 173 | do |
gke | 0:62a1c91a859a | 174 | ch = PollRxChar(); |
gke | 0:62a1c91a859a | 175 | while ( ((ch < '0') || (ch > '9')) && |
gke | 0:62a1c91a859a | 176 | (ch != '-') ); |
gke | 0:62a1c91a859a | 177 | if ( ch == '-' ) { |
gke | 0:62a1c91a859a | 178 | Neg = true; |
gke | 0:62a1c91a859a | 179 | do |
gke | 0:62a1c91a859a | 180 | ch = PollRxChar(); |
gke | 0:62a1c91a859a | 181 | while ( (ch < '0') || (ch > '9') ); |
gke | 0:62a1c91a859a | 182 | } |
gke | 0:62a1c91a859a | 183 | n = (ch - '0') * 10; |
gke | 0:62a1c91a859a | 184 | |
gke | 0:62a1c91a859a | 185 | do |
gke | 0:62a1c91a859a | 186 | ch = PollRxChar(); |
gke | 0:62a1c91a859a | 187 | while ( (ch < '0') || (ch > '9') ); |
gke | 0:62a1c91a859a | 188 | n += ch - '0'; |
gke | 0:62a1c91a859a | 189 | if ( Neg ) |
gke | 0:62a1c91a859a | 190 | n = -n; |
gke | 0:62a1c91a859a | 191 | return(n); |
gke | 0:62a1c91a859a | 192 | } // RxNumS |
gke | 0:62a1c91a859a | 193 | |
gke | 0:62a1c91a859a | 194 | void TxVal32(int32 V, int8 dp, uint8 Separator) { |
gke | 0:62a1c91a859a | 195 | uint8 S[16]; |
gke | 0:62a1c91a859a | 196 | int8 c, zeros, i; |
gke | 0:62a1c91a859a | 197 | int32 NewV, Rem; |
gke | 0:62a1c91a859a | 198 | |
gke | 0:62a1c91a859a | 199 | if (V<0) { |
gke | 0:62a1c91a859a | 200 | TxChar('-'); |
gke | 0:62a1c91a859a | 201 | V=-V; |
gke | 0:62a1c91a859a | 202 | } |
gke | 0:62a1c91a859a | 203 | // else |
gke | 0:62a1c91a859a | 204 | // TxChar(' '); |
gke | 0:62a1c91a859a | 205 | |
gke | 0:62a1c91a859a | 206 | c=0; |
gke | 0:62a1c91a859a | 207 | do { |
gke | 0:62a1c91a859a | 208 | NewV=V/10; |
gke | 0:62a1c91a859a | 209 | Rem=V-(NewV*10); |
gke | 0:62a1c91a859a | 210 | S[c++]=Rem + '0'; |
gke | 0:62a1c91a859a | 211 | V=NewV; |
gke | 0:62a1c91a859a | 212 | } while (V>0); |
gke | 0:62a1c91a859a | 213 | |
gke | 0:62a1c91a859a | 214 | if ((c < ( dp + 1 ) ) && (dp > 0 )) { |
gke | 0:62a1c91a859a | 215 | TxChar('0'); |
gke | 0:62a1c91a859a | 216 | TxChar('.'); |
gke | 0:62a1c91a859a | 217 | } |
gke | 0:62a1c91a859a | 218 | |
gke | 0:62a1c91a859a | 219 | zeros = (int8)dp-c-1; |
gke | 0:62a1c91a859a | 220 | if ( zeros >= 0 ) |
gke | 0:62a1c91a859a | 221 | for (i = zeros; i>=0; i--) |
gke | 0:62a1c91a859a | 222 | TxChar('0'); |
gke | 0:62a1c91a859a | 223 | |
gke | 0:62a1c91a859a | 224 | do { |
gke | 0:62a1c91a859a | 225 | c--; |
gke | 0:62a1c91a859a | 226 | TxChar(S[c]); |
gke | 0:62a1c91a859a | 227 | if ((c==dp)&&(c>0)) |
gke | 0:62a1c91a859a | 228 | TxChar('.'); |
gke | 0:62a1c91a859a | 229 | } while ( c > 0 ); |
gke | 0:62a1c91a859a | 230 | |
gke | 0:62a1c91a859a | 231 | if ( Separator != NUL ) |
gke | 0:62a1c91a859a | 232 | TxChar(Separator); |
gke | 0:62a1c91a859a | 233 | } // TxVal32 |
gke | 0:62a1c91a859a | 234 | |
gke | 0:62a1c91a859a | 235 | void TxESCu8(uint8 ch) { |
gke | 0:62a1c91a859a | 236 | if ((ch==SOH)||(ch==EOT)||(ch==ESC)) |
gke | 0:62a1c91a859a | 237 | TxChar(ESC); |
gke | 0:62a1c91a859a | 238 | TxChar(ch); |
gke | 0:62a1c91a859a | 239 | } // TxESCu8 |
gke | 0:62a1c91a859a | 240 | |
gke | 0:62a1c91a859a | 241 | void TxESCi8(int8 b) { |
gke | 0:62a1c91a859a | 242 | if (((uint8)b==SOH)||((uint8)b==EOT)||((uint8)b==ESC)) |
gke | 0:62a1c91a859a | 243 | TxChar(ESC); |
gke | 0:62a1c91a859a | 244 | TxChar(b); |
gke | 0:62a1c91a859a | 245 | } // TxESCu8 |
gke | 0:62a1c91a859a | 246 | |
gke | 0:62a1c91a859a | 247 | void Sendi16(int16 v) { |
gke | 0:62a1c91a859a | 248 | i16u Temp; |
gke | 0:62a1c91a859a | 249 | |
gke | 0:62a1c91a859a | 250 | Temp.i16 = v; |
gke | 0:62a1c91a859a | 251 | TxChar(Temp.b0); |
gke | 0:62a1c91a859a | 252 | TxChar(Temp.b1); |
gke | 0:62a1c91a859a | 253 | } // Sendi16 |
gke | 0:62a1c91a859a | 254 | |
gke | 0:62a1c91a859a | 255 | void TxESCi16(int16 v) { |
gke | 0:62a1c91a859a | 256 | i16u Temp; |
gke | 0:62a1c91a859a | 257 | |
gke | 0:62a1c91a859a | 258 | Temp.i16 = v; |
gke | 0:62a1c91a859a | 259 | TxESCu8(Temp.b0); |
gke | 0:62a1c91a859a | 260 | TxESCu8(Temp.b1); |
gke | 0:62a1c91a859a | 261 | } // Sendi16 |
gke | 0:62a1c91a859a | 262 | |
gke | 0:62a1c91a859a | 263 | void TxESCi24(int24 v) { |
gke | 0:62a1c91a859a | 264 | i24u Temp; |
gke | 0:62a1c91a859a | 265 | |
gke | 0:62a1c91a859a | 266 | Temp.i24 = v; |
gke | 0:62a1c91a859a | 267 | TxESCu8(Temp.b0); |
gke | 0:62a1c91a859a | 268 | TxESCu8(Temp.b1); |
gke | 0:62a1c91a859a | 269 | TxESCu8(Temp.b2); |
gke | 0:62a1c91a859a | 270 | } // Sendi16 |
gke | 0:62a1c91a859a | 271 | |
gke | 0:62a1c91a859a | 272 | void TxESCi32(int32 v) { |
gke | 0:62a1c91a859a | 273 | i32u Temp; |
gke | 0:62a1c91a859a | 274 | |
gke | 0:62a1c91a859a | 275 | Temp.i32 = v; |
gke | 0:62a1c91a859a | 276 | TxESCi16(Temp.w0); |
gke | 0:62a1c91a859a | 277 | TxESCi16(Temp.w1); |
gke | 0:62a1c91a859a | 278 | } // TxESCi32 |
gke | 0:62a1c91a859a | 279 |