src/main/io/opflow_msp.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_OPFLOW_MSP)
  40
  41 #include "drivers/opflow/opflow_virtual.h"
  42 #include "drivers/time.h"
  43 #include "io/opflow.h"
  44
  45 #include "msp/msp_protocol_v2_sensor_msg.h"
  46
  47
  48 static bool hasNewData = false;
  49 static timeUs_t updatedTimeUs = 0;
  50 static opflowData_t sensorData = { 0 };
  51
  52 static bool mspOpflowDetect(void)
  53 {
  54     // Always detectable
  55     return true;
  56 }
  57
  58 static bool mspOpflowInit(void)
  59 {
  60     return true;
  61 }
  62
  63 static bool mspOpflowUpdate(opflowData_t * data)
  64 {
  65     if (hasNewData) {
  66         hasNewData = false;
  67         *data = sensorData;
  68         return true;
  69     }
  70
  71     return false;
  72 }
  73
  74 void mspOpflowReceiveNewData(uint8_t * bufferPtr)
  75 {
  76     const timeUs_t currentTimeUs = micros();
  77     const mspSensorOpflowDataMessage_t * pkt = (const mspSensorOpflowDataMessage_t *)bufferPtr;
  78
  79     sensorData.deltaTime = currentTimeUs - updatedTimeUs;
  80     sensorData.flowRateRaw[0] = pkt->motionX;
  81     sensorData.flowRateRaw[1] = pkt->motionY;
  82     sensorData.flowRateRaw[2] = 0;
  83     sensorData.quality = (int)pkt->quality * 100 / 255;
  84     hasNewData = true;
  85
  86     updatedTimeUs = currentTimeUs;
  87 }
  88
  89 virtualOpflowVTable_t opflowMSPVtable = {
  90     .detect = mspOpflowDetect,
  91     .init = mspOpflowInit,
  92     .update = mspOpflowUpdate
  93 };
  94
  95 #endif