src/main/io/rangefinder_benewake.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_RANGEFINDER_BENEWAKE)
  40 #include "drivers/rangefinder/rangefinder_virtual.h"
  41 #include "drivers/time.h"
  42 #include "io/rangefinder.h"
  43
  44 typedef struct __attribute__((packed)) {
  45     uint8_t     header0;
  46     uint8_t     header1;
  47     uint8_t     distL;
  48     uint8_t     distH;
  49     uint8_t     strengthL;
  50     uint8_t     strengthH;
  51     uint8_t     res;
  52     uint8_t     rawQual;
  53     uint8_t     checksum;
  54 } tfminiPacket_t;
  55
  56 #define BENEWAKE_PACKET_SIZE    sizeof(tfminiPacket_t)
  57 #define BENEWAKE_MIN_QUALITY    0
  58 #define BENEWAKE_TIMEOUT_MS     200 // 200ms
  59
  60 static serialPort_t * serialPort = NULL;
  61 static serialPortConfig_t * portConfig;
  62 static uint8_t  buffer[BENEWAKE_PACKET_SIZE];
  63 static unsigned bufferPtr;
  64 static timeMs_t lastProtocolActivityMs;
  65
  66 static bool hasNewData = false;
  67 static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
  68
  69 // TFmini command to initiate 100Hz sampling
  70 static const uint8_t initCmd100Hz[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
  71
  72 static bool benewakeRangefinderDetect(void)
  73 {
  74     portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
  75     if (!portConfig) {
  76         return false;
  77     }
  78
  79     return true;
  80 }
  81
  82 static void benewakeRangefinderInit(void)
  83 {
  84     if (!portConfig) {
  85         return;
  86     }
  87
  88     serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
  89     if (!serialPort) {
  90         return;
  91     }
  92
  93     lastProtocolActivityMs = 0;
  94     bufferPtr = 0;
  95 }
  96
  97 static void benewakeRangefinderUpdate(void)
  98 {
  99     // Initialize the sensor
 100     if (lastProtocolActivityMs == 0 || (millis() - lastProtocolActivityMs) > BENEWAKE_TIMEOUT_MS) {
 101         // Initialize the sensor to do 100Hz sampling to make sure we get the most recent data always
 102         serialWriteBuf(serialPort, initCmd100Hz, sizeof(initCmd100Hz));
 103
 104         // Send the init command every BENEWAKE_TIMEOUT_MS
 105         lastProtocolActivityMs = millis();
 106     }
 107
 108     // Process incoming bytes
 109     tfminiPacket_t *tfminiPacket = (tfminiPacket_t *)buffer;
 110     while (serialRxBytesWaiting(serialPort) > 0) {
 111         uint8_t c = serialRead(serialPort);
 112
 113         // Add byte to buffer
 114         if (bufferPtr < BENEWAKE_PACKET_SIZE) {
 115             buffer[bufferPtr++] = c;
 116         }
 117
 118         // Check header bytes
 119         if ((bufferPtr == 1) && (tfminiPacket->header0 != 0x59)) {
 120             bufferPtr = 0;
 121             continue;
 122         }
 123
 124         if ((bufferPtr == 2) && (tfminiPacket->header1 != 0x59)) {
 125             bufferPtr = 0;
 126             continue;
 127         }
 128
 129         // Check for complete packet
 130         if (bufferPtr == BENEWAKE_PACKET_SIZE) {
 131             const uint8_t checksum = buffer[0] + buffer[1] + buffer[2] + buffer[3] + buffer[4] + buffer[5] + buffer[6] + buffer[7];
 132             if (tfminiPacket->checksum == checksum) {
 133                 // Valid packet
 134                 hasNewData = true;
 135                 sensorData = (tfminiPacket->distL << 0) | (tfminiPacket->distH << 8);
 136                 lastProtocolActivityMs = millis();
 137
 138                 uint16_t qual = (tfminiPacket->strengthL << 0) | (tfminiPacket->strengthH << 8);
 139
 140                 if (sensorData == 0 || qual <= BENEWAKE_MIN_QUALITY) {
 141                     sensorData = -1;
 142                 }
 143             }
 144
 145             // Prepare for new packet
 146             bufferPtr = 0;
 147         }
 148     }
 149 }
 150
 151 static int32_t benewakeRangefinderGetDistance(void)
 152 {
 153     if (hasNewData) {
 154         hasNewData = false;
 155         return (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
 156     }
 157     else {
 158         return RANGEFINDER_NO_NEW_DATA;
 159     }
 160 }
 161
 162 virtualRangefinderVTable_t rangefinderBenewakeVtable = {
 163     .detect = benewakeRangefinderDetect,
 164     .init = benewakeRangefinderInit,
 165     .update = benewakeRangefinderUpdate,
 166     .read = benewakeRangefinderGetDistance
 167 };
 168
 169 #endif