src/main/drivers/rangefinder/rangefinder_lidartf.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include "platform.h"
  25
  26 #ifdef USE_RANGEFINDER_TF
  27
  28 #include "build/debug.h"
  29 #include "build/build_config.h"
  30
  31 #include "io/serial.h"
  32
  33 #include "drivers/time.h"
  34 #include "drivers/rangefinder/rangefinder.h"
  35 #include "drivers/rangefinder/rangefinder_lidartf.h"
  36
  37 #define TF_DEVTYPE_NONE 0
  38 #define TF_DEVTYPE_MINI 1
  39 #define TF_DEVTYPE_02   2
  40
  41 static uint8_t tfDevtype = TF_DEVTYPE_NONE;
  42
  43 #define TF_FRAME_LENGTH    6             // Excluding sync bytes (0x59) x 2 and checksum
  44 #define TF_FRAME_SYNC_BYTE 0x59
  45 #define TF_TIMEOUT_MS      (100 * 2)
  46
  47 //
  48 // Benewake TFmini frame format
  49 // Byte Off Description
  50 // 1    -   SYNC
  51 // 2    -   SYNC
  52 // 3    0   Measured distance (LSB)
  53 // 4    1   Measured distance (MSB)
  54 // 5    2   Signal strength (LSB)
  55 // 6    3   Signal strength (MSB)
  56 // 7    4   Integral time
  57 // 8    5   Reserved
  58 // 9    -   Checksum (Unsigned 8-bit sum of bytes 0~7)
  59 //
  60 // Credibility
  61 // 1. If distance is 12m (1200cm), then OoR.
  62 //
  63 #define TF_MINI_FRAME_INTEGRAL_TIME 4
  64
  65 //
  66 // Benewake TF02 frame format (From SJ-GU-TF02-01 Version: A01)
  67 // Byte Off Description
  68 // 1    -   SYNC
  69 // 2    -   SYNC
  70 // 3    0   Measured distance (LSB)
  71 // 4    1   Measured distance (MSB)
  72 // 5    2   Signal strength (LSB)
  73 // 6    3   Signal strength (MSB)
  74 // 7    4   SIG (Reliability in 1~8, less than 7 is unreliable)
  75 // 8    5   TIME (Exposure time, 3 or 6)
  76 // 9    -   Checksum (Unsigned 8-bit sum of bytes 0~7)
  77 //
  78 // Credibility
  79 // 1. If SIG is less than 7, unreliable
  80 // 2. If distance is 22m (2200cm), then OoR.
  81 //
  82 #define TF_02_FRAME_SIG 4
  83
  84 // Maximum ratings
  85
  86 #define TF_MINI_RANGE_MIN 40
  87 #define TF_MINI_RANGE_MAX 1200
  88
  89 #define TF_02_RANGE_MIN 40
  90 #define TF_02_RANGE_MAX 2200
  91
  92 #define TF_DETECTION_CONE_DECIDEGREES 900
  93
  94 static serialPort_t *tfSerialPort = NULL;
  95
  96 typedef enum {
  97     TF_FRAME_STATE_WAIT_START1,
  98     TF_FRAME_STATE_WAIT_START2,
  99     TF_FRAME_STATE_READING_PAYLOAD,
 100     TF_FRAME_STATE_WAIT_CKSUM,
 101 } tfFrameState_e;
 102
 103 static tfFrameState_e tfFrameState;
 104 static uint8_t tfFrame[TF_FRAME_LENGTH];
 105 static uint8_t tfReceivePosition;
 106
 107 // TFmini
 108 // Command for 100Hz sampling (10msec interval)
 109 // At 100Hz scheduling, skew will cause 10msec delay at the most.
 110 static uint8_t tfCmdTFmini[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
 111
 112 // TF02
 113 // Same as TFmini for now..
 114 static uint8_t tfCmdTF02[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
 115
 116 static int32_t lidarTFValue;
 117 static uint16_t lidarTFerrors = 0;
 118
 119 static void lidarTFSendCommand(void)
 120 {
 121     switch (tfDevtype) {
 122     case TF_DEVTYPE_MINI:
 123         serialWriteBuf(tfSerialPort, tfCmdTFmini, sizeof(tfCmdTFmini));
 124         break;
 125     case TF_DEVTYPE_02:
 126         serialWriteBuf(tfSerialPort, tfCmdTF02, sizeof(tfCmdTF02));
 127         break;
 128     }
 129 }
 130
 131 void lidarTFInit(rangefinderDev_t *dev)
 132 {
 133     UNUSED(dev);
 134
 135     tfFrameState = TF_FRAME_STATE_WAIT_START1;
 136     tfReceivePosition = 0;
 137 }
 138
 139 void lidarTFUpdate(rangefinderDev_t *dev)
 140 {
 141     UNUSED(dev);
 142     static timeMs_t lastFrameReceivedMs = 0;
 143     const timeMs_t timeNowMs = millis();
 144
 145     if (tfSerialPort == NULL) {
 146         return;
 147     }
 148
 149     while (serialRxBytesWaiting(tfSerialPort)) {
 150         uint8_t c = serialRead(tfSerialPort);
 151         switch (tfFrameState) {
 152         case TF_FRAME_STATE_WAIT_START1:
 153             if (c == TF_FRAME_SYNC_BYTE) {
 154                 tfFrameState = TF_FRAME_STATE_WAIT_START2;
 155             }
 156             break;
 157
 158         case TF_FRAME_STATE_WAIT_START2:
 159             if (c == TF_FRAME_SYNC_BYTE) {
 160                 tfFrameState = TF_FRAME_STATE_READING_PAYLOAD;
 161             } else {
 162                 tfFrameState = TF_FRAME_STATE_WAIT_START1;
 163             }
 164             break;
 165
 166         case TF_FRAME_STATE_READING_PAYLOAD:
 167             tfFrame[tfReceivePosition++] = c;
 168             if (tfReceivePosition == TF_FRAME_LENGTH) {
 169                 tfFrameState = TF_FRAME_STATE_WAIT_CKSUM;
 170             }
 171             break;
 172
 173         case TF_FRAME_STATE_WAIT_CKSUM:
 174             {
 175                 uint8_t cksum = TF_FRAME_SYNC_BYTE + TF_FRAME_SYNC_BYTE;
 176                 for (int i = 0 ; i < TF_FRAME_LENGTH ; i++) {
 177                     cksum += tfFrame[i];
 178                 }
 179
 180                 if (c == cksum) {
 181
 182                     uint16_t distance = tfFrame[0] | (tfFrame[1] << 8);
 183                     uint16_t strength = tfFrame[2] | (tfFrame[3] << 8);
 184
 185                     DEBUG_SET(DEBUG_LIDAR_TF, 0, distance);
 186                     DEBUG_SET(DEBUG_LIDAR_TF, 1, strength);
 187                     DEBUG_SET(DEBUG_LIDAR_TF, 2, tfFrame[4]);
 188                     DEBUG_SET(DEBUG_LIDAR_TF, 3, tfFrame[5]);
 189
 190                     switch (tfDevtype) {
 191                     case TF_DEVTYPE_MINI:
 192                         if (distance >= TF_MINI_RANGE_MIN && distance < TF_MINI_RANGE_MAX) {
 193                             lidarTFValue = distance;
 194                             if (tfFrame[TF_MINI_FRAME_INTEGRAL_TIME] == 7) {
 195                                 // When integral time is long (7), measured distance tends to be longer by 12~13.
 196                                 lidarTFValue -= 13;
 197                             }
 198                         } else {
 199                             lidarTFValue = -1;
 200                         }
 201                         break;
 202
 203                     case TF_DEVTYPE_02:
 204                         if (distance >= TF_02_RANGE_MIN && distance < TF_02_RANGE_MAX && tfFrame[TF_02_FRAME_SIG] >= 7) {
 205                             lidarTFValue = distance;
 206                         } else {
 207                             lidarTFValue = -1;
 208                         }
 209                         break;
 210                     }
 211                     lastFrameReceivedMs = timeNowMs;
 212                 } else {
 213                     // Checksum error. Simply discard the current frame.
 214                     ++lidarTFerrors;
 215                     //DEBUG_SET(DEBUG_LIDAR_TF, 3, lidarTFerrors);
 216                 }
 217             }
 218
 219             tfFrameState = TF_FRAME_STATE_WAIT_START1;
 220             tfReceivePosition = 0;
 221
 222             break;
 223         }
 224     }
 225
 226     // If valid frame hasn't been received for more than a timeout, resend command.
 227
 228     if (timeNowMs - lastFrameReceivedMs > TF_TIMEOUT_MS) {
 229         lidarTFSendCommand();
 230     }
 231 }
 232
 233 // Return most recent device output in cm
 234
 235 int32_t lidarTFGetDistance(rangefinderDev_t *dev)
 236 {
 237     UNUSED(dev);
 238
 239     return lidarTFValue;
 240 }
 241
 242 static bool lidarTFDetect(rangefinderDev_t *dev, uint8_t devtype)
 243 {
 244     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LIDAR_TF);
 245
 246     if (!portConfig) {
 247         return false;
 248     }
 249
 250     tfSerialPort = openSerialPort(portConfig->identifier, FUNCTION_LIDAR_TF, NULL, NULL, 115200, MODE_RXTX, 0);
 251
 252     if (tfSerialPort == NULL) {
 253         return false;
 254     }
 255
 256     tfDevtype = devtype;
 257
 258     dev->delayMs = 10;
 259     dev->maxRangeCm = (devtype == TF_DEVTYPE_MINI) ? TF_MINI_RANGE_MAX : TF_02_RANGE_MAX;
 260     dev->detectionConeDeciDegrees = TF_DETECTION_CONE_DECIDEGREES;
 261     dev->detectionConeExtendedDeciDegrees = TF_DETECTION_CONE_DECIDEGREES;
 262
 263     dev->init = &lidarTFInit;
 264     dev->update = &lidarTFUpdate;
 265     dev->read = &lidarTFGetDistance;
 266
 267     return true;
 268 }
 269
 270 bool lidarTFminiDetect(rangefinderDev_t *dev)
 271 {
 272     return lidarTFDetect(dev, TF_DEVTYPE_MINI);
 273 }
 274
 275 bool lidarTF02Detect(rangefinderDev_t *dev)
 276 {
 277     return lidarTFDetect(dev, TF_DEVTYPE_02);
 278 }
 279 #endif