cob detection

[aversive.git] / projects / microb2010 / mainboard / cmdline.c

diff --git a/projects/microb2010/mainboard/cmdline.c b/projects/microb2010/mainboard/cmdline.c
index 7c3ec9e..3ae4d30 100644 (file)
--- a/projects/microb2010/mainboard/cmdline.c
+++ b/projects/microb2010/mainboard/cmdline.c
@@ -1,7 +1,7 @@
-/*  
+/*
  *  Copyright Droids Corporation
  *  Olivier Matz <zer0@droids-corp.org>
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -26,12 +26,14 @@
 #include <aversive.h>
 #include <aversive/error.h>
 
+#include <hostsim.h>
+
 #include <parse.h>
 #include <rdline.h>
 #include <ax12.h>
 #include <uart.h>
 #include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
 
 #include <pid.h>
 #include <quadramp.h>
@@ -53,13 +55,13 @@
 /******** See in commands.c for the list of commands. */
 extern parse_pgm_ctx_t main_ctx[];
 
-static void write_char(char c) 
+static void write_char(char c)
 {
        uart_send(CMDLINE_UART, c);
 }
 
-static void 
-valid_buffer(const char *buf, uint8_t size) 
+static void
+valid_buffer(const char *buf, uint8_t size)
 {
        int8_t ret;
 
@@ -76,7 +78,7 @@ valid_buffer(const char *buf, uint8_t size)
                printf_P(PSTR("Bad arguments\r\n"));
 }
 
-static int8_t 
+static int8_t
 complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
                int16_t *state)
 {
@@ -88,34 +90,40 @@ complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
 void emergency(char c)
 {
        static uint8_t i = 0;
-       
+
        /* interrupt traj here */
        if (c == '\003')
                interrupt_traj();
-       
+
        if ((i == 0 && c == 'p') ||
            (i == 1 && c == 'o') ||
-           (i == 2 && c == 'p')) 
+           (i == 2 && c == 'p'))
                i++;
        else if ( !(i == 1 && c == 'p') )
                i = 0;
-       if (i == 3)
+       if (i == 3) {
+#ifdef HOST_VERSION
+               hostsim_exit();
+#endif
                reset();
+       }
 }
 
 /* log function, add a command to configure
  * it dynamically */
-void mylog(struct error * e, ...) 
+void mylog(struct error * e, ...)
 {
        va_list ap;
+#ifndef HOST_VERSION
        u16 stream_flags = stdout->flags;
+#endif
        uint8_t i;
        time_h tv;
 
        if (e->severity > ERROR_SEVERITY_ERROR) {
                if (gen.log_level < e->severity)
                        return;
-               
+
                for (i=0; i<NB_LOGS+1; i++)
                        if (gen.logs[i] == e->err_num)
                                break;
@@ -125,17 +133,19 @@ void mylog(struct error * e, ...)
 
        va_start(ap, e);
        tv = time_get_time();
-       printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
-       
+       printf_P(PSTR("%d.%.3d: "), (int)tv.s, (int)(tv.us/1000UL));
+
        printf_P(PSTR("(%d,%d,%d) "),
                 position_get_x_s16(&mainboard.pos),
                 position_get_y_s16(&mainboard.pos),
                 position_get_a_deg_s16(&mainboard.pos));
-       
+
        vfprintf_P(stdout, e->text, ap);
        printf_P(PSTR("\r\n"));
        va_end(ap);
+#ifndef HOST_VERSION
        stdout->flags = stream_flags;
+#endif
 }
 
 int cmdline_interact(void)
@@ -143,14 +153,14 @@ int cmdline_interact(void)
        const char *history, *buffer;
        int8_t ret, same = 0;
        int16_t c;
-       
+
        rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
-       snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > ");       
+       snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > ");
        rdline_newline(&gen.rdl, gen.prompt);
 
        while (1) {
                c = uart_recv_nowait(CMDLINE_UART);
-               if (c == -1) 
+               if (c == -1)
                        continue;
                ret = rdline_char_in(&gen.rdl, c);
                if (ret != 2 && ret != 0) {