src/main/io/opflow_cxof.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <ctype.h>
  28 #include <math.h>
  29
  30 #include "platform.h"
  31
  32 #include "build/build_config.h"
  33 #include "build/debug.h"
  34
  35 #include "common/maths.h"
  36
  37 #include "io/serial.h"
  38
  39 #if defined(USE_OPFLOW_CXOF)
  40 #include "drivers/opflow/opflow_virtual.h"
  41 #include "drivers/time.h"
  42 #include "io/opflow.h"
  43
  44 #define CXOF_PACKET_SIZE    9
  45 static serialPort_t * flowPort = NULL;
  46 static serialPortConfig_t * portConfig;
  47 static uint8_t  buffer[10];
  48 static int      bufferPtr;
  49
  50 typedef struct __attribute__((packed)) {
  51     uint8_t     header;     // 0xFE
  52     uint8_t     res0;       // Seems to be 0x04 all the time
  53     int16_t     motionX;
  54     int16_t     motionY;
  55     int8_t      motionT;    // ???
  56     uint8_t     squal;      // Not sure about this
  57     uint8_t     footer;     // 0xAA
  58 } cxofPacket_t;
  59
  60 static bool cxofOpflowDetect(void)
  61 {
  62     portConfig = findSerialPortConfig(FUNCTION_OPTICAL_FLOW);
  63     if (!portConfig) {
  64         return false;
  65     }
  66
  67     return true;
  68 }
  69
  70 static bool cxofOpflowInit(void)
  71 {
  72     if (!portConfig) {
  73         return false;
  74     }
  75
  76     flowPort = openSerialPort(portConfig->identifier, FUNCTION_OPTICAL_FLOW, NULL, NULL, baudRates[portConfig->gps_baudrateIndex], MODE_RX, SERIAL_NOT_INVERTED);
  77     if (!flowPort) {
  78         return false;
  79     }
  80
  81     bufferPtr = 0;
  82
  83     return true;
  84 }
  85
  86 static bool cxofOpflowUpdate(opflowData_t * data)
  87 {
  88     static timeUs_t previousTimeUs = 0;
  89     const timeUs_t currentTimeUs = micros();
  90
  91     bool newPacket = false;
  92     opflowData_t tmpData = { 0 };
  93
  94     while (serialRxBytesWaiting(flowPort) > 0) {
  95         uint8_t c = serialRead(flowPort);
  96
  97         // Wait for header
  98         if (bufferPtr == 0) {
  99             if (c != 0xFE) {
 100                 break;
 101             }
 102         }
 103
 104         // Consume received byte
 105         if (bufferPtr < CXOF_PACKET_SIZE) {
 106             buffer[bufferPtr++] = c;
 107
 108             if (bufferPtr == CXOF_PACKET_SIZE) {
 109                 cxofPacket_t * pkt = (cxofPacket_t *)&buffer[0];
 110
 111                 if (pkt->header == 0xFE && pkt->footer == 0xAA) {
 112                     // Valid packet
 113                     tmpData.deltaTime += (currentTimeUs - previousTimeUs);
 114                     tmpData.flowRateRaw[0] += pkt->motionX;
 115                     tmpData.flowRateRaw[1] += pkt->motionY;
 116                     tmpData.flowRateRaw[2] = 0;
 117                     tmpData.quality = (constrain(pkt->squal, 64, 78) - 64) * 100 / 14;
 118
 119                     previousTimeUs = currentTimeUs;
 120                     newPacket = true;
 121                 }
 122
 123                 // Reset the decoder
 124                 bufferPtr = 0;
 125             }
 126         }
 127         else {
 128             // In case of buffer overflow - reset the decoder
 129             bufferPtr = 0;
 130         }
 131     }
 132
 133     if (newPacket) {
 134         *data = tmpData;
 135     }
 136
 137     return newPacket;
 138 }
 139
 140 virtualOpflowVTable_t opflowCxofVtable = {
 141     .detect = cxofOpflowDetect,
 142     .init = cxofOpflowInit,
 143     .update = cxofOpflowUpdate
 144 };
 145
 146 #endif