diff --git a/device/serialstream.cc b/device/serialstream.cc index 3cab162..778b7eb 100644 --- a/device/serialstream.cc +++ b/device/serialstream.cc @@ -1,4 +1,5 @@ #include "serialstream.h" +#include "../utils/string.h" SerialStream::SerialStream(ComPort port, BaudRate baud_rate, DataBits data_bits, StopBits stop_bits, Parity parity) @@ -6,6 +7,46 @@ SerialStream::SerialStream(ComPort port, BaudRate baud_rate, DataBits data_bits, void SerialStream::flush() {} +// https://stackoverflow.com/questions/3440726/what-is-the-proper-way-of-implementing-a-good-itoa-function +// Yet, another good itoa implementation +// returns: the length of the number string +int itoa(int value, char *sp, int radix) +{ + char tmp[16];// be careful with the length of the buffer + char *tp = tmp; + int i; + unsigned v; + + int sign = (radix == 10 && value < 0); + if (sign) + v = -value; + else + v = (unsigned)value; + + while (v || tp == tmp) + { + i = v % radix; + v /= radix; + if (i < 10) + *tp++ = i+'0'; + else + *tp++ = i + 'a' - 10; + } + + int len = tp - tmp; + + if (sign) + { + *sp++ = '-'; + len++; + } + + while (tp > tmp) + *sp++ = *--tp; + + return len; +} + void SerialStream::setForeground(Color c) { write(0x1b); write('['); @@ -29,11 +70,16 @@ void SerialStream::setAttribute(Attrib a) { write('m'); } -void SerialStream::reset() {} +void SerialStream::reset() { + write(0x1b); + write('c'); +} void SerialStream::setPos(int x, int y) { - (void)x; - (void)y; + char out[] = {0x1b, '[', 0, 0, ';', 0, 0, 'H', 0}; + itoa(x, &out[2], 10); + itoa(y, &out[5], 10); + print(out, strlen(out)); } void SerialStream::print(char* str, int length) { diff --git a/main.cc b/main.cc index 6c1086e..a6c23e7 100644 --- a/main.cc +++ b/main.cc @@ -48,6 +48,8 @@ extern "C" int main() { ss.print("test", 4); ss.setBackground(SerialStream::CYAN); ss.print("test", 4); + ss.setPos(10, 10); + ss.print("test", 4); /* Start application processors * To avoid unexpected behaviour, make sure that interrupts are not