src/main/drivers/rangefinder/rangefinder_lidarmt.c

   1 /*
   2  * This file is part of Betaflight and INAV
   3  *
   4  * Betaflight and INAV are free software. You can
   5  * redistribute this software and/or modify this software under
   6  * the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, either version 3 of the License,
   8  * or (at your option) any later version.
   9  *
  10  * Betaflight and INAV are distributed in the hope that
  11  * they will be useful, but WITHOUT ANY WARRANTY; without even the
  12  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  13  * PURPOSE. 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 /*
  22  * This is a bridge driver between a sensor reader that operates at high level (i.e. on top of UART)
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <string.h>
  28
  29 #include "platform.h"
  30
  31 #ifdef USE_RANGEFINDER_MT
  32
  33 #include "build/build_config.h"
  34
  35 #include "common/utils.h"
  36
  37 #include "drivers/rangefinder/rangefinder_lidarmt.h"
  38 #include "sensors/rangefinder.h"
  39
  40 static bool hasNewData = false;
  41 static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
  42
  43 // Initialize the table with values for each rangefinder type
  44 static const MTRangefinderConfig rangefinderConfigs[] = {
  45     { .deviceType = RANGEFINDER_MTF01,  .delayMs = 10, .maxRangeCm = 800  },
  46     { .deviceType = RANGEFINDER_MTF02,  .delayMs = 20, .maxRangeCm = 250  },
  47     { .deviceType = RANGEFINDER_MTF01P, .delayMs = 10, .maxRangeCm = 1200 },
  48     { .deviceType = RANGEFINDER_MTF02P, .delayMs = 20, .maxRangeCm = 600  },
  49 };
  50
  51 typedef struct __attribute__((packed)) {
  52     uint8_t quality;    // [0;255]
  53     int32_t distanceMm; // Negative value for out of range
  54 } mspSensorRangefinderLidarMtDataMessage_t;
  55
  56 static void mtRangefinderInit(rangefinderDev_t * dev) {
  57     UNUSED(dev);
  58 }
  59
  60 static void mtRangefinderUpdate(rangefinderDev_t * dev) {
  61     UNUSED(dev);
  62 }
  63
  64 static int32_t mtRangefinderGetDistance(rangefinderDev_t * dev) {
  65     UNUSED(dev);
  66     if (hasNewData) {
  67         hasNewData = false;
  68         return (sensorData >= 0.0f) ? sensorData : RANGEFINDER_OUT_OF_RANGE;
  69     } else {
  70         return RANGEFINDER_NO_NEW_DATA;
  71     }
  72 }
  73
  74 bool mtRangefinderDetect(rangefinderDev_t * dev, rangefinderType_e mtRangefinderToUse) {
  75     const MTRangefinderConfig* deviceConf = getMTRangefinderDeviceConf(mtRangefinderToUse);
  76     if (!deviceConf) {
  77         return false;
  78     }
  79     dev->delayMs    = deviceConf->delayMs;
  80     dev->maxRangeCm = deviceConf->maxRangeCm;
  81
  82     dev->detectionConeDeciDegrees = RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES;
  83     dev->detectionConeExtendedDeciDegrees = RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES;
  84
  85     dev->init = &mtRangefinderInit;
  86     dev->update = &mtRangefinderUpdate;
  87     dev->read = &mtRangefinderGetDistance;
  88
  89     return true;
  90 }
  91
  92 void mtRangefinderReceiveNewData(const uint8_t * bufferPtr) {
  93     const mspSensorRangefinderLidarMtDataMessage_t * pkt = (const mspSensorRangefinderLidarMtDataMessage_t *)bufferPtr;
  94
  95     sensorData = pkt->distanceMm / 10;
  96     hasNewData = true;
  97 }
  98
  99 const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse){
 100     for (const MTRangefinderConfig* cfg =  rangefinderConfigs; cfg < ARRAYEND(rangefinderConfigs); cfg++) {
 101         if (cfg->deviceType == mtRangefinderToUse) {
 102             return cfg;
 103         }
 104     }
 105     return NULL;
 106 }
 107
 108 #endif // USE_RANGEFINDER_MT