flight/pios/common/pios_i2c_esc.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_I2C_ESC Code for controlling I2C based ESCs
   6  * @brief Deals with the hardware interface to the magnetometers
   7  * @{
   8  *
   9  * @file       pios_i2c_esc.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      HMC5843 Magnetic Sensor Functions from AHRS
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  ******************************************************************************
  15  */
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "pios.h"
  33
  34 #ifdef PIOS_INCLUDE_I2C_ESC
  35
  36 /* Known i2c ESC addresses */
  37 #define MK_I2C_ADDR     0x29
  38 #define ASTEC4_I2C_ADDR 0x02
  39
  40 bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed);
  41
  42 uint8_t base_address = MK_I2C_ADDR;
  43 uint8_t valid_motors = 0;
  44 bool PIOS_I2C_ESC_Config()
  45 {
  46     base_address = MK_I2C_ADDR;
  47     valid_motors = 0;
  48     for (uint8_t i = 0; i < 4; i++) {
  49         if (PIOS_SetMKSpeed(i, 0)) {
  50             valid_motors |= (1 << i);
  51         }
  52     }
  53
  54     return true;
  55 }
  56
  57 bool PIOS_I2C_ESC_SetSpeed(uint8_t speed[4])
  58 {
  59     /*bool success = true;
  60        for(uint8_t i = 0; i < 4; i++) {
  61             //if(valid_motors & (1 << i))
  62                     success &= PIOS_SetMKSpeed(i, speed[i]);
  63        }
  64        return success; */
  65
  66     const struct pios_i2c_txn txn_list[] = {
  67         {
  68             .info = __func__,
  69             .addr = MK_I2C_ADDR + 0,
  70             .rw   = PIOS_I2C_TXN_WRITE,
  71             .len  = sizeof(speed[0]),
  72             .buf  = &speed[0],
  73         },
  74         {
  75             .info = __func__,
  76             .addr = MK_I2C_ADDR + 1,
  77             .rw   = PIOS_I2C_TXN_WRITE,
  78             .len  = sizeof(speed[1]),
  79             .buf  = &speed[1],
  80         },
  81         {
  82             .info = __func__,
  83             .addr = MK_I2C_ADDR + 2,
  84             .rw   = PIOS_I2C_TXN_WRITE,
  85             .len  = sizeof(speed[2]),
  86             .buf  = &speed[2],
  87         },
  88         {
  89             .info = __func__,
  90             .addr = MK_I2C_ADDR + 3,
  91             .rw   = PIOS_I2C_TXN_WRITE,
  92             .len  = sizeof(speed[3]),
  93             .buf  = &speed[3],
  94         }
  95     };
  96
  97     return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list));
  98 }
  99
 100 bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed)
 101 {
 102     static uint8_t speeds[4] = { 0, 0, 0, 0 };
 103
 104     if (motornum >= 4) {
 105         return false;
 106     }
 107
 108     if (speeds[motornum] == speed) {
 109         return true;
 110     }
 111
 112     const struct pios_i2c_txn txn_list[] = {
 113         {
 114             .info = __func__,
 115             .addr = MK_I2C_ADDR + motornum,
 116             .rw   = PIOS_I2C_TXN_WRITE,
 117             .len  = sizeof(speed),
 118             .buf  = &speed,
 119         }
 120     };
 121
 122     return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list));
 123 }
 124
 125 bool PIOS_SetAstec4Address(uint8_t new_address)
 126 {
 127     if ((new_address < 0) || (new_address > 4)) {
 128         return false;
 129     }
 130
 131     uint8_t data[4] = { 250, 0, new_address, 230 + new_address };
 132
 133     const struct pios_i2c_txn txn_list[] = {
 134         {
 135             .info = __func__,
 136             .addr = ASTEC4_I2C_ADDR,
 137             .rw   = PIOS_I2C_TXN_WRITE,
 138             .len  = sizeof(data),
 139             .buf  = &data[0],
 140         }
 141     };
 142
 143     return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list));
 144 }
 145
 146 bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed)
 147 {
 148     static uint8_t speeds[5] = { 0, 0, 0, 0 };
 149
 150     if ((motornum < 0) || (motornum >= 4)) {
 151         return false;
 152     }
 153
 154     speeds[motornum] = speed;
 155
 156     if (motornum != 3) {
 157         return true;
 158     }
 159
 160     /* Write in chunks of four */
 161     speeds[4] = 0xAA + speeds[0] + speeds[1] + speeds[2] + speeds[3];
 162
 163     const struct pios_i2c_txn txn_list[] = {
 164         {
 165             .info = __func__,
 166             .addr = ASTEC4_I2C_ADDR,
 167             .rw   = PIOS_I2C_TXN_WRITE,
 168             .len  = sizeof(speeds),
 169             .buf  = &speeds[0],
 170         }
 171     };
 172
 173     return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list));
 174 }
 175
 176 #endif /* PIOS_INCLUDE_I2C_ESC */
 177
 178 /**
 179  * @}
 180  * @}
 181  */