X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=rc_proto.c;h=4ced1232883f73e565f5aea04b8b25e8f2eb6799;hp=22b6f28464c72df6758d551eea48eed262718666;hb=0373c0e13f4a81426ee5084c0c1ee148bc6b935a;hpb=c17da94fafe8e0fa1a467f0c8fb7922fec8386e5

diff --git a/rc_proto.c b/rc_proto.c
index 22b6f28..4ced123 100644
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -52,12 +52,17 @@ struct rc_proto_power_levels {
 static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
 
 /* update power level when we receive the answer from DB */
-static int update_power_level(void *frame, unsigned len, void *arg)
+static int8_t update_power_level(int8_t retcode, void *frame, unsigned len,
+	void *arg)
 {
 	struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
 	int level = (intptr_t)arg;
 	uint8_t db;
 
+	/* nothing more to do, error is already logged by xbee_user */
+	if (retcode < 0)
+		return retcode;
+
 	/* XXX check if this test is correct */
 	if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t)) {
 		/* XXX stats */
@@ -74,26 +79,25 @@ static int update_power_level(void *frame, unsigned len, void *arg)
 void rc_proto_rx_power_probe(int power_level)
 {
 	(void)power_level;
-	xbeeapp_send_atcmd("DB", NULL, 0, 0, update_power_level, NULL);
+	xbeeapp_send_atcmd("DB", NULL, 0, update_power_level, NULL);
 }
 
 /* send a hello message */
-// XXX iovec for xbee ?
 int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len)
 {
-	struct {
-		struct rc_proto_echo_req hdr;
-		char buf[XBEE_MAX_FRAME_LEN - sizeof(struct rc_proto_echo_req)];
-	} frame;
+	struct rc_proto_echo_req hdr;
+	struct xbee_msg msg;
 
-	if (data_len > sizeof(frame.buf))
-		return -1;
+	hdr.type = RC_PROTO_HELLO;
+	hdr.datalen = data_len;
 
-	frame.hdr.type = RC_PROTO_HELLO;
-	frame.hdr.datalen = data_len;
-	memcpy(frame.buf, data, data_len);
-	return xbeeapp_send_msg(addr, &frame,
-				data_len + sizeof(struct rc_proto_echo_req), 1);
+	msg.iovlen = 2;
+	msg.iov[0].buf = &hdr;
+	msg.iov[0].len = sizeof(hdr);
+	msg.iov[1].buf = data;
+	msg.iov[1].len = data_len;
+
+	return xbeeapp_send_msg(addr, &msg, NULL, NULL);
 }
 
 
@@ -239,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;
+}