Fix softwere serial

1 files changed, 17 insertions, 27 deletions

diff --git a/protocol/serial_soft.c b/protocol/serial_soft.c
index 47b1e4571d..af5110b4f6 100644
--- a/protocol/serial_soft.c
+++ b/protocol/serial_soft.c
@@ -48,23 +48,6 @@ POSSIBILITY OF SUCH DAMAGE.
 
 #define WAIT_US     (1000000/SERIAL_BAUD)
 
-#if 1
-#define WAIT_TICK     (1000000/SERIAL_BAUD)
-#define WAIT4(tick) _delay_us(tick)
-#else
-#define WAIT_TICK   ((16000000/SERIAL_BAUD)/4 - 5)
-static inline void  WAIT4(uint8_t tick)
-{
-	__asm__ __volatile__ (
-		"1: dec %0" "\n\t"
-                "nop"       "\n\t"
-		"brne 1b"
-		: 
-		: "r" (tick)
-	);
-}
-#endif
-
 void serial_init(void)
 {
     SERIAL_RXD_INIT();
@@ -90,6 +73,18 @@ uint8_t serial_recv(void)
     return data;
 }
 
+int16_t serial_recv2(void)
+{
+    uint8_t data = 0;
+    if (rbuf_head == rbuf_tail) {
+        return -1;
+    }
+
+    data = rbuf[rbuf_tail];
+    rbuf_tail = (rbuf_tail + 1) % RBUF_SIZE;
+    return data;
+}
+
 void serial_send(uint8_t data)
 {
     /* signal state: IDLE: ON, START: OFF, STOP: ON, DATA0: OFF, DATA1: ON */
@@ -139,13 +134,11 @@ PORTD ^= 1<<7;
 #endif
 
     /* to center of start bit */
-    //_delay_us(WAIT_US/2);
-    WAIT4(WAIT_TICK/2);
+    _delay_us(WAIT_US/2);
 PORTD ^= 1<<7;
     do {
         /* to center of next bit */
-        //_delay_us(WAIT_US);
-        WAIT4(WAIT_TICK);
+        _delay_us(WAIT_US);
 
 PORTD ^= 1<<7;
         if (SERIAL_RXD_READ()) {
@@ -160,18 +153,15 @@ PORTD ^= 1<<7;
     } while (mask);
 
     /* to center of parity bit */
-    //_delay_us(WAIT_US);
-    WAIT4(WAIT_TICK);
+    _delay_us(WAIT_US);
     if (SERIAL_RXD_READ()) { parity ^= 1; }
 PORTD ^= 1<<7;
 
     /* to center of stop bit */
-    //_delay_us(WAIT_US);
-    WAIT4(WAIT_TICK);
+    _delay_us(WAIT_US);
 
     uint8_t next = (rbuf_head + 1) % RBUF_SIZE;
-    //if (parity && next != rbuf_tail) {
-    if (next != rbuf_tail) {
+    if (parity && next != rbuf_tail) {
         rbuf[rbuf_head] = data;
         rbuf_head = next;
     }