src/main/io/rangefinder_nanoradar.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 #include "platform.h"
  29 #include "io/serial.h"
  30 #include "drivers/time.h"
  31
  32 #if defined(USE_RANGEFINDER_NANORADAR)
  33 #include "drivers/rangefinder/rangefinder_virtual.h"
  34
  35 #define NANORADAR_HDR 0xAA // Header Byte
  36 #define NANORADAR_END 0x55
  37
  38 #define NANORADAR_COMMAND_TARGET_INFO 0x70C
  39
  40 typedef struct __attribute__((packed)) {
  41     uint8_t     header0;
  42     uint8_t     header1;
  43     uint8_t     commandH;
  44     uint8_t     commandL;
  45     uint8_t     index; // Target ID
  46     uint8_t     rcs; // The section of radar reflection
  47     uint8_t     rangeH; // Target distance high 8 bi
  48     uint8_t     rangeL; // Target distance low 8 bit
  49     uint8_t     rsvd1;
  50     uint8_t     info; // VrelH Rsvd1 RollCount
  51     uint8_t     vrelL;
  52     uint8_t     SNR; // Signal-Noise Ratio
  53     uint8_t     end0;
  54     uint8_t     end1;
  55 } nanoradarPacket_t;
  56
  57 #define NANORADAR_PACKET_SIZE    sizeof(nanoradarPacket_t)
  58 #define NANORADAR_TIMEOUT_MS     2000 // 2s
  59
  60 static bool hasNewData = false;
  61 static bool hasEverData = false;
  62 static serialPort_t * serialPort = NULL;
  63 static serialPortConfig_t * portConfig;
  64 static uint8_t buffer[NANORADAR_PACKET_SIZE];
  65 static unsigned bufferPtr;
  66
  67 static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
  68 static timeMs_t lastProtocolActivityMs;
  69
  70 static bool nanoradarRangefinderDetect(void)
  71 {
  72     portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
  73     if (!portConfig) {
  74         return false;
  75     }
  76
  77     return true;
  78 }
  79
  80 static void nanoradarRangefinderInit(void)
  81 {
  82     if (!portConfig) {
  83         return;
  84     }
  85
  86     serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
  87     if (!serialPort) {
  88         return;
  89     }
  90
  91     lastProtocolActivityMs = 0;
  92 }
  93
  94 static void nanoradarRangefinderUpdate(void)
  95 {
  96
  97     nanoradarPacket_t *nanoradarPacket = (nanoradarPacket_t *)buffer;
  98     while (serialRxBytesWaiting(serialPort) > 0) {
  99         uint8_t c = serialRead(serialPort);
 100
 101         if (bufferPtr < NANORADAR_PACKET_SIZE) {
 102             buffer[bufferPtr++] = c;
 103         }
 104
 105         if ((bufferPtr == 1) && (nanoradarPacket->header0 != NANORADAR_HDR)) {
 106             bufferPtr = 0;
 107             continue;
 108         }
 109
 110         if ((bufferPtr == 2) && (nanoradarPacket->header1 != NANORADAR_HDR)) {
 111             bufferPtr = 0;
 112             continue;
 113         }
 114
 115         //only target info packet we are interested
 116         if (bufferPtr == 4) {
 117             uint16_t command = nanoradarPacket->commandH + (nanoradarPacket->commandL * 0x100);
 118
 119             if (command != NANORADAR_COMMAND_TARGET_INFO) {
 120                 bufferPtr = 0;
 121                 continue;
 122             }
 123         }
 124
 125         // Check for complete packet
 126         if (bufferPtr == NANORADAR_PACKET_SIZE) {
 127             if (nanoradarPacket->end0 == NANORADAR_END && nanoradarPacket->end1 == NANORADAR_END) {
 128                 // Valid packet
 129                 hasNewData = true;
 130                 hasEverData = true;
 131                 lastProtocolActivityMs = millis();
 132
 133                 sensorData = ((nanoradarPacket->rangeH * 0x100) + nanoradarPacket->rangeL);
 134             }
 135
 136             // Prepare for new packet
 137             bufferPtr = 0;
 138         }
 139     }
 140 }
 141
 142 static int32_t nanoradarRangefinderGetDistance(void)
 143 {
 144     int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
 145
 146     if (hasNewData) {
 147         hasNewData = false;
 148         return altitude;
 149     }
 150     else {
 151         //keep last value for timeout, because radar sends data only if change
 152         if ((millis() - lastProtocolActivityMs) < NANORADAR_TIMEOUT_MS) {
 153             return altitude;
 154         }
 155
 156         return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
 157     }
 158 }
 159
 160 virtualRangefinderVTable_t rangefinderNanoradarVtable = {
 161     .detect = nanoradarRangefinderDetect,
 162     .init = nanoradarRangefinderInit,
 163     .update = nanoradarRangefinderUpdate,
 164     .read = nanoradarRangefinderGetDistance
 165 };
 166
 167 #endif