Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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?

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 // 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