src/main/sensors/irlock.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "stdbool.h"
  19 #include "stdint.h"
  20 #include <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 #include "build/debug.h"
  26
  27 #include "common/maths.h"
  28 #include "common/log.h"
  29 #include "common/printf.h"
  30
  31 #include "drivers/irlock.h"
  32 #include "drivers/time.h"
  33
  34 #include "fc/runtime_config.h"
  35
  36 #include "flight/imu.h"
  37
  38 #include "navigation/navigation.h"
  39
  40 #include "sensors/sensors.h"
  41 #include "sensors/barometer.h"
  42 #include "sensors/irlock.h"
  43
  44
  45 #define IRLOCK_TIMEOUT 100
  46
  47 #if defined(USE_IRLOCK)
  48
  49 static irlockDev_t irlockDev;
  50 static bool irlockDetected = false;
  51 static bool measurementValid = false;
  52 static irlockData_t irlockData;
  53 static timeMs_t lastUpdateMs = 0;
  54
  55 void irlockInit(void)
  56 {
  57     irlockDetected = irlockDetect(&irlockDev);
  58 }
  59
  60 bool irlockHasBeenDetected(void)
  61 {
  62     return irlockDetected;
  63 }
  64
  65 bool irlockMeasurementIsValid(void) {
  66     return measurementValid;
  67 }
  68
  69 void irlockUpdate(void)
  70 {
  71     if (irlockDetected && irlockDev.read(&irlockDev, &irlockData)) lastUpdateMs = millis();
  72     measurementValid = millis() - lastUpdateMs < IRLOCK_TIMEOUT;
  73 }
  74
  75 #define X_TO_DISTANCE_FACTOR -0.0029387573f
  76 #define X_TO_DISTANCE_OFFSET 0.4702011635f
  77 #define Y_TO_DISTANCE_FACTOR -0.0030568431f
  78 #define Y_TO_DISTANCE_OFFSET 0.3056843086f
  79
  80 #define LENS_X_CORRECTION 4.4301355e-6f
  81 #define LENS_Y_CORRECTION 4.7933139e-6f
  82
  83 // calculate distance relative to center at 1 meter distance from absolute object coordinates and taking into account lens distortion
  84 static void irlockCoordinatesToDistance(uint16_t pixX, uint16_t pixY, float *distX, float *distY)
  85 {
  86     int16_t xCenterOffset = pixX - IRLOCK_RES_X / 2;
  87     int16_t yCenterOffset = pixY - IRLOCK_RES_Y / 2;
  88     float lensDistortionCorrectionFactor = LENS_X_CORRECTION * xCenterOffset * xCenterOffset + LENS_Y_CORRECTION * yCenterOffset * yCenterOffset - 1.0f;
  89     *distX = (X_TO_DISTANCE_FACTOR * pixX + X_TO_DISTANCE_OFFSET) / lensDistortionCorrectionFactor;
  90     *distY = (Y_TO_DISTANCE_FACTOR * pixY + Y_TO_DISTANCE_OFFSET) / lensDistortionCorrectionFactor;
  91 }
  92
  93 bool irlockGetPosition(float *distX, float *distY)
  94 {
  95     if (!measurementValid) return false;
  96
  97     // calculate edges of the object
  98     int16_t corner1X = irlockData.posX - irlockData.sizeX / 2;
  99     int16_t corner1Y = irlockData.posY - irlockData.sizeY / 2;
 100     int16_t corner2X = irlockData.posX + irlockData.sizeX / 2;
 101     int16_t corner2Y = irlockData.posY + irlockData.sizeY / 2;
 102
 103     // convert pixel position to distance
 104     float corner1DistX, corner1DistY, corner2DistX, corner2DistY;
 105     irlockCoordinatesToDistance(corner1X, corner1Y, &corner1DistX, &corner1DistY);
 106     irlockCoordinatesToDistance(corner2X, corner2Y, &corner2DistX, &corner2DistY);
 107
 108     // return center of object
 109     float uDistX = (corner1DistX + corner2DistX) / 2.0f; // lateral movement, positive means the aircraft is to the left of the beacon
 110     float uDistY = (corner1DistY + corner2DistY) / 2.0f; // longitudinal movement, positive means the aircraft is in front of the beacon
 111
 112     // compensate for altitude and attitude
 113     float altitude = CENTIMETERS_TO_METERS(getEstimatedActualPosition(Z));
 114     *distX = altitude * tan_approx(atan2_approx(uDistX, 1.0f) - DECIDEGREES_TO_RADIANS(attitude.values.roll));
 115     *distY = altitude * tan_approx(atan2_approx(uDistY, 1.0f) + DECIDEGREES_TO_RADIANS(attitude.values.pitch));
 116
 117     return true;
 118 }
 119
 120 #endif /* USE_IRLOCK */