diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index e0c50535eb..364356342f 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -406,8 +406,6 @@ main_sources(COMMON_SRC rx/sumd.h rx/sumh.c rx/sumh.h - rx/uib_rx.c - rx/uib_rx.h rx/xbus.c rx/xbus.h @@ -435,10 +433,6 @@ main_sources(COMMON_SRC sensors/temperature.c sensors/temperature.h - uav_interconnect/uav_interconnect.h - uav_interconnect/uav_interconnect_bus.c - uav_interconnect/uav_interconnect_rangefinder.c - blackbox/blackbox.c blackbox/blackbox.h blackbox/blackbox_encoding.c diff --git a/src/main/drivers/opflow/opflow_virtual.c b/src/main/drivers/opflow/opflow_virtual.c index c6f9857e81..a559d5be19 100755 --- a/src/main/drivers/opflow/opflow_virtual.c +++ b/src/main/drivers/opflow/opflow_virtual.c @@ -23,7 +23,7 @@ */ /* - * This is a bridge driver between a sensor reader that operates at high level (on top of UART or UIB) + * This is a bridge driver between a sensor reader that operates at high level (i.e. on top of UART) */ #include diff --git a/src/main/drivers/rangefinder/rangefinder_virtual.c b/src/main/drivers/rangefinder/rangefinder_virtual.c index 4d535db8bb..c3bf85b89b 100644 --- a/src/main/drivers/rangefinder/rangefinder_virtual.c +++ b/src/main/drivers/rangefinder/rangefinder_virtual.c @@ -23,7 +23,7 @@ */ /* - * This is a bridge driver between a sensor reader that operates at high level (on top of UART or UIB) + * This is a bridge driver between a sensor reader that operates at high level (i.e. on top of UART) */ #include diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index b8cb792156..025861d35e 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -148,8 +148,6 @@ #include "telemetry/telemetry.h" -#include "uav_interconnect/uav_interconnect.h" - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -570,10 +568,6 @@ void init(void) } #endif -#ifdef USE_UAV_INTERCONNECT - uavInterconnectBusInit(); -#endif - #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL) // Register the srxl Textgen telemetry sensor as a displayport device cmsDisplayPortRegister(displayPortSrxlInit()); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b000de7017..260d5b2f63 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -91,8 +91,6 @@ #include "config/feature.h" -#include "uav_interconnect/uav_interconnect.h" - void taskHandleSerial(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); @@ -365,9 +363,6 @@ void fcTasksInit(void) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif -#ifdef USE_UAV_INTERCONNECT - setTaskEnabled(TASK_UAV_INTERCONNECT, uavInterconnectBusIsInitialized()); -#endif #ifdef USE_RCDEVICE setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled()); #endif @@ -572,16 +567,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_OPFLOW] = { .taskName = "OPFLOW", .taskFunc = taskUpdateOpticalFlow, - .desiredPeriod = TASK_PERIOD_HZ(100), // I2C/SPI sensor will work at higher rate and accumulate, UIB/UART sensor will work at lower rate w/o accumulation - .staticPriority = TASK_PRIORITY_MEDIUM, - }, -#endif - -#ifdef USE_UAV_INTERCONNECT - [TASK_UAV_INTERCONNECT] = { - .taskName = "UIB", - .taskFunc = uavInterconnectBusTask, - .desiredPeriod = 1000000 / 500, // 500 Hz + .desiredPeriod = TASK_PERIOD_HZ(100), // I2C/SPI sensor will work at higher rate and accumulate, UART sensor will work at lower rate w/o accumulation .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 95199bde63..f5fde879ef 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -7,7 +7,7 @@ tables: values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"] + values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"] enum: rangefinderType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"] @@ -22,7 +22,7 @@ tables: values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"] enum: pitotSensor_e - name: receiver_type - values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"] + values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UNUSED"] enum: rxReceiverType_e - name: serial_rx values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST"] diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 9ea7869ab1..7b4f05c76b 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -44,7 +44,7 @@ typedef enum { FUNCTION_RCDEVICE = (1 << 10), // 1024 FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_VTX_TRAMP = (1 << 12), // 4096 - FUNCTION_UAV_INTERCONNECT = (1 << 13), // 8192 + FUNCTION_UNUSED_1 = (1 << 13), // 8192: former UAV_INTERCONNECT FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 FUNCTION_LOG = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index c8baee9322..1a9ef844bd 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -64,7 +64,6 @@ #include "rx/srxl2.h" #include "rx/sumd.h" #include "rx/sumh.h" -#include "rx/uib_rx.h" #include "rx/xbus.h" #include "rx/ghst.h" @@ -330,12 +329,6 @@ void rxInit(void) break; #endif -#ifdef USE_RX_UIB - case RX_TYPE_UIB: - rxUIBInit(rxConfig(), &rxRuntimeConfig); - break; -#endif - #ifdef USE_RX_SPI case RX_TYPE_SPI: if (!rxSpiInit(rxConfig(), &rxRuntimeConfig)) { diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7c7b7367b4..0aee1e8353 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -65,7 +65,7 @@ typedef enum { RX_TYPE_SERIAL = 2, RX_TYPE_MSP = 3, RX_TYPE_SPI = 4, - RX_TYPE_UIB = 5 + RX_TYPE_UNUSED_1 = 5 } rxReceiverType_e; typedef enum { diff --git a/src/main/rx/uib_rx.c b/src/main/rx/uib_rx.c deleted file mode 100755 index e8ab466927..0000000000 --- a/src/main/rx/uib_rx.c +++ /dev/null @@ -1,126 +0,0 @@ -/* - * This file is part of INAV. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this file, - * You can obtain one at http://mozilla.org/MPL/2.0/. - * - * Alternatively, the contents of this file may be used under the terms - * of the GNU General Public License Version 3, as described below: - * - * This file is free software: you may copy, redistribute and/or modify - * it under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General - * Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see http://www.gnu.org/licenses/. - * - * @author Konstantin Sharlaimov - */ - -#include -#include -#include - -#include "platform.h" - -#if defined(USE_UAV_INTERCONNECT) && defined(USE_RX_UIB) - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/utils.h" -#include "common/maths.h" - -#include "rx/rx.h" -#include "rx/uib_rx.h" - -#include "uav_interconnect/uav_interconnect.h" - -#define UIB_DEVICE_ADDRESS UIB_DEVICE_ID_RC_RECEIVER - -typedef struct __attribute__((packed)) { - uint8_t flags; // UIB_DATA_VALID (0x01) - link ok - uint8_t rssi; - uint8_t sticks[4]; // Values in range [0;255], center = 127 - uint8_t aux[8]; // Analog AUX channels - values in range [0;255], center = 127 - uint16_t reserved; // Reserved for future use -} rcUibReceiverData_t; - -static rcUibReceiverData_t uibData; - -#define UIB_RX_MAX_CHANNEL_COUNT 16 - -static uint16_t rxUIBReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) -{ - UNUSED(rxRuntimeConfigPtr); - - switch (chan) { - case 0 ... 3: - return scaleRange(uibData.sticks[chan], 0, 255, 1000, 2000); - - case 4 ... 11: - return scaleRange(uibData.aux[chan - 4], 0, 255, 1000, 2000); - - case 12: - case 13: - case 14: - return 1500; - - case 15: - return scaleRange(uibData.rssi, 0, 255, 1000, 2000); - - default: - return 1500; - } -} - -static uint8_t rxUIBFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) -{ - UNUSED(rxRuntimeConfig); - - if (!uavInterconnectBusIsInitialized()) { - return RX_FRAME_FAILSAFE; - } - - // If bus didn't detect the yet - report failure - if (!uibDetectAndActivateDevice(UIB_DEVICE_ADDRESS)) { - return RX_FRAME_FAILSAFE; - } - - if (uibGetUnansweredRequests(UIB_DEVICE_ADDRESS) > 10) { // Tolerate 200ms loss (10 packet loss) - return RX_FRAME_FAILSAFE; - } - - if (uibDataAvailable(UIB_DEVICE_ADDRESS)) { - rcUibReceiverData_t uibDataTmp; - uibRead(UIB_DEVICE_ADDRESS, (uint8_t*)&uibDataTmp, sizeof(uibDataTmp)); - - if (!(uibDataTmp.flags & UIB_DATA_VALID)) - return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; - - memcpy(&uibData, &uibDataTmp, sizeof(rcUibReceiverData_t)); - return RX_FRAME_COMPLETE; - } - else { - return RX_FRAME_PENDING; - } -} - -void rxUIBInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) -{ - UNUSED(rxConfig); - - rxRuntimeConfig->channelCount = UIB_RX_MAX_CHANNEL_COUNT; - rxRuntimeConfig->rxRefreshRate = 20000; - rxRuntimeConfig->rxSignalTimeout = DELAY_5_HZ; - rxRuntimeConfig->rcReadRawFn = rxUIBReadRawRC; - rxRuntimeConfig->rcFrameStatusFn = rxUIBFrameStatus; -} -#endif diff --git a/src/main/rx/uib_rx.h b/src/main/rx/uib_rx.h deleted file mode 100755 index 1db7a10221..0000000000 --- a/src/main/rx/uib_rx.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * This file is part of INAV. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this file, - * You can obtain one at http://mozilla.org/MPL/2.0/. - * - * Alternatively, the contents of this file may be used under the terms - * of the GNU General Public License Version 3, as described below: - * - * This file is free software: you may copy, redistribute and/or modify - * it under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General - * Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see http://www.gnu.org/licenses/. - * - * @author Konstantin Sharlaimov - */ - -#pragma once - -#include "rx/rx.h" - -void rxUIBInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index b4661d16e0..233eaec99c 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -101,9 +101,6 @@ typedef enum { #ifdef USE_OPFLOW TASK_OPFLOW, #endif -#ifdef USE_UAV_INTERCONNECT - TASK_UAV_INTERCONNECT, -#endif #ifdef USE_RCDEVICE TASK_RCDEVICE, #endif diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index aec1e065aa..29955a42d5 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -53,8 +53,6 @@ #include "scheduler/scheduler.h" -#include "uav_interconnect/uav_interconnect.h" - rangefinder_t rangefinder; #define RANGEFINDER_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise @@ -142,15 +140,6 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; - case RANGEFINDER_UIB: -#if defined(USE_RANGEFINDER_UIB) - if (uibRangefinderDetect(dev)) { - rangefinderHardware = RANGEFINDER_UIB; - rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_UIB_TASK_PERIOD_MS)); - } -#endif - break; - case RANGEFINDER_BENEWAKE: #if defined(USE_RANGEFINDER_BENEWAKE) if (virtualRangefinderDetect(dev, &rangefinderBenewakeVtable)) { diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index c80d18ac1e..2cd7d01e1c 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -28,7 +28,7 @@ typedef enum { RANGEFINDER_HCSR04I2C = 3, RANGEFINDER_VL53L0X = 4, RANGEFINDER_MSP = 5, - RANGEFINDER_UIB = 6, + RANGEFINDER_UNUSED = 6, // Was UIB RANGEFINDER_BENEWAKE = 7, } rangefinderType_e; diff --git a/src/main/target/common.h b/src/main/target/common.h index dbe28d00a5..9d0ddc3346 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -87,8 +87,6 @@ #define USE_DYNAMIC_FILTERS #define USE_GYRO_KALMAN #define USE_EXTENDED_CMS_MENUS -#define USE_UAV_INTERCONNECT -#define USE_RX_UIB #define USE_HOTT_TEXTMODE // NAZA GPS support for F4+ only diff --git a/src/main/uav_interconnect/uav_interconnect.h b/src/main/uav_interconnect/uav_interconnect.h deleted file mode 100755 index 95e9e46e41..0000000000 --- a/src/main/uav_interconnect/uav_interconnect.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * This file is part of INAV. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this file, - * You can obtain one at http://mozilla.org/MPL/2.0/. - * - * Alternatively, the contents of this file may be used under the terms - * of the GNU General Public License Version 3, as described below: - * - * This file is free software: you may copy, redistribute and/or modify - * it under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General - * Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see http://www.gnu.org/licenses/. - * - * @author Konstantin Sharlaimov - */ - -#include -#include -#include -#include - -#include "platform.h" -#include "build/build_config.h" -#include "drivers/rangefinder/rangefinder.h" - -#ifdef USE_UAV_INTERCONNECT - -#define UIB_MAX_PACKET_SIZE 32 - -#define UIB_DEVICE_ID_RANGEFINDER 0x12 -#define UIB_DEVICE_ID_GPS 0x13 -#define UIB_DEVICE_ID_COMPASS 0x14 -#define UIB_DEVICE_ID_RC_RECEIVER 0x80 - -typedef enum { - UIB_DATA_NONE = 0, - UIB_DATA_VALID = (1 << 0), // Data is valid -} uibDataFlags_t; - -/* Bus task */ -bool uavInterconnectBusCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime); -void uavInterconnectBusTask(timeUs_t currentTimeUs); -void uavInterconnectBusInit(void); -bool uavInterconnectBusIsInitialized(void); - -/* Bus device API */ -bool uibRegisterDevice(uint8_t devId); -bool uibDetectAndActivateDevice(uint8_t devId); -bool uibGetDeviceParams(uint8_t devId, uint8_t * params); -timeUs_t uibGetPollRateUs(uint8_t devId); -uint32_t uibGetUnansweredRequests(uint8_t devId); -uint8_t uibDataAvailable(uint8_t devId); -uint8_t uibRead(uint8_t devId, uint8_t * buffer, uint8_t bufSize); -bool uibCanWrite(uint8_t devId); -bool uibWrite(uint8_t devId, const uint8_t * buffer, uint8_t bufSize); - -#define RANGEFINDER_UIB_TASK_PERIOD_MS 100 -bool uibRangefinderDetect(rangefinderDev_t *dev); - -#endif diff --git a/src/main/uav_interconnect/uav_interconnect_bus.c b/src/main/uav_interconnect/uav_interconnect_bus.c deleted file mode 100755 index 9eb0fca674..0000000000 --- a/src/main/uav_interconnect/uav_interconnect_bus.c +++ /dev/null @@ -1,635 +0,0 @@ -/* - * This file is part of INAV. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this file, - * You can obtain one at http://mozilla.org/MPL/2.0/. - * - * Alternatively, the contents of this file may be used under the terms - * of the GNU General Public License Version 3, as described below: - * - * This file is free software: you may copy, redistribute and/or modify - * it under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General - * Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see http://www.gnu.org/licenses/. - * - * @author Konstantin Sharlaimov - */ - -#include -#include -#include -#include -#include - -#include "platform.h" -#include "build/build_config.h" - -#ifdef USE_UAV_INTERCONNECT - -#include "build/debug.h" - -#include "common/maths.h" -#include "common/axis.h" -#include "common/utils.h" -#include "common/crc.h" - -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" - -#include "drivers/serial.h" -#include "drivers/time.h" - -#include "io/serial.h" - -#include "uav_interconnect/uav_interconnect.h" - -typedef enum { - UIB_COMMAND_IDENTIFY = (0x00 << 5), // 0x00 - UIB_COMMAND_NOTIFY = (0x01 << 5), // 0x20 - UIB_COMMAND_READ = (0x02 << 5), // 0x40 - UIB_COMMAND_WRITE = (0x03 << 5), // 0x60 - UIB_COMMAND_RES_1 = (0x04 << 5), // 0x80 - UIB_COMMAND_RES_2 = (0x05 << 5), // 0xA0 - UIB_COMMAND_RES_3 = (0x06 << 5), // 0xC0 - UIB_COMMAND_RES_4 = (0x07 << 5), // 0xE0 -} uibCommand_e; - -typedef enum { - UIB_FLAG_HAS_READ = (1 << 0), // Device supports READ command (sensor) - UIB_FLAG_HAS_WRITE = (1 << 1), // Device supports WRITE command (sensor configuration or executive device) -} uibDeviceFlags_e; - -#define UIB_PROTOCOL_VERSION 0x00 -#define UIB_MAX_SLOTS 16 // 32 is design maximum -#define UIB_PORT_OPTIONS (SERIAL_NOT_INVERTED | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_UNIDIR) - -#define UIB_DISCOVERY_DELAY_US 2000000 // 2 seconds from power-up to allow all devices to boot -#define UIB_GUARD_INTERVAL_US 1000 -#define UIB_REFRESH_INTERVAL_US 200000 - -typedef struct { - bool allocated; - bool activated; - uint8_t deviceAddress; - uint16_t deviceFlags; - uint8_t devParams[4]; - timeUs_t pollIntervalUs; - timeUs_t lastPollTimeUs; - uint32_t unrepliedRequests; // 0 - all answered, 1 - request in progress, 2 or more - device failed to answer one or more requests - - uint8_t rxDataReadySize; - uint8_t txDataReadySize; - uint8_t rxPacket[UIB_MAX_PACKET_SIZE]; - uint8_t txPacket[UIB_MAX_PACKET_SIZE]; -} uavInterconnectSlot_t; - -typedef enum { - STATE_INITIALIZE, - STATE_DISCOVER, - STATE_READY, -} uavInterconnectState_t; - -typedef struct { - int sentCommands; - int discoveredDevices; - int failedCRC; - int commandTimeouts; -} uavInterconnectStats_t; - -static serialPort_t * busPort; -static bool uibInitialized = false; - -static uavInterconnectSlot_t slots[UIB_MAX_SLOTS]; -static timeUs_t slotStartTimeUs; -static uavInterconnectState_t busState = STATE_INITIALIZE; - -static int discoveryAddress; -static int refreshSlot; -static timeUs_t refreshStartTimeUs; - -static uint8_t slotDataBuffer[20]; // Max transaction length is 20 bytes -static unsigned slotDataBufferCount; -static timeUs_t slotLastActivityUs; - -// Statistics -static uavInterconnectStats_t uibStats; - -static void switchState(uavInterconnectState_t newState) -{ - if (busState == newState) - return; - - busState = newState; -} - -static void sendDiscover(timeUs_t currentTimeUs, uint8_t slot, uint8_t devId) -{ - uint8_t buf[4]; - buf[0] = UIB_COMMAND_IDENTIFY | slot; - buf[1] = devId; - buf[2] = UIB_PROTOCOL_VERSION; - buf[3] = crc8_dvb_s2_update(0x00, &buf[0], 3); - slotStartTimeUs = currentTimeUs; - serialWriteBuf(busPort, buf, 4); - uibStats.sentCommands++; -} - -static void sendNotify(timeUs_t currentTimeUs, uint8_t slot, uint8_t devId) -{ - uint8_t buf[4]; - buf[0] = UIB_COMMAND_NOTIFY | slot; - buf[1] = devId; - buf[2] = UIB_PROTOCOL_VERSION; - buf[3] = crc8_dvb_s2_update(0x00, &buf[0], 3); - slotStartTimeUs = currentTimeUs; - serialWriteBuf(busPort, buf, 4); - uibStats.sentCommands++; -} - -static void sendRead(timeUs_t currentTimeUs, uint8_t slot) -{ - uint8_t buf[2]; - buf[0] = UIB_COMMAND_READ | slot; - buf[1] = crc8_dvb_s2_update(0x00, &buf[0], 1); - slotStartTimeUs = currentTimeUs; - serialWriteBuf(busPort, buf, 2); - uibStats.sentCommands++; -} - -static void sendWrite(timeUs_t currentTimeUs, uint8_t slot, uint8_t * data, uint8_t len) -{ - uint8_t buf[UIB_MAX_PACKET_SIZE + 3]; - buf[0] = UIB_COMMAND_WRITE | slot; - buf[1] = len; - memcpy(&buf[2], data, len); - buf[UIB_MAX_PACKET_SIZE + 2] = crc8_dvb_s2_update(0x00, &buf[0], UIB_MAX_PACKET_SIZE + 2); - slotStartTimeUs = currentTimeUs; - serialWriteBuf(busPort, buf, UIB_MAX_PACKET_SIZE + 3); - uibStats.sentCommands++; -} - -static uavInterconnectSlot_t * findDevice(uint8_t devId) -{ - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - if (slots[i].allocated && slots[i].deviceAddress == devId) { - return &slots[i]; - } - } - - return NULL; -} - -static int findEmptySlot(void) -{ - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - if (!slots[i].allocated) - return i; - } - - return -1; -} - -static void registerDeviceSlot(uint8_t slotId, uint8_t devId, uint16_t deviceFlags, uint32_t pollIntervalUs, const uint8_t * devParams) -{ - slots[slotId].allocated = true; - slots[slotId].deviceAddress = devId; - slots[slotId].deviceFlags = deviceFlags; - slots[slotId].pollIntervalUs = pollIntervalUs; - - if (devParams) { - slots[slotId].devParams[0] = devParams[0]; - slots[slotId].devParams[1] = devParams[1]; - slots[slotId].devParams[2] = devParams[2]; - slots[slotId].devParams[3] = devParams[3]; - } - else { - slots[slotId].devParams[0] = 0; - slots[slotId].devParams[1] = 0; - slots[slotId].devParams[2] = 0; - slots[slotId].devParams[3] = 0; - } - - slots[slotId].rxDataReadySize = 0; - slots[slotId].txDataReadySize = 0; - slots[slotId].lastPollTimeUs = 0; - slots[slotId].unrepliedRequests = 0; -} - -typedef struct __attribute__((packed)) { - uint8_t slotAndCmd; - uint8_t devId; - uint8_t protoVersion; - uint8_t crc1; - uint16_t pollIntervalMs; - uint16_t devFlags; - uint8_t devParams[4]; - uint8_t crc2; -} uibPktIdentify_t; - -typedef struct __attribute__((packed)) { - uint8_t slotAndCmd; - uint8_t devId; - uint8_t protoVersion; - uint8_t crc1; -} uibPktNotify_t; - -typedef struct __attribute__((packed)) { - uint8_t slotAndCmd; - uint8_t crc1; - uint8_t size; - uint8_t data[0]; -} uibPktRead_t; - -typedef struct __attribute__((packed)) { - uint8_t slotAndCmd; - uint8_t size; - uint8_t data[0]; -} uibPktWrite_t; - -typedef union { - uibPktIdentify_t ident; - uibPktNotify_t notify; - uibPktRead_t read; - uibPktWrite_t write; -} uibPktData_t; - -static void processSlot(void) -{ - // First byte is command / slot - const uint8_t cmd = slotDataBuffer[0] & 0xE0; - const uint8_t slot = slotDataBuffer[0] & 0x1F; - const uibPktData_t * pkt = (const uibPktData_t *)&slotDataBuffer[0]; - const int lastPacketByteIndex = slotDataBufferCount - 1; - - // CRC is calculated over the whole slot, including command byte(s) sent by FC. This ensures integrity of the transaction as a whole - switch (cmd) { - // Identify command (13 bytes) - // FC: IDENTIFY[1] + DevID[1] + ProtoVersion [1] + CRC1[1] - // DEV: PollInterval[2] + Flags[2] + DevParams[4] + CRC2[1] - case UIB_COMMAND_IDENTIFY: - if (slotDataBufferCount == sizeof(uibPktIdentify_t)) { - if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) == pkt->ident.crc2) { - // CRC valid - process valid IDENTIFY slot - registerDeviceSlot(slot, pkt->ident.devId, pkt->ident.devFlags, pkt->ident.pollIntervalMs * 1000, pkt->ident.devParams); - uibStats.discoveredDevices++; - } - else { - uibStats.failedCRC++; - } - - // Regardless of CRC validity - discard buffer data - slotDataBufferCount = 0; - } - break; - - // NOTIFY command (4 bytes) - // FC: IDENTIFY[1] + DevID[1] + ProtoVersion [1] + CRC1[1] - case UIB_COMMAND_NOTIFY: - if (slotDataBufferCount == sizeof(uibPktNotify_t)) { - // Record failed CRC. Do nothing else - NOTIFY is only sent for devices that already have a slot assigned - if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) != pkt->notify.crc1) { - uibStats.failedCRC++; - } - - slotDataBufferCount = 0; - } - break; - - // Read command (min 4 bytes) - // FC: READ[1] + CRC1[1] - // DEV: DATA_LEN[1] + DATA[variable] + CRC2[1] - case UIB_COMMAND_READ: - if ((slotDataBufferCount >= sizeof(uibPktRead_t)) && (slotDataBufferCount == ((unsigned)pkt->read.size + 4)) && (pkt->read.size <= UIB_MAX_PACKET_SIZE)) { - if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) == slotDataBuffer[lastPacketByteIndex]) { - // CRC valid - process valid READ slot - // Check if this slot has read capability and is allocated - if (slots[slot].allocated && (slots[slot].deviceFlags & UIB_FLAG_HAS_READ)) { - if ((slots[slot].rxDataReadySize == 0) && (pkt->read.size > 0)) { - memcpy(slots[slot].rxPacket, pkt->read.data, pkt->read.size); - slots[slot].rxDataReadySize = pkt->read.size; - } - - slots[slot].unrepliedRequests = 0; - } - } - else { - uibStats.failedCRC++; - } - - // Regardless of CRC validity - discard buffer data - slotDataBufferCount = 0; - } - break; - - // Write command (min 3 bytes) - // FC: WRITE[1] + DATA_LEN[1] + DATA[variable] + CRC1[1] - case UIB_COMMAND_WRITE: - if ((slotDataBufferCount >= sizeof(uibPktWrite_t)) && (slotDataBufferCount == ((unsigned)pkt->write.size + 3)) && (pkt->write.size <= UIB_MAX_PACKET_SIZE)) { - // Keep track of failed CRC events - if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) != slotDataBuffer[lastPacketByteIndex]) { - uibStats.failedCRC++; - } - - // Regardless of CRC validity - discard buffer data - slotDataBufferCount = 0; - } - break; - - default: - break; - } -} - -static void processScheduledTransactions(timeUs_t currentTimeUs) -{ - int slotPrio = 0x100; - int slotId = -1; - - // First - find device with highest priority that has the READ capability and is scheduled for READ - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - // Only do scheduled READs on allocated and active slots - if (!(slots[i].allocated && slots[i].activated)) - continue; - - if ((slots[i].deviceFlags & UIB_FLAG_HAS_READ) && ((currentTimeUs - slots[i].lastPollTimeUs) >= slots[i].pollIntervalUs) && (slotPrio > slots[i].deviceAddress)) { - slotId = i; - slotPrio = slots[i].deviceAddress; - } - } - - // READ command - if (slotId >= 0) { - sendRead(currentTimeUs, slotId); - slots[slotId].unrepliedRequests++; - slots[slotId].lastPollTimeUs = currentTimeUs; - return; - } - - // No READ command executed - check if we have data to WRITE - slotPrio = 0x100; - slotId = -1; - - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - // Only do WRITEs on allocated and active slots - if (!(slots[i].allocated && slots[i].activated)) - continue; - - if (slots[i].txDataReadySize && (slots[i].deviceFlags & UIB_FLAG_HAS_WRITE) && (slotPrio > slots[i].deviceAddress)) { - slotId = i; - slotPrio = slots[i].deviceAddress; - } - } - - // WRITE command - if (slotId >= 0) { - sendWrite(currentTimeUs, slotId, slots[slotId].txPacket, slots[slotId].txDataReadySize); - slots[slotId].unrepliedRequests++; - slots[slotId].txDataReadySize = 0; - return; - } - - // Neither READ nor WRITE command are queued - issue IDENTIFY once in a while - if ((currentTimeUs - refreshStartTimeUs) > UIB_REFRESH_INTERVAL_US) { - // Send notifications for allocated slots - if (slots[refreshSlot].allocated) { - sendNotify(currentTimeUs, refreshSlot, slots[refreshSlot].deviceAddress); - refreshStartTimeUs = currentTimeUs; - } - - if (++refreshSlot >= UIB_MAX_SLOTS) { - refreshSlot = 0; - } - } -} - -static bool canSendNewRequest(timeUs_t currentTimeUs) -{ - return ((currentTimeUs - slotLastActivityUs) >= UIB_GUARD_INTERVAL_US); -} - -void uavInterconnectBusTask(timeUs_t currentTimeUs) -{ - if (!uibInitialized) - return; - - // Receive bytes to the buffer - bool hasNewBytes = false; - while (serialRxBytesWaiting(busPort) > 0) { - uint8_t c = serialRead(busPort); - if (slotDataBufferCount < (int)(sizeof(slotDataBuffer) / sizeof(slotDataBuffer[0]))) { - slotDataBuffer[slotDataBufferCount++] = c; - hasNewBytes = true; - } - - slotLastActivityUs = currentTimeUs; - } - - // Flush receive buffer if guard interval elapsed - if ((currentTimeUs - slotLastActivityUs) >= UIB_GUARD_INTERVAL_US && slotDataBufferCount > 0) { - uibStats.commandTimeouts++; - slotDataBufferCount = 0; - } - - // If we have new bytes - process packet - if (hasNewBytes && slotDataBufferCount >= 4) { // minimum transaction length is 4 bytes - no point in processing something smaller - processSlot(); - } - - // Process request scheduling - we can initiate another slot if guard interval has elapsed and slot interval has elapsed as well - if (canSendNewRequest(currentTimeUs)) { - // We get here only if we can send requests - no timeout checking should be done beyond this point - switch (busState) { - case STATE_INITIALIZE: - if ((currentTimeUs - slotStartTimeUs) > UIB_DISCOVERY_DELAY_US) { - discoveryAddress = 0; - switchState(STATE_DISCOVER); - } - break; - - case STATE_DISCOVER: - { - int discoverySlot = findEmptySlot(); - if (discoverySlot >= 0) { - sendDiscover(currentTimeUs, discoverySlot, discoveryAddress); - if (discoveryAddress == 0xFF) { - // All addresses have been polled - switchState(STATE_READY); - } - else { - // Query next address and stick here - discoveryAddress++; - } - } - else { - // All slots are allocated - can't discover more devices - refreshSlot = 0; - refreshStartTimeUs = currentTimeUs; - switchState(STATE_READY); - } - } - break; - - case STATE_READY: - // Bus ready - process scheduled transfers - processScheduledTransactions(currentTimeUs); - break; - } - } -} - -void uavInterconnectBusInit(void) -{ - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - slots[i].allocated = false; - slots[i].activated = false; - } - - serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_UAV_INTERCONNECT); - if (!portConfig) - return; - - baudRate_e baudRateIndex = portConfig->peripheral_baudrateIndex; - busPort = openSerialPort(portConfig->identifier, FUNCTION_UAV_INTERCONNECT, NULL, NULL, baudRates[baudRateIndex], MODE_RXTX, UIB_PORT_OPTIONS); - if (!busPort) - return; - - slotStartTimeUs = 0; - uibInitialized = true; -} - -bool uavInterconnectBusIsInitialized(void) -{ - return uibInitialized; -} - -bool uibDetectAndActivateDevice(uint8_t devId) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return false; - - /* We have discovered device in out registry and code is asking to access it - activate device */ - slot->activated = true; - - return slot->allocated; -} - -bool uibRegisterDevice(uint8_t devId) -{ - int slotId = findEmptySlot(); - if (slotId >= 0) { - registerDeviceSlot(slotId, devId, UIB_FLAG_HAS_WRITE, 0, NULL); - return uibDetectAndActivateDevice(devId); - } - - return false; -} - -bool uibGetDeviceParams(uint8_t devId, uint8_t * params) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL || params == NULL) - return false; - - params[0] = slot->devParams[0]; - params[1] = slot->devParams[1]; - params[2] = slot->devParams[2]; - params[3] = slot->devParams[3]; - return true; -} - -timeUs_t uibGetPollRateUs(uint8_t devId) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return 0; - - return slot->pollIntervalUs; -} - -uint32_t uibGetUnansweredRequests(uint8_t devId) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return 0; - - return (slot->unrepliedRequests < 2) ? 0 : slot->unrepliedRequests - 1; -} - -uint8_t uibDataAvailable(uint8_t devId) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return 0; - - return slot->rxDataReadySize; -} - -uint8_t uibRead(uint8_t devId, uint8_t * buffer, uint8_t bufSize) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return 0; - - // If no READ capability - fail - if (!(slot->deviceFlags & UIB_FLAG_HAS_READ)) - return false; - - // If no data ready - fail - if (!slot->rxDataReadySize) - return 0; - - uint8_t bytes = slot->rxDataReadySize; - memcpy(buffer, slot->rxPacket, MIN(bytes, bufSize)); - slot->rxDataReadySize = 0; - - return bytes; -} - -static bool slotCanWrite(const uavInterconnectSlot_t * slot) -{ - // If no WRITE capability - fail - if (!(slot->deviceFlags & UIB_FLAG_HAS_WRITE)) - return false; - - // If we have unsent data in the buffer - fail - if (slot->txDataReadySize > 0) - return false; - - return true; -} - -bool uibCanWrite(uint8_t devId) -{ - // No device available - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return false; - - return slotCanWrite(slot); -} - -bool uibWrite(uint8_t devId, const uint8_t * buffer, uint8_t len) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return false; - - if (slotCanWrite(slot)) { - memcpy(slot->txPacket, buffer, len); - slot->txDataReadySize = len; - return true; - } - - return false; -} -#endif diff --git a/src/main/uav_interconnect/uav_interconnect_rangefinder.c b/src/main/uav_interconnect/uav_interconnect_rangefinder.c deleted file mode 100755 index 6fd9d825b2..0000000000 --- a/src/main/uav_interconnect/uav_interconnect_rangefinder.c +++ /dev/null @@ -1,121 +0,0 @@ -/* - * This file is part of INAV. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this file, - * You can obtain one at http://mozilla.org/MPL/2.0/. - * - * Alternatively, the contents of this file may be used under the terms - * of the GNU General Public License Version 3, as described below: - * - * This file is free software: you may copy, redistribute and/or modify - * it under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General - * Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see http://www.gnu.org/licenses/. - * - * @author Konstantin Sharlaimov - */ - -#include -#include -#include -#include -#include - -#include "platform.h" -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/maths.h" -#include "common/axis.h" -#include "common/utils.h" - -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" - -#ifdef USE_RANGEFINDER_UIB -#include "drivers/rangefinder/rangefinder.h" -#include "uav_interconnect/uav_interconnect.h" - -#define UIB_DEVICE_ADDRESS UIB_DEVICE_ID_RANGEFINDER - -#define RANGEFINDER_MAX_RANGE_CM 1000 -#define RANGEFINDER_DETECTION_CONE_DECIDEGREES 450 - -typedef struct __attribute__((packed)) { - uint8_t flags; - uint16_t distanceCm; -} rangefinderData_t; - -static int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; -static rangefinderData_t uibData; - -static void uibRangefinderInit(rangefinderDev_t *dev) -{ - UNUSED(dev); -} - -static void uibRangefinderUpdate(rangefinderDev_t *dev) -{ - UNUSED(dev); - - if (!uavInterconnectBusIsInitialized()) { - lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; - return; - } - - // If bus didn't detect the yet - report failure - if (!uibDetectAndActivateDevice(UIB_DEVICE_ADDRESS)) { - lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; - return; - } - - // If device is not responding - report failure - if (uibGetUnansweredRequests(UIB_DEVICE_ADDRESS) > 0) { - lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; - return; - } - - if (uibDataAvailable(UIB_DEVICE_ADDRESS)) { - uibRead(UIB_DEVICE_ADDRESS, (uint8_t*)&uibData, sizeof(uibData)); - - if (uibData.flags & UIB_DATA_VALID) { - lastCalculatedDistance = uibData.distanceCm; - dev->delayMs = uibGetPollRateUs(UIB_DEVICE_ADDRESS) / 1000; - } - else { - lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; - } - } -} - -static int32_t uibRangefinderGetDistance(rangefinderDev_t *dev) -{ - UNUSED(dev); - return lastCalculatedDistance; -} - -bool uibRangefinderDetect(rangefinderDev_t *dev) -{ - // This always succeed - dev->delayMs = RANGEFINDER_UIB_TASK_PERIOD_MS; - dev->maxRangeCm = RANGEFINDER_MAX_RANGE_CM; - dev->detectionConeDeciDegrees = RANGEFINDER_DETECTION_CONE_DECIDEGREES; - dev->detectionConeExtendedDeciDegrees = RANGEFINDER_DETECTION_CONE_DECIDEGREES; - - dev->init = &uibRangefinderInit; - dev->update = &uibRangefinderUpdate; - dev->read = &uibRangefinderGetDistance; - - return true; -} - -#endif