src/main/io/rangefinder_usd1_v0.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 <ctype.h>
  27
  28
  29 #include "platform.h"
  30 #include "io/serial.h"
  31 #include "drivers/time.h"
  32
  33 #if defined(USE_RANGEFINDER_USD1_V0)
  34 #include "drivers/rangefinder/rangefinder_virtual.h"
  35
  36 #define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48)
  37
  38 #define USD1_PACKET_SIZE 3
  39 #define USD1_KEEP_DATA_TIMEOUT 2000 // 2s
  40
  41
  42 static serialPort_t * serialPort = NULL;
  43 static serialPortConfig_t * portConfig;
  44
  45 static bool hasNewData = false;
  46 static bool hasEverData = false;
  47 static uint8_t  lineBuf[USD1_PACKET_SIZE];
  48 static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
  49 static timeMs_t lastProtocolActivityMs;
  50
  51 static bool usd1RangefinderDetect(void)
  52 {
  53     portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
  54     if (!portConfig) {
  55         return false;
  56     }
  57
  58     return true;
  59 }
  60
  61 static void usd1RangefinderInit(void)
  62 {
  63     if (!portConfig) {
  64         return;
  65     }
  66
  67     serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
  68     if (!serialPort) {
  69         return;
  70     }
  71
  72     lastProtocolActivityMs = 0;
  73 }
  74
  75 static void usd1RangefinderUpdate(void)
  76 {
  77     float sum = 0;
  78     uint16_t count = 0;
  79     uint8_t  index = 0;
  80
  81     while (serialRxBytesWaiting(serialPort) > 0) {
  82         uint8_t c = serialRead(serialPort);
  83
  84         if (c == USD1_HDR_V0 && index == 0) {
  85             lineBuf[index] = c;
  86             index = 1;
  87             continue;
  88         }
  89
  90         if (index > 0) {
  91             lineBuf[index] = c;
  92             index++;
  93             if (index == 3) {
  94                 index = 0;
  95                 sum += (float)((lineBuf[2]&0x7F) * 128 + (lineBuf[1]&0x7F));
  96                 count++;
  97             }
  98         }
  99     }
 100
 101     if (count == 0) {
 102         return;
 103     }
 104
 105     hasNewData = true;
 106     hasEverData = true;
 107     lastProtocolActivityMs = millis();
 108     sensorData = (int32_t)(2.5f * sum / (float)count);
 109 }
 110
 111 static int32_t usd1RangefinderGetDistance(void)
 112 {
 113     int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
 114
 115     if (hasNewData) {
 116         hasNewData = false;
 117         return altitude;
 118     }
 119     else {
 120         //keep last value for timeout, because radar sends data only if change
 121         if ((millis() - lastProtocolActivityMs) < USD1_KEEP_DATA_TIMEOUT) {
 122             return altitude;
 123         }
 124
 125         return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
 126     }
 127 }
 128
 129 virtualRangefinderVTable_t rangefinderUSD1Vtable = {
 130     .detect = usd1RangefinderDetect,
 131     .init = usd1RangefinderInit,
 132     .update = usd1RangefinderUpdate,
 133     .read = usd1RangefinderGetDistance
 134 };
 135
 136 #endif