src/main/drivers/irlock.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 #include "drivers/sensor.h"
  26 #include "drivers/irlock.h"
  27 #include "drivers/time.h"
  28
  29 #define IRLOCK_OBJECT_SYNC ((uint16_t)0xaa55)
  30 #define IRLOCK_FRAME_SYNC ((uint32_t)(IRLOCK_OBJECT_SYNC | (IRLOCK_OBJECT_SYNC << 16)))
  31
  32
  33 #if defined(USE_IRLOCK)
  34
  35 static bool irlockHealthy = false;
  36
  37 static bool irlockFrameSync(irlockDev_t *irlockDev)
  38 {
  39     uint32_t sync_word = 0;
  40     uint8_t count = 10;
  41     while (count-- && sync_word != IRLOCK_FRAME_SYNC) {
  42         uint8_t sync_byte;
  43         irlockHealthy = busRead(irlockDev->busDev, 0xFF, &sync_byte);
  44         if (!(irlockHealthy && sync_byte)) return false;
  45         sync_word = (sync_word >> 8) | (((uint32_t)sync_byte) << 24);
  46     }
  47     return sync_word == IRLOCK_FRAME_SYNC;
  48 }
  49
  50 static bool irlockRead(irlockDev_t *irlockDev, irlockData_t *irlockData)
  51 {
  52     if (irlockFrameSync(irlockDev) && busReadBuf(irlockDev->busDev, 0xFF, (void*)irlockData, sizeof(*irlockData))) {
  53         uint16_t cksum = irlockData->signature + irlockData->posX + irlockData->posY + irlockData->sizeX + irlockData->sizeY;
  54         if (irlockData->cksum == cksum) return true;
  55     }
  56     return false;
  57 }
  58
  59 static bool deviceDetect(irlockDev_t *irlockDev)
  60 {
  61     uint8_t buf;
  62     bool detected = busRead(irlockDev->busDev, 0xFF, &buf);
  63     return !!detected;
  64 }
  65
  66 bool irlockDetect(irlockDev_t *irlockDev)
  67 {
  68     irlockDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_IRLOCK, 0, OWNER_IRLOCK);
  69     if (irlockDev->busDev == NULL) {
  70         return false;
  71     }
  72
  73     if (!deviceDetect(irlockDev)) {
  74         busDeviceDeInit(irlockDev->busDev);
  75         return false;
  76     }
  77
  78     irlockDev->read = irlockRead;
  79
  80     return true;
  81 }
  82
  83 bool irlockIsHealthy(void)
  84 {
  85     return irlockHealthy;
  86 }
  87
  88 #endif /* USE_IRLOCK */