From 93e278d345d286301b3d0b4fe209dd04c734d7bf Mon Sep 17 00:00:00 2001 From: Fabrice Desclaux Date: Mon, 20 Feb 2012 22:46:45 +0100 Subject: [PATCH] rc_proto: choose best power level for rc transmit --- commands.c | 9 +---- main.c | 54 ++++++++++++++++++++++-------- main.h | 2 ++ rc_proto.c | 96 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ rc_proto.h | 12 +++++++ 5 files changed, 152 insertions(+), 21 deletions(-) diff --git a/commands.c b/commands.c index 1489cb5..9e000bd 100644 --- a/commands.c +++ b/commands.c @@ -76,7 +76,6 @@ static int range_cur_count = 0; static struct event rc_send_event; static int rc_send_period_ms = 100; -static uint64_t rc_send_dstaddr = 0xFFFF; /* broadcast by default */ static int rc_send_running = 0; static const char *xbee_logfilename = "/tmp/xbee.log"; @@ -134,14 +133,8 @@ static void rc_send_cb(int fd, short event, void *arg) { struct cmdline *cl = arg; struct timeval tv; - struct rc_proto_channel rc_chan; - int i; - rc_chan.type = RC_PROTO_TYPE_CHANNEL; - for (i = 0; i< AXIS_NUMBER; i++){ - rc_chan.axis[i] = htons(joyinfo.axis[i]); - } - xbeeapp_send_msg(rc_send_dstaddr, &rc_chan, sizeof(rc_chan), 0); + rc_proto_send_channels(); evtimer_set(&rc_send_event, rc_send_cb, cl); tv.tv_sec = 0; diff --git a/main.c b/main.c index 4847d11..fd658a0 100644 --- a/main.c +++ b/main.c @@ -59,6 +59,7 @@ #include "xbee_proto.h" #include "xbee.h" #include "joystick.h" +#include "rc_proto.h" #include "main.h" #define TIMEOUT_US 1000000 @@ -260,6 +261,41 @@ static int dump_atcmd(struct xbee_ctx *ctx, struct xbee_atresp_hdr *frame, return 0; } +int xbee_recv_data(struct xbee_recv_hdr *recvframe, unsigned len) +{ + int datalen = len - sizeof(*recvframe); + struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data; + + if (datalen < sizeof(struct rc_proto_hdr)) + return -1; + + switch (rch->type) { + case RC_PROTO_TYPE_CHANNEL: + if (datalen != sizeof(struct rc_proto_channel)) + return -1; + break; + case RC_PROTO_TYPE_RANGE: { + struct rc_proto_range *rcr = + (struct rc_proto_range *) recvframe->data; + + if (datalen != sizeof(struct rc_proto_range)) + return -1; + + if (rcr->power_level >= MAX_POWER_LEVEL) + return -1; + + rc_proto_rx_range(rcr->power_level); + + break; + } + default: + return -1; + } + + return 0; +} + + void xbee_rx(struct xbee_dev *dev, int channel, int type, void *frame, unsigned len, void *opaque) { @@ -326,19 +362,8 @@ void xbee_rx(struct xbee_dev *dev, int channel, int type, } case XBEE_TYPE_RECV: { - struct xbee_recv_hdr *recvframe = frame; - int recvlen = len - sizeof(*recvframe); - int on_stdout = 1; - - /* if we receive a range-test frame, ask for RSSI now */ - if (recvlen >= strlen("range") && - !strncmp((char *)recvframe->data, - "range", strlen("range"))) { - xbeeapp_send_atcmd("DB", NULL, 0, 0, NULL, NULL); - on_stdout = 0; - } - hexdump(on_stdout, "rx data", recvframe->data, - recvlen); + if (xbee_recv_data(frame, len) < 0) + do_hexdump = 1; break; } @@ -781,6 +806,9 @@ int main(int argc, char **argv) if (err < 0) return -1; + /* init rc_proto */ + rc_proto_init(); + /* init joystick */ if (joystick_devname != NULL) { if (joystick_init(joystick_devname, &joyinfo) < 0) { diff --git a/main.h b/main.h index 693a5c8..88629df 100644 --- a/main.h +++ b/main.h @@ -32,6 +32,8 @@ enum xbee_ctx_type { DUMP_CONF, }; +#define MAX_POWER_LEVEL 5 + /* used for timeouts and xbee rx callback */ struct xbee_ctx { enum xbee_ctx_type type; diff --git a/rc_proto.c b/rc_proto.c index 4242477..495ddcc 100644 --- a/rc_proto.c +++ b/rc_proto.c @@ -28,6 +28,102 @@ #include #include #include +#include +#include + +#include +#include #include "rc_proto.h" +#include "xbee_proto.h" +#include "joystick.h" + +#include "main.h" + + +struct power_levels { + int ttl; + int power_db; +}; + +struct power_levels power_levels[MAX_POWER_LEVEL]; +static struct event power_level_event; + +static uint8_t power_level_global; + +uint64_t rc_send_dstaddr = 0xFFFF; /* broadcast by default */ + +static int set_power_level(void *frame, unsigned len, void *arg) +{ + struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame; + int level = (intptr_t)arg; + uint8_t db; + db = atresp->data[0]; + + power_levels[level].power_db = db; + power_levels[level].ttl = 2; + return 0; +} + +void rc_proto_rx_range(int power_level) +{ + xbeeapp_send_atcmd("DB", NULL, 0, 0, + set_power_level, (void *)(intptr_t)power_level); + +} + + +static void compute_best_power_cb(int s, short event, void *arg) +{ + struct timeval tv; + int i; + int best_power_level = MAX_POWER_LEVEL - 1; + + for (i = 0; i < MAX_POWER_LEVEL; i++) { + if (power_levels[i].ttl > 0) + power_levels[i].ttl--; + } + for (i = 0; i < MAX_POWER_LEVEL; i++) { + if (power_levels[i].ttl == 0) + continue; + if (power_levels[i].power_db < RX_DB_THRESHOLD) { + best_power_level = i; + break; + } + } + /* + if (power_level_global != best_power_level) + printf("changing power level %d => %d\n", + power_level_global, best_power_level); + */ + power_level_global = best_power_level; + + tv.tv_sec = 1; + tv.tv_usec = 0; + evtimer_add(&power_level_event, &tv); +} + +void rc_proto_send_channels(void) +{ + struct rc_proto_channel rc_chan; + int i; + + xbeeapp_send_atcmd("PL", &power_level_global, + sizeof(power_level_global), 0, NULL, NULL); + rc_chan.type = RC_PROTO_TYPE_CHANNEL; + for (i = 0; i< AXIS_NUMBER; i++){ + rc_chan.axis[i] = htons(joyinfo.axis[i]); + } + xbeeapp_send_msg(rc_send_dstaddr, &rc_chan, sizeof(rc_chan), 0); +} + +void rc_proto_init(void) +{ + struct timeval tv; + + evtimer_set(&power_level_event, compute_best_power_cb, NULL); + tv.tv_sec = 1; + tv.tv_usec = 0; + evtimer_add(&power_level_event, &tv); +} diff --git a/rc_proto.h b/rc_proto.h index a9a826b..dd58951 100644 --- a/rc_proto.h +++ b/rc_proto.h @@ -34,8 +34,16 @@ #define RC_PROTO_TYPE_RANGE 1 +#define RX_DB_THRESHOLD 0x40 + /* TODO: Authenticate packet!! */ +extern uint64_t rc_send_dstaddr; + +struct rc_proto_hdr { + uint8_t type; +} __attribute__((packed)); + struct rc_proto_channel { uint8_t type; int16_t axis[AXIS_NUMBER]; @@ -47,4 +55,8 @@ struct rc_proto_range { } __attribute__((packed)); +void rc_proto_rx_range(int power_level); +void rc_proto_send_channels(void); +void rc_proto_init(void); + #endif -- 2.20.1