/**
* @brief Reads a whole line from the input channel.
*
* @param[in] chp pointer to a @p BaseSequentialStream object
* @param[in] line pointer to the line buffer
* @param[in] size buffer maximum length
* @return The operation status.
* @retval true the channel was reset or CTRL-D pressed.
* @retval false operation successful.
*
* @api
*/
bool shellGetLine(BaseSequentialStream *chp, char *line, unsigned size) {
char *p = line;
while (true) {
char c;
if (chSequentialStreamRead(chp, (uint8_t *)&c, 1) == 0)
return true;
if (c == 4) {
chprintf(chp, "^D");
return true;
}
if ((c == 8) || (c == 127)) {
if (p != line) {
chSequentialStreamPut(chp, c);
chSequentialStreamPut(chp, 0x20);
chSequentialStreamPut(chp, c);
p--;
}
continue;
}
if (c == '\r') {
chprintf(chp, "\r\n");
*p = 0;
return false;
}
if (c < 0x20)
continue;
if (p < line + size - 1) {
chSequentialStreamPut(chp, c);
*p++ = (char)c;
}
}
}
static void printn(uint32_t n) {
char buf[16], *p;
if (!n)
chSequentialStreamPut(&SD1, '0');
else {
p = buf;
while (n)
*p++ = (n % 10) + '0', n /= 10;
while (p > buf)
chSequentialStreamPut(&SD1, *--p);
}
}
static void print_line(void) {
unsigned i;
for (i = 0; i < 76; i++)
chSequentialStreamPut(chp, '-');
chSequentialStreamWrite(chp, (const uint8_t *)"\r\n", 2);
}
static THD_FUNCTION(thSerEcho, arg)
{
(void)arg;
chRegSetThreadName("SerEcho");
event_listener_t elSerData;
eventflags_t flags;
chEvtRegisterMask((event_source_t *)chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));
while (!chThdShouldTerminateX())
{
chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
flags = chEvtGetAndClearFlags(&elSerData);
if (flags & CHN_INPUT_AVAILABLE)
{
msg_t charbuf;
do
{
charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
if ( charbuf != Q_TIMEOUT )
{
chSequentialStreamPut(&SD1, charbuf);
}
}
while (charbuf != Q_TIMEOUT);
}
}
}
static void cmd_gps_passthrough(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argc;
(void)argv;
static const SerialConfig sc = {
9600, 0, USART_CR2_STOP1_BITS | USART_CR2_LINEN, 0};
sdStart(&SD1, &sc);
EventListener elSerData;
flagsmask_t flags;
chEvtRegisterMask(chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));
while (TRUE)
{
chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
flags = chEvtGetAndClearFlags(&elSerData);
if (flags & CHN_INPUT_AVAILABLE)
{
msg_t charbuf;
do
{
charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
if ( charbuf != Q_TIMEOUT )
{
chSequentialStreamPut(chp, charbuf);
}
}
while (charbuf != Q_TIMEOUT);
}
}
}
static void println(char *p) {
while (*p) {
chSequentialStreamPut(&SD1, *p++);
}
chSequentialStreamWrite(&SD1, (uint8_t *)"\r\n", 2);
}
/**
* @brief System formatted output function.
* @details This function implements a minimal @p vprintf()-like functionality
* with output on a @p BaseSequentialStream.
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
* The following parameter types (p) are supported:
* - <b>x</b> hexadecimal integer.
* - <b>X</b> hexadecimal long.
* - <b>o</b> octal integer.
* - <b>O</b> octal long.
* - <b>d</b> decimal signed integer.
* - <b>D</b> decimal signed long.
* - <b>u</b> decimal unsigned integer.
* - <b>U</b> decimal unsigned long.
* - <b>c</b> character.
* - <b>s</b> string.
* .
*
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
* @param[in] fmt formatting string
* @param[in] ap list of parameters
* @return The number of bytes that would have been
* written to @p chp if no stream error occurs
*
* @api
*/
int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
char *p, *s, c, filler;
int i, precision, width;
int n = 0;
bool is_long, left_align;
long l;
#if CHPRINTF_USE_FLOAT
float f;
char tmpbuf[2*MAX_FILLER + 1];
#else
char tmpbuf[MAX_FILLER + 1];
#endif
while (TRUE) {
c = *fmt++;
if (c == 0)
return n;
if (c != '%') {
chSequentialStreamPut(chp, (uint8_t)c);
n++;
continue;
}
p = tmpbuf;
s = tmpbuf;
left_align = FALSE;
if (*fmt == '-') {
fmt++;
left_align = TRUE;
}
filler = ' ';
if (*fmt == '0') {
fmt++;
filler = '0';
}
width = 0;
while (TRUE) {
c = *fmt++;
if (c >= '0' && c <= '9')
c -= '0';
else if (c == '*')
c = va_arg(ap, int);
else
break;
width = width * 10 + c;
}
precision = 0;
if (c == '.') {
while (TRUE) {
c = *fmt++;
if (c >= '0' && c <= '9')
c -= '0';
else if (c == '*')
c = va_arg(ap, int);
else
break;
precision *= 10;
precision += c;
}
}
static size_t chstream_write(void *ip, const void *data, size_t size) {
BaseSequentialStream *chp = (BaseSequentialStream *) ip;
int cr_send = 0;
int counter = 0;
while (counter < size) {
char *ptr = (char*) data;
#if 1
if (ptr[counter] == '\r')
cr_send = 1;
else if (ptr[counter] == '\n' && !cr_send)
chSequentialStreamPut(chp, '\r');
else
cr_send = 0;
#endif
if (chSequentialStreamPut(chp, ptr[counter]) != Q_OK)
return counter;
counter++;
}
return counter;
}
int _write_r(struct _reent *r, int file, char * ptr, int len)
{
(void)r;
(void)file;
(void)ptr;
while(len) {
chSequentialStreamPut(&SDU2, *ptr);
ptr++;
len--;
}
return len;
}
void LogTextMessage(const char* format, ...) {
if ((usbGetDriverStateI(BDU1.config->usbp) == USB_ACTIVE) && (connected)) {
int h = 0x546F7841; // "AxoT"
chSequentialStreamWrite((BaseSequentialStream * )&BDU1,
(const unsigned char* )&h, 4);
va_list ap;
va_start(ap, format);
chvprintf((BaseSequentialStream *)&BDU1, format, ap);
va_end(ap);
chSequentialStreamPut((BaseSequentialStream * )&BDU1, 0);
}
}
static void print(char *p) {
while (*p) {
chSequentialStreamPut(&SD1, *p++);
}
}
static void print_tokens(void) {
char *cp = tokens_buffer;
while (cp < tokp)
chSequentialStreamPut(chp, *cp++);
}
/**
* @brief Prints a line without final end-of-line.
*
* @param[in] msgp the message
*/
void test_print(const char *msgp) {
while (*msgp)
chSequentialStreamPut(chp, *msgp++);
}
/**
* @brief Function required to properly using format library
* @param c char
* @return
*/
int fm_putchar(int c)
{
chThdSleepMilliseconds(2);
chSequentialStreamPut(&SDU1, (uint8_t)c);
return 1;
}
void chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
char *p, *s, c, filler;
int i, precision, width;
bool is_long, left_align;
long l;
#if CHPRINTF_USE_FLOAT
double d;
char tmpbuf[2*MAX_FILLER + 1];
int fprec=0;
#else
char tmpbuf[MAX_FILLER + 1];
#endif
while (TRUE) {
c = *fmt++;
if (c == 0) {
return;
}
if (c != '%') {
chSequentialStreamPut(chp, (uint8_t)c);
continue;
}
p = tmpbuf;
s = tmpbuf;
left_align = false;
if (*fmt == '-') {
fmt++;
left_align = true;
}
filler = ' ';
if (*fmt == '.') {
fmt++;
filler = '0';
#if CHPRINTF_USE_FLOAT
fprec = intPow (10, (*fmt)-'0');
#endif
}
width = 0;
while (TRUE) {
c = *fmt++;
if (c >= '0' && c <= '9')
c -= '0';
else if (c == '*')
c = va_arg(ap, int);
else
break;
width = width * 10 + c;
}
precision = 0;
if (c == '.') {
while (TRUE) {
c = *fmt++;
if (c >= '0' && c <= '9') {
c -= '0';
#if CHPRINTF_USE_FLOAT
fprec = intPow (10, c);
#endif
}
else if (c == '*')
c = va_arg(ap, int);
else
break;
precision *= 10;
precision += c;
}
}
void send_command( char *p )
{
while (*p) chSequentialStreamPut( &SD6, *p++ );
}
void spicallback(SPIDriver *spip){
chSequentialStreamPut(&SD1, '.');
spip->txbuf = 0; // terminating null
}
void consolePutChar(int x) {
chSequentialStreamPut(getConsoleChannel(), (uint8_t )(x));
}
