imuboard/i2c_protocol.c

   1 /*
   2  * Copyright (c) 2014, 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 <string.h>
  29
  30 #include <aversive.h>
  31 #include <aversive/error.h>
  32
  33 #include <i2c.h>
  34
  35 #include "../common/i2c_commands.h"
  36 #include "gps_venus.h"
  37 #include "imu.h"
  38 #include "main.h"
  39
  40 void i2c_protocol_init(void)
  41 {
  42 }
  43
  44 /*** LED CONTROL ***/
  45 void i2c_led_control(uint8_t l, uint8_t state)
  46 {
  47         switch(l) {
  48         case 1:
  49                 state? LED1_ON():LED1_OFF();
  50                 break;
  51         case 2:
  52                 state? LED2_ON():LED2_OFF();
  53                 break;
  54         default:
  55                 break;
  56         }
  57 }
  58
  59 #ifdef notyet
  60 static void i2c_test(uint16_t val)
  61 {
  62         static uint16_t prev=0;
  63
  64         if ( (val-prev) != 1 ) {
  65                 WARNING(E_USER_I2C_PROTO, "Duplicated msg %d", val);
  66         }
  67         prev = val;
  68         ext.nb_test_cmd ++;
  69 }
  70 #endif
  71
  72 static void i2c_send_status(void)
  73 {
  74         struct i2c_ans_imuboard_status ans;
  75         struct gps_pos gps_pos;
  76         struct imu_euler_int imu_pos;
  77         uint8_t irq_flags;
  78
  79         i2c_flush();
  80         ans.hdr.cmd =  I2C_ANS_IMUBOARD_STATUS;
  81
  82         /* gps */
  83         IRQ_LOCK(irq_flags);
  84         gps_get_pos(&gps_pos);
  85         IRQ_UNLOCK(irq_flags);
  86
  87         /* imu */
  88         IRQ_LOCK(irq_flags);
  89         imu_get_pos_euler_int(&imu_pos);
  90         IRQ_UNLOCK(irq_flags);
  91
  92         ans.flags = 0;
  93         if (imuboard.flags & IMUBOARD_F_BOOT_OK)
  94                 ans.flags |= I2C_IMUBOARD_STATUS_BOOT_OK;
  95         if (imuboard.flags & IMUBOARD_F_SDCARD_OK)
  96                 ans.flags |= I2C_IMUBOARD_STATUS_SDCARD_OK;
  97
  98         if (gps_pos_valid(&gps_pos)) {
  99                 ans.flags |= I2C_IMUBOARD_STATUS_GPS_OK;
 100                 ans.latitude = gps_pos.latitude;
 101                 ans.longitude = gps_pos.longitude;
 102                 ans.altitude = gps_pos.altitude;
 103         }
 104
 105         ans.roll = imu_pos.roll;
 106         ans.pitch = imu_pos.pitch;
 107         ans.yaw = imu_pos.yaw;
 108         i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
 109                         sizeof(ans), I2C_CTRL_GENERIC);
 110 }
 111
 112 void i2c_recvevent(uint8_t * buf, int8_t size)
 113 {
 114         void *void_cmd = buf;
 115
 116 #if 0 /* XXX */
 117         static uint8_t a = 0;
 118
 119         a++;
 120         if (a & 0x10)
 121                 LED2_TOGGLE();
 122
 123         if (size <= 0) {
 124                 goto error;
 125         }
 126 #endif
 127
 128         switch (buf[0]) {
 129
 130         /* Commands (no answer needed) */
 131         case I2C_CMD_GENERIC_LED_CONTROL:
 132                 {
 133                         struct i2c_cmd_led_control *cmd = void_cmd;
 134                         if (size != sizeof (*cmd))
 135                                 goto error;
 136                         i2c_led_control(cmd->led_num, cmd->state);
 137                         break;
 138                 }
 139
 140
 141         /* Add other commands here ...*/
 142
 143         case I2C_REQ_IMUBOARD_STATUS:
 144                 {
 145                         //struct i2c_req_imuboard_status *cmd = void_cmd;
 146                         if (size != sizeof (struct i2c_req_imuboard_status))
 147                                 goto error;
 148
 149                         i2c_send_status();
 150                         break;
 151                 }
 152
 153         default:
 154                 goto error;
 155         }
 156
 157  error:
 158         /* log error on a led ? */
 159         return;
 160 }
 161
 162 void i2c_recvbyteevent(__attribute__((unused)) uint8_t hwstatus,
 163                        __attribute__((unused)) uint8_t i,
 164                        __attribute__((unused)) int8_t c)
 165 {
 166 }
 167
 168 void i2c_sendevent(__attribute__((unused)) int8_t size)
 169 {
 170 }
 171
 172