move xbee rx in rc proto

[protos/xbee-avr.git] / rc_proto.c

diff --git a/rc_proto.c b/rc_proto.c
index 442eb90..4ced123 100644 (file)
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -243,3 +243,61 @@ int8_t buf2servo(uint8_t *buf, uint8_t len)
        return 0;
 }
 #endif
+
+int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
+{
+       unsigned int datalen;
+       struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
+
+       if (len <  sizeof(*recvframe))
+               return -1;
+
+       datalen = len - sizeof(*recvframe);
+       if (datalen < sizeof(struct rc_proto_hdr))
+               return -1;
+
+       switch (rch->type) {
+#if 0
+               case RC_PROTO_TYPE_CHANNEL: {
+                       struct rc_proto_channel *rcc =
+                               (struct rc_proto_channel *) recvframe->data;
+                       int16_t val;
+                       if (datalen != sizeof(struct rc_proto_channel))
+                               return -1;
+                       val = ntohs(rcc->axis[0]);
+                       val >>= 6;
+                       val += 512;
+                       spi_servo_set(0, val);
+                       break;
+               }
+#endif
+               case RC_PROTO_POWER_PROBE: {
+                       struct rc_proto_power_probe *rcpb =
+                               (struct rc_proto_power_probe *) recvframe->data;
+
+                       if (datalen != sizeof(*rcpb))
+                               return -1;
+
+                       if (rcpb->power_level >= MAX_POWER_LEVEL)
+                               return -1;
+
+                       //rc_proto_rx_range(rcpb->power_level);
+
+                       break;
+               }
+
+               case RC_PROTO_HELLO: {
+                       struct rc_proto_hello *rch =
+                               (struct rc_proto_hello *) recvframe->data;
+
+                       NOTICE(E_USER_XBEE, "recv hello len=%d",
+                               rch->datalen);
+                       /* XXX stats */
+                       break;
+               }
+               default:
+                       return -1;
+       }
+
+       return 0;
+}