rc_proto.c

   1 /*
   2  * Copyright (c) 2012, Olivier MATZ <zer0@droids-corp.org>
   3  * All rights reserved.
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions are met:
   6  *
   7  *     * Redistributions of source code must retain the above copyright
   8  *       notice, this list of conditions and the following disclaimer.
   9  *     * Redistributions in binary form must reproduce the above copyright
  10  *       notice, this list of conditions and the following disclaimer in the
  11  *       documentation and/or other materials provided with the distribution.
  12  *     * Neither the name of the University of California, Berkeley nor the
  13  *       names of its contributors may be used to endorse or promote products
  14  *       derived from this software without specific prior written permission.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
  17  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19  * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
  20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26  */
  27
  28 #include <stdio.h>
  29 #include <stdlib.h>
  30 #include <stdint.h>
  31 #include <event.h>
  32 #include <linux/joystick.h>
  33
  34 #include <cmdline_parse.h>
  35 #include <cmdline.h>
  36
  37 #include "rc_proto.h"
  38 #include "xbee_proto.h"
  39 #include "joystick.h"
  40
  41 #include "main.h"
  42
  43
  44 struct power_levels {
  45         int ttl;
  46         int power_db;
  47 };
  48
  49 struct power_levels power_levels[MAX_POWER_LEVEL];
  50 static struct event power_level_event;
  51
  52 static uint8_t power_level_global;
  53
  54 uint64_t rc_send_dstaddr = 0xFFFF; /* broadcast by default */
  55
  56 static int set_power_level(void *frame, unsigned len, void *arg)
  57 {
  58         struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
  59         int level = (intptr_t)arg;
  60         uint8_t db;
  61         db = atresp->data[0];
  62
  63         power_levels[level].power_db = db;
  64         power_levels[level].ttl = 2;
  65         return 0;
  66 }
  67
  68 void rc_proto_rx_range(int power_level)
  69 {
  70         xbeeapp_send_atcmd("DB", NULL, 0, 0,
  71                            set_power_level, (void *)(intptr_t)power_level);
  72
  73 }
  74
  75
  76 static void compute_best_power_cb(int s, short event, void *arg)
  77 {
  78         struct timeval tv;
  79         int i;
  80         int best_power_level = MAX_POWER_LEVEL - 1;
  81
  82         for (i = 0; i < MAX_POWER_LEVEL; i++) {
  83                 if (power_levels[i].ttl > 0)
  84                         power_levels[i].ttl--;
  85         }
  86         for (i = 0; i < MAX_POWER_LEVEL; i++) {
  87                 if (power_levels[i].ttl == 0)
  88                         continue;
  89                 if (power_levels[i].power_db < RX_DB_THRESHOLD) {
  90                         best_power_level = i;
  91                         break;
  92                 }
  93         }
  94         /*
  95         if (power_level_global != best_power_level)
  96                 printf("changing power level %d => %d\n",
  97                        power_level_global, best_power_level);
  98         */
  99         power_level_global = best_power_level;
 100
 101         tv.tv_sec = 1;
 102         tv.tv_usec = 0;
 103         evtimer_add(&power_level_event, &tv);
 104 }
 105
 106 void rc_proto_send_channels(void)
 107 {
 108         struct rc_proto_channel rc_chan;
 109         int i;
 110
 111         xbeeapp_send_atcmd("PL", &power_level_global,
 112                            sizeof(power_level_global), 0, NULL, NULL);
 113         rc_chan.type = RC_PROTO_TYPE_CHANNEL;
 114         for (i = 0; i< AXIS_NUMBER; i++){
 115                 rc_chan.axis[i] = htons(joyinfo.axis[i]);
 116         }
 117         xbeeapp_send_msg(rc_send_dstaddr, &rc_chan, sizeof(rc_chan), 0);
 118 }
 119
 120 void rc_proto_init(void)
 121 {
 122         struct timeval tv;
 123
 124         evtimer_set(&power_level_event, compute_best_power_cb, NULL);
 125         tv.tv_sec = 1;
 126         tv.tv_usec = 0;
 127         evtimer_add(&power_level_event, &tv);
 128 }
 129