Reading ITG-3200 to USB.

1 files changed, 84 insertions, 0 deletions

diff --git a/foo.h b/foo.h
new file mode 100644
index 0000000..d6bb028
--- /dev/null
+++ b/foo.h
@@ -0,0 +1,84 @@
+int usbprintf(USBSerial& usbs, const char* format, ...) {
+	int32_t* argp = (int32_t*)&format;
+	int num;
+	
+	while(*format) {
+		if(*format != '%') {
+			usbs.putc(*format++);
+			num++;
+			continue;
+		}
+		format++;
+		
+		bool is_signed = false;
+		bool zero_pad = false;
+		int radix = 16;
+		int min_len = 0;
+		
+		if(*format == '0') {
+			zero_pad = true;
+		}
+		
+		while(*format >= '0' && *format <= '9') {
+			min_len = min_len * 10 + *format++ - '0';
+		}
+		
+		switch(*format++) {
+			case '%':
+				usbs.putc('%');
+				num++;
+				break;
+			
+			case 'c':
+				usbs.putc((char)*++argp);
+				num++;
+				break;
+			
+			case 's': {
+				char* str = (char*)*++argp;
+				while(*str) {
+					usbs.putc(*str++);
+					num++;
+				}
+			} break;
+			
+			case 'd':
+				is_signed = true;
+			case 'u':
+				radix = 10;
+			case 'x':
+				;
+				uint32_t x = (uint32_t)*(++argp);
+				if(is_signed && (int32_t)x < 0) {
+					x = -x;
+					usbs.putc('-');
+					min_len--;
+				}
+				
+				char buf[11];
+				char* bufp = &buf[sizeof(buf)];
+				*--bufp = 0;
+				
+				do {
+					int d = x % radix;
+					*--bufp = d < 10 ? '0' + d : 'a' - 10 + d;
+					x /= radix;
+					min_len--;
+				} while(x);
+				
+				while(min_len > 0) {
+					usbs.putc(zero_pad ? '0' : ' ');
+					num++;
+					min_len--;
+				}
+				
+				while(*bufp) {
+					usbs.putc(*bufp++);
+					num++;
+				}
+				break;
+		}
+	}
+	
+	return num;
+}