From 09603277b5320b015189d665d1e50d39f072d1bd Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 8 Nov 2023 16:26:10 -0500 Subject: [PATCH 01/29] Add AAT VRX type, needs fix to servoUpdate --- lib/CrsfProtocol/crsf_protocol.h | 65 ++++++++++++++- platformio.ini | 1 + src/Vrx_main.cpp | 15 +++- src/module_aat.cpp | 137 +++++++++++++++++++++++++++++++ src/module_aat.h | 33 ++++++++ src/module_base.h | 2 + targets/aat.ini | 17 ++++ 7 files changed, 265 insertions(+), 5 deletions(-) create mode 100644 src/module_aat.cpp create mode 100644 src/module_aat.h create mode 100644 targets/aat.ini diff --git a/lib/CrsfProtocol/crsf_protocol.h b/lib/CrsfProtocol/crsf_protocol.h index 4e44160..a0bfb96 100644 --- a/lib/CrsfProtocol/crsf_protocol.h +++ b/lib/CrsfProtocol/crsf_protocol.h @@ -2,5 +2,68 @@ #define CRSF_CRC_POLY 0xd5 +#define CRSF_FRAMETYPE_GPS 0x02 #define CRSF_FRAMETYPE_LINK_STATISTICS 0x14 -#define CRSF_FRAMETYPE_BATTERY_SENSOR 0x08 \ No newline at end of file +#define CRSF_FRAMETYPE_BATTERY_SENSOR 0x08 + +#define PACKED __attribute__((packed)) + +/** + * Define the shape of a standard header + */ +typedef struct crsf_header_s +{ + uint8_t device_addr; // from crsf_addr_e + uint8_t frame_size; // counts size after this byte, so it must be the payload size + 2 (type and crc) + uint8_t type; // from crsf_frame_type_e +} PACKED crsf_header_t; + +#define CRSF_MK_FRAME_T(payload) struct payload##_frame_s { crsf_header_t h; payload p; uint8_t crc; } PACKED + +typedef struct crsf_sensor_gps_s { + int32_t lat; // degrees * 1e7 + int32_t lon; // degrees * 1e7 + uint16_t speed; // big-endian km/h * 10 + uint16_t heading; // big-endian degrees * 10 + uint16_t altitude; // big endian meters + 1000 + uint8_t satcnt; // number of satellites +} crsf_sensor_gps_t; +typedef CRSF_MK_FRAME_T(crsf_sensor_gps_t) crsf_packet_gps_t; + +#if !defined(__linux__) +static inline uint16_t htobe16(uint16_t val) +{ +#if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) + return val; +#else + return __builtin_bswap16(val); +#endif +} + +static inline uint16_t be16toh(uint16_t val) +{ +#if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) + return val; +#else + return __builtin_bswap16(val); +#endif +} + +static inline uint32_t htobe32(uint32_t val) +{ +#if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) + return val; +#else + return __builtin_bswap32(val); +#endif +} + +static inline uint32_t be32toh(uint32_t val) +{ +#if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) + return val; +#else + return __builtin_bswap32(val); +#endif +} +#endif \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 2b26767..fd1ff16 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,6 +8,7 @@ extra_configs = targets/txbp_esp.ini targets/txbp_stm.ini # defined one by one to maintain the order + targets/aat.ini targets/fusion.ini targets/hdzero.ini targets/orqa.ini diff --git a/src/Vrx_main.cpp b/src/Vrx_main.cpp index d6c7e23..d2af43a 100644 --- a/src/Vrx_main.cpp +++ b/src/Vrx_main.cpp @@ -38,6 +38,8 @@ #include "skyzone_msp.h" #elif defined(ORQA_BACKPACK) #include "orqa.h" +#elif defined(AAT_BACKPACK) + #include "module_aat.h" #endif /////////// DEFINES /////////// @@ -108,6 +110,8 @@ VrxBackpackConfig config; SkyzoneMSP vrxModule(&Serial); #elif defined(ORQA_BACKPACK) Orqa vrxModule; +#elif defined(AAT_BACKPACK) + AatModule vrxModule; #endif /////////// FUNCTION DEFS /////////// @@ -135,15 +139,15 @@ void OnDataRecv(uint8_t * mac_addr, uint8_t *data, uint8_t data_len) void OnDataRecv(const uint8_t * mac_addr, const uint8_t *data, int data_len) #endif { - DBGLN("ESP NOW DATA:"); + //DBGLN("ESP NOW DATA:"); for(int i = 0; i < data_len; i++) { - DBG("%x", data[i]); // Debug prints - DBG(","); + //DBG("%x", data[i]); // Debug prints + //DBG(","); if (msp.processReceivedByte(data[i])) { - DBGLN(""); // Extra line for serial output readability + //DBGLN(""); // Extra line for serial output readability // Finished processing a complete packet // Only process packets from a bound MAC address if (connectionState == binding || @@ -236,6 +240,9 @@ void ProcessMSPPacket(mspPacket_t *packet) break; } switch (packet->payload[2]) { + case CRSF_FRAMETYPE_GPS: + vrxModule.SendGpsTelemetry((crsf_packet_gps_t *)packet->payload); + break; case CRSF_FRAMETYPE_BATTERY_SENSOR: vrxModule.SendBatteryTelemetry(packet->payload); break; diff --git a/src/module_aat.cpp b/src/module_aat.cpp new file mode 100644 index 0000000..2fffd1f --- /dev/null +++ b/src/module_aat.cpp @@ -0,0 +1,137 @@ +#include "module_aat.h" +#include "logging.h" +#include +#include + +#define DEG2RAD(deg) ((deg) * M_PI / 180.0) +#define RAD2DEG(rad) ((rad) * 180.0 / M_PI) + +void AatModule::Init() +{ +#if defined(PIN_SERVO_AZIM) + _servo_Azim.attach(PIN_SERVO_AZIM); +#endif +#if defined(PIN_SERVO_ELEV) + _servo_Elev.attach(PIN_SERVO_ELEV); +#endif + ModuleBase::Init(); +} + +void AatModule::SendGpsTelemetry(crsf_packet_gps_t *packet) +{ + _gpsLast.lat = be32toh(packet->p.lat); + _gpsLast.lon = be32toh(packet->p.lon); + _gpsLast.speed = be16toh(packet->p.speed); + _gpsLast.heading = be16toh(packet->p.heading); + _gpsLast.altitude = be16toh(packet->p.altitude); + _gpsLast.satcnt = packet->p.satcnt; + + DBGLN("GPS: (%d,%d) %dm %u sats", _gpsLast.lat, _gpsLast.lon, + (int32_t)_gpsLast.altitude - 1000, _gpsLast.satcnt); + _gpsUpdated = true; +} + +void calcDistAndAzimuth(int32_t srcLat, int32_t srcLon, int32_t dstLat, int32_t dstLon, + uint32_t *out_dist, uint32_t *out_azimuth) +{ + // https://www.movable-type.co.uk/scripts/latlong.html + // https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/ + + // Have to use doubles for at least some of these, due to short distances getting rounded + // particularly cos(deltaLon) for <2000 m rounds to 1.0000000000 + double deltaLon = DEG2RAD((float)(dstLon - srcLon) / 1e7); + double thetaA = DEG2RAD((float)srcLat / 1e7); + double thetaB = DEG2RAD((float)dstLat / 1e7); + double cosThetaA = cos(thetaA); + double cosThetaB = cos(thetaB); + double sinThetaA = sin(thetaA); + double sinThetaB = sin(thetaB); + double cosDeltaLon = cos(deltaLon); + double sinDeltaLon = sin(deltaLon); + + if (out_dist) + { + const double R = 6371e3; + double dist = acos(sinThetaA * sinThetaB + cosThetaA * cosThetaB * cosDeltaLon) * R; + *out_dist = (uint32_t)dist; + } + + if (out_azimuth) + { + double X = cosThetaB * sinDeltaLon; + double Y = cosThetaA * sinThetaB - sinThetaA * cosThetaB * cosDeltaLon; + + // Convert to degrees, normalized to 0-360 + uint32_t hdg = RAD2DEG(atan2(X, Y)); + *out_azimuth = (hdg + 360) % 360; + } +} + +static int32_t calcElevation(uint32_t distance, int32_t altitude) +{ + return RAD2DEG(atan2(altitude, distance)); +} + +void AatModule::processGps() +{ + // Check if need to set home position + if (_homeLat == 0 && _homeLon == 0) + { + if (_gpsLast.satcnt >= HOME_MIN_SATS) + { + _homeLat = _gpsLast.lat; + _homeLon = _gpsLast.lon; + DBGLN("GPS Home set to (%d,%d)", _homeLat, _homeLon); + } + else + return; + } + + uint32_t azimuth; + uint32_t distance; + calcDistAndAzimuth(_homeLat, _homeLon, _gpsLast.lat, _gpsLast.lon, &distance, &azimuth); + uint8_t elevation = constrain(calcElevation(distance, (int32_t)_gpsLast.altitude - 1000), 0, 180); + DBGLN("Azimuth: %udeg Elevation: %udeg Distance: %um", azimuth, elevation, distance); + + _targetDistance = distance; + _targetElev = elevation; + _targetAzim = azimuth; +} + +void AatModule::servoUpdate(uint32_t now) +{ + if (now - _lastServoUpdateMs < 20U) + return; + _lastServoUpdateMs = now; + + // Smooth the updates + _currentAzim = ((_currentAzim * 8) + (_targetAzim * 200)) / 10; + _currentElev = ((_currentElev * 8) + (_targetElev * 200)) / 10; + + // 90-270 azim inverts the elevation servo + int32_t projectedAzim = _currentAzim; + int32_t projectedElev = _currentElev; + if (projectedAzim > 18000) + { + projectedElev = 18000 - projectedElev; + projectedAzim = projectedAzim - 18000; + } +#if defined(PIN_SERVO_AZIM) + _servo_Azim.writeMicroseconds(map(projectedAzim, 0, 18000, 500, 2500)); +#endif +#if defined(PIN_SERVO_ELEV) + _servo_Elev.writeMicroseconds(map(projectedElev, 0, 18000, 500, 2500)); +#endif +} + +void AatModule::Loop(uint32_t now) +{ + if (_gpsUpdated) + { + _gpsUpdated = false; + processGps(); + } + + servoUpdate(now); + ModuleBase::Loop(now); +} \ No newline at end of file diff --git a/src/module_aat.h b/src/module_aat.h new file mode 100644 index 0000000..faede41 --- /dev/null +++ b/src/module_aat.h @@ -0,0 +1,33 @@ +#pragma once + +#include "module_base.h" +#include "crsf_protocol.h" +#include + +class AatModule : public ModuleBase +{ +public: + void Init(); + void Loop(uint32_t now); + + void SendGpsTelemetry(crsf_packet_gps_t *packet); +private: + // Minimum number of satellites to lock in the home position + static constexpr uint8_t HOME_MIN_SATS = 5; + void processGps(); + void servoUpdate(uint32_t now); + + bool _gpsUpdated; + crsf_sensor_gps_t _gpsLast; + int32_t _homeLat; + int32_t _homeLon; + // Servo Position + Servo _servo_Azim; + Servo _servo_Elev; + uint32_t _lastServoUpdateMs; + uint32_t _targetDistance; // meters + uint8_t _targetElev; // degrees + uint8_t _targetAzim; // degrees + int32_t _currentElev; // degrees * 100 + int32_t _currentAzim; // degrees * 100 +}; diff --git a/src/module_base.h b/src/module_base.h index 1ef59b5..153a28a 100644 --- a/src/module_base.h +++ b/src/module_base.h @@ -1,6 +1,7 @@ #pragma once #include +#include "crsf_protocol.h" #include "msp.h" class ModuleBase @@ -14,6 +15,7 @@ class ModuleBase void SetRTC(); void SendLinkTelemetry(uint8_t *rawCrsfPacket); void SendBatteryTelemetry(uint8_t *rawCrsfPacket); + void SendGpsTelemetry(crsf_packet_gps_t *packet); void Loop(uint32_t now); }; diff --git a/targets/aat.ini b/targets/aat.ini new file mode 100644 index 0000000..b019fb7 --- /dev/null +++ b/targets/aat.ini @@ -0,0 +1,17 @@ + +[env:AAT_ESP_Backpack_via_UART] +extends = env_common_esp12e +build_flags = + ${common_env_data.build_flags} + ${env_common_esp12e.build_flags} + -D TARGET_VRX_BACKPACK + -D AAT_BACKPACK + -D PIN_LED=2 + -D PIN_SERVO_AZIM=4 + -D PIN_SERVO_ELEV=5 + -D DEBUG_LOG +lib_ignore = STM32UPDATE +build_src_filter = ${common_env_data.build_src_filter} - - - - + +[env:AAT_ESP_Backpack_via_WIFI] +extends = env:AAT_ESP_Backpack_via_UART From e56ba97229a2448de544073c1095a60edae228ef Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Fri, 22 Dec 2023 11:21:59 -0500 Subject: [PATCH 02/29] First working AAT backpack with OLED --- src/module_aat.cpp | 88 +++++++++++++++++++++++++++++++++++++--------- src/module_aat.h | 41 ++++++++++++++++++--- targets/aat.ini | 21 ++++++++--- 3 files changed, 124 insertions(+), 26 deletions(-) diff --git a/src/module_aat.cpp b/src/module_aat.cpp index 2fffd1f..eae12a8 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -1,3 +1,4 @@ +#include "common.h" #include "module_aat.h" #include "logging.h" #include @@ -6,13 +7,39 @@ #define DEG2RAD(deg) ((deg) * M_PI / 180.0) #define RAD2DEG(rad) ((rad) * 180.0 / M_PI) +AatModule::AatModule() : +#if defined(PIN_SERVO_AZIM) + _servo_Azim(), +#endif +#if defined(PIN_SERVO_ELEV) + _servo_Elev(), +#endif +#if defined(PIN_OLED_SDA) + display(SCREEN_WIDTH, SCREEN_HEIGHT) +#endif +{ + // Init is called manually +} + void AatModule::Init() { #if defined(PIN_SERVO_AZIM) - _servo_Azim.attach(PIN_SERVO_AZIM); + _servo_Azim.attach(PIN_SERVO_AZIM, 500, 2500); #endif #if defined(PIN_SERVO_ELEV) - _servo_Elev.attach(PIN_SERVO_ELEV); + _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500); +#endif +#if defined(PIN_OLED_SDA) + Wire.begin(PIN_OLED_SDA, PIN_OLED_SCL); + display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); + display.setTextSize(2); + display.setTextColor(SSD1306_WHITE); + display.setCursor(0, 0); + if (connectionState == binding) + display.print("Bind\nmode..."); + else + display.print("AAT\nBackpack"); + display.display(); #endif ModuleBase::Init(); } @@ -23,11 +50,12 @@ void AatModule::SendGpsTelemetry(crsf_packet_gps_t *packet) _gpsLast.lon = be32toh(packet->p.lon); _gpsLast.speed = be16toh(packet->p.speed); _gpsLast.heading = be16toh(packet->p.heading); - _gpsLast.altitude = be16toh(packet->p.altitude); + _gpsLast.altitude = (int32_t)be16toh(packet->p.altitude) - 1000; _gpsLast.satcnt = packet->p.satcnt; DBGLN("GPS: (%d,%d) %dm %u sats", _gpsLast.lat, _gpsLast.lon, - (int32_t)_gpsLast.altitude - 1000, _gpsLast.satcnt); + _gpsLast.altitude, _gpsLast.satcnt); + _gpsUpdated = true; } @@ -75,12 +103,13 @@ static int32_t calcElevation(uint32_t distance, int32_t altitude) void AatModule::processGps() { // Check if need to set home position - if (_homeLat == 0 && _homeLon == 0) + if (!isHomeSet()) { if (_gpsLast.satcnt >= HOME_MIN_SATS) { _homeLat = _gpsLast.lat; _homeLon = _gpsLast.lon; + _homeAlt = _gpsLast.altitude; DBGLN("GPS Home set to (%d,%d)", _homeLat, _homeLon); } else @@ -90,7 +119,7 @@ void AatModule::processGps() uint32_t azimuth; uint32_t distance; calcDistAndAzimuth(_homeLat, _homeLon, _gpsLast.lat, _gpsLast.lon, &distance, &azimuth); - uint8_t elevation = constrain(calcElevation(distance, (int32_t)_gpsLast.altitude - 1000), 0, 180); + uint8_t elevation = constrain(calcElevation(distance, _gpsLast.altitude - _homeAlt), 0, 90); DBGLN("Azimuth: %udeg Elevation: %udeg Distance: %um", azimuth, elevation, distance); _targetDistance = distance; @@ -100,28 +129,53 @@ void AatModule::processGps() void AatModule::servoUpdate(uint32_t now) { + if (!isHomeSet()) + return; if (now - _lastServoUpdateMs < 20U) return; _lastServoUpdateMs = now; - // Smooth the updates - _currentAzim = ((_currentAzim * 8) + (_targetAzim * 200)) / 10; - _currentElev = ((_currentElev * 8) + (_targetElev * 200)) / 10; + // Smooth the updates and scale to degrees * 100 + _currentAzim = ((_currentAzim * 9) + (_targetAzim * 100)) / 10; + _currentElev = ((_currentElev * 9) + (_targetElev * 100)) / 10; - // 90-270 azim inverts the elevation servo int32_t projectedAzim = _currentAzim; int32_t projectedElev = _currentElev; - if (projectedAzim > 18000) - { - projectedElev = 18000 - projectedElev; - projectedAzim = projectedAzim - 18000; - } + + // 90-270 azim inverts the elevation servo + // if (projectedAzim > 18000) + // { + // projectedElev = 18000 - projectedElev; + // projectedAzim = projectedAzim - 18000; + // } + // For straight geared servos with 180 degree elev flip + // int32_t usAzim = map(projectedAzim, 0, 18000, 500, 2500); + // int32_t usElev = map(projectedElev, 0, 18000, 500, 2500); + + // For 1:2 gearing on the azim servo to allow 360 rotation + // For Elev servos that only go 0-90 and the azim does 360 + projectedAzim = (projectedAzim + 18000) % 36000; // convert so 0 maps to 1500us + int32_t usAzim = map(projectedAzim, 0, 36000, 700, 2400); + int32_t usElev = map(projectedElev, 0, 9000, 1100, 1900); + #if defined(PIN_SERVO_AZIM) - _servo_Azim.writeMicroseconds(map(projectedAzim, 0, 18000, 500, 2500)); + _servo_Azim.writeMicroseconds(usAzim); #endif #if defined(PIN_SERVO_ELEV) - _servo_Elev.writeMicroseconds(map(projectedElev, 0, 18000, 500, 2500)); + _servo_Elev.writeMicroseconds(usElev); #endif + +#if defined(PIN_OLED_SDA) + display.clearDisplay(); + display.setCursor(0, 0); + //display.printf("GPS (%d,%d)\n", _gpsLast.lat, _gpsLast.lon); + //display.printf("%dm %u sats\n", _gpsLast.altitude, _gpsLast.satcnt); + display.printf("Az:%u %dm\nEl:%u %dm\n", _targetAzim, _targetDistance, + _targetElev, _gpsLast.altitude - _homeAlt); + display.printf("SAz:%dus\nSEl:%dus\n", usAzim, usElev); + display.display(); +#endif + } void AatModule::Loop(uint32_t now) diff --git a/src/module_aat.h b/src/module_aat.h index faede41..4794061 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -2,32 +2,65 @@ #include "module_base.h" #include "crsf_protocol.h" + +#if defined(PIN_SERVO_AZIM) || defined(PIN_SERVO_ELEV) #include +#endif + +#if defined(PIN_OLED_SDA) +#include +#include +#include + +#define SCREEN_WIDTH 128 +#define SCREEN_HEIGHT 64 +#define SCREEN_ADDRESS 0x3C +#endif class AatModule : public ModuleBase { public: + AatModule(); + void Init(); void Loop(uint32_t now); void SendGpsTelemetry(crsf_packet_gps_t *packet); + bool isHomeSet() const { return _homeLat !=0 && _homeLon != 0; } private: // Minimum number of satellites to lock in the home position static constexpr uint8_t HOME_MIN_SATS = 5; + void processGps(); void servoUpdate(uint32_t now); +#if defined(PIN_SERVO_AZIM) + Servo _servo_Azim; +#endif +#if defined(PIN_SERVO_ELEV) + Servo _servo_Elev; +#endif +#if defined(PIN_OLED_SDA) + Adafruit_SSD1306 display; +#endif + bool _gpsUpdated; - crsf_sensor_gps_t _gpsLast; + struct { + int32_t lat; + int32_t lon; + uint32_t speed; // km/h * 10 + uint32_t heading; // degrees * 10 + int32_t altitude; // meters + uint8_t satcnt; // number of satellites + } _gpsLast; int32_t _homeLat; int32_t _homeLon; + int32_t _homeAlt; // Servo Position - Servo _servo_Azim; - Servo _servo_Elev; uint32_t _lastServoUpdateMs; uint32_t _targetDistance; // meters + uint16_t _targetAzim; // degrees uint8_t _targetElev; // degrees - uint8_t _targetAzim; // degrees int32_t _currentElev; // degrees * 100 int32_t _currentAzim; // degrees * 100 }; diff --git a/targets/aat.ini b/targets/aat.ini index b019fb7..7329c47 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -1,4 +1,3 @@ - [env:AAT_ESP_Backpack_via_UART] extends = env_common_esp12e build_flags = @@ -6,11 +5,23 @@ build_flags = ${env_common_esp12e.build_flags} -D TARGET_VRX_BACKPACK -D AAT_BACKPACK - -D PIN_LED=2 - -D PIN_SERVO_AZIM=4 - -D PIN_SERVO_ELEV=5 - -D DEBUG_LOG + -D SSD1306_NO_SPLASH + -D PIN_BUTTON=0 + -D PIN_LED=16 + -D LED_INVERTED + -D PIN_SERVO_AZIM=9 + -D PIN_SERVO_ELEV=10 + -D PIN_OLED_SCL=1 + -D PIN_OLED_SDA=3 + ; -D PIN_SERVO_AZIM=1 + ; -D PIN_SERVO_ELEV=3 + ; -D PIN_OLED_SCL=9 + ; -D PIN_OLED_SDA=10 + ; -D DEBUG_LOG lib_ignore = STM32UPDATE +lib_deps = + ${env.lib_deps} + adafruit/Adafruit SSD1306 @ 2.5.9 build_src_filter = ${common_env_data.build_src_filter} - - - - [env:AAT_ESP_Backpack_via_WIFI] From f9ad3280f35759abc7c520400889048789acc3ae Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 27 Dec 2023 09:43:04 -0500 Subject: [PATCH 03/29] WIP --- lib/CrsfProtocol/crsf_protocol.h | 3 +- src/module_aat.cpp | 117 ++++++++++++++++++++++++++++++- src/module_aat.h | 21 ++++++ targets/aat.ini | 1 + 4 files changed, 139 insertions(+), 3 deletions(-) diff --git a/lib/CrsfProtocol/crsf_protocol.h b/lib/CrsfProtocol/crsf_protocol.h index a0bfb96..856b877 100644 --- a/lib/CrsfProtocol/crsf_protocol.h +++ b/lib/CrsfProtocol/crsf_protocol.h @@ -1,6 +1,7 @@ #pragma once #define CRSF_CRC_POLY 0xd5 +#define CRSF_SYNC_BYTE 0xc8 #define CRSF_FRAMETYPE_GPS 0x02 #define CRSF_FRAMETYPE_LINK_STATISTICS 0x14 @@ -13,7 +14,7 @@ */ typedef struct crsf_header_s { - uint8_t device_addr; // from crsf_addr_e + uint8_t sync_byte; // CRSF_SYNC_BYTE uint8_t frame_size; // counts size after this byte, so it must be the payload size + 2 (type and crc) uint8_t type; // from crsf_frame_type_e } PACKED crsf_header_t; diff --git a/src/module_aat.cpp b/src/module_aat.cpp index eae12a8..2ce5f55 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -1,3 +1,4 @@ +#if defined(AAT_BACKPACK) #include "common.h" #include "module_aat.h" #include "logging.h" @@ -15,7 +16,10 @@ AatModule::AatModule() : _servo_Elev(), #endif #if defined(PIN_OLED_SDA) - display(SCREEN_WIDTH, SCREEN_HEIGHT) + display(SCREEN_WIDTH, SCREEN_HEIGHT), +#endif +#if defined (AAT_SERIAL_INPUT) + _crc(CRSF_CRC_POLY) #endif { // Init is called manually @@ -102,11 +106,18 @@ static int32_t calcElevation(uint32_t distance, int32_t altitude) void AatModule::processGps() { + // Actually want to track time between _processing_ each GPS update + uint32_t now = millis(); + uint32_t interval = _gpsLast.lastUpdateMs - now; + _gpsLast.lastUpdateMs = now; + // Check if need to set home position + bool didSetHome = false; if (!isHomeSet()) { if (_gpsLast.satcnt >= HOME_MIN_SATS) { + didSetHome = true; _homeLat = _gpsLast.lat; _homeLon = _gpsLast.lon; _homeAlt = _gpsLast.altitude; @@ -122,6 +133,14 @@ void AatModule::processGps() uint8_t elevation = constrain(calcElevation(distance, _gpsLast.altitude - _homeAlt), 0, 90); DBGLN("Azimuth: %udeg Elevation: %udeg Distance: %um", azimuth, elevation, distance); + // Calculate angular velocity to allow dead reckoning projection + if (!didSetHome) + { + int32_t azimDelta = azimuth - _targetAzim; + int32_t azimMsPerDelta = interval / azimDelta; + DBGLN("%d delta in %ums, %dms/d", azimDelta, interval, azimMsPerDelta); + } + _targetDistance = distance; _targetElev = elevation; _targetAzim = azimuth; @@ -178,8 +197,101 @@ void AatModule::servoUpdate(uint32_t now) } +#if defined(AAT_SERIAL_INPUT) +void AatModule::processPacketIn(uint8_t len) +{ + const crsf_header_t *hdr = (crsf_header_t *)_rxBuf; + if (hdr->sync_byte == CRSF_SYNC_BYTE) // || hdr->sync_byte == CRSF_ADDRESS_FLIGHT_CONTROLLER) + { + switch (hdr->type) + { + case CRSF_FRAMETYPE_GPS: + SendGpsTelemetry((crsf_packet_gps_t *)hdr); + break; + } + } +} + +// Shift the bytes in the RxBuf down by cnt bytes +void AatModule::shiftRxBuffer(uint8_t cnt) +{ + // If removing the whole thing, just set pos to 0 + if (cnt >= _rxBufPos) + { + _rxBufPos = 0; + return; + } + + // Otherwise do the slow shift down + uint8_t *src = &_rxBuf[cnt]; + uint8_t *dst = &_rxBuf[0]; + _rxBufPos -= cnt; + uint8_t left = _rxBufPos; + while (left--) + *dst++ = *src++; +} + +void AatModule::handleByteReceived() +{ + bool reprocess; + do + { + reprocess = false; + if (_rxBufPos > 1) + { + uint8_t len = _rxBuf[1]; + // Sanity check the declared length isn't outside Type + X{1,CRSF_MAX_PAYLOAD_LEN} + CRC + // assumes there never will be a CRSF message that just has a type and no data (X) + if (len < 3 || len > (CRSF_MAX_PAYLOAD_LEN + 2)) + { + shiftRxBuffer(1); + reprocess = true; + } + + else if (_rxBufPos >= (len + 2)) + { + uint8_t inCrc = _rxBuf[2 + len - 1]; + uint8_t crc = _crc.calc(&_rxBuf[2], len - 1); + if (crc == inCrc) + { + processPacketIn(len); + shiftRxBuffer(len + 2); + reprocess = true; + } + else + { + shiftRxBuffer(1); + reprocess = true; + } + } // if complete packet + } // if pos > 1 + } while (reprocess); +} +#endif + +void AatModule::checkSerialInput() +{ +#if defined(AAT_SERIAL_INPUT) + while (Serial.available()) + { + uint8_t b = Serial.read(); + _rxBuf[_rxBufPos++] = b; + + handleByteReceived(); + + if (_rxBufPos == (sizeof(_rxBuf)/sizeof(_rxBuf[0]))) + { + // Packet buffer filled and no valid packet found, dump the whole thing + _rxBufPos = 0; + } + } +#endif +} + void AatModule::Loop(uint32_t now) { + checkSerialInput(); + if (_gpsUpdated) { _gpsUpdated = false; @@ -188,4 +300,5 @@ void AatModule::Loop(uint32_t now) servoUpdate(now); ModuleBase::Loop(now); -} \ No newline at end of file +} +#endif /* AAT_BACKPACK */ \ No newline at end of file diff --git a/src/module_aat.h b/src/module_aat.h index 4794061..6fdc78c 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -1,8 +1,13 @@ #pragma once +#if defined(AAT_BACKPACK) #include "module_base.h" #include "crsf_protocol.h" +#if defined (AAT_SERIAL_INPUT) +#include "crc.h" +#endif + #if defined(PIN_SERVO_AZIM) || defined(PIN_SERVO_ELEV) #include #endif @@ -31,6 +36,7 @@ class AatModule : public ModuleBase // Minimum number of satellites to lock in the home position static constexpr uint8_t HOME_MIN_SATS = 5; + void checkSerialInput(); void processGps(); void servoUpdate(uint32_t now); @@ -51,6 +57,7 @@ class AatModule : public ModuleBase uint32_t speed; // km/h * 10 uint32_t heading; // degrees * 10 int32_t altitude; // meters + uint32_t lastUpdateMs; // timestamp of last update uint8_t satcnt; // number of satellites } _gpsLast; int32_t _homeLat; @@ -63,4 +70,18 @@ class AatModule : public ModuleBase uint8_t _targetElev; // degrees int32_t _currentElev; // degrees * 100 int32_t _currentAzim; // degrees * 100 + +#if defined (AAT_SERIAL_INPUT) + static constexpr uint8_t CRSF_MAX_PACKET_SIZE = 64U; + static constexpr uint8_t CRSF_MAX_PAYLOAD_LEN = (CRSF_MAX_PACKET_SIZE - 4U); + + GENERIC_CRC8 _crc; + uint8_t _rxBufPos; + uint8_t _rxBuf[CRSF_MAX_PACKET_SIZE]; + + void processPacketIn(uint8_t len); + void shiftRxBuffer(uint8_t cnt); + void handleByteReceived(); +#endif }; +#endif /* AAT_BACKPACK */ \ No newline at end of file diff --git a/targets/aat.ini b/targets/aat.ini index 7329c47..5a778f5 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -5,6 +5,7 @@ build_flags = ${env_common_esp12e.build_flags} -D TARGET_VRX_BACKPACK -D AAT_BACKPACK + -D AAT_SERIAL_INPUT -D SSD1306_NO_SPLASH -D PIN_BUTTON=0 -D PIN_LED=16 From be36dee18c9b3235eab1a28d0f6de582aba9e179 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Thu, 28 Dec 2023 12:29:39 -0500 Subject: [PATCH 04/29] Add CrsfModuleBase --- src/module_crsf.cpp | 76 +++++++++++++++++++++++++++++++++++++++++++++ src/module_crsf.h | 31 ++++++++++++++++++ 2 files changed, 107 insertions(+) create mode 100644 src/module_crsf.cpp create mode 100644 src/module_crsf.h diff --git a/src/module_crsf.cpp b/src/module_crsf.cpp new file mode 100644 index 0000000..eff7fbd --- /dev/null +++ b/src/module_crsf.cpp @@ -0,0 +1,76 @@ +#include "module_crsf.h" + +void CrsfModuleBase::Loop(uint32_t now) +{ + while (_port.available()) + { + uint8_t b = _port.read(); + _rxBuf[_rxBufPos++] = b; + + handleByteReceived(); + + if (_rxBufPos == (sizeof(_rxBuf)/sizeof(_rxBuf[0]))) + { + // Packet buffer filled and no valid packet found, dump the whole thing + _rxBufPos = 0; + } + } + ModuleBase::Loop(now); +} + +void CrsfModuleBase::handleByteReceived() +{ + bool reprocess; + do + { + reprocess = false; + if (_rxBufPos > 1) + { + uint8_t len = _rxBuf[1]; + // Sanity check the declared length isn't outside Type + X{1,CRSF_MAX_PAYLOAD_LEN} + CRC + // assumes there never will be a CRSF message that just has a type and no data (X) + if (len < 3 || len > (CRSF_MAX_PAYLOAD_LEN + 2)) + { + shiftRxBuffer(1); + reprocess = true; + } + + else if (_rxBufPos >= (len + 2)) + { + uint8_t inCrc = _rxBuf[2 + len - 1]; + uint8_t crc = _crc.calc(&_rxBuf[2], len - 1); + if (crc == inCrc) + { + onCrsfPacketIn((const crsf_header_t *)_rxBuf); + + shiftRxBuffer(len + 2); + reprocess = true; + } + else + { + shiftRxBuffer(1); + reprocess = true; + } + } // if complete packet + } // if pos > 1 + } while (reprocess); +} + +// Shift the bytes in the RxBuf down by cnt bytes +void CrsfModuleBase::shiftRxBuffer(uint8_t cnt) +{ + // If removing the whole thing, just set pos to 0 + if (cnt >= _rxBufPos) + { + _rxBufPos = 0; + return; + } + + // Otherwise do the slow shift down + uint8_t *src = &_rxBuf[cnt]; + uint8_t *dst = &_rxBuf[0]; + _rxBufPos -= cnt; + uint8_t left = _rxBufPos; + while (left--) + *dst++ = *src++; +} diff --git a/src/module_crsf.h b/src/module_crsf.h new file mode 100644 index 0000000..0ddacfe --- /dev/null +++ b/src/module_crsf.h @@ -0,0 +1,31 @@ +#pragma once + +#include "module_base.h" +#include "crsf_protocol.h" +#include "crc.h" + +class CrsfModuleBase : public ModuleBase +{ +public: + CrsfModuleBase() = delete; + CrsfModuleBase(Stream &port) : + _port(port), _crc(CRSF_CRC_POLY) + {}; + void Loop(uint32_t now); + + virtual void onCrsfPacketIn(const crsf_header_t *pkt) {}; + +protected: + Stream &_port; + + static constexpr uint8_t CRSF_MAX_PACKET_SIZE = 64U; + static constexpr uint8_t CRSF_MAX_PAYLOAD_LEN = (CRSF_MAX_PACKET_SIZE - 4U); + + GENERIC_CRC8 _crc; + uint8_t _rxBufPos; + uint8_t _rxBuf[CRSF_MAX_PACKET_SIZE]; + + void processPacketIn(uint8_t len); + void shiftRxBuffer(uint8_t cnt); + void handleByteReceived(); +}; \ No newline at end of file From 8f78ff8b05221fa5c438511b1ba7c0e361e7aa3a Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Thu, 28 Dec 2023 12:45:21 -0500 Subject: [PATCH 05/29] Add AAT azimuth projection, update ticker, config, test util --- lib/WIFI/devWIFI.h | 1 + lib/config/config.cpp | 16 +- lib/config/config.h | 19 ++ python/utils/gpscsv_to_crsf.py | 108 +++++++++ src/Vrx_main.cpp | 2 +- src/module_aat.cpp | 407 ++++++++++++++++++--------------- src/module_aat.h | 68 +++--- 7 files changed, 395 insertions(+), 226 deletions(-) create mode 100644 python/utils/gpscsv_to_crsf.py diff --git a/lib/WIFI/devWIFI.h b/lib/WIFI/devWIFI.h index e4fa66d..439632a 100644 --- a/lib/WIFI/devWIFI.h +++ b/lib/WIFI/devWIFI.h @@ -4,5 +4,6 @@ #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266) extern device_t WIFI_device; +extern const char *VERSION; #define HAS_WIFI #endif \ No newline at end of file diff --git a/lib/config/config.cpp b/lib/config/config.cpp index 0fc1266..adb1545 100644 --- a/lib/config/config.cpp +++ b/lib/config/config.cpp @@ -135,12 +135,18 @@ VrxBackpackConfig::SetStorageProvider(ELRS_EEPROM *eeprom) void VrxBackpackConfig::SetDefaults() { + memset(&m_config, 0, sizeof(m_config)); m_config.version = VRX_BACKPACK_CONFIG_VERSION | VRX_BACKPACK_CONFIG_MAGIC; - m_config.bootCount = 0; - m_config.startWiFi = false; - m_config.ssid[0] = 0; - m_config.password[0] = 0; - memset(m_config.address, 0, 6); + +#if defined(AAT_BACKPACK) + m_config.project = 0xff; + m_config.servoSmooth = 9; + m_config.servoLowAzim = 700; + m_config.servoLowElev = 1100; + m_config.servoHighAzim = 2400; + m_config.servoHighElev = 1900; +#endif + m_modified = true; Commit(); } diff --git a/lib/config/config.h b/lib/config/config.h index ae8f9e1..d58846b 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -59,6 +59,16 @@ typedef struct { char ssid[33]; char password[65]; uint8_t address[6]; + +#if defined(AAT_BACKPACK) + uint8_t servoSmooth; + uint8_t project; // 0=none, 1=projectAzim, 2=projectElev, 3=projectBoth + uint8_t servoMode; // reserved to declare 2:1 or 180+flip servo + uint16_t servoLowAzim; + uint16_t servoLowElev; + uint16_t servoHighAzim; + uint16_t servoHighElev; +#endif } vrx_backpack_config_t; class VrxBackpackConfig @@ -84,6 +94,15 @@ class VrxBackpackConfig void SetPassword(const char *ssid); void SetGroupAddress(const uint8_t address[6]); +#if defined(AAT_BACKPACK) + uint8_t GetAatServoSmooth() const { return m_config.servoSmooth; } + uint8_t GetAatProject() const { return m_config.project; } + uint16_t GetAatServoLowAzim() const { return m_config.servoLowAzim; } + uint16_t GetAatServoLowElev() const { return m_config.servoLowElev; } + uint16_t GetAatServoHighAzim() const { return m_config.servoHighAzim; } + uint16_t GetAatServoHighElev() const { return m_config.servoHighElev; } +#endif + private: vrx_backpack_config_t m_config; ELRS_EEPROM *m_eeprom; diff --git a/python/utils/gpscsv_to_crsf.py b/python/utils/gpscsv_to_crsf.py new file mode 100644 index 0000000..9c49755 --- /dev/null +++ b/python/utils/gpscsv_to_crsf.py @@ -0,0 +1,108 @@ +import sys +import time +import csv +import argparse +import serial +import random + +_crc_tab = [ + 0x00,0xD5,0x7F,0xAA,0xFE,0x2B,0x81,0x54,0x29,0xFC,0x56,0x83,0xD7,0x02,0xA8,0x7D,0x52,0x87,0x2D,0xF8,0xAC,0x79,0xD3,0x06,0x7B,0xAE,0x04,0xD1,0x85,0x50,0xFA,0x2F,\ + 0xA4,0x71,0xDB,0x0E,0x5A,0x8F,0x25,0xF0,0x8D,0x58,0xF2,0x27,0x73,0xA6,0x0C,0xD9,0xF6,0x23,0x89,0x5C,0x08,0xDD,0x77,0xA2,0xDF,0x0A,0xA0,0x75,0x21,0xF4,0x5E,0x8B,\ + 0x9D,0x48,0xE2,0x37,0x63,0xB6,0x1C,0xC9,0xB4,0x61,0xCB,0x1E,0x4A,0x9F,0x35,0xE0,0xCF,0x1A,0xB0,0x65,0x31,0xE4,0x4E,0x9B,0xE6,0x33,0x99,0x4C,0x18,0xCD,0x67,0xB2,\ + 0x39,0xEC,0x46,0x93,0xC7,0x12,0xB8,0x6D,0x10,0xC5,0x6F,0xBA,0xEE,0x3B,0x91,0x44,0x6B,0xBE,0x14,0xC1,0x95,0x40,0xEA,0x3F,0x42,0x97,0x3D,0xE8,0xBC,0x69,0xC3,0x16,\ + 0xEF,0x3A,0x90,0x45,0x11,0xC4,0x6E,0xBB,0xC6,0x13,0xB9,0x6C,0x38,0xED,0x47,0x92,0xBD,0x68,0xC2,0x17,0x43,0x96,0x3C,0xE9,0x94,0x41,0xEB,0x3E,0x6A,0xBF,0x15,0xC0,\ + 0x4B,0x9E,0x34,0xE1,0xB5,0x60,0xCA,0x1F,0x62,0xB7,0x1D,0xC8,0x9C,0x49,0xE3,0x36,0x19,0xCC,0x66,0xB3,0xE7,0x32,0x98,0x4D,0x30,0xE5,0x4F,0x9A,0xCE,0x1B,0xB1,0x64,\ + 0x72,0xA7,0x0D,0xD8,0x8C,0x59,0xF3,0x26,0x5B,0x8E,0x24,0xF1,0xA5,0x70,0xDA,0x0F,0x20,0xF5,0x5F,0x8A,0xDE,0x0B,0xA1,0x74,0x09,0xDC,0x76,0xA3,0xF7,0x22,0x88,0x5D,\ + 0xD6,0x03,0xA9,0x7C,0x28,0xFD,0x57,0x82,0xFF,0x2A,0x80,0x55,0x01,0xD4,0x7E,0xAB,0x84,0x51,0xFB,0x2E,0x7A,0xAF,0x05,0xD0,0xAD,0x78,0xD2,0x07,0x53,0x86,0x2C,0xF9,\ +] + +def crsf_frame_crc(frame: bytes) -> int: + crc_frame = frame[2:-1] + return crsf_crc(crc_frame) + +def crsf_crc(data: bytes) -> int: + crc = 0 + for i in data: + crc = _crc_tab[crc ^ i] + return crc + +def idle(duration: float, port: serial.Serial): + if port: + start = time.monotonic() + while time.monotonic() - start < duration: + b = port.read(128) + if len(b) > 0: + sys.stdout.write(b.decode('utf-8')) + else: + time.sleep(duration) + +def processFile(fname, interval, port, baud, jitter): + random.seed() + + if port is not None: + uart = serial.Serial(port=port, baudrate=baud, timeout=0.1) + else: + uart = None + + with open(fname, 'r') as csv_file: + reader = csv.reader(csv_file) + # Skip header. DictReader is not used for performance and simplicity + # ['time (us)', 'GPS_fixType', ' GPS_numSat', ' GPS_coord[0]', ' GPS_coord[1]', ' GPS_altitude', ' GPS_speed (m/s)', ' GPS_ground_course', ' GPS_hdop', ' GPS_eph', ' GPS_epv', ' GPS_velned[0]', ' GPS_velned[1]', ' GPS_velned[2]'] + next(reader) + + lastTime = None + for row in reader: + timeS = float(row[0]) / 1e6 + + if lastTime != None: + dur = timeS - lastTime + if dur < interval: + continue + idle(dur + (random.randrange(0, int(jitter * 1000.0)) / 1000.0), uart) + + # All values here are immediately loaded into their CRSF representations + sats = int(row[2]) + lat = int(float(row[3]) * 1e7) + lon = int(float(row[4]) * 1e7) + alt = int(row[5]) + 1000 + spd = int(float(row[6]) * 360) # m/s to km/h*10 + hdg = int(float(row[7]) * 100) # deg to centideg + #print(f'{timeS:0.6f} GPS: ({lat},{lon}) {alt-1000}m {sats} sats') + + # CRSF_SYNC, len, type (GPS), payload, crc + buf = bytearray(b'\xc8\x00\x02') + buf += int.to_bytes(lat, length=4, byteorder='big', signed=True) + buf += int.to_bytes(lon, length=4, byteorder='big', signed=True) + buf += int.to_bytes(spd, length=2, byteorder='big', signed=False) + buf += int.to_bytes(hdg, length=2, byteorder='big', signed=False) + buf += int.to_bytes(alt, length=2, byteorder='big', signed=False) + buf += int.to_bytes(sats, length=1, byteorder='big', signed=False) + buf += b'\x00' # place for the CRC + buf[-1] = crsf_frame_crc(buf) + buf[1] = len(buf) - 2 + if uart: + uart.write(buf) + + lastTime = timeS + +parser = argparse.ArgumentParser( + prog='gpscsv_to_crsf', + description='Convert iNav GPS CSV to CRSF and send in real time' +) +parser.add_argument('filename') +parser.add_argument('-b', '--baud', + default='460800', + help='Baud rate used with sending CRSF data to UART') +parser.add_argument('-P', '--port',\ + help='UART to send CRSF data to') +parser.add_argument('-I', '--interval', + default=5.0, + type=float, + help='If non-zero, skip GPS frames so that GPS updates are limited to this interval (s)') +parser.add_argument('-J', '--jitter', + default=0.0, + type=float, + help='Adds random jitter to the GPS output to simulate real world telemetry delay (s)') + +args = parser.parse_args() +processFile(args.filename, args.interval, args.port, args.baud, args.jitter) \ No newline at end of file diff --git a/src/Vrx_main.cpp b/src/Vrx_main.cpp index d2af43a..893d9a5 100644 --- a/src/Vrx_main.cpp +++ b/src/Vrx_main.cpp @@ -111,7 +111,7 @@ VrxBackpackConfig config; #elif defined(ORQA_BACKPACK) Orqa vrxModule; #elif defined(AAT_BACKPACK) - AatModule vrxModule; + AatModule vrxModule(Serial); #endif /////////// FUNCTION DEFS /////////// diff --git a/src/module_aat.cpp b/src/module_aat.cpp index 2ce5f55..a824f9a 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -2,24 +2,70 @@ #include "common.h" #include "module_aat.h" #include "logging.h" +#include "devWifi.h" + #include #include #define DEG2RAD(deg) ((deg) * M_PI / 180.0) #define RAD2DEG(rad) ((rad) * 180.0 / M_PI) +#define DELAY_IDLE (20U) // sleep used when not tracking +#define DELAY_FIRST_UPDATE (5000U) // absolute delay before first servo update + +static void calcDistAndAzimuth(int32_t srcLat, int32_t srcLon, int32_t dstLat, int32_t dstLon, + uint32_t *out_dist, uint32_t *out_azimuth) +{ + // https://www.movable-type.co.uk/scripts/latlong.html + // https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/ + + // Have to use doubles for at least some of these, due to short distances getting rounded + // particularly cos(deltaLon) for <2000 m rounds to 1.0000000000 + double deltaLon = DEG2RAD((float)(dstLon - srcLon) / 1e7); + double thetaA = DEG2RAD((float)srcLat / 1e7); + double thetaB = DEG2RAD((float)dstLat / 1e7); + double cosThetaA = cos(thetaA); + double cosThetaB = cos(thetaB); + double sinThetaA = sin(thetaA); + double sinThetaB = sin(thetaB); + double cosDeltaLon = cos(deltaLon); + double sinDeltaLon = sin(deltaLon); + + if (out_dist) + { + const double R = 6371e3; + double dist = acos(sinThetaA * sinThetaB + cosThetaA * cosThetaB * cosDeltaLon) * R; + *out_dist = (uint32_t)dist; + } + + if (out_azimuth) + { + double X = cosThetaB * sinDeltaLon; + double Y = cosThetaA * sinThetaB - sinThetaA * cosThetaB * cosDeltaLon; + + // Convert to degrees, normalized to 0-360 + uint32_t hdg = RAD2DEG(atan2(X, Y)); + *out_azimuth = (hdg + 360) % 360; + } +} + +static int32_t calcElevation(uint32_t distance, int32_t altitude) +{ + return RAD2DEG(atan2(altitude, distance)); +} -AatModule::AatModule() : +AatModule::AatModule(Stream &port) : + CrsfModuleBase(port), _gpsLast{0}, _home{0}, + _gpsAvgUpdateInterval(0), _lastServoUpdateMs(0), _targetDistance(0), + _targetAzim(0), _targetElev(0), _azimMsPerDelta(0), + _currentElev(0), _currentAzim(0) #if defined(PIN_SERVO_AZIM) - _servo_Azim(), + , _servo_Azim() #endif #if defined(PIN_SERVO_ELEV) - _servo_Elev(), + , _servo_Elev() #endif #if defined(PIN_OLED_SDA) - display(SCREEN_WIDTH, SCREEN_HEIGHT), -#endif -#if defined (AAT_SERIAL_INPUT) - _crc(CRSF_CRC_POLY) + , _display(SCREEN_WIDTH, SCREEN_HEIGHT) #endif { // Init is called manually @@ -33,18 +79,7 @@ void AatModule::Init() #if defined(PIN_SERVO_ELEV) _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500); #endif -#if defined(PIN_OLED_SDA) - Wire.begin(PIN_OLED_SDA, PIN_OLED_SCL); - display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); - display.setTextSize(2); - display.setTextColor(SSD1306_WHITE); - display.setCursor(0, 0); - if (connectionState == binding) - display.print("Bind\nmode..."); - else - display.print("AAT\nBackpack"); - display.display(); -#endif + displayInit(); ModuleBase::Init(); } @@ -57,58 +92,44 @@ void AatModule::SendGpsTelemetry(crsf_packet_gps_t *packet) _gpsLast.altitude = (int32_t)be16toh(packet->p.altitude) - 1000; _gpsLast.satcnt = packet->p.satcnt; - DBGLN("GPS: (%d,%d) %dm %u sats", _gpsLast.lat, _gpsLast.lon, - _gpsLast.altitude, _gpsLast.satcnt); + //DBGLN("GPS: (%d,%d) %dm %usats", _gpsLast.lat, _gpsLast.lon, + // _gpsLast.altitude, _gpsLast.satcnt); - _gpsUpdated = true; + _gpsLast.updated = true; } -void calcDistAndAzimuth(int32_t srcLat, int32_t srcLon, int32_t dstLat, int32_t dstLon, - uint32_t *out_dist, uint32_t *out_azimuth) +void AatModule::updateGpsInterval(uint32_t interval) { - // https://www.movable-type.co.uk/scripts/latlong.html - // https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/ - - // Have to use doubles for at least some of these, due to short distances getting rounded - // particularly cos(deltaLon) for <2000 m rounds to 1.0000000000 - double deltaLon = DEG2RAD((float)(dstLon - srcLon) / 1e7); - double thetaA = DEG2RAD((float)srcLat / 1e7); - double thetaB = DEG2RAD((float)dstLat / 1e7); - double cosThetaA = cos(thetaA); - double cosThetaB = cos(thetaB); - double sinThetaA = sin(thetaA); - double sinThetaB = sin(thetaB); - double cosDeltaLon = cos(deltaLon); - double sinDeltaLon = sin(deltaLon); + // Avg is in ms * 100 + interval *= 100; + // Low pass filter. Note there is no fast init of the average, so it will take some time to grow + // this prevents overprojection caused by the first update after setting home + _gpsAvgUpdateInterval += ((int32_t)interval - (int32_t)_gpsAvgUpdateInterval) / 4; + + // Limit the maximum interval to provent projecting for too long + const uint32_t GPS_UPDATE_INTERVAL_MAX = (10U * 1000U * 100U); + if (_gpsAvgUpdateInterval > GPS_UPDATE_INTERVAL_MAX) + _gpsAvgUpdateInterval = GPS_UPDATE_INTERVAL_MAX; +} - if (out_dist) +uint8_t AatModule::calcGpsIntervalPct(uint32_t now) +{ + if (_gpsAvgUpdateInterval) { - const double R = 6371e3; - double dist = acos(sinThetaA * sinThetaB + cosThetaA * cosThetaB * cosDeltaLon) * R; - *out_dist = (uint32_t)dist; + return constrain((now - _gpsLast.lastUpdateMs) * (100U * 100U) / (uint32_t)_gpsAvgUpdateInterval, 0U, 100U); } - if (out_azimuth) - { - double X = cosThetaB * sinDeltaLon; - double Y = cosThetaA * sinThetaB - sinThetaA * cosThetaB * cosDeltaLon; - - // Convert to degrees, normalized to 0-360 - uint32_t hdg = RAD2DEG(atan2(X, Y)); - *out_azimuth = (hdg + 360) % 360; - } + return 0; } -static int32_t calcElevation(uint32_t distance, int32_t altitude) +void AatModule::processGps(uint32_t now) { - return RAD2DEG(atan2(altitude, distance)); -} + if (!_gpsLast.updated) + return; + _gpsLast.updated = false; -void AatModule::processGps() -{ // Actually want to track time between _processing_ each GPS update - uint32_t now = millis(); - uint32_t interval = _gpsLast.lastUpdateMs - now; + uint32_t interval = now - _gpsLast.lastUpdateMs; _gpsLast.lastUpdateMs = now; // Check if need to set home position @@ -118,10 +139,10 @@ void AatModule::processGps() if (_gpsLast.satcnt >= HOME_MIN_SATS) { didSetHome = true; - _homeLat = _gpsLast.lat; - _homeLon = _gpsLast.lon; - _homeAlt = _gpsLast.altitude; - DBGLN("GPS Home set to (%d,%d)", _homeLat, _homeLon); + _home.lat = _gpsLast.lat; + _home.lon = _gpsLast.lon; + _home.alt = _gpsLast.altitude; + DBGLN("GPS Home set to (%d,%d)", _home.lat, _home.lon); } else return; @@ -129,16 +150,18 @@ void AatModule::processGps() uint32_t azimuth; uint32_t distance; - calcDistAndAzimuth(_homeLat, _homeLon, _gpsLast.lat, _gpsLast.lon, &distance, &azimuth); - uint8_t elevation = constrain(calcElevation(distance, _gpsLast.altitude - _homeAlt), 0, 90); + calcDistAndAzimuth(_home.lat, _home.lon, _gpsLast.lat, _gpsLast.lon, &distance, &azimuth); + uint8_t elevation = constrain(calcElevation(distance, _gpsLast.altitude - _home.alt), 0, 90); DBGLN("Azimuth: %udeg Elevation: %udeg Distance: %um", azimuth, elevation, distance); // Calculate angular velocity to allow dead reckoning projection if (!didSetHome) { - int32_t azimDelta = azimuth - _targetAzim; - int32_t azimMsPerDelta = interval / azimDelta; - DBGLN("%d delta in %ums, %dms/d", azimDelta, interval, azimMsPerDelta); + updateGpsInterval(interval); + // azimDelta is the azimuth change since the last packet, -180 to +180 + int32_t azimDelta = (azimuth - _targetAzim + 540) % 360 - 180; + _azimMsPerDelta = (azimDelta == 0) ? 0 : (int32_t)interval / azimDelta; + DBGLN("%d delta in %ums, %dms/d %uavg", azimDelta, interval, _azimMsPerDelta, _gpsAvgUpdateInterval); } _targetDistance = distance; @@ -146,36 +169,140 @@ void AatModule::processGps() _targetAzim = azimuth; } +int32_t AatModule::calcProjectedAzim(uint32_t now) +{ + // Attempt to do a linear projection of the last + if (config.GetAatProject() && _gpsAvgUpdateInterval && _azimMsPerDelta) + { + uint32_t elapsed = constrain(now - _gpsLast.lastUpdateMs, 0U, _gpsAvgUpdateInterval / 100U); + int32_t target = (((int32_t)elapsed / _azimMsPerDelta) + _targetAzim + 360) % 360; + return target; + } + + return _targetAzim; +} + +void AatModule::displayInit() +{ +#if defined(PIN_OLED_SDA) + Wire.begin(PIN_OLED_SDA, PIN_OLED_SCL); + _display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); + _display.setTextSize(2); + _display.setTextColor(SSD1306_WHITE); + _display.setCursor(0, 0); + if (connectionState == binding) + _display.print("Bind\nmode...\n\n"); + else + _display.print("AAT\nBackpack\n\n"); + _display.setTextSize(1); + _display.print(VERSION); + _display.display(); +#endif +} + +void AatModule::displayIdle(uint32_t now) +{ +#if defined(PIN_OLED_SDA) + // A screen with just the GPS position, sat count, and interval bar + _display.clearDisplay(); + _display.setCursor(0, 0); + + _display.setTextSize(2); + _display.printf("Sats: %u\n", _gpsLast.satcnt); + + _display.setTextSize(1); + _display.printf("\nLat: %d.%07d\nLon: %d.%07d", + _gpsLast.lat / 10000000, abs(_gpsLast.lat) % 10000000, + _gpsLast.lon / 10000000, abs(_gpsLast.lon) % 10000000 + ); + + displayGpsIntervalBar(now); + _display.display(); +#endif +} + +void AatModule::displayActive(uint32_t now, int32_t projectedAzim, int32_t usElev, int32_t usAzim) +{ +#if defined(PIN_OLED_SDA) + // El:[deg] [alt]m + // Az:[deg] [dist] + // Se:[servo elev]us + // Sa:[servo azim]us + // With interval bar + _display.clearDisplay(); + _display.setTextSize(2); + _display.setCursor(0, 0) + ; + _display.printf("El:%02u %dm\nAz:%03u ", _targetElev, + constrain(_gpsLast.altitude - _home.alt, -99, 999), + projectedAzim); + + // Target distance has variable width/height but all fits in 3x1 (doublesized) characters + if (_targetDistance > 999) + { + _display.setTextSize(1); + _display.printf("%u.%03u\nkm\n", _targetDistance / 1000, (_targetDistance % 1000)); // X.XXX km small font + _display.setTextSize(2); + } + else if (_targetDistance > 99) + { + _display.printf("%u\n", _targetDistance); // XXX + } + else + { + _display.printf("%um\n", _targetDistance); // XXm + } + + _display.printf("Se:%4dus\nSa:%4dus\n", usElev, usAzim); + displayGpsIntervalBar(now); + _display.display(); +#endif +} + +void AatModule::displayGpsIntervalBar(uint32_t now) +{ +#if defined(PIN_OLED_SDA) + if (_gpsAvgUpdateInterval) + { + uint8_t gpsIntervalPct = calcGpsIntervalPct(now); + uint8_t pxHeight = SCREEN_HEIGHT * (100U - gpsIntervalPct) / 100U; + _display.fillRect(SCREEN_WIDTH - 3, SCREEN_HEIGHT - pxHeight, 2, pxHeight, SSD1306_WHITE); + } +#endif +} + void AatModule::servoUpdate(uint32_t now) { - if (!isHomeSet()) - return; - if (now - _lastServoUpdateMs < 20U) + uint32_t interval = now - _lastServoUpdateMs; + if (interval < 20U) return; _lastServoUpdateMs = now; + int32_t projectedAzim = calcProjectedAzim(now); + // Smooth the updates and scale to degrees * 100 - _currentAzim = ((_currentAzim * 9) + (_targetAzim * 100)) / 10; - _currentElev = ((_currentElev * 9) + (_targetElev * 100)) / 10; + const int8_t servoSmoothFactor = config.GetAatServoSmooth(); + _currentAzim = ((_currentAzim * servoSmoothFactor) + (projectedAzim * (10 - servoSmoothFactor) * 100)) / 10; + _currentElev = ((_currentElev * servoSmoothFactor) + (_targetElev * (10 - servoSmoothFactor) * 100)) / 10; - int32_t projectedAzim = _currentAzim; - int32_t projectedElev = _currentElev; + int32_t transformedAzim = _currentAzim; + int32_t transformedElev = _currentElev; // 90-270 azim inverts the elevation servo - // if (projectedAzim > 18000) + // if (transformedAzim > 18000) // { - // projectedElev = 18000 - projectedElev; - // projectedAzim = projectedAzim - 18000; + // transformedElev = 18000 - transformedElev; + // transformedAzim = transformedAzim - 18000; // } // For straight geared servos with 180 degree elev flip - // int32_t usAzim = map(projectedAzim, 0, 18000, 500, 2500); - // int32_t usElev = map(projectedElev, 0, 18000, 500, 2500); + // int32_t usAzim = map(transformedAzim, 0, 18000, 500, 2500); + // int32_t usElev = map(transformedElev, 0, 18000, 500, 2500); // For 1:2 gearing on the azim servo to allow 360 rotation // For Elev servos that only go 0-90 and the azim does 360 - projectedAzim = (projectedAzim + 18000) % 36000; // convert so 0 maps to 1500us - int32_t usAzim = map(projectedAzim, 0, 36000, 700, 2400); - int32_t usElev = map(projectedElev, 0, 9000, 1100, 1900); + transformedAzim = (transformedAzim + 18000) % 36000; // convert so 0 maps to 1500us + int32_t usAzim = map(transformedAzim, 0, 36000, config.GetAatServoLowAzim(), config.GetAatServoHighAzim()); + int32_t usElev = map(transformedElev, 0, 9000, config.GetAatServoLowElev(), config.GetAatServoHighElev()); #if defined(PIN_SERVO_AZIM) _servo_Azim.writeMicroseconds(usAzim); @@ -184,121 +311,31 @@ void AatModule::servoUpdate(uint32_t now) _servo_Elev.writeMicroseconds(usElev); #endif -#if defined(PIN_OLED_SDA) - display.clearDisplay(); - display.setCursor(0, 0); - //display.printf("GPS (%d,%d)\n", _gpsLast.lat, _gpsLast.lon); - //display.printf("%dm %u sats\n", _gpsLast.altitude, _gpsLast.satcnt); - display.printf("Az:%u %dm\nEl:%u %dm\n", _targetAzim, _targetDistance, - _targetElev, _gpsLast.altitude - _homeAlt); - display.printf("SAz:%dus\nSEl:%dus\n", usAzim, usElev); - display.display(); -#endif - + displayActive(now, projectedAzim, usElev, usAzim); } -#if defined(AAT_SERIAL_INPUT) -void AatModule::processPacketIn(uint8_t len) +void AatModule::onCrsfPacketIn(const crsf_header_t *pkt) { - const crsf_header_t *hdr = (crsf_header_t *)_rxBuf; - if (hdr->sync_byte == CRSF_SYNC_BYTE) // || hdr->sync_byte == CRSF_ADDRESS_FLIGHT_CONTROLLER) + if (pkt->sync_byte == CRSF_SYNC_BYTE) { - switch (hdr->type) - { - case CRSF_FRAMETYPE_GPS: - SendGpsTelemetry((crsf_packet_gps_t *)hdr); - break; - } + if (pkt->type == CRSF_FRAMETYPE_GPS) + SendGpsTelemetry((crsf_packet_gps_t *)pkt); } } -// Shift the bytes in the RxBuf down by cnt bytes -void AatModule::shiftRxBuffer(uint8_t cnt) -{ - // If removing the whole thing, just set pos to 0 - if (cnt >= _rxBufPos) - { - _rxBufPos = 0; - return; - } - - // Otherwise do the slow shift down - uint8_t *src = &_rxBuf[cnt]; - uint8_t *dst = &_rxBuf[0]; - _rxBufPos -= cnt; - uint8_t left = _rxBufPos; - while (left--) - *dst++ = *src++; -} - -void AatModule::handleByteReceived() -{ - bool reprocess; - do - { - reprocess = false; - if (_rxBufPos > 1) - { - uint8_t len = _rxBuf[1]; - // Sanity check the declared length isn't outside Type + X{1,CRSF_MAX_PAYLOAD_LEN} + CRC - // assumes there never will be a CRSF message that just has a type and no data (X) - if (len < 3 || len > (CRSF_MAX_PAYLOAD_LEN + 2)) - { - shiftRxBuffer(1); - reprocess = true; - } - - else if (_rxBufPos >= (len + 2)) - { - uint8_t inCrc = _rxBuf[2 + len - 1]; - uint8_t crc = _crc.calc(&_rxBuf[2], len - 1); - if (crc == inCrc) - { - processPacketIn(len); - shiftRxBuffer(len + 2); - reprocess = true; - } - else - { - shiftRxBuffer(1); - reprocess = true; - } - } // if complete packet - } // if pos > 1 - } while (reprocess); -} -#endif - -void AatModule::checkSerialInput() -{ -#if defined(AAT_SERIAL_INPUT) - while (Serial.available()) - { - uint8_t b = Serial.read(); - _rxBuf[_rxBufPos++] = b; - - handleByteReceived(); - - if (_rxBufPos == (sizeof(_rxBuf)/sizeof(_rxBuf[0]))) - { - // Packet buffer filled and no valid packet found, dump the whole thing - _rxBufPos = 0; - } - } -#endif -} - void AatModule::Loop(uint32_t now) { - checkSerialInput(); + processGps(now); - if (_gpsUpdated) + if (isHomeSet() && now > DELAY_FIRST_UPDATE) + servoUpdate(now); + else { - _gpsUpdated = false; - processGps(); + if (isGpsActive()) + displayIdle(now); + delay(DELAY_IDLE); } - servoUpdate(now); - ModuleBase::Loop(now); + CrsfModuleBase::Loop(now); } #endif /* AAT_BACKPACK */ \ No newline at end of file diff --git a/src/module_aat.h b/src/module_aat.h index 6fdc78c..98b8093 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -1,13 +1,10 @@ #pragma once #if defined(AAT_BACKPACK) -#include "module_base.h" +#include "config.h" +#include "module_crsf.h" #include "crsf_protocol.h" -#if defined (AAT_SERIAL_INPUT) -#include "crc.h" -#endif - #if defined(PIN_SERVO_AZIM) || defined(PIN_SERVO_ELEV) #include #endif @@ -22,35 +19,34 @@ #define SCREEN_ADDRESS 0x3C #endif -class AatModule : public ModuleBase +class AatModule : public CrsfModuleBase { public: - AatModule(); + AatModule() = delete; + AatModule(Stream &port); void Init(); void Loop(uint32_t now); void SendGpsTelemetry(crsf_packet_gps_t *packet); - bool isHomeSet() const { return _homeLat !=0 && _homeLon != 0; } + bool isHomeSet() const { return _home.lat != 0 || _home.lon != 0; } + bool isGpsActive() const { return _gpsLast.lat != 0 || _gpsLast.lon != 0; }; +protected: + virtual void onCrsfPacketIn(const crsf_header_t *pkt); private: // Minimum number of satellites to lock in the home position static constexpr uint8_t HOME_MIN_SATS = 5; - void checkSerialInput(); - void processGps(); + void displayInit(); + void updateGpsInterval(uint32_t interval); + uint8_t calcGpsIntervalPct(uint32_t now); + int32_t calcProjectedAzim(uint32_t now); + void processGps(uint32_t now); void servoUpdate(uint32_t now); + void displayIdle(uint32_t now); + void displayActive(uint32_t now, int32_t projectedAzim, int32_t usElev, int32_t usAzim); + void displayGpsIntervalBar(uint32_t now); -#if defined(PIN_SERVO_AZIM) - Servo _servo_Azim; -#endif -#if defined(PIN_SERVO_ELEV) - Servo _servo_Elev; -#endif -#if defined(PIN_OLED_SDA) - Adafruit_SSD1306 display; -#endif - - bool _gpsUpdated; struct { int32_t lat; int32_t lon; @@ -59,29 +55,31 @@ class AatModule : public ModuleBase int32_t altitude; // meters uint32_t lastUpdateMs; // timestamp of last update uint8_t satcnt; // number of satellites + bool updated; } _gpsLast; - int32_t _homeLat; - int32_t _homeLon; - int32_t _homeAlt; + struct { + int32_t lat; + int32_t lon; + int32_t alt; + } _home; + uint32_t _gpsAvgUpdateInterval; // ms * 100 // Servo Position uint32_t _lastServoUpdateMs; uint32_t _targetDistance; // meters uint16_t _targetAzim; // degrees uint8_t _targetElev; // degrees + int32_t _azimMsPerDelta; // milliseconds per degree int32_t _currentElev; // degrees * 100 int32_t _currentAzim; // degrees * 100 -#if defined (AAT_SERIAL_INPUT) - static constexpr uint8_t CRSF_MAX_PACKET_SIZE = 64U; - static constexpr uint8_t CRSF_MAX_PAYLOAD_LEN = (CRSF_MAX_PACKET_SIZE - 4U); - - GENERIC_CRC8 _crc; - uint8_t _rxBufPos; - uint8_t _rxBuf[CRSF_MAX_PACKET_SIZE]; - - void processPacketIn(uint8_t len); - void shiftRxBuffer(uint8_t cnt); - void handleByteReceived(); +#if defined(PIN_SERVO_AZIM) + Servo _servo_Azim; +#endif +#if defined(PIN_SERVO_ELEV) + Servo _servo_Elev; +#endif +#if defined(PIN_OLED_SDA) + Adafruit_SSD1306 _display; #endif }; #endif /* AAT_BACKPACK */ \ No newline at end of file From 095b1b5ffca315e17c1f414c394de682091f2694 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Thu, 28 Dec 2023 12:46:42 -0500 Subject: [PATCH 06/29] Swap SDA/SCL --- targets/aat.ini | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/targets/aat.ini b/targets/aat.ini index 5a778f5..1d22b25 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -5,19 +5,18 @@ build_flags = ${env_common_esp12e.build_flags} -D TARGET_VRX_BACKPACK -D AAT_BACKPACK - -D AAT_SERIAL_INPUT -D SSD1306_NO_SPLASH -D PIN_BUTTON=0 -D PIN_LED=16 -D LED_INVERTED -D PIN_SERVO_AZIM=9 -D PIN_SERVO_ELEV=10 - -D PIN_OLED_SCL=1 - -D PIN_OLED_SDA=3 + -D PIN_OLED_SDA=1 + -D PIN_OLED_SCL=3 ; -D PIN_SERVO_AZIM=1 ; -D PIN_SERVO_ELEV=3 - ; -D PIN_OLED_SCL=9 - ; -D PIN_OLED_SDA=10 + ; -D PIN_OLED_SDA=9 + ; -D PIN_OLED_SCL=10 ; -D DEBUG_LOG lib_ignore = STM32UPDATE lib_deps = From 309b08e981d374db5a87b912d72dde4d11cfee01 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Thu, 28 Dec 2023 14:58:32 -0500 Subject: [PATCH 07/29] Add MSP output mode --- python/utils/gpscsv_to_crsf.py | 40 ++++++++++++++++++++++++++-------- 1 file changed, 31 insertions(+), 9 deletions(-) diff --git a/python/utils/gpscsv_to_crsf.py b/python/utils/gpscsv_to_crsf.py index 9c49755..1d2dc0c 100644 --- a/python/utils/gpscsv_to_crsf.py +++ b/python/utils/gpscsv_to_crsf.py @@ -26,17 +26,36 @@ def crsf_crc(data: bytes) -> int: crc = _crc_tab[crc ^ i] return crc -def idle(duration: float, port: serial.Serial): - if port: +def idle(duration: float, uart: serial.Serial): + if uart: start = time.monotonic() while time.monotonic() - start < duration: - b = port.read(128) + b = uart.read(128) if len(b) > 0: - sys.stdout.write(b.decode('utf-8')) + sys.stdout.write(b.decode('ascii', 'replace')) else: time.sleep(duration) -def processFile(fname, interval, port, baud, jitter): +def output_Crsf(uart: serial.Serial, data): + if uart is None: + return + uart.write(data) + +def output_Msp(uart: serial.Serial, data): + if uart is None: + return + + # MSPv2 header + MSP_COMMAND + flags + buf = b'$X<\x00' + # Command MSP_ELRS_BACKPACK_CRSF_TLM + buf += int.to_bytes(0x0011, length=2, byteorder='little', signed=False) + buf += int.to_bytes(len(data), length=2, byteorder='little', signed=False) + buf += data + buf += int.to_bytes(crsf_crc(buf[3:]), length=1, byteorder='big', signed=False) + + uart.write(buf) + +def processFile(fname, interval, port, baud, jitter, output): random.seed() if port is not None: @@ -67,7 +86,7 @@ def processFile(fname, interval, port, baud, jitter): alt = int(row[5]) + 1000 spd = int(float(row[6]) * 360) # m/s to km/h*10 hdg = int(float(row[7]) * 100) # deg to centideg - #print(f'{timeS:0.6f} GPS: ({lat},{lon}) {alt-1000}m {sats} sats') + print(f'{timeS:0.6f} GPS: ({lat},{lon}) {alt-1000}m {sats}sats') # CRSF_SYNC, len, type (GPS), payload, crc buf = bytearray(b'\xc8\x00\x02') @@ -80,8 +99,7 @@ def processFile(fname, interval, port, baud, jitter): buf += b'\x00' # place for the CRC buf[-1] = crsf_frame_crc(buf) buf[1] = len(buf) - 2 - if uart: - uart.write(buf) + output(uart, buf) lastTime = timeS @@ -103,6 +121,10 @@ def processFile(fname, interval, port, baud, jitter): default=0.0, type=float, help='Adds random jitter to the GPS output to simulate real world telemetry delay (s)') +parser.add_argument('-m', '--msp', + dest='output', + action='store_const', const=output_Msp, default=output_Crsf, + help='Wrap CRSF with MSP when sending to UART (default: send just CRSF)') args = parser.parse_args() -processFile(args.filename, args.interval, args.port, args.baud, args.jitter) \ No newline at end of file +processFile(args.filename, args.interval, args.port, args.baud, args.jitter, args.output) \ No newline at end of file From d6d333a52ab655357ada66624c928a075787de8f Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Thu, 28 Dec 2023 15:53:08 -0500 Subject: [PATCH 08/29] Limit projection tracking speed to 100deg/s, config satmin --- lib/config/config.cpp | 1 + lib/config/config.h | 2 ++ src/module_aat.cpp | 26 ++++++++++++++++++++------ src/module_aat.h | 7 ++----- 4 files changed, 25 insertions(+), 11 deletions(-) diff --git a/lib/config/config.cpp b/lib/config/config.cpp index adb1545..c16607d 100644 --- a/lib/config/config.cpp +++ b/lib/config/config.cpp @@ -139,6 +139,7 @@ VrxBackpackConfig::SetDefaults() m_config.version = VRX_BACKPACK_CONFIG_VERSION | VRX_BACKPACK_CONFIG_MAGIC; #if defined(AAT_BACKPACK) + m_config.satelliteHomeMin = 5; m_config.project = 0xff; m_config.servoSmooth = 9; m_config.servoLowAzim = 700; diff --git a/lib/config/config.h b/lib/config/config.h index d58846b..1643d7e 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -61,6 +61,7 @@ typedef struct { uint8_t address[6]; #if defined(AAT_BACKPACK) + uint8_t satelliteHomeMin; // minimum number of satellites to establish home uint8_t servoSmooth; uint8_t project; // 0=none, 1=projectAzim, 2=projectElev, 3=projectBoth uint8_t servoMode; // reserved to declare 2:1 or 180+flip servo @@ -95,6 +96,7 @@ class VrxBackpackConfig void SetGroupAddress(const uint8_t address[6]); #if defined(AAT_BACKPACK) + uint8_t GetAatSatelliteHomeMin() const { return m_config.satelliteHomeMin; } uint8_t GetAatServoSmooth() const { return m_config.servoSmooth; } uint8_t GetAatProject() const { return m_config.project; } uint16_t GetAatServoLowAzim() const { return m_config.servoLowAzim; } diff --git a/src/module_aat.cpp b/src/module_aat.cpp index a824f9a..65fe176 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -56,7 +56,7 @@ static int32_t calcElevation(uint32_t distance, int32_t altitude) AatModule::AatModule(Stream &port) : CrsfModuleBase(port), _gpsLast{0}, _home{0}, _gpsAvgUpdateInterval(0), _lastServoUpdateMs(0), _targetDistance(0), - _targetAzim(0), _targetElev(0), _azimMsPerDelta(0), + _targetAzim(0), _targetElev(0), _azimMsPerDegree(0), _currentElev(0), _currentAzim(0) #if defined(PIN_SERVO_AZIM) , _servo_Azim() @@ -136,7 +136,7 @@ void AatModule::processGps(uint32_t now) bool didSetHome = false; if (!isHomeSet()) { - if (_gpsLast.satcnt >= HOME_MIN_SATS) + if (_gpsLast.satcnt >= config.GetAatSatelliteHomeMin()) { didSetHome = true; _home.lat = _gpsLast.lat; @@ -160,8 +160,8 @@ void AatModule::processGps(uint32_t now) updateGpsInterval(interval); // azimDelta is the azimuth change since the last packet, -180 to +180 int32_t azimDelta = (azimuth - _targetAzim + 540) % 360 - 180; - _azimMsPerDelta = (azimDelta == 0) ? 0 : (int32_t)interval / azimDelta; - DBGLN("%d delta in %ums, %dms/d %uavg", azimDelta, interval, _azimMsPerDelta, _gpsAvgUpdateInterval); + _azimMsPerDegree = (azimDelta == 0) ? 0 : (int32_t)interval / azimDelta; + DBGLN("%d delta in %ums, %dms/d %uavg", azimDelta, interval, _azimMsPerDegree, _gpsAvgUpdateInterval); } _targetDistance = distance; @@ -172,10 +172,22 @@ void AatModule::processGps(uint32_t now) int32_t AatModule::calcProjectedAzim(uint32_t now) { // Attempt to do a linear projection of the last - if (config.GetAatProject() && _gpsAvgUpdateInterval && _azimMsPerDelta) + // If enabled, we know the GPS update rate, the azimuth has changed, and more than a few meters away + if (config.GetAatProject() && _gpsAvgUpdateInterval && _azimMsPerDegree && _targetDistance > 3) { uint32_t elapsed = constrain(now - _gpsLast.lastUpdateMs, 0U, _gpsAvgUpdateInterval / 100U); - int32_t target = (((int32_t)elapsed / _azimMsPerDelta) + _targetAzim + 360) % 360; + + // Prevent excessive rotational velocity (100 degrees per second) + int32_t azimMsPDLimited; + if (_azimMsPerDegree > -10 && _azimMsPerDegree < 10) + if (_azimMsPerDegree > 0) + azimMsPDLimited = 10; + else + azimMsPDLimited = -10; + else + azimMsPDLimited = _azimMsPerDegree; + + int32_t target = (((int32_t)elapsed / azimMsPDLimited) + _targetAzim + 360) % 360; return target; } @@ -328,7 +340,9 @@ void AatModule::Loop(uint32_t now) processGps(now); if (isHomeSet() && now > DELAY_FIRST_UPDATE) + { servoUpdate(now); + } else { if (isGpsActive()) diff --git a/src/module_aat.h b/src/module_aat.h index 98b8093..20a0fa3 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -30,13 +30,10 @@ class AatModule : public CrsfModuleBase void SendGpsTelemetry(crsf_packet_gps_t *packet); bool isHomeSet() const { return _home.lat != 0 || _home.lon != 0; } - bool isGpsActive() const { return _gpsLast.lat != 0 || _gpsLast.lon != 0; }; + bool isGpsActive() const { return _gpsLast.lat != 0 || _gpsLast.lon != 0 || _gpsLast.satcnt > 0; }; protected: virtual void onCrsfPacketIn(const crsf_header_t *pkt); private: - // Minimum number of satellites to lock in the home position - static constexpr uint8_t HOME_MIN_SATS = 5; - void displayInit(); void updateGpsInterval(uint32_t interval); uint8_t calcGpsIntervalPct(uint32_t now); @@ -68,7 +65,7 @@ class AatModule : public CrsfModuleBase uint32_t _targetDistance; // meters uint16_t _targetAzim; // degrees uint8_t _targetElev; // degrees - int32_t _azimMsPerDelta; // milliseconds per degree + int32_t _azimMsPerDegree; // milliseconds per degree int32_t _currentElev; // degrees * 100 int32_t _currentAzim; // degrees * 100 From da654f9427bf685d67cb25795f24cec30e9a4ef7 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Thu, 28 Dec 2023 17:41:28 -0500 Subject: [PATCH 09/29] Missing ModuleBase::SendGpsTelemetry() --- src/module_base.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/module_base.h b/src/module_base.h index 153a28a..cace5ab 100644 --- a/src/module_base.h +++ b/src/module_base.h @@ -15,7 +15,7 @@ class ModuleBase void SetRTC(); void SendLinkTelemetry(uint8_t *rawCrsfPacket); void SendBatteryTelemetry(uint8_t *rawCrsfPacket); - void SendGpsTelemetry(crsf_packet_gps_t *packet); + void SendGpsTelemetry(crsf_packet_gps_t *packet) {} void Loop(uint32_t now); }; From c22ecf4744127f0a673ad73c806f584f1de81108 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Thu, 28 Dec 2023 17:58:45 -0500 Subject: [PATCH 10/29] CRLF line endings? In this economy? --- python/utils/gpscsv_to_crsf.py | 258 ++++++------ src/module_aat.cpp | 708 ++++++++++++++++----------------- src/module_aat.h | 162 ++++---- src/module_crsf.cpp | 152 +++---- src/module_crsf.h | 60 +-- targets/aat.ini | 60 +-- 6 files changed, 702 insertions(+), 698 deletions(-) diff --git a/python/utils/gpscsv_to_crsf.py b/python/utils/gpscsv_to_crsf.py index 1d2dc0c..b466d6c 100644 --- a/python/utils/gpscsv_to_crsf.py +++ b/python/utils/gpscsv_to_crsf.py @@ -1,130 +1,130 @@ -import sys -import time -import csv -import argparse -import serial -import random - -_crc_tab = [ - 0x00,0xD5,0x7F,0xAA,0xFE,0x2B,0x81,0x54,0x29,0xFC,0x56,0x83,0xD7,0x02,0xA8,0x7D,0x52,0x87,0x2D,0xF8,0xAC,0x79,0xD3,0x06,0x7B,0xAE,0x04,0xD1,0x85,0x50,0xFA,0x2F,\ - 0xA4,0x71,0xDB,0x0E,0x5A,0x8F,0x25,0xF0,0x8D,0x58,0xF2,0x27,0x73,0xA6,0x0C,0xD9,0xF6,0x23,0x89,0x5C,0x08,0xDD,0x77,0xA2,0xDF,0x0A,0xA0,0x75,0x21,0xF4,0x5E,0x8B,\ - 0x9D,0x48,0xE2,0x37,0x63,0xB6,0x1C,0xC9,0xB4,0x61,0xCB,0x1E,0x4A,0x9F,0x35,0xE0,0xCF,0x1A,0xB0,0x65,0x31,0xE4,0x4E,0x9B,0xE6,0x33,0x99,0x4C,0x18,0xCD,0x67,0xB2,\ - 0x39,0xEC,0x46,0x93,0xC7,0x12,0xB8,0x6D,0x10,0xC5,0x6F,0xBA,0xEE,0x3B,0x91,0x44,0x6B,0xBE,0x14,0xC1,0x95,0x40,0xEA,0x3F,0x42,0x97,0x3D,0xE8,0xBC,0x69,0xC3,0x16,\ - 0xEF,0x3A,0x90,0x45,0x11,0xC4,0x6E,0xBB,0xC6,0x13,0xB9,0x6C,0x38,0xED,0x47,0x92,0xBD,0x68,0xC2,0x17,0x43,0x96,0x3C,0xE9,0x94,0x41,0xEB,0x3E,0x6A,0xBF,0x15,0xC0,\ - 0x4B,0x9E,0x34,0xE1,0xB5,0x60,0xCA,0x1F,0x62,0xB7,0x1D,0xC8,0x9C,0x49,0xE3,0x36,0x19,0xCC,0x66,0xB3,0xE7,0x32,0x98,0x4D,0x30,0xE5,0x4F,0x9A,0xCE,0x1B,0xB1,0x64,\ - 0x72,0xA7,0x0D,0xD8,0x8C,0x59,0xF3,0x26,0x5B,0x8E,0x24,0xF1,0xA5,0x70,0xDA,0x0F,0x20,0xF5,0x5F,0x8A,0xDE,0x0B,0xA1,0x74,0x09,0xDC,0x76,0xA3,0xF7,0x22,0x88,0x5D,\ - 0xD6,0x03,0xA9,0x7C,0x28,0xFD,0x57,0x82,0xFF,0x2A,0x80,0x55,0x01,0xD4,0x7E,0xAB,0x84,0x51,0xFB,0x2E,0x7A,0xAF,0x05,0xD0,0xAD,0x78,0xD2,0x07,0x53,0x86,0x2C,0xF9,\ -] - -def crsf_frame_crc(frame: bytes) -> int: - crc_frame = frame[2:-1] - return crsf_crc(crc_frame) - -def crsf_crc(data: bytes) -> int: - crc = 0 - for i in data: - crc = _crc_tab[crc ^ i] - return crc - -def idle(duration: float, uart: serial.Serial): - if uart: - start = time.monotonic() - while time.monotonic() - start < duration: - b = uart.read(128) - if len(b) > 0: - sys.stdout.write(b.decode('ascii', 'replace')) - else: - time.sleep(duration) - -def output_Crsf(uart: serial.Serial, data): - if uart is None: - return - uart.write(data) - -def output_Msp(uart: serial.Serial, data): - if uart is None: - return - - # MSPv2 header + MSP_COMMAND + flags - buf = b'$X<\x00' - # Command MSP_ELRS_BACKPACK_CRSF_TLM - buf += int.to_bytes(0x0011, length=2, byteorder='little', signed=False) - buf += int.to_bytes(len(data), length=2, byteorder='little', signed=False) - buf += data - buf += int.to_bytes(crsf_crc(buf[3:]), length=1, byteorder='big', signed=False) - - uart.write(buf) - -def processFile(fname, interval, port, baud, jitter, output): - random.seed() - - if port is not None: - uart = serial.Serial(port=port, baudrate=baud, timeout=0.1) - else: - uart = None - - with open(fname, 'r') as csv_file: - reader = csv.reader(csv_file) - # Skip header. DictReader is not used for performance and simplicity - # ['time (us)', 'GPS_fixType', ' GPS_numSat', ' GPS_coord[0]', ' GPS_coord[1]', ' GPS_altitude', ' GPS_speed (m/s)', ' GPS_ground_course', ' GPS_hdop', ' GPS_eph', ' GPS_epv', ' GPS_velned[0]', ' GPS_velned[1]', ' GPS_velned[2]'] - next(reader) - - lastTime = None - for row in reader: - timeS = float(row[0]) / 1e6 - - if lastTime != None: - dur = timeS - lastTime - if dur < interval: - continue - idle(dur + (random.randrange(0, int(jitter * 1000.0)) / 1000.0), uart) - - # All values here are immediately loaded into their CRSF representations - sats = int(row[2]) - lat = int(float(row[3]) * 1e7) - lon = int(float(row[4]) * 1e7) - alt = int(row[5]) + 1000 - spd = int(float(row[6]) * 360) # m/s to km/h*10 - hdg = int(float(row[7]) * 100) # deg to centideg - print(f'{timeS:0.6f} GPS: ({lat},{lon}) {alt-1000}m {sats}sats') - - # CRSF_SYNC, len, type (GPS), payload, crc - buf = bytearray(b'\xc8\x00\x02') - buf += int.to_bytes(lat, length=4, byteorder='big', signed=True) - buf += int.to_bytes(lon, length=4, byteorder='big', signed=True) - buf += int.to_bytes(spd, length=2, byteorder='big', signed=False) - buf += int.to_bytes(hdg, length=2, byteorder='big', signed=False) - buf += int.to_bytes(alt, length=2, byteorder='big', signed=False) - buf += int.to_bytes(sats, length=1, byteorder='big', signed=False) - buf += b'\x00' # place for the CRC - buf[-1] = crsf_frame_crc(buf) - buf[1] = len(buf) - 2 - output(uart, buf) - - lastTime = timeS - -parser = argparse.ArgumentParser( - prog='gpscsv_to_crsf', - description='Convert iNav GPS CSV to CRSF and send in real time' -) -parser.add_argument('filename') -parser.add_argument('-b', '--baud', - default='460800', - help='Baud rate used with sending CRSF data to UART') -parser.add_argument('-P', '--port',\ - help='UART to send CRSF data to') -parser.add_argument('-I', '--interval', - default=5.0, - type=float, - help='If non-zero, skip GPS frames so that GPS updates are limited to this interval (s)') -parser.add_argument('-J', '--jitter', - default=0.0, - type=float, - help='Adds random jitter to the GPS output to simulate real world telemetry delay (s)') -parser.add_argument('-m', '--msp', - dest='output', - action='store_const', const=output_Msp, default=output_Crsf, - help='Wrap CRSF with MSP when sending to UART (default: send just CRSF)') - -args = parser.parse_args() +import sys +import time +import csv +import argparse +import serial +import random + +_crc_tab = [ + 0x00,0xD5,0x7F,0xAA,0xFE,0x2B,0x81,0x54,0x29,0xFC,0x56,0x83,0xD7,0x02,0xA8,0x7D,0x52,0x87,0x2D,0xF8,0xAC,0x79,0xD3,0x06,0x7B,0xAE,0x04,0xD1,0x85,0x50,0xFA,0x2F,\ + 0xA4,0x71,0xDB,0x0E,0x5A,0x8F,0x25,0xF0,0x8D,0x58,0xF2,0x27,0x73,0xA6,0x0C,0xD9,0xF6,0x23,0x89,0x5C,0x08,0xDD,0x77,0xA2,0xDF,0x0A,0xA0,0x75,0x21,0xF4,0x5E,0x8B,\ + 0x9D,0x48,0xE2,0x37,0x63,0xB6,0x1C,0xC9,0xB4,0x61,0xCB,0x1E,0x4A,0x9F,0x35,0xE0,0xCF,0x1A,0xB0,0x65,0x31,0xE4,0x4E,0x9B,0xE6,0x33,0x99,0x4C,0x18,0xCD,0x67,0xB2,\ + 0x39,0xEC,0x46,0x93,0xC7,0x12,0xB8,0x6D,0x10,0xC5,0x6F,0xBA,0xEE,0x3B,0x91,0x44,0x6B,0xBE,0x14,0xC1,0x95,0x40,0xEA,0x3F,0x42,0x97,0x3D,0xE8,0xBC,0x69,0xC3,0x16,\ + 0xEF,0x3A,0x90,0x45,0x11,0xC4,0x6E,0xBB,0xC6,0x13,0xB9,0x6C,0x38,0xED,0x47,0x92,0xBD,0x68,0xC2,0x17,0x43,0x96,0x3C,0xE9,0x94,0x41,0xEB,0x3E,0x6A,0xBF,0x15,0xC0,\ + 0x4B,0x9E,0x34,0xE1,0xB5,0x60,0xCA,0x1F,0x62,0xB7,0x1D,0xC8,0x9C,0x49,0xE3,0x36,0x19,0xCC,0x66,0xB3,0xE7,0x32,0x98,0x4D,0x30,0xE5,0x4F,0x9A,0xCE,0x1B,0xB1,0x64,\ + 0x72,0xA7,0x0D,0xD8,0x8C,0x59,0xF3,0x26,0x5B,0x8E,0x24,0xF1,0xA5,0x70,0xDA,0x0F,0x20,0xF5,0x5F,0x8A,0xDE,0x0B,0xA1,0x74,0x09,0xDC,0x76,0xA3,0xF7,0x22,0x88,0x5D,\ + 0xD6,0x03,0xA9,0x7C,0x28,0xFD,0x57,0x82,0xFF,0x2A,0x80,0x55,0x01,0xD4,0x7E,0xAB,0x84,0x51,0xFB,0x2E,0x7A,0xAF,0x05,0xD0,0xAD,0x78,0xD2,0x07,0x53,0x86,0x2C,0xF9,\ +] + +def crsf_frame_crc(frame: bytes) -> int: + crc_frame = frame[2:-1] + return crsf_crc(crc_frame) + +def crsf_crc(data: bytes) -> int: + crc = 0 + for i in data: + crc = _crc_tab[crc ^ i] + return crc + +def idle(duration: float, uart: serial.Serial): + if uart: + start = time.monotonic() + while time.monotonic() - start < duration: + b = uart.read(128) + if len(b) > 0: + sys.stdout.write(b.decode('ascii', 'replace')) + else: + time.sleep(duration) + +def output_Crsf(uart: serial.Serial, data): + if uart is None: + return + uart.write(data) + +def output_Msp(uart: serial.Serial, data): + if uart is None: + return + + # MSPv2 header + MSP_COMMAND + flags + buf = b'$X<\x00' + # Command MSP_ELRS_BACKPACK_CRSF_TLM + buf += int.to_bytes(0x0011, length=2, byteorder='little', signed=False) + buf += int.to_bytes(len(data), length=2, byteorder='little', signed=False) + buf += data + buf += int.to_bytes(crsf_crc(buf[3:]), length=1, byteorder='big', signed=False) + + uart.write(buf) + +def processFile(fname, interval, port, baud, jitter, output): + random.seed() + + if port is not None: + uart = serial.Serial(port=port, baudrate=baud, timeout=0.1) + else: + uart = None + + with open(fname, 'r') as csv_file: + reader = csv.reader(csv_file) + # Skip header. DictReader is not used for performance and simplicity + # ['time (us)', 'GPS_fixType', ' GPS_numSat', ' GPS_coord[0]', ' GPS_coord[1]', ' GPS_altitude', ' GPS_speed (m/s)', ' GPS_ground_course', ' GPS_hdop', ' GPS_eph', ' GPS_epv', ' GPS_velned[0]', ' GPS_velned[1]', ' GPS_velned[2]'] + next(reader) + + lastTime = None + for row in reader: + timeS = float(row[0]) / 1e6 + + if lastTime != None: + dur = timeS - lastTime + if dur < interval: + continue + idle(dur + (random.randrange(0, int(jitter * 1000.0)) / 1000.0), uart) + + # All values here are immediately loaded into their CRSF representations + sats = int(row[2]) + lat = int(float(row[3]) * 1e7) + lon = int(float(row[4]) * 1e7) + alt = int(row[5]) + 1000 + spd = int(float(row[6]) * 360) # m/s to km/h*10 + hdg = int(float(row[7]) * 100) # deg to centideg + print(f'{timeS:0.6f} GPS: ({lat},{lon}) {alt-1000}m {sats}sats') + + # CRSF_SYNC, len, type (GPS), payload, crc + buf = bytearray(b'\xc8\x00\x02') + buf += int.to_bytes(lat, length=4, byteorder='big', signed=True) + buf += int.to_bytes(lon, length=4, byteorder='big', signed=True) + buf += int.to_bytes(spd, length=2, byteorder='big', signed=False) + buf += int.to_bytes(hdg, length=2, byteorder='big', signed=False) + buf += int.to_bytes(alt, length=2, byteorder='big', signed=False) + buf += int.to_bytes(sats, length=1, byteorder='big', signed=False) + buf += b'\x00' # place for the CRC + buf[-1] = crsf_frame_crc(buf) + buf[1] = len(buf) - 2 + output(uart, buf) + + lastTime = timeS + +parser = argparse.ArgumentParser( + prog='gpscsv_to_crsf', + description='Convert iNav GPS CSV to CRSF and send in real time' +) +parser.add_argument('filename') +parser.add_argument('-b', '--baud', + default='460800', + help='Baud rate used with sending CRSF data to UART') +parser.add_argument('-P', '--port',\ + help='UART to send CRSF data to') +parser.add_argument('-I', '--interval', + default=5.0, + type=float, + help='If non-zero, skip GPS frames so that GPS updates are limited to this interval (s)') +parser.add_argument('-J', '--jitter', + default=0.0, + type=float, + help='Adds random jitter to the GPS output to simulate real world telemetry delay (s)') +parser.add_argument('-m', '--msp', + dest='output', + action='store_const', const=output_Msp, default=output_Crsf, + help='Wrap CRSF with MSP when sending to UART (default: send just CRSF)') + +args = parser.parse_args() processFile(args.filename, args.interval, args.port, args.baud, args.jitter, args.output) \ No newline at end of file diff --git a/src/module_aat.cpp b/src/module_aat.cpp index 65fe176..a41e0cc 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -1,355 +1,355 @@ -#if defined(AAT_BACKPACK) -#include "common.h" -#include "module_aat.h" -#include "logging.h" -#include "devWifi.h" - -#include -#include - -#define DEG2RAD(deg) ((deg) * M_PI / 180.0) -#define RAD2DEG(rad) ((rad) * 180.0 / M_PI) -#define DELAY_IDLE (20U) // sleep used when not tracking -#define DELAY_FIRST_UPDATE (5000U) // absolute delay before first servo update - -static void calcDistAndAzimuth(int32_t srcLat, int32_t srcLon, int32_t dstLat, int32_t dstLon, - uint32_t *out_dist, uint32_t *out_azimuth) -{ - // https://www.movable-type.co.uk/scripts/latlong.html - // https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/ - - // Have to use doubles for at least some of these, due to short distances getting rounded - // particularly cos(deltaLon) for <2000 m rounds to 1.0000000000 - double deltaLon = DEG2RAD((float)(dstLon - srcLon) / 1e7); - double thetaA = DEG2RAD((float)srcLat / 1e7); - double thetaB = DEG2RAD((float)dstLat / 1e7); - double cosThetaA = cos(thetaA); - double cosThetaB = cos(thetaB); - double sinThetaA = sin(thetaA); - double sinThetaB = sin(thetaB); - double cosDeltaLon = cos(deltaLon); - double sinDeltaLon = sin(deltaLon); - - if (out_dist) - { - const double R = 6371e3; - double dist = acos(sinThetaA * sinThetaB + cosThetaA * cosThetaB * cosDeltaLon) * R; - *out_dist = (uint32_t)dist; - } - - if (out_azimuth) - { - double X = cosThetaB * sinDeltaLon; - double Y = cosThetaA * sinThetaB - sinThetaA * cosThetaB * cosDeltaLon; - - // Convert to degrees, normalized to 0-360 - uint32_t hdg = RAD2DEG(atan2(X, Y)); - *out_azimuth = (hdg + 360) % 360; - } -} - -static int32_t calcElevation(uint32_t distance, int32_t altitude) -{ - return RAD2DEG(atan2(altitude, distance)); -} - -AatModule::AatModule(Stream &port) : - CrsfModuleBase(port), _gpsLast{0}, _home{0}, - _gpsAvgUpdateInterval(0), _lastServoUpdateMs(0), _targetDistance(0), - _targetAzim(0), _targetElev(0), _azimMsPerDegree(0), - _currentElev(0), _currentAzim(0) -#if defined(PIN_SERVO_AZIM) - , _servo_Azim() -#endif -#if defined(PIN_SERVO_ELEV) - , _servo_Elev() -#endif -#if defined(PIN_OLED_SDA) - , _display(SCREEN_WIDTH, SCREEN_HEIGHT) -#endif -{ - // Init is called manually -} - -void AatModule::Init() -{ -#if defined(PIN_SERVO_AZIM) - _servo_Azim.attach(PIN_SERVO_AZIM, 500, 2500); -#endif -#if defined(PIN_SERVO_ELEV) - _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500); -#endif - displayInit(); - ModuleBase::Init(); -} - -void AatModule::SendGpsTelemetry(crsf_packet_gps_t *packet) -{ - _gpsLast.lat = be32toh(packet->p.lat); - _gpsLast.lon = be32toh(packet->p.lon); - _gpsLast.speed = be16toh(packet->p.speed); - _gpsLast.heading = be16toh(packet->p.heading); - _gpsLast.altitude = (int32_t)be16toh(packet->p.altitude) - 1000; - _gpsLast.satcnt = packet->p.satcnt; - - //DBGLN("GPS: (%d,%d) %dm %usats", _gpsLast.lat, _gpsLast.lon, - // _gpsLast.altitude, _gpsLast.satcnt); - - _gpsLast.updated = true; -} - -void AatModule::updateGpsInterval(uint32_t interval) -{ - // Avg is in ms * 100 - interval *= 100; - // Low pass filter. Note there is no fast init of the average, so it will take some time to grow - // this prevents overprojection caused by the first update after setting home - _gpsAvgUpdateInterval += ((int32_t)interval - (int32_t)_gpsAvgUpdateInterval) / 4; - - // Limit the maximum interval to provent projecting for too long - const uint32_t GPS_UPDATE_INTERVAL_MAX = (10U * 1000U * 100U); - if (_gpsAvgUpdateInterval > GPS_UPDATE_INTERVAL_MAX) - _gpsAvgUpdateInterval = GPS_UPDATE_INTERVAL_MAX; -} - -uint8_t AatModule::calcGpsIntervalPct(uint32_t now) -{ - if (_gpsAvgUpdateInterval) - { - return constrain((now - _gpsLast.lastUpdateMs) * (100U * 100U) / (uint32_t)_gpsAvgUpdateInterval, 0U, 100U); - } - - return 0; -} - -void AatModule::processGps(uint32_t now) -{ - if (!_gpsLast.updated) - return; - _gpsLast.updated = false; - - // Actually want to track time between _processing_ each GPS update - uint32_t interval = now - _gpsLast.lastUpdateMs; - _gpsLast.lastUpdateMs = now; - - // Check if need to set home position - bool didSetHome = false; - if (!isHomeSet()) - { - if (_gpsLast.satcnt >= config.GetAatSatelliteHomeMin()) - { - didSetHome = true; - _home.lat = _gpsLast.lat; - _home.lon = _gpsLast.lon; - _home.alt = _gpsLast.altitude; - DBGLN("GPS Home set to (%d,%d)", _home.lat, _home.lon); - } - else - return; - } - - uint32_t azimuth; - uint32_t distance; - calcDistAndAzimuth(_home.lat, _home.lon, _gpsLast.lat, _gpsLast.lon, &distance, &azimuth); - uint8_t elevation = constrain(calcElevation(distance, _gpsLast.altitude - _home.alt), 0, 90); - DBGLN("Azimuth: %udeg Elevation: %udeg Distance: %um", azimuth, elevation, distance); - - // Calculate angular velocity to allow dead reckoning projection - if (!didSetHome) - { - updateGpsInterval(interval); - // azimDelta is the azimuth change since the last packet, -180 to +180 - int32_t azimDelta = (azimuth - _targetAzim + 540) % 360 - 180; - _azimMsPerDegree = (azimDelta == 0) ? 0 : (int32_t)interval / azimDelta; - DBGLN("%d delta in %ums, %dms/d %uavg", azimDelta, interval, _azimMsPerDegree, _gpsAvgUpdateInterval); - } - - _targetDistance = distance; - _targetElev = elevation; - _targetAzim = azimuth; -} - -int32_t AatModule::calcProjectedAzim(uint32_t now) -{ - // Attempt to do a linear projection of the last - // If enabled, we know the GPS update rate, the azimuth has changed, and more than a few meters away - if (config.GetAatProject() && _gpsAvgUpdateInterval && _azimMsPerDegree && _targetDistance > 3) - { - uint32_t elapsed = constrain(now - _gpsLast.lastUpdateMs, 0U, _gpsAvgUpdateInterval / 100U); - - // Prevent excessive rotational velocity (100 degrees per second) - int32_t azimMsPDLimited; - if (_azimMsPerDegree > -10 && _azimMsPerDegree < 10) - if (_azimMsPerDegree > 0) - azimMsPDLimited = 10; - else - azimMsPDLimited = -10; - else - azimMsPDLimited = _azimMsPerDegree; - - int32_t target = (((int32_t)elapsed / azimMsPDLimited) + _targetAzim + 360) % 360; - return target; - } - - return _targetAzim; -} - -void AatModule::displayInit() -{ -#if defined(PIN_OLED_SDA) - Wire.begin(PIN_OLED_SDA, PIN_OLED_SCL); - _display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); - _display.setTextSize(2); - _display.setTextColor(SSD1306_WHITE); - _display.setCursor(0, 0); - if (connectionState == binding) - _display.print("Bind\nmode...\n\n"); - else - _display.print("AAT\nBackpack\n\n"); - _display.setTextSize(1); - _display.print(VERSION); - _display.display(); -#endif -} - -void AatModule::displayIdle(uint32_t now) -{ -#if defined(PIN_OLED_SDA) - // A screen with just the GPS position, sat count, and interval bar - _display.clearDisplay(); - _display.setCursor(0, 0); - - _display.setTextSize(2); - _display.printf("Sats: %u\n", _gpsLast.satcnt); - - _display.setTextSize(1); - _display.printf("\nLat: %d.%07d\nLon: %d.%07d", - _gpsLast.lat / 10000000, abs(_gpsLast.lat) % 10000000, - _gpsLast.lon / 10000000, abs(_gpsLast.lon) % 10000000 - ); - - displayGpsIntervalBar(now); - _display.display(); -#endif -} - -void AatModule::displayActive(uint32_t now, int32_t projectedAzim, int32_t usElev, int32_t usAzim) -{ -#if defined(PIN_OLED_SDA) - // El:[deg] [alt]m - // Az:[deg] [dist] - // Se:[servo elev]us - // Sa:[servo azim]us - // With interval bar - _display.clearDisplay(); - _display.setTextSize(2); - _display.setCursor(0, 0) - ; - _display.printf("El:%02u %dm\nAz:%03u ", _targetElev, - constrain(_gpsLast.altitude - _home.alt, -99, 999), - projectedAzim); - - // Target distance has variable width/height but all fits in 3x1 (doublesized) characters - if (_targetDistance > 999) - { - _display.setTextSize(1); - _display.printf("%u.%03u\nkm\n", _targetDistance / 1000, (_targetDistance % 1000)); // X.XXX km small font - _display.setTextSize(2); - } - else if (_targetDistance > 99) - { - _display.printf("%u\n", _targetDistance); // XXX - } - else - { - _display.printf("%um\n", _targetDistance); // XXm - } - - _display.printf("Se:%4dus\nSa:%4dus\n", usElev, usAzim); - displayGpsIntervalBar(now); - _display.display(); -#endif -} - -void AatModule::displayGpsIntervalBar(uint32_t now) -{ -#if defined(PIN_OLED_SDA) - if (_gpsAvgUpdateInterval) - { - uint8_t gpsIntervalPct = calcGpsIntervalPct(now); - uint8_t pxHeight = SCREEN_HEIGHT * (100U - gpsIntervalPct) / 100U; - _display.fillRect(SCREEN_WIDTH - 3, SCREEN_HEIGHT - pxHeight, 2, pxHeight, SSD1306_WHITE); - } -#endif -} - -void AatModule::servoUpdate(uint32_t now) -{ - uint32_t interval = now - _lastServoUpdateMs; - if (interval < 20U) - return; - _lastServoUpdateMs = now; - - int32_t projectedAzim = calcProjectedAzim(now); - - // Smooth the updates and scale to degrees * 100 - const int8_t servoSmoothFactor = config.GetAatServoSmooth(); - _currentAzim = ((_currentAzim * servoSmoothFactor) + (projectedAzim * (10 - servoSmoothFactor) * 100)) / 10; - _currentElev = ((_currentElev * servoSmoothFactor) + (_targetElev * (10 - servoSmoothFactor) * 100)) / 10; - - int32_t transformedAzim = _currentAzim; - int32_t transformedElev = _currentElev; - - // 90-270 azim inverts the elevation servo - // if (transformedAzim > 18000) - // { - // transformedElev = 18000 - transformedElev; - // transformedAzim = transformedAzim - 18000; - // } - // For straight geared servos with 180 degree elev flip - // int32_t usAzim = map(transformedAzim, 0, 18000, 500, 2500); - // int32_t usElev = map(transformedElev, 0, 18000, 500, 2500); - - // For 1:2 gearing on the azim servo to allow 360 rotation - // For Elev servos that only go 0-90 and the azim does 360 - transformedAzim = (transformedAzim + 18000) % 36000; // convert so 0 maps to 1500us - int32_t usAzim = map(transformedAzim, 0, 36000, config.GetAatServoLowAzim(), config.GetAatServoHighAzim()); - int32_t usElev = map(transformedElev, 0, 9000, config.GetAatServoLowElev(), config.GetAatServoHighElev()); - -#if defined(PIN_SERVO_AZIM) - _servo_Azim.writeMicroseconds(usAzim); -#endif -#if defined(PIN_SERVO_ELEV) - _servo_Elev.writeMicroseconds(usElev); -#endif - - displayActive(now, projectedAzim, usElev, usAzim); -} - -void AatModule::onCrsfPacketIn(const crsf_header_t *pkt) -{ - if (pkt->sync_byte == CRSF_SYNC_BYTE) - { - if (pkt->type == CRSF_FRAMETYPE_GPS) - SendGpsTelemetry((crsf_packet_gps_t *)pkt); - } -} - -void AatModule::Loop(uint32_t now) -{ - processGps(now); - - if (isHomeSet() && now > DELAY_FIRST_UPDATE) - { - servoUpdate(now); - } - else - { - if (isGpsActive()) - displayIdle(now); - delay(DELAY_IDLE); - } - - CrsfModuleBase::Loop(now); -} +#if defined(AAT_BACKPACK) +#include "common.h" +#include "module_aat.h" +#include "logging.h" +#include "devWifi.h" + +#include +#include + +#define DEG2RAD(deg) ((deg) * M_PI / 180.0) +#define RAD2DEG(rad) ((rad) * 180.0 / M_PI) +#define DELAY_IDLE (20U) // sleep used when not tracking +#define DELAY_FIRST_UPDATE (5000U) // absolute delay before first servo update + +static void calcDistAndAzimuth(int32_t srcLat, int32_t srcLon, int32_t dstLat, int32_t dstLon, + uint32_t *out_dist, uint32_t *out_azimuth) +{ + // https://www.movable-type.co.uk/scripts/latlong.html + // https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/ + + // Have to use doubles for at least some of these, due to short distances getting rounded + // particularly cos(deltaLon) for <2000 m rounds to 1.0000000000 + double deltaLon = DEG2RAD((float)(dstLon - srcLon) / 1e7); + double thetaA = DEG2RAD((float)srcLat / 1e7); + double thetaB = DEG2RAD((float)dstLat / 1e7); + double cosThetaA = cos(thetaA); + double cosThetaB = cos(thetaB); + double sinThetaA = sin(thetaA); + double sinThetaB = sin(thetaB); + double cosDeltaLon = cos(deltaLon); + double sinDeltaLon = sin(deltaLon); + + if (out_dist) + { + const double R = 6371e3; + double dist = acos(sinThetaA * sinThetaB + cosThetaA * cosThetaB * cosDeltaLon) * R; + *out_dist = (uint32_t)dist; + } + + if (out_azimuth) + { + double X = cosThetaB * sinDeltaLon; + double Y = cosThetaA * sinThetaB - sinThetaA * cosThetaB * cosDeltaLon; + + // Convert to degrees, normalized to 0-360 + uint32_t hdg = RAD2DEG(atan2(X, Y)); + *out_azimuth = (hdg + 360) % 360; + } +} + +static int32_t calcElevation(uint32_t distance, int32_t altitude) +{ + return RAD2DEG(atan2(altitude, distance)); +} + +AatModule::AatModule(Stream &port) : + CrsfModuleBase(port), _gpsLast{0}, _home{0}, + _gpsAvgUpdateInterval(0), _lastServoUpdateMs(0), _targetDistance(0), + _targetAzim(0), _targetElev(0), _azimMsPerDegree(0), + _currentElev(0), _currentAzim(0) +#if defined(PIN_SERVO_AZIM) + , _servo_Azim() +#endif +#if defined(PIN_SERVO_ELEV) + , _servo_Elev() +#endif +#if defined(PIN_OLED_SDA) + , _display(SCREEN_WIDTH, SCREEN_HEIGHT) +#endif +{ + // Init is called manually +} + +void AatModule::Init() +{ +#if defined(PIN_SERVO_AZIM) + _servo_Azim.attach(PIN_SERVO_AZIM, 500, 2500); +#endif +#if defined(PIN_SERVO_ELEV) + _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500); +#endif + displayInit(); + ModuleBase::Init(); +} + +void AatModule::SendGpsTelemetry(crsf_packet_gps_t *packet) +{ + _gpsLast.lat = be32toh(packet->p.lat); + _gpsLast.lon = be32toh(packet->p.lon); + _gpsLast.speed = be16toh(packet->p.speed); + _gpsLast.heading = be16toh(packet->p.heading); + _gpsLast.altitude = (int32_t)be16toh(packet->p.altitude) - 1000; + _gpsLast.satcnt = packet->p.satcnt; + + //DBGLN("GPS: (%d,%d) %dm %usats", _gpsLast.lat, _gpsLast.lon, + // _gpsLast.altitude, _gpsLast.satcnt); + + _gpsLast.updated = true; +} + +void AatModule::updateGpsInterval(uint32_t interval) +{ + // Avg is in ms * 100 + interval *= 100; + // Low pass filter. Note there is no fast init of the average, so it will take some time to grow + // this prevents overprojection caused by the first update after setting home + _gpsAvgUpdateInterval += ((int32_t)interval - (int32_t)_gpsAvgUpdateInterval) / 4; + + // Limit the maximum interval to provent projecting for too long + const uint32_t GPS_UPDATE_INTERVAL_MAX = (10U * 1000U * 100U); + if (_gpsAvgUpdateInterval > GPS_UPDATE_INTERVAL_MAX) + _gpsAvgUpdateInterval = GPS_UPDATE_INTERVAL_MAX; +} + +uint8_t AatModule::calcGpsIntervalPct(uint32_t now) +{ + if (_gpsAvgUpdateInterval) + { + return constrain((now - _gpsLast.lastUpdateMs) * (100U * 100U) / (uint32_t)_gpsAvgUpdateInterval, 0U, 100U); + } + + return 0; +} + +void AatModule::processGps(uint32_t now) +{ + if (!_gpsLast.updated) + return; + _gpsLast.updated = false; + + // Actually want to track time between _processing_ each GPS update + uint32_t interval = now - _gpsLast.lastUpdateMs; + _gpsLast.lastUpdateMs = now; + + // Check if need to set home position + bool didSetHome = false; + if (!isHomeSet()) + { + if (_gpsLast.satcnt >= config.GetAatSatelliteHomeMin()) + { + didSetHome = true; + _home.lat = _gpsLast.lat; + _home.lon = _gpsLast.lon; + _home.alt = _gpsLast.altitude; + DBGLN("GPS Home set to (%d,%d)", _home.lat, _home.lon); + } + else + return; + } + + uint32_t azimuth; + uint32_t distance; + calcDistAndAzimuth(_home.lat, _home.lon, _gpsLast.lat, _gpsLast.lon, &distance, &azimuth); + uint8_t elevation = constrain(calcElevation(distance, _gpsLast.altitude - _home.alt), 0, 90); + DBGLN("Azimuth: %udeg Elevation: %udeg Distance: %um", azimuth, elevation, distance); + + // Calculate angular velocity to allow dead reckoning projection + if (!didSetHome) + { + updateGpsInterval(interval); + // azimDelta is the azimuth change since the last packet, -180 to +180 + int32_t azimDelta = (azimuth - _targetAzim + 540) % 360 - 180; + _azimMsPerDegree = (azimDelta == 0) ? 0 : (int32_t)interval / azimDelta; + DBGLN("%d delta in %ums, %dms/d %uavg", azimDelta, interval, _azimMsPerDegree, _gpsAvgUpdateInterval); + } + + _targetDistance = distance; + _targetElev = elevation; + _targetAzim = azimuth; +} + +int32_t AatModule::calcProjectedAzim(uint32_t now) +{ + // Attempt to do a linear projection of the last + // If enabled, we know the GPS update rate, the azimuth has changed, and more than a few meters away + if (config.GetAatProject() && _gpsAvgUpdateInterval && _azimMsPerDegree && _targetDistance > 3) + { + uint32_t elapsed = constrain(now - _gpsLast.lastUpdateMs, 0U, _gpsAvgUpdateInterval / 100U); + + // Prevent excessive rotational velocity (100 degrees per second) + int32_t azimMsPDLimited; + if (_azimMsPerDegree > -10 && _azimMsPerDegree < 10) + if (_azimMsPerDegree > 0) + azimMsPDLimited = 10; + else + azimMsPDLimited = -10; + else + azimMsPDLimited = _azimMsPerDegree; + + int32_t target = (((int32_t)elapsed / azimMsPDLimited) + _targetAzim + 360) % 360; + return target; + } + + return _targetAzim; +} + +void AatModule::displayInit() +{ +#if defined(PIN_OLED_SDA) + Wire.begin(PIN_OLED_SDA, PIN_OLED_SCL); + _display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); + _display.setTextSize(2); + _display.setTextColor(SSD1306_WHITE); + _display.setCursor(0, 0); + if (connectionState == binding) + _display.print("Bind\nmode...\n\n"); + else + _display.print("AAT\nBackpack\n\n"); + _display.setTextSize(1); + _display.print(VERSION); + _display.display(); +#endif +} + +void AatModule::displayIdle(uint32_t now) +{ +#if defined(PIN_OLED_SDA) + // A screen with just the GPS position, sat count, and interval bar + _display.clearDisplay(); + _display.setCursor(0, 0); + + _display.setTextSize(2); + _display.printf("Sats: %u\n", _gpsLast.satcnt); + + _display.setTextSize(1); + _display.printf("\nLat: %d.%07d\nLon: %d.%07d", + _gpsLast.lat / 10000000, abs(_gpsLast.lat) % 10000000, + _gpsLast.lon / 10000000, abs(_gpsLast.lon) % 10000000 + ); + + displayGpsIntervalBar(now); + _display.display(); +#endif +} + +void AatModule::displayActive(uint32_t now, int32_t projectedAzim, int32_t usElev, int32_t usAzim) +{ +#if defined(PIN_OLED_SDA) + // El:[deg] [alt]m + // Az:[deg] [dist] + // Se:[servo elev]us + // Sa:[servo azim]us + // With interval bar + _display.clearDisplay(); + _display.setTextSize(2); + _display.setCursor(0, 0) + ; + _display.printf("El:%02u %dm\nAz:%03u ", _targetElev, + constrain(_gpsLast.altitude - _home.alt, -99, 999), + projectedAzim); + + // Target distance has variable width/height but all fits in 3x1 (doublesized) characters + if (_targetDistance > 999) + { + _display.setTextSize(1); + _display.printf("%u.%03u\nkm\n", _targetDistance / 1000, (_targetDistance % 1000)); // X.XXX km small font + _display.setTextSize(2); + } + else if (_targetDistance > 99) + { + _display.printf("%u\n", _targetDistance); // XXX + } + else + { + _display.printf("%um\n", _targetDistance); // XXm + } + + _display.printf("Se:%4dus\nSa:%4dus\n", usElev, usAzim); + displayGpsIntervalBar(now); + _display.display(); +#endif +} + +void AatModule::displayGpsIntervalBar(uint32_t now) +{ +#if defined(PIN_OLED_SDA) + if (_gpsAvgUpdateInterval) + { + uint8_t gpsIntervalPct = calcGpsIntervalPct(now); + uint8_t pxHeight = SCREEN_HEIGHT * (100U - gpsIntervalPct) / 100U; + _display.fillRect(SCREEN_WIDTH - 3, SCREEN_HEIGHT - pxHeight, 2, pxHeight, SSD1306_WHITE); + } +#endif +} + +void AatModule::servoUpdate(uint32_t now) +{ + uint32_t interval = now - _lastServoUpdateMs; + if (interval < 20U) + return; + _lastServoUpdateMs = now; + + int32_t projectedAzim = calcProjectedAzim(now); + + // Smooth the updates and scale to degrees * 100 + const int8_t servoSmoothFactor = config.GetAatServoSmooth(); + _currentAzim = ((_currentAzim * servoSmoothFactor) + (projectedAzim * (10 - servoSmoothFactor) * 100)) / 10; + _currentElev = ((_currentElev * servoSmoothFactor) + (_targetElev * (10 - servoSmoothFactor) * 100)) / 10; + + int32_t transformedAzim = _currentAzim; + int32_t transformedElev = _currentElev; + + // 90-270 azim inverts the elevation servo + // if (transformedAzim > 18000) + // { + // transformedElev = 18000 - transformedElev; + // transformedAzim = transformedAzim - 18000; + // } + // For straight geared servos with 180 degree elev flip + // int32_t usAzim = map(transformedAzim, 0, 18000, 500, 2500); + // int32_t usElev = map(transformedElev, 0, 18000, 500, 2500); + + // For 1:2 gearing on the azim servo to allow 360 rotation + // For Elev servos that only go 0-90 and the azim does 360 + transformedAzim = (transformedAzim + 18000) % 36000; // convert so 0 maps to 1500us + int32_t usAzim = map(transformedAzim, 0, 36000, config.GetAatServoLowAzim(), config.GetAatServoHighAzim()); + int32_t usElev = map(transformedElev, 0, 9000, config.GetAatServoLowElev(), config.GetAatServoHighElev()); + +#if defined(PIN_SERVO_AZIM) + _servo_Azim.writeMicroseconds(usAzim); +#endif +#if defined(PIN_SERVO_ELEV) + _servo_Elev.writeMicroseconds(usElev); +#endif + + displayActive(now, projectedAzim, usElev, usAzim); +} + +void AatModule::onCrsfPacketIn(const crsf_header_t *pkt) +{ + if (pkt->sync_byte == CRSF_SYNC_BYTE) + { + if (pkt->type == CRSF_FRAMETYPE_GPS) + SendGpsTelemetry((crsf_packet_gps_t *)pkt); + } +} + +void AatModule::Loop(uint32_t now) +{ + processGps(now); + + if (isHomeSet() && now > DELAY_FIRST_UPDATE) + { + servoUpdate(now); + } + else + { + if (isGpsActive()) + displayIdle(now); + delay(DELAY_IDLE); + } + + CrsfModuleBase::Loop(now); +} #endif /* AAT_BACKPACK */ \ No newline at end of file diff --git a/src/module_aat.h b/src/module_aat.h index 20a0fa3..3b6d5b8 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -1,82 +1,82 @@ -#pragma once - -#if defined(AAT_BACKPACK) -#include "config.h" -#include "module_crsf.h" -#include "crsf_protocol.h" - -#if defined(PIN_SERVO_AZIM) || defined(PIN_SERVO_ELEV) -#include -#endif - -#if defined(PIN_OLED_SDA) -#include -#include -#include - -#define SCREEN_WIDTH 128 -#define SCREEN_HEIGHT 64 -#define SCREEN_ADDRESS 0x3C -#endif - -class AatModule : public CrsfModuleBase -{ -public: - AatModule() = delete; - AatModule(Stream &port); - - void Init(); - void Loop(uint32_t now); - - void SendGpsTelemetry(crsf_packet_gps_t *packet); - bool isHomeSet() const { return _home.lat != 0 || _home.lon != 0; } - bool isGpsActive() const { return _gpsLast.lat != 0 || _gpsLast.lon != 0 || _gpsLast.satcnt > 0; }; -protected: - virtual void onCrsfPacketIn(const crsf_header_t *pkt); -private: - void displayInit(); - void updateGpsInterval(uint32_t interval); - uint8_t calcGpsIntervalPct(uint32_t now); - int32_t calcProjectedAzim(uint32_t now); - void processGps(uint32_t now); - void servoUpdate(uint32_t now); - void displayIdle(uint32_t now); - void displayActive(uint32_t now, int32_t projectedAzim, int32_t usElev, int32_t usAzim); - void displayGpsIntervalBar(uint32_t now); - - struct { - int32_t lat; - int32_t lon; - uint32_t speed; // km/h * 10 - uint32_t heading; // degrees * 10 - int32_t altitude; // meters - uint32_t lastUpdateMs; // timestamp of last update - uint8_t satcnt; // number of satellites - bool updated; - } _gpsLast; - struct { - int32_t lat; - int32_t lon; - int32_t alt; - } _home; - uint32_t _gpsAvgUpdateInterval; // ms * 100 - // Servo Position - uint32_t _lastServoUpdateMs; - uint32_t _targetDistance; // meters - uint16_t _targetAzim; // degrees - uint8_t _targetElev; // degrees - int32_t _azimMsPerDegree; // milliseconds per degree - int32_t _currentElev; // degrees * 100 - int32_t _currentAzim; // degrees * 100 - -#if defined(PIN_SERVO_AZIM) - Servo _servo_Azim; -#endif -#if defined(PIN_SERVO_ELEV) - Servo _servo_Elev; -#endif -#if defined(PIN_OLED_SDA) - Adafruit_SSD1306 _display; -#endif -}; +#pragma once + +#if defined(AAT_BACKPACK) +#include "config.h" +#include "module_crsf.h" +#include "crsf_protocol.h" + +#if defined(PIN_SERVO_AZIM) || defined(PIN_SERVO_ELEV) +#include +#endif + +#if defined(PIN_OLED_SDA) +#include +#include +#include + +#define SCREEN_WIDTH 128 +#define SCREEN_HEIGHT 64 +#define SCREEN_ADDRESS 0x3C +#endif + +class AatModule : public CrsfModuleBase +{ +public: + AatModule() = delete; + AatModule(Stream &port); + + void Init(); + void Loop(uint32_t now); + + void SendGpsTelemetry(crsf_packet_gps_t *packet); + bool isHomeSet() const { return _home.lat != 0 || _home.lon != 0; } + bool isGpsActive() const { return _gpsLast.lat != 0 || _gpsLast.lon != 0 || _gpsLast.satcnt > 0; }; +protected: + virtual void onCrsfPacketIn(const crsf_header_t *pkt); +private: + void displayInit(); + void updateGpsInterval(uint32_t interval); + uint8_t calcGpsIntervalPct(uint32_t now); + int32_t calcProjectedAzim(uint32_t now); + void processGps(uint32_t now); + void servoUpdate(uint32_t now); + void displayIdle(uint32_t now); + void displayActive(uint32_t now, int32_t projectedAzim, int32_t usElev, int32_t usAzim); + void displayGpsIntervalBar(uint32_t now); + + struct { + int32_t lat; + int32_t lon; + uint32_t speed; // km/h * 10 + uint32_t heading; // degrees * 10 + int32_t altitude; // meters + uint32_t lastUpdateMs; // timestamp of last update + uint8_t satcnt; // number of satellites + bool updated; + } _gpsLast; + struct { + int32_t lat; + int32_t lon; + int32_t alt; + } _home; + uint32_t _gpsAvgUpdateInterval; // ms * 100 + // Servo Position + uint32_t _lastServoUpdateMs; + uint32_t _targetDistance; // meters + uint16_t _targetAzim; // degrees + uint8_t _targetElev; // degrees + int32_t _azimMsPerDegree; // milliseconds per degree + int32_t _currentElev; // degrees * 100 + int32_t _currentAzim; // degrees * 100 + +#if defined(PIN_SERVO_AZIM) + Servo _servo_Azim; +#endif +#if defined(PIN_SERVO_ELEV) + Servo _servo_Elev; +#endif +#if defined(PIN_OLED_SDA) + Adafruit_SSD1306 _display; +#endif +}; #endif /* AAT_BACKPACK */ \ No newline at end of file diff --git a/src/module_crsf.cpp b/src/module_crsf.cpp index eff7fbd..b29c9ba 100644 --- a/src/module_crsf.cpp +++ b/src/module_crsf.cpp @@ -1,76 +1,76 @@ -#include "module_crsf.h" - -void CrsfModuleBase::Loop(uint32_t now) -{ - while (_port.available()) - { - uint8_t b = _port.read(); - _rxBuf[_rxBufPos++] = b; - - handleByteReceived(); - - if (_rxBufPos == (sizeof(_rxBuf)/sizeof(_rxBuf[0]))) - { - // Packet buffer filled and no valid packet found, dump the whole thing - _rxBufPos = 0; - } - } - ModuleBase::Loop(now); -} - -void CrsfModuleBase::handleByteReceived() -{ - bool reprocess; - do - { - reprocess = false; - if (_rxBufPos > 1) - { - uint8_t len = _rxBuf[1]; - // Sanity check the declared length isn't outside Type + X{1,CRSF_MAX_PAYLOAD_LEN} + CRC - // assumes there never will be a CRSF message that just has a type and no data (X) - if (len < 3 || len > (CRSF_MAX_PAYLOAD_LEN + 2)) - { - shiftRxBuffer(1); - reprocess = true; - } - - else if (_rxBufPos >= (len + 2)) - { - uint8_t inCrc = _rxBuf[2 + len - 1]; - uint8_t crc = _crc.calc(&_rxBuf[2], len - 1); - if (crc == inCrc) - { - onCrsfPacketIn((const crsf_header_t *)_rxBuf); - - shiftRxBuffer(len + 2); - reprocess = true; - } - else - { - shiftRxBuffer(1); - reprocess = true; - } - } // if complete packet - } // if pos > 1 - } while (reprocess); -} - -// Shift the bytes in the RxBuf down by cnt bytes -void CrsfModuleBase::shiftRxBuffer(uint8_t cnt) -{ - // If removing the whole thing, just set pos to 0 - if (cnt >= _rxBufPos) - { - _rxBufPos = 0; - return; - } - - // Otherwise do the slow shift down - uint8_t *src = &_rxBuf[cnt]; - uint8_t *dst = &_rxBuf[0]; - _rxBufPos -= cnt; - uint8_t left = _rxBufPos; - while (left--) - *dst++ = *src++; -} +#include "module_crsf.h" + +void CrsfModuleBase::Loop(uint32_t now) +{ + while (_port.available()) + { + uint8_t b = _port.read(); + _rxBuf[_rxBufPos++] = b; + + handleByteReceived(); + + if (_rxBufPos == (sizeof(_rxBuf)/sizeof(_rxBuf[0]))) + { + // Packet buffer filled and no valid packet found, dump the whole thing + _rxBufPos = 0; + } + } + ModuleBase::Loop(now); +} + +void CrsfModuleBase::handleByteReceived() +{ + bool reprocess; + do + { + reprocess = false; + if (_rxBufPos > 1) + { + uint8_t len = _rxBuf[1]; + // Sanity check the declared length isn't outside Type + X{1,CRSF_MAX_PAYLOAD_LEN} + CRC + // assumes there never will be a CRSF message that just has a type and no data (X) + if (len < 3 || len > (CRSF_MAX_PAYLOAD_LEN + 2)) + { + shiftRxBuffer(1); + reprocess = true; + } + + else if (_rxBufPos >= (len + 2)) + { + uint8_t inCrc = _rxBuf[2 + len - 1]; + uint8_t crc = _crc.calc(&_rxBuf[2], len - 1); + if (crc == inCrc) + { + onCrsfPacketIn((const crsf_header_t *)_rxBuf); + + shiftRxBuffer(len + 2); + reprocess = true; + } + else + { + shiftRxBuffer(1); + reprocess = true; + } + } // if complete packet + } // if pos > 1 + } while (reprocess); +} + +// Shift the bytes in the RxBuf down by cnt bytes +void CrsfModuleBase::shiftRxBuffer(uint8_t cnt) +{ + // If removing the whole thing, just set pos to 0 + if (cnt >= _rxBufPos) + { + _rxBufPos = 0; + return; + } + + // Otherwise do the slow shift down + uint8_t *src = &_rxBuf[cnt]; + uint8_t *dst = &_rxBuf[0]; + _rxBufPos -= cnt; + uint8_t left = _rxBufPos; + while (left--) + *dst++ = *src++; +} diff --git a/src/module_crsf.h b/src/module_crsf.h index 0ddacfe..7b299d3 100644 --- a/src/module_crsf.h +++ b/src/module_crsf.h @@ -1,31 +1,31 @@ -#pragma once - -#include "module_base.h" -#include "crsf_protocol.h" -#include "crc.h" - -class CrsfModuleBase : public ModuleBase -{ -public: - CrsfModuleBase() = delete; - CrsfModuleBase(Stream &port) : - _port(port), _crc(CRSF_CRC_POLY) - {}; - void Loop(uint32_t now); - - virtual void onCrsfPacketIn(const crsf_header_t *pkt) {}; - -protected: - Stream &_port; - - static constexpr uint8_t CRSF_MAX_PACKET_SIZE = 64U; - static constexpr uint8_t CRSF_MAX_PAYLOAD_LEN = (CRSF_MAX_PACKET_SIZE - 4U); - - GENERIC_CRC8 _crc; - uint8_t _rxBufPos; - uint8_t _rxBuf[CRSF_MAX_PACKET_SIZE]; - - void processPacketIn(uint8_t len); - void shiftRxBuffer(uint8_t cnt); - void handleByteReceived(); +#pragma once + +#include "module_base.h" +#include "crsf_protocol.h" +#include "crc.h" + +class CrsfModuleBase : public ModuleBase +{ +public: + CrsfModuleBase() = delete; + CrsfModuleBase(Stream &port) : + _port(port), _crc(CRSF_CRC_POLY) + {}; + void Loop(uint32_t now); + + virtual void onCrsfPacketIn(const crsf_header_t *pkt) {}; + +protected: + Stream &_port; + + static constexpr uint8_t CRSF_MAX_PACKET_SIZE = 64U; + static constexpr uint8_t CRSF_MAX_PAYLOAD_LEN = (CRSF_MAX_PACKET_SIZE - 4U); + + GENERIC_CRC8 _crc; + uint8_t _rxBufPos; + uint8_t _rxBuf[CRSF_MAX_PACKET_SIZE]; + + void processPacketIn(uint8_t len); + void shiftRxBuffer(uint8_t cnt); + void handleByteReceived(); }; \ No newline at end of file diff --git a/targets/aat.ini b/targets/aat.ini index 1d22b25..9211b30 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -1,28 +1,32 @@ -[env:AAT_ESP_Backpack_via_UART] -extends = env_common_esp12e -build_flags = - ${common_env_data.build_flags} - ${env_common_esp12e.build_flags} - -D TARGET_VRX_BACKPACK - -D AAT_BACKPACK - -D SSD1306_NO_SPLASH - -D PIN_BUTTON=0 - -D PIN_LED=16 - -D LED_INVERTED - -D PIN_SERVO_AZIM=9 - -D PIN_SERVO_ELEV=10 - -D PIN_OLED_SDA=1 - -D PIN_OLED_SCL=3 - ; -D PIN_SERVO_AZIM=1 - ; -D PIN_SERVO_ELEV=3 - ; -D PIN_OLED_SDA=9 - ; -D PIN_OLED_SCL=10 - ; -D DEBUG_LOG -lib_ignore = STM32UPDATE -lib_deps = - ${env.lib_deps} - adafruit/Adafruit SSD1306 @ 2.5.9 -build_src_filter = ${common_env_data.build_src_filter} - - - - - -[env:AAT_ESP_Backpack_via_WIFI] -extends = env:AAT_ESP_Backpack_via_UART +# ******************************** +# AAT VRX backpack -- Automatic antenna tracking +# ******************************** + +[env:AAT_ESP_Backpack_via_UART] +extends = env_common_esp12e +build_flags = + ${common_env_data.build_flags} + ${env_common_esp12e.build_flags} + -D TARGET_VRX_BACKPACK + -D AAT_BACKPACK + -D SSD1306_NO_SPLASH + -D PIN_BUTTON=0 + -D PIN_LED=16 + -D LED_INVERTED + -D PIN_SERVO_AZIM=9 + -D PIN_SERVO_ELEV=10 + -D PIN_OLED_SDA=1 + -D PIN_OLED_SCL=3 + ; -D PIN_SERVO_AZIM=1 + ; -D PIN_SERVO_ELEV=3 + ; -D PIN_OLED_SDA=9 + ; -D PIN_OLED_SCL=10 + ; -D DEBUG_LOG +lib_ignore = STM32UPDATE +lib_deps = + ${env.lib_deps} + adafruit/Adafruit SSD1306 @ 2.5.9 +build_src_filter = ${common_env_data.build_src_filter} - - - - + +[env:AAT_ESP_Backpack_via_WIFI] +extends = env:AAT_ESP_Backpack_via_UART From e49df6beef3807dabedd4d2817220adbf79f169c Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Fri, 29 Dec 2023 09:40:05 -0500 Subject: [PATCH 11/29] Idle on startup, fix 0 jitter error --- python/utils/gpscsv_to_crsf.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/python/utils/gpscsv_to_crsf.py b/python/utils/gpscsv_to_crsf.py index b466d6c..ec0bc43 100644 --- a/python/utils/gpscsv_to_crsf.py +++ b/python/utils/gpscsv_to_crsf.py @@ -60,6 +60,8 @@ def processFile(fname, interval, port, baud, jitter, output): if port is not None: uart = serial.Serial(port=port, baudrate=baud, timeout=0.1) + # Delay enough time for the MCU to reboot if DTR/RTS hooked to RST + idle(1.0, uart) else: uart = None @@ -77,7 +79,8 @@ def processFile(fname, interval, port, baud, jitter, output): dur = timeS - lastTime if dur < interval: continue - idle(dur + (random.randrange(0, int(jitter * 1000.0)) / 1000.0), uart) + j = random.randrange(0, int(jitter * 1000.0)) / 1000.0 if jitter > 0 else 0 + idle(dur + j, uart) # All values here are immediately loaded into their CRSF representations sats = int(row[2]) From d7879f82c71011dc4a1f9979d596a4244e87b39c Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Fri, 29 Dec 2023 12:32:53 -0500 Subject: [PATCH 12/29] Servo center on init, smooth serovos not angles, swap I2C --- lib/config/config.cpp | 14 +++++----- lib/config/config.h | 30 ++++++++++---------- src/module_aat.cpp | 65 ++++++++++++++++++++++++++----------------- src/module_aat.h | 8 +++--- src/module_crsf.h | 3 +- targets/aat.ini | 8 +++--- 6 files changed, 72 insertions(+), 56 deletions(-) diff --git a/lib/config/config.cpp b/lib/config/config.cpp index c16607d..aa15c4d 100644 --- a/lib/config/config.cpp +++ b/lib/config/config.cpp @@ -139,13 +139,13 @@ VrxBackpackConfig::SetDefaults() m_config.version = VRX_BACKPACK_CONFIG_VERSION | VRX_BACKPACK_CONFIG_MAGIC; #if defined(AAT_BACKPACK) - m_config.satelliteHomeMin = 5; - m_config.project = 0xff; - m_config.servoSmooth = 9; - m_config.servoLowAzim = 700; - m_config.servoLowElev = 1100; - m_config.servoHighAzim = 2400; - m_config.servoHighElev = 1900; + m_config.aat.satelliteHomeMin = 5; + m_config.aat.project = 0xff; + m_config.aat.servoSmooth = 9; + m_config.aat.servoEndpoints[0].low = 700; // AZIM + m_config.aat.servoEndpoints[0].high = 2400; + m_config.aat.servoEndpoints[1].low = 1100; // ELEV + m_config.aat.servoEndpoints[1].high = 1900; #endif m_modified = true; diff --git a/lib/config/config.h b/lib/config/config.h index 1643d7e..7fabdb4 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -61,14 +61,16 @@ typedef struct { uint8_t address[6]; #if defined(AAT_BACKPACK) - uint8_t satelliteHomeMin; // minimum number of satellites to establish home - uint8_t servoSmooth; - uint8_t project; // 0=none, 1=projectAzim, 2=projectElev, 3=projectBoth - uint8_t servoMode; // reserved to declare 2:1 or 180+flip servo - uint16_t servoLowAzim; - uint16_t servoLowElev; - uint16_t servoHighAzim; - uint16_t servoHighElev; + struct __attribute__((packed)) tagAatConfig { + uint8_t satelliteHomeMin; // minimum number of satellites to establish home + uint8_t servoSmooth; // 0-9 for no smoothing to 1/10th smoothing + uint8_t project; // 0=none, 1=projectAzim, 2=projectElev, 3=projectBoth + uint8_t servoMode; // reserved to declare 2:1 or 180+flip servo + struct tagServoEndoint { + uint16_t low; + uint16_t high; + } servoEndpoints[2]; // us endpoints for servos + } aat; #endif } vrx_backpack_config_t; @@ -96,13 +98,11 @@ class VrxBackpackConfig void SetGroupAddress(const uint8_t address[6]); #if defined(AAT_BACKPACK) - uint8_t GetAatSatelliteHomeMin() const { return m_config.satelliteHomeMin; } - uint8_t GetAatServoSmooth() const { return m_config.servoSmooth; } - uint8_t GetAatProject() const { return m_config.project; } - uint16_t GetAatServoLowAzim() const { return m_config.servoLowAzim; } - uint16_t GetAatServoLowElev() const { return m_config.servoLowElev; } - uint16_t GetAatServoHighAzim() const { return m_config.servoHighAzim; } - uint16_t GetAatServoHighElev() const { return m_config.servoHighElev; } + uint8_t GetAatSatelliteHomeMin() const { return m_config.aat.satelliteHomeMin; } + uint8_t GetAatServoSmooth() const { return m_config.aat.servoSmooth; } + uint8_t GetAatProject() const { return m_config.aat.project; } + uint16_t GetAatServoLow(uint8_t idx) const { return m_config.aat.servoEndpoints[idx].low; } + uint16_t GetAatServoHigh(uint8_t idx) const { return m_config.aat.servoEndpoints[idx].high; } #endif private: diff --git a/src/module_aat.cpp b/src/module_aat.cpp index a41e0cc..0c94519 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -57,7 +57,7 @@ AatModule::AatModule(Stream &port) : CrsfModuleBase(port), _gpsLast{0}, _home{0}, _gpsAvgUpdateInterval(0), _lastServoUpdateMs(0), _targetDistance(0), _targetAzim(0), _targetElev(0), _azimMsPerDegree(0), - _currentElev(0), _currentAzim(0) + _servoUs{0} #if defined(PIN_SERVO_AZIM) , _servo_Azim() #endif @@ -73,11 +73,17 @@ AatModule::AatModule(Stream &port) : void AatModule::Init() { +#if !defined(DEBUG_LOG) + // Need to call _port's end but it is a stream reference not the HardwareSerial + Serial.end(); +#endif #if defined(PIN_SERVO_AZIM) - _servo_Azim.attach(PIN_SERVO_AZIM, 500, 2500); + _servoUs[IDX_AZIM] = (config.GetAatServoLow(IDX_AZIM) + config.GetAatServoHigh(IDX_AZIM)) / 2; + _servo_Azim.attach(PIN_SERVO_AZIM, 500, 2500, _servoUs[IDX_AZIM]); #endif #if defined(PIN_SERVO_ELEV) - _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500); + _servoUs[IDX_ELEV] = (config.GetAatServoLow(IDX_ELEV) + config.GetAatServoHigh(IDX_ELEV)) / 2; + _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500, _servoUs[IDX_ELEV]); #endif displayInit(); ModuleBase::Init(); @@ -177,7 +183,7 @@ int32_t AatModule::calcProjectedAzim(uint32_t now) { uint32_t elapsed = constrain(now - _gpsLast.lastUpdateMs, 0U, _gpsAvgUpdateInterval / 100U); - // Prevent excessive rotational velocity (100 degrees per second) + // Prevent excessive rotational velocity (100 degrees per second / 10ms per degree) int32_t azimMsPDLimited; if (_azimMsPerDegree > -10 && _azimMsPerDegree < 10) if (_azimMsPerDegree > 0) @@ -188,6 +194,7 @@ int32_t AatModule::calcProjectedAzim(uint32_t now) azimMsPDLimited = _azimMsPerDegree; int32_t target = (((int32_t)elapsed / azimMsPDLimited) + _targetAzim + 360) % 360; + //DBGLN("%u t=%d p=%d", elapsed, _targetAzim, target); return target; } @@ -233,7 +240,7 @@ void AatModule::displayIdle(uint32_t now) #endif } -void AatModule::displayActive(uint32_t now, int32_t projectedAzim, int32_t usElev, int32_t usAzim) +void AatModule::displayActive(uint32_t now, int32_t projectedAzim) { #if defined(PIN_OLED_SDA) // El:[deg] [alt]m @@ -265,7 +272,7 @@ void AatModule::displayActive(uint32_t now, int32_t projectedAzim, int32_t usEle _display.printf("%um\n", _targetDistance); // XXm } - _display.printf("Se:%4dus\nSa:%4dus\n", usElev, usAzim); + _display.printf("Se:%4dus\nSa:%4dus\n", _servoUs[IDX_ELEV], _servoUs[IDX_AZIM]); displayGpsIntervalBar(now); _display.display(); #endif @@ -291,39 +298,47 @@ void AatModule::servoUpdate(uint32_t now) _lastServoUpdateMs = now; int32_t projectedAzim = calcProjectedAzim(now); - - // Smooth the updates and scale to degrees * 100 - const int8_t servoSmoothFactor = config.GetAatServoSmooth(); - _currentAzim = ((_currentAzim * servoSmoothFactor) + (projectedAzim * (10 - servoSmoothFactor) * 100)) / 10; - _currentElev = ((_currentElev * servoSmoothFactor) + (_targetElev * (10 - servoSmoothFactor) * 100)) / 10; - - int32_t transformedAzim = _currentAzim; - int32_t transformedElev = _currentElev; + int32_t transformedAzim = projectedAzim; + int32_t transformedElev = _targetElev; // 90-270 azim inverts the elevation servo - // if (transformedAzim > 18000) + // if (transformedAzim > 180) // { - // transformedElev = 18000 - transformedElev; - // transformedAzim = transformedAzim - 18000; + // transformedElev = 180 - transformedElev; + // transformedAzim = transformedAzim - 180; // } // For straight geared servos with 180 degree elev flip - // int32_t usAzim = map(transformedAzim, 0, 18000, 500, 2500); - // int32_t usElev = map(transformedElev, 0, 18000, 500, 2500); + // int32_t usAzim = map(transformedAzim, 0, 180, 500, 2500); + // int32_t usElev = map(transformedElev, 0, 180, 500, 2500); // For 1:2 gearing on the azim servo to allow 360 rotation // For Elev servos that only go 0-90 and the azim does 360 - transformedAzim = (transformedAzim + 18000) % 36000; // convert so 0 maps to 1500us - int32_t usAzim = map(transformedAzim, 0, 36000, config.GetAatServoLowAzim(), config.GetAatServoHighAzim()); - int32_t usElev = map(transformedElev, 0, 9000, config.GetAatServoLowElev(), config.GetAatServoHighElev()); + transformedAzim = (transformedAzim + 180) % 360; // convert so 0 maps to 1500us + int32_t newServoPos[IDX_COUNT]; + newServoPos[IDX_AZIM] = map(transformedAzim, 0, 360, config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM)); + newServoPos[IDX_ELEV] = map(transformedElev, 0, 90, config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV)); + + for (uint32_t idx=IDX_AZIM; idx 80) + _servoUs[idx] = newServoPos[idx]; + else + _servoUs[idx] += diff / (config.GetAatServoSmooth() + 1); + } + //DBGLN("t=%u pro=%d us=%d smoo=%d", _targetAzim, projectedAzim, newServoPos[IDX_AZIM], _servoUs[IDX_AZIM]); #if defined(PIN_SERVO_AZIM) - _servo_Azim.writeMicroseconds(usAzim); + _servo_Azim.writeMicroseconds(_servoUs[IDX_AZIM]); #endif #if defined(PIN_SERVO_ELEV) - _servo_Elev.writeMicroseconds(usElev); + _servo_Elev.writeMicroseconds(_servoUs[IDX_ELEV]); #endif - displayActive(now, projectedAzim, usElev, usAzim); + displayActive(now, projectedAzim); } void AatModule::onCrsfPacketIn(const crsf_header_t *pkt) diff --git a/src/module_aat.h b/src/module_aat.h index 3b6d5b8..ac248f4 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -30,10 +30,11 @@ class AatModule : public CrsfModuleBase void SendGpsTelemetry(crsf_packet_gps_t *packet); bool isHomeSet() const { return _home.lat != 0 || _home.lon != 0; } - bool isGpsActive() const { return _gpsLast.lat != 0 || _gpsLast.lon != 0 || _gpsLast.satcnt > 0; }; + bool isGpsActive() const { return _gpsLast.lastUpdateMs != 0; }; protected: virtual void onCrsfPacketIn(const crsf_header_t *pkt); private: + enum tagServoIndex { IDX_AZIM, IDX_ELEV, IDX_COUNT }; void displayInit(); void updateGpsInterval(uint32_t interval); uint8_t calcGpsIntervalPct(uint32_t now); @@ -41,7 +42,7 @@ class AatModule : public CrsfModuleBase void processGps(uint32_t now); void servoUpdate(uint32_t now); void displayIdle(uint32_t now); - void displayActive(uint32_t now, int32_t projectedAzim, int32_t usElev, int32_t usAzim); + void displayActive(uint32_t now, int32_t projectedAzim); void displayGpsIntervalBar(uint32_t now); struct { @@ -66,8 +67,7 @@ class AatModule : public CrsfModuleBase uint16_t _targetAzim; // degrees uint8_t _targetElev; // degrees int32_t _azimMsPerDegree; // milliseconds per degree - int32_t _currentElev; // degrees * 100 - int32_t _currentAzim; // degrees * 100 + int32_t _servoUs[IDX_COUNT]; // smoothed azim servo output #if defined(PIN_SERVO_AZIM) Servo _servo_Azim; diff --git a/src/module_crsf.h b/src/module_crsf.h index 7b299d3..7ecbb77 100644 --- a/src/module_crsf.h +++ b/src/module_crsf.h @@ -9,7 +9,8 @@ class CrsfModuleBase : public ModuleBase public: CrsfModuleBase() = delete; CrsfModuleBase(Stream &port) : - _port(port), _crc(CRSF_CRC_POLY) + _port(port), _crc(CRSF_CRC_POLY), + _rxBufPos(0) {}; void Loop(uint32_t now); diff --git a/targets/aat.ini b/targets/aat.ini index 9211b30..9f48604 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -15,12 +15,12 @@ build_flags = -D LED_INVERTED -D PIN_SERVO_AZIM=9 -D PIN_SERVO_ELEV=10 - -D PIN_OLED_SDA=1 - -D PIN_OLED_SCL=3 + -D PIN_OLED_SCL=1 + -D PIN_OLED_SDA=3 ; -D PIN_SERVO_AZIM=1 ; -D PIN_SERVO_ELEV=3 - ; -D PIN_OLED_SDA=9 - ; -D PIN_OLED_SCL=10 + ; -D PIN_OLED_SCL=9 + ; -D PIN_OLED_SDA=10 ; -D DEBUG_LOG lib_ignore = STM32UPDATE lib_deps = From 0a6d1a39e493f9bdafe53f9163749c5d65506a66 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Fri, 29 Dec 2023 14:13:59 -0500 Subject: [PATCH 13/29] Store servopos in us*100 for rounding --- src/module_aat.cpp | 42 +++++++++++++++++------------------------- src/module_aat.h | 2 +- targets/aat.ini | 2 -- 3 files changed, 18 insertions(+), 28 deletions(-) diff --git a/src/module_aat.cpp b/src/module_aat.cpp index 0c94519..be6fce3 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -57,7 +57,7 @@ AatModule::AatModule(Stream &port) : CrsfModuleBase(port), _gpsLast{0}, _home{0}, _gpsAvgUpdateInterval(0), _lastServoUpdateMs(0), _targetDistance(0), _targetAzim(0), _targetElev(0), _azimMsPerDegree(0), - _servoUs{0} + _servoPos{0} #if defined(PIN_SERVO_AZIM) , _servo_Azim() #endif @@ -78,12 +78,14 @@ void AatModule::Init() Serial.end(); #endif #if defined(PIN_SERVO_AZIM) - _servoUs[IDX_AZIM] = (config.GetAatServoLow(IDX_AZIM) + config.GetAatServoHigh(IDX_AZIM)) / 2; - _servo_Azim.attach(PIN_SERVO_AZIM, 500, 2500, _servoUs[IDX_AZIM]); + _servoPos[IDX_AZIM] = (config.GetAatServoLow(IDX_AZIM) + config.GetAatServoHigh(IDX_AZIM)) / 2; + _servo_Azim.attach(PIN_SERVO_AZIM, 500, 2500, _servoPos[IDX_AZIM]); + _servoPos[IDX_AZIM] *= 100; #endif #if defined(PIN_SERVO_ELEV) - _servoUs[IDX_ELEV] = (config.GetAatServoLow(IDX_ELEV) + config.GetAatServoHigh(IDX_ELEV)) / 2; - _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500, _servoUs[IDX_ELEV]); + _servoPos[IDX_ELEV] = (config.GetAatServoLow(IDX_ELEV) + config.GetAatServoHigh(IDX_ELEV)) / 2; + _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500, _servoPos[IDX_ELEV]); + _servoPos[IDX_ELEV] *= 100; #endif displayInit(); ModuleBase::Init(); @@ -272,7 +274,7 @@ void AatModule::displayActive(uint32_t now, int32_t projectedAzim) _display.printf("%um\n", _targetDistance); // XXm } - _display.printf("Se:%4dus\nSa:%4dus\n", _servoUs[IDX_ELEV], _servoUs[IDX_AZIM]); + _display.printf("Se:%4dus\nSa:%4dus\n", _servoPos[IDX_ELEV]/100, _servoPos[IDX_AZIM]/100); displayGpsIntervalBar(now); _display.display(); #endif @@ -301,41 +303,31 @@ void AatModule::servoUpdate(uint32_t now) int32_t transformedAzim = projectedAzim; int32_t transformedElev = _targetElev; - // 90-270 azim inverts the elevation servo - // if (transformedAzim > 180) - // { - // transformedElev = 180 - transformedElev; - // transformedAzim = transformedAzim - 180; - // } - // For straight geared servos with 180 degree elev flip - // int32_t usAzim = map(transformedAzim, 0, 180, 500, 2500); - // int32_t usElev = map(transformedElev, 0, 180, 500, 2500); - // For 1:2 gearing on the azim servo to allow 360 rotation // For Elev servos that only go 0-90 and the azim does 360 transformedAzim = (transformedAzim + 180) % 360; // convert so 0 maps to 1500us int32_t newServoPos[IDX_COUNT]; - newServoPos[IDX_AZIM] = map(transformedAzim, 0, 360, config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM)); - newServoPos[IDX_ELEV] = map(transformedElev, 0, 90, config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV)); + newServoPos[IDX_AZIM] = 100 * map(transformedAzim, 0, 360, config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM)); + newServoPos[IDX_ELEV] = 100 * map(transformedElev, 0, 90, config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV)); for (uint32_t idx=IDX_AZIM; idx 80) - _servoUs[idx] = newServoPos[idx]; + _servoPos[idx] = newServoPos[idx]; else - _servoUs[idx] += diff / (config.GetAatServoSmooth() + 1); + _servoPos[idx] += diff / (config.GetAatServoSmooth() + 1); } - //DBGLN("t=%u pro=%d us=%d smoo=%d", _targetAzim, projectedAzim, newServoPos[IDX_AZIM], _servoUs[IDX_AZIM]); + //DBGLN("t=%u pro=%d us=%d smoo=%d", _targetAzim, projectedAzim, newServoPos[IDX_AZIM]/100, _servoPos[IDX_AZIM]/100); #if defined(PIN_SERVO_AZIM) - _servo_Azim.writeMicroseconds(_servoUs[IDX_AZIM]); + _servo_Azim.writeMicroseconds(_servoPos[IDX_AZIM]/100); #endif #if defined(PIN_SERVO_ELEV) - _servo_Elev.writeMicroseconds(_servoUs[IDX_ELEV]); + _servo_Elev.writeMicroseconds(_servoPos[IDX_ELEV]/100); #endif displayActive(now, projectedAzim); diff --git a/src/module_aat.h b/src/module_aat.h index ac248f4..78e9100 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -67,7 +67,7 @@ class AatModule : public CrsfModuleBase uint16_t _targetAzim; // degrees uint8_t _targetElev; // degrees int32_t _azimMsPerDegree; // milliseconds per degree - int32_t _servoUs[IDX_COUNT]; // smoothed azim servo output + int32_t _servoPos[IDX_COUNT]; // smoothed azim servo output us * 100 #if defined(PIN_SERVO_AZIM) Servo _servo_Azim; diff --git a/targets/aat.ini b/targets/aat.ini index 9f48604..efde9eb 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -17,8 +17,6 @@ build_flags = -D PIN_SERVO_ELEV=10 -D PIN_OLED_SCL=1 -D PIN_OLED_SDA=3 - ; -D PIN_SERVO_AZIM=1 - ; -D PIN_SERVO_ELEV=3 ; -D PIN_OLED_SCL=9 ; -D PIN_OLED_SDA=10 ; -D DEBUG_LOG From 0c2c96ffef3d4af3fe28e8f51bfffb5b27c50fc7 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 3 Jan 2024 16:05:18 -0500 Subject: [PATCH 14/29] Add VBat, smoothing is now dps, new tracking screen --- lib/config/config.cpp | 5 +- lib/config/config.h | 8 ++ src/module_aat.cpp | 284 +++++++++++++++++++++++++++++++++--------- src/module_aat.h | 31 ++++- 4 files changed, 264 insertions(+), 64 deletions(-) diff --git a/lib/config/config.cpp b/lib/config/config.cpp index aa15c4d..f336b4d 100644 --- a/lib/config/config.cpp +++ b/lib/config/config.cpp @@ -141,11 +141,14 @@ VrxBackpackConfig::SetDefaults() #if defined(AAT_BACKPACK) m_config.aat.satelliteHomeMin = 5; m_config.aat.project = 0xff; - m_config.aat.servoSmooth = 9; + m_config.aat.servoSmooth = 5; m_config.aat.servoEndpoints[0].low = 700; // AZIM m_config.aat.servoEndpoints[0].high = 2400; m_config.aat.servoEndpoints[1].low = 1100; // ELEV m_config.aat.servoEndpoints[1].high = 1900; + + m_config.vbat.scale = 410; + m_config.vbat.offset = 12; #endif m_modified = true; diff --git a/lib/config/config.h b/lib/config/config.h index 7fabdb4..17749f7 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -71,6 +71,11 @@ typedef struct { uint16_t high; } servoEndpoints[2]; // us endpoints for servos } aat; + + struct __attribute__((packed)) tagVbatConfig { + uint16_t scale; + int16_t offset; + } vbat; #endif } vrx_backpack_config_t; @@ -103,6 +108,9 @@ class VrxBackpackConfig uint8_t GetAatProject() const { return m_config.aat.project; } uint16_t GetAatServoLow(uint8_t idx) const { return m_config.aat.servoEndpoints[idx].low; } uint16_t GetAatServoHigh(uint8_t idx) const { return m_config.aat.servoEndpoints[idx].high; } + + uint16_t GetVbatScale() const { return m_config.vbat.scale; } + int16_t GetVBatOffset() const { return m_config.vbat.offset; } #endif private: diff --git a/src/module_aat.cpp b/src/module_aat.cpp index be6fce3..5111550 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -11,6 +11,8 @@ #define RAD2DEG(rad) ((rad) * 180.0 / M_PI) #define DELAY_IDLE (20U) // sleep used when not tracking #define DELAY_FIRST_UPDATE (5000U) // absolute delay before first servo update +#define FONT_W (6) // Actually 5x7 + 1 pixel space +#define FONT_H (8) static void calcDistAndAzimuth(int32_t srcLat, int32_t srcLon, int32_t dstLat, int32_t dstLon, uint32_t *out_dist, uint32_t *out_azimuth) @@ -53,11 +55,40 @@ static int32_t calcElevation(uint32_t distance, int32_t altitude) return RAD2DEG(atan2(altitude, distance)); } +VbatSampler::VbatSampler() : + _lastUpdate(0), _value(0), _sum(0), _cnt(0) +{ +} + +void VbatSampler::update(uint32_t now) +{ + if (_lastUpdate != 0 && now - _lastUpdate < VBAT_UPDATE_INTERVAL) + return; + _lastUpdate = now; + + uint32_t adc = analogRead(A0); + _sum += adc; + ++_cnt; + + if (_cnt == VBAT_UPDATE_COUNT) + { + adc = _sum / _cnt; + // For negative offsets, anything between abs(OFFSET) and 0 is considered 0 + if (config.GetVBatOffset() < 0 && (int32_t)adc <= -config.GetVBatOffset()) + _value = 0; + else + _value = ((int32_t)adc - config.GetVBatOffset()) * 100 / config.GetVbatScale(); + + _sum = 0; + _cnt = 0; + } +} + AatModule::AatModule(Stream &port) : CrsfModuleBase(port), _gpsLast{0}, _home{0}, - _gpsAvgUpdateInterval(0), _lastServoUpdateMs(0), _targetDistance(0), + _gpsAvgUpdateIntervalMs(0), _lastServoUpdateMs(0), _targetDistance(0), _targetAzim(0), _targetElev(0), _azimMsPerDegree(0), - _servoPos{0} + _servoPos{0}, _lastAzimFlipMs(0) #if defined(PIN_SERVO_AZIM) , _servo_Azim() #endif @@ -77,17 +108,17 @@ void AatModule::Init() // Need to call _port's end but it is a stream reference not the HardwareSerial Serial.end(); #endif -#if defined(PIN_SERVO_AZIM) _servoPos[IDX_AZIM] = (config.GetAatServoLow(IDX_AZIM) + config.GetAatServoHigh(IDX_AZIM)) / 2; +#if defined(PIN_SERVO_AZIM) _servo_Azim.attach(PIN_SERVO_AZIM, 500, 2500, _servoPos[IDX_AZIM]); - _servoPos[IDX_AZIM] *= 100; #endif -#if defined(PIN_SERVO_ELEV) _servoPos[IDX_ELEV] = (config.GetAatServoLow(IDX_ELEV) + config.GetAatServoHigh(IDX_ELEV)) / 2; +#if defined(PIN_SERVO_ELEV) _servo_Elev.attach(PIN_SERVO_ELEV, 500, 2500, _servoPos[IDX_ELEV]); - _servoPos[IDX_ELEV] *= 100; #endif +#if defined(PIN_OLED_SDA) displayInit(); +#endif ModuleBase::Init(); } @@ -108,23 +139,21 @@ void AatModule::SendGpsTelemetry(crsf_packet_gps_t *packet) void AatModule::updateGpsInterval(uint32_t interval) { - // Avg is in ms * 100 - interval *= 100; // Low pass filter. Note there is no fast init of the average, so it will take some time to grow // this prevents overprojection caused by the first update after setting home - _gpsAvgUpdateInterval += ((int32_t)interval - (int32_t)_gpsAvgUpdateInterval) / 4; + _gpsAvgUpdateIntervalMs += ((int32_t)interval - (int32_t)_gpsAvgUpdateIntervalMs) / 4; // Limit the maximum interval to provent projecting for too long - const uint32_t GPS_UPDATE_INTERVAL_MAX = (10U * 1000U * 100U); - if (_gpsAvgUpdateInterval > GPS_UPDATE_INTERVAL_MAX) - _gpsAvgUpdateInterval = GPS_UPDATE_INTERVAL_MAX; + const uint32_t GPS_UPDATE_INTERVAL_MAX = (10U * 1000U); + if (_gpsAvgUpdateIntervalMs > GPS_UPDATE_INTERVAL_MAX) + _gpsAvgUpdateIntervalMs = GPS_UPDATE_INTERVAL_MAX; } uint8_t AatModule::calcGpsIntervalPct(uint32_t now) { - if (_gpsAvgUpdateInterval) + if (_gpsAvgUpdateIntervalMs) { - return constrain((now - _gpsLast.lastUpdateMs) * (100U * 100U) / (uint32_t)_gpsAvgUpdateInterval, 0U, 100U); + return constrain((now - _gpsLast.lastUpdateMs) * 100U / (uint32_t)_gpsAvgUpdateIntervalMs, 0U, 100U); } return 0; @@ -169,7 +198,7 @@ void AatModule::processGps(uint32_t now) // azimDelta is the azimuth change since the last packet, -180 to +180 int32_t azimDelta = (azimuth - _targetAzim + 540) % 360 - 180; _azimMsPerDegree = (azimDelta == 0) ? 0 : (int32_t)interval / azimDelta; - DBGLN("%d delta in %ums, %dms/d %uavg", azimDelta, interval, _azimMsPerDegree, _gpsAvgUpdateInterval); + DBGLN("%d delta in %ums, %dms/d %uavg", azimDelta, interval, _azimMsPerDegree, _gpsAvgUpdateIntervalMs); } _targetDistance = distance; @@ -181,9 +210,9 @@ int32_t AatModule::calcProjectedAzim(uint32_t now) { // Attempt to do a linear projection of the last // If enabled, we know the GPS update rate, the azimuth has changed, and more than a few meters away - if (config.GetAatProject() && _gpsAvgUpdateInterval && _azimMsPerDegree && _targetDistance > 3) + if (config.GetAatProject() && _gpsAvgUpdateIntervalMs && _azimMsPerDegree && _targetDistance > 3) { - uint32_t elapsed = constrain(now - _gpsLast.lastUpdateMs, 0U, _gpsAvgUpdateInterval / 100U); + uint32_t elapsed = constrain(now - _gpsLast.lastUpdateMs, 0U, _gpsAvgUpdateIntervalMs); // Prevent excessive rotational velocity (100 degrees per second / 10ms per degree) int32_t azimMsPDLimited; @@ -203,27 +232,28 @@ int32_t AatModule::calcProjectedAzim(uint32_t now) return _targetAzim; } +#if defined(PIN_OLED_SDA) void AatModule::displayInit() { -#if defined(PIN_OLED_SDA) Wire.begin(PIN_OLED_SDA, PIN_OLED_SCL); _display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); + _display.setTextWrap(false); _display.setTextSize(2); _display.setTextColor(SSD1306_WHITE); _display.setCursor(0, 0); if (connectionState == binding) _display.print("Bind\nmode...\n\n"); + else if (connectionState == wifiUpdate) + _display.print("Wifi\nmode...\n\n"); else _display.print("AAT\nBackpack\n\n"); _display.setTextSize(1); _display.print(VERSION); _display.display(); -#endif } void AatModule::displayIdle(uint32_t now) { -#if defined(PIN_OLED_SDA) // A screen with just the GPS position, sat count, and interval bar _display.clearDisplay(); _display.setCursor(0, 0); @@ -239,58 +269,169 @@ void AatModule::displayIdle(uint32_t now) displayGpsIntervalBar(now); _display.display(); -#endif } -void AatModule::displayActive(uint32_t now, int32_t projectedAzim) +void AatModule::displayAzimuth(int32_t projectedAzim) { -#if defined(PIN_OLED_SDA) - // El:[deg] [alt]m - // Az:[deg] [dist] - // Se:[servo elev]us - // Sa:[servo azim]us - // With interval bar - _display.clearDisplay(); - _display.setTextSize(2); - _display.setCursor(0, 0) - ; - _display.printf("El:%02u %dm\nAz:%03u ", _targetElev, - constrain(_gpsLast.altitude - _home.alt, -99, 999), - projectedAzim); - - // Target distance has variable width/height but all fits in 3x1 (doublesized) characters - if (_targetDistance > 999) + int32_t y = SCREEN_HEIGHT - FONT_H + 1 - 6; + + // start with horizon line with arrow and unprojected azimuth + // |-----/\---|-| + _display.drawFastVLine(0, y, 5, SSD1306_WHITE); + _display.drawFastHLine(0, y+2, SCREEN_WIDTH-1, SSD1306_WHITE); + _display.drawFastVLine(SCREEN_WIDTH-1, y, 5, SSD1306_WHITE); + int32_t azimPos = map((_targetAzim + 180) % 360, 0, 360, 0, SCREEN_WIDTH); + _display.drawFastVLine(azimPos, y+1, 3, SSD1306_WHITE); + // Projected Azimuth arrow + azimPos = map((projectedAzim + 180) % 360, 0, 360, 0, SCREEN_WIDTH); + const uint8_t oledim_arrowup[] = { 0x10, 0x38, 0x7c }; // 'arrowup', 7x3px + _display.drawBitmap(azimPos-7/2, y+1, oledim_arrowup, 7, 3, SSD1306_WHITE, SSD1306_BLACK); + + // S W N E S under that + y += 6; + _display.drawChar(0, y, 'S', SSD1306_WHITE, SSD1306_BLACK, 1); + _display.drawChar(SCREEN_WIDTH-FONT_W+1, y, 'S', SSD1306_WHITE, SSD1306_BLACK, 1); + _display.drawChar(SCREEN_WIDTH/4-FONT_W/2, y, 'W', SSD1306_WHITE, SSD1306_BLACK, 1); + _display.drawChar(SCREEN_WIDTH/2-FONT_W/2+1, y, 'N', SSD1306_WHITE, SSD1306_BLACK, 1); + _display.drawChar(3*SCREEN_WIDTH/4-FONT_W/2, y, 'E', SSD1306_WHITE, SSD1306_BLACK, 1); + + // Projected azim value as 3 digit number on a white background (max 2px each side) + y -= 2; + uint32_t azimStart = constrain(azimPos-((3*FONT_W)/2), -2, SCREEN_WIDTH-(3*FONT_W)-2); + _display.fillRoundRect(azimStart, y, 3*FONT_W + 3, FONT_H + 1, 2, SSD1306_WHITE); + _display.setTextSize(1); + _display.setTextColor(SSD1306_BLACK); + _display.setCursor(azimStart+2, y+1); + _display.printf("%03u", projectedAzim); +} + +void AatModule::displayTargetDistance(int32_t azimPos, int32_t elevPos) +{ + if (_targetDistance == 0) + return; + + // Target distance over the X: 9m, 99m, 999m, 9.9k, 99.9k, 999k + char dist[16]; + if (_targetDistance > 99999) // > 99.9km = "xxxk" can you believe that?! so far. { - _display.setTextSize(1); - _display.printf("%u.%03u\nkm\n", _targetDistance / 1000, (_targetDistance % 1000)); // X.XXX km small font - _display.setTextSize(2); + snprintf(dist, sizeof(dist), "%3uk", _targetDistance / 1000); } - else if (_targetDistance > 99) + else if (_targetDistance > 999) // >999m = "xx.xk" { - _display.printf("%u\n", _targetDistance); // XXX + snprintf(dist, sizeof(dist), "%u.%uk", _targetDistance / 1000, (_targetDistance % 1000) / 100); } - else + else // "xxxm" { - _display.printf("%um\n", _targetDistance); // XXm + snprintf(dist, sizeof(dist), "%um", _targetDistance); } - _display.printf("Se:%4dus\nSa:%4dus\n", _servoPos[IDX_ELEV]/100, _servoPos[IDX_AZIM]/100); + // If elevation is low, put distance over the pip, if elev high put the distance below it + // and allow the units to go off the screen to the right if needed + int32_t strWidth = strlen(dist) * FONT_W; + int32_t distX = constrain(azimPos - (strWidth/2) + 1, 0, SCREEN_WIDTH + FONT_W - strWidth); + int32_t distY = (_targetElev < 50) ? elevPos - FONT_H - 5 : elevPos + 6; + _display.setTextColor(SSD1306_WHITE); + _display.setCursor(distX, distY); + _display.write(dist); +} + +void AatModule::displayTargetCircle(int32_t projectedAzim) +{ + const int32_t elevH = 33; + const int32_t elevOff = 16; + // Dotted line to separate top of screen from tracking area + for (int32_t x=0; x (3*_gpsAvgUpdateIntervalMs)) { + // Inverse "OLD" in the center of where the update bar would be + _display.fillRoundRect(80, 0, SCREEN_WIDTH-1-80, FONT_H-1, 2, SSD1306_WHITE); + _display.setTextSize(1); + _display.setTextColor(SSD1306_BLACK); + _display.setCursor(104 - (3*FONT_W)/2, 0); + _display.write("OLD"); + } + else + { + // A depleting bar going from screen center to right uint8_t gpsIntervalPct = calcGpsIntervalPct(now); - uint8_t pxHeight = SCREEN_HEIGHT * (100U - gpsIntervalPct) / 100U; - _display.fillRect(SCREEN_WIDTH - 3, SCREEN_HEIGHT - pxHeight, 2, pxHeight, SSD1306_WHITE); + uint8_t pxWidth = SCREEN_WIDTH/2 * (100U - gpsIntervalPct) / 100U; + _display.fillRect(SCREEN_WIDTH-pxWidth, 0, pxWidth, 2, SSD1306_WHITE); } -#endif } +#endif /* defined(PIN_OLED_SDA) */ void AatModule::servoUpdate(uint32_t now) { @@ -307,30 +448,48 @@ void AatModule::servoUpdate(uint32_t now) // For Elev servos that only go 0-90 and the azim does 360 transformedAzim = (transformedAzim + 180) % 360; // convert so 0 maps to 1500us int32_t newServoPos[IDX_COUNT]; - newServoPos[IDX_AZIM] = 100 * map(transformedAzim, 0, 360, config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM)); - newServoPos[IDX_ELEV] = 100 * map(transformedElev, 0, 90, config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV)); + newServoPos[IDX_AZIM] = map(transformedAzim, 0, 360, config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM)); + newServoPos[IDX_ELEV] = map(transformedElev, 0, 90, config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV)); for (uint32_t idx=IDX_AZIM; idx 80) - _servoPos[idx] = newServoPos[idx]; + if (idx == IDX_AZIM && (abs(diff) * 100 / range) > 80) + { + // Prevent the servo from flipping back and forth around the 180 point + // by only allowing 1 flip ever Xms. Just keep pushing toward the limit + const uint32_t AZIM_FLIP_MIN_DELAY = 2000U; + if (now - _lastAzimFlipMs > AZIM_FLIP_MIN_DELAY) + { + _servoPos[idx] = newServoPos[idx]; + _lastAzimFlipMs = now; + } + else if (diff < 0) + _servoPos[idx] = constrain(_servoPos[idx] + maxDiff, _servoPos[idx], config.GetAatServoHigh(idx)); + else + _servoPos[idx] = constrain(_servoPos[idx] - maxDiff, config.GetAatServoLow(idx), _servoPos[idx]); + } else - _servoPos[idx] += diff / (config.GetAatServoSmooth() + 1); + _servoPos[idx] += constrain(diff, -maxDiff, maxDiff); } - //DBGLN("t=%u pro=%d us=%d smoo=%d", _targetAzim, projectedAzim, newServoPos[IDX_AZIM]/100, _servoPos[IDX_AZIM]/100); + //DBGLN("t=%u pro=%d us=%d smoo=%d", _targetAzim, projectedAzim, newServoPos[IDX_AZIM], _servoPos[IDX_AZIM]); #if defined(PIN_SERVO_AZIM) - _servo_Azim.writeMicroseconds(_servoPos[IDX_AZIM]/100); + _servo_Azim.writeMicroseconds(_servoPos[IDX_AZIM]); #endif #if defined(PIN_SERVO_ELEV) - _servo_Elev.writeMicroseconds(_servoPos[IDX_ELEV]/100); + _servo_Elev.writeMicroseconds(_servoPos[IDX_ELEV]); #endif +#if defined(PIN_OLED_SDA) displayActive(now, projectedAzim); +#endif } void AatModule::onCrsfPacketIn(const crsf_header_t *pkt) @@ -345,6 +504,7 @@ void AatModule::onCrsfPacketIn(const crsf_header_t *pkt) void AatModule::Loop(uint32_t now) { processGps(now); + _vbat.update(now); if (isHomeSet() && now > DELAY_FIRST_UPDATE) { @@ -352,8 +512,10 @@ void AatModule::Loop(uint32_t now) } else { +#if defined(PIN_OLED_SDA) if (isGpsActive()) displayIdle(now); +#endif delay(DELAY_IDLE); } diff --git a/src/module_aat.h b/src/module_aat.h index 78e9100..3b8a846 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -19,6 +19,23 @@ #define SCREEN_ADDRESS 0x3C #endif +class VbatSampler +{ +public: + VbatSampler(); + + void update(uint32_t now); + uint32_t value() const { return 1250; } //_value; } +private: + const uint32_t VBAT_UPDATE_INTERVAL = 100U; + const uint32_t VBAT_UPDATE_COUNT = 5U; + + uint32_t _lastUpdate; + uint32_t _value; + uint32_t _sum; // in progress sum/count + uint32_t _cnt; +}; + class AatModule : public CrsfModuleBase { public: @@ -41,9 +58,17 @@ class AatModule : public CrsfModuleBase int32_t calcProjectedAzim(uint32_t now); void processGps(uint32_t now); void servoUpdate(uint32_t now); + +#if defined(PIN_OLED_SDA) void displayIdle(uint32_t now); void displayActive(uint32_t now, int32_t projectedAzim); void displayGpsIntervalBar(uint32_t now); + void displayAzimuth(int32_t projectedAzim); + void displayTargetDistance(int32_t azimPos, int32_t elevPos); + void displayTargetCircle(int32_t projectedAzim); + void displayAltitude(); + void displayVBat(); +#endif struct { int32_t lat; @@ -60,14 +85,16 @@ class AatModule : public CrsfModuleBase int32_t lon; int32_t alt; } _home; - uint32_t _gpsAvgUpdateInterval; // ms * 100 + uint32_t _gpsAvgUpdateIntervalMs; // Servo Position uint32_t _lastServoUpdateMs; uint32_t _targetDistance; // meters uint16_t _targetAzim; // degrees uint8_t _targetElev; // degrees int32_t _azimMsPerDegree; // milliseconds per degree - int32_t _servoPos[IDX_COUNT]; // smoothed azim servo output us * 100 + int32_t _servoPos[IDX_COUNT]; // smoothed azim servo output us + uint32_t _lastAzimFlipMs; + VbatSampler _vbat; #if defined(PIN_SERVO_AZIM) Servo _servo_Azim; From 167e0b1817326aacfb2aab712e4c75f06a3a1710 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 3 Jan 2024 16:41:47 -0500 Subject: [PATCH 15/29] Reserve units config --- lib/config/config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/config/config.h b/lib/config/config.h index 17749f7..30972a4 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -70,6 +70,7 @@ typedef struct { uint16_t low; uint16_t high; } servoEndpoints[2]; // us endpoints for servos + uint8_t units; // 0=meters, anything else=also meters :-D } aat; struct __attribute__((packed)) tagVbatConfig { From 0839bc348fa6e2d1bf95e12dfc7f8d2ac2254374 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 3 Jan 2024 16:42:35 -0500 Subject: [PATCH 16/29] Swap altitude and distance displays --- src/module_aat.cpp | 78 ++++++++++++++++++++++++++-------------------- src/module_aat.h | 4 +-- 2 files changed, 47 insertions(+), 35 deletions(-) diff --git a/src/module_aat.cpp b/src/module_aat.cpp index 5111550..bcf7e1b 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -290,10 +290,10 @@ void AatModule::displayAzimuth(int32_t projectedAzim) // S W N E S under that y += 6; _display.drawChar(0, y, 'S', SSD1306_WHITE, SSD1306_BLACK, 1); - _display.drawChar(SCREEN_WIDTH-FONT_W+1, y, 'S', SSD1306_WHITE, SSD1306_BLACK, 1); _display.drawChar(SCREEN_WIDTH/4-FONT_W/2, y, 'W', SSD1306_WHITE, SSD1306_BLACK, 1); _display.drawChar(SCREEN_WIDTH/2-FONT_W/2+1, y, 'N', SSD1306_WHITE, SSD1306_BLACK, 1); _display.drawChar(3*SCREEN_WIDTH/4-FONT_W/2, y, 'E', SSD1306_WHITE, SSD1306_BLACK, 1); + _display.drawChar(SCREEN_WIDTH-FONT_W+1, y, 'S', SSD1306_WHITE, SSD1306_BLACK, 1); // Projected azim value as 3 digit number on a white background (max 2px each side) y -= 2; @@ -305,34 +305,23 @@ void AatModule::displayAzimuth(int32_t projectedAzim) _display.printf("%03u", projectedAzim); } -void AatModule::displayTargetDistance(int32_t azimPos, int32_t elevPos) +void AatModule::displayAltitude(int32_t azimPos, int32_t elevPos) { - if (_targetDistance == 0) + int alt = constrain(_gpsLast.altitude - _home.alt, -99, 999); + if (alt == 0) return; - // Target distance over the X: 9m, 99m, 999m, 9.9k, 99.9k, 999k - char dist[16]; - if (_targetDistance > 99999) // > 99.9km = "xxxk" can you believe that?! so far. - { - snprintf(dist, sizeof(dist), "%3uk", _targetDistance / 1000); - } - else if (_targetDistance > 999) // >999m = "xx.xk" - { - snprintf(dist, sizeof(dist), "%u.%uk", _targetDistance / 1000, (_targetDistance % 1000) / 100); - } - else // "xxxm" - { - snprintf(dist, sizeof(dist), "%um", _targetDistance); - } + char buf[16]; + snprintf(buf, sizeof(buf), "%dm", alt); - // If elevation is low, put distance over the pip, if elev high put the distance below it + // If elevation is low, put alt over the pip, if elev high put the alt below it // and allow the units to go off the screen to the right if needed - int32_t strWidth = strlen(dist) * FONT_W; + int32_t strWidth = strlen(buf) * FONT_W; int32_t distX = constrain(azimPos - (strWidth/2) + 1, 0, SCREEN_WIDTH + FONT_W - strWidth); int32_t distY = (_targetElev < 50) ? elevPos - FONT_H - 5 : elevPos + 6; _display.setTextColor(SSD1306_WHITE); _display.setCursor(distX, distY); - _display.write(dist); + _display.write(buf); } void AatModule::displayTargetCircle(int32_t projectedAzim) @@ -366,7 +355,40 @@ void AatModule::displayTargetCircle(int32_t projectedAzim) _display.drawFastVLine(servoX-3, servoY-1, 3, SSD1306_WHITE); _display.drawFastVLine(servoX+3, servoY-1, 3, SSD1306_WHITE); - displayTargetDistance(azimPos, elevPos); + displayAltitude(azimPos, elevPos); +} + +void AatModule::displayTargetDistance() +{ + // Target distance over the X: 9m, 99m, 999m, 9.9k, 99.9k, 999k + char dist[16]; + const char *units; + if (_targetDistance > 99999) // >99.99m = "xxx.x" (5 chars) + { + snprintf(dist, sizeof(dist), "%u.%u", _targetDistance / 1000, (_targetDistance % 1000) / 100); + units = "km"; + } + else if (_targetDistance > 999) // >999m = "[x]x.xx" (4-5 chars) + { + snprintf(dist, sizeof(dist), "%u.%02u", _targetDistance / 1000, (_targetDistance % 1000) / 10); + units = "km"; + } + else // "[x]xx" (2-3 chars) + { + snprintf(dist, sizeof(dist), "%2u", _targetDistance); + units = "m"; + } + _display.setTextColor(SSD1306_WHITE); + _display.setTextSize(1); + _display.setCursor(0, 0); + _display.write("Dst"); + _display.setTextSize(2); + _display.setCursor(3 * FONT_W + 3, 0); + _display.printf(dist); + + _display.setTextSize(1); + _display.setCursor(_display.getCursorX() + 1, FONT_H); + _display.write(units); } void AatModule::displayVBat() @@ -386,16 +408,6 @@ void AatModule::displayVBat() _display.write(buf); } -void AatModule::displayAltitude() -{ - _display.setTextColor(SSD1306_WHITE); - _display.setTextSize(1); - _display.setCursor(0, 0); - _display.write("Alt "); - _display.setTextSize(2); - _display.printf("%3dm", constrain(_gpsLast.altitude - _home.alt, -99, 999)); -} - void AatModule::displayActive(uint32_t now, int32_t projectedAzim) { _display.clearDisplay(); @@ -403,7 +415,7 @@ void AatModule::displayActive(uint32_t now, int32_t projectedAzim) displayGpsIntervalBar(now); displayAzimuth(projectedAzim); displayTargetCircle(projectedAzim); - displayAltitude(); + displayTargetDistance(); displayVBat(); _display.display(); @@ -427,7 +439,7 @@ void AatModule::displayGpsIntervalBar(uint32_t now) { // A depleting bar going from screen center to right uint8_t gpsIntervalPct = calcGpsIntervalPct(now); - uint8_t pxWidth = SCREEN_WIDTH/2 * (100U - gpsIntervalPct) / 100U; + uint8_t pxWidth = (SCREEN_WIDTH-1-80) * (100U - gpsIntervalPct) / 100U; _display.fillRect(SCREEN_WIDTH-pxWidth, 0, pxWidth, 2, SSD1306_WHITE); } } diff --git a/src/module_aat.h b/src/module_aat.h index 3b8a846..a4b5321 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -64,9 +64,9 @@ class AatModule : public CrsfModuleBase void displayActive(uint32_t now, int32_t projectedAzim); void displayGpsIntervalBar(uint32_t now); void displayAzimuth(int32_t projectedAzim); - void displayTargetDistance(int32_t azimPos, int32_t elevPos); + void displayAltitude(int32_t azimPos, int32_t elevPos); void displayTargetCircle(int32_t projectedAzim); - void displayAltitude(); + void displayTargetDistance(); void displayVBat(); #endif From c2add6212fca919b32a3a62e3db6642ea0c625b8 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 3 Jan 2024 18:10:40 -0500 Subject: [PATCH 17/29] Maybe fix pio dependency on devWifi --- src/module_aat.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/module_aat.h b/src/module_aat.h index a4b5321..944e689 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -2,6 +2,7 @@ #if defined(AAT_BACKPACK) #include "config.h" +#include "devWifi.h" #include "module_crsf.h" #include "crsf_protocol.h" @@ -25,7 +26,7 @@ class VbatSampler VbatSampler(); void update(uint32_t now); - uint32_t value() const { return 1250; } //_value; } + uint32_t value() const { return _value + 1170; } private: const uint32_t VBAT_UPDATE_INTERVAL = 100U; const uint32_t VBAT_UPDATE_COUNT = 5U; From 2c890cdc275d2d1c067b322110609b2cce4e9000 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 3 Jan 2024 18:16:39 -0500 Subject: [PATCH 18/29] Add wifi to libdeps? --- targets/aat.ini | 1 + 1 file changed, 1 insertion(+) diff --git a/targets/aat.ini b/targets/aat.ini index efde9eb..c4abcbc 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -23,6 +23,7 @@ build_flags = lib_ignore = STM32UPDATE lib_deps = ${env.lib_deps} + WIFI adafruit/Adafruit SSD1306 @ 2.5.9 build_src_filter = ${common_env_data.build_src_filter} - - - - From 08a669fa47edd0bfa494578ff01c7d879faefcbb Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 3 Jan 2024 18:20:23 -0500 Subject: [PATCH 19/29] Hax us better, stronger, extern const char --- lib/WIFI/devWIFI.h | 1 - src/module_aat.cpp | 2 +- src/module_aat.h | 1 - 3 files changed, 1 insertion(+), 3 deletions(-) diff --git a/lib/WIFI/devWIFI.h b/lib/WIFI/devWIFI.h index 439632a..e4fa66d 100644 --- a/lib/WIFI/devWIFI.h +++ b/lib/WIFI/devWIFI.h @@ -4,6 +4,5 @@ #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266) extern device_t WIFI_device; -extern const char *VERSION; #define HAS_WIFI #endif \ No newline at end of file diff --git a/src/module_aat.cpp b/src/module_aat.cpp index bcf7e1b..867b9e6 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -2,7 +2,6 @@ #include "common.h" #include "module_aat.h" #include "logging.h" -#include "devWifi.h" #include #include @@ -248,6 +247,7 @@ void AatModule::displayInit() else _display.print("AAT\nBackpack\n\n"); _display.setTextSize(1); + extern const char *VERSION; // in devWifi / created during build _display.print(VERSION); _display.display(); } diff --git a/src/module_aat.h b/src/module_aat.h index 944e689..71235a9 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -2,7 +2,6 @@ #if defined(AAT_BACKPACK) #include "config.h" -#include "devWifi.h" #include "module_crsf.h" #include "crsf_protocol.h" From edc494b7899fef4cce3e3c20069316f00e775b34 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Wed, 3 Jan 2024 18:49:43 -0500 Subject: [PATCH 20/29] WIFI lib not needed --- targets/aat.ini | 1 - 1 file changed, 1 deletion(-) diff --git a/targets/aat.ini b/targets/aat.ini index c4abcbc..efde9eb 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -23,7 +23,6 @@ build_flags = lib_ignore = STM32UPDATE lib_deps = ${env.lib_deps} - WIFI adafruit/Adafruit SSD1306 @ 2.5.9 build_src_filter = ${common_env_data.build_src_filter} - - - - From a24ea5cd6dbaae60dedd1d059f92019d083f27e5 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Thu, 4 Jan 2024 10:50:58 -0500 Subject: [PATCH 21/29] Remove debug VBAT, vbat is x 10 --- src/module_aat.cpp | 10 ++-------- src/module_aat.h | 3 ++- targets/aat.ini | 2 -- 3 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/module_aat.cpp b/src/module_aat.cpp index 867b9e6..6ae4d0c 100644 --- a/src/module_aat.cpp +++ b/src/module_aat.cpp @@ -396,16 +396,10 @@ void AatModule::displayVBat() if (_vbat.value() == 0) return; - //_display.setTextColor(SSD1306_WHITE); - //_display.setTextSize(1); - //_display.setCursor(0, SCREEN_HEIGHT-FONT_H+1); // SCREEN_WIDTH-(5*FONT_W)+1 - char buf[16]; - snprintf(buf, sizeof(buf), "%2d.%01dV", _vbat.value() / 100, (_vbat.value() % 100) / 10); - _display.setTextColor(SSD1306_WHITE); _display.setTextSize(1); - _display.setCursor(SCREEN_WIDTH - strlen(buf)*FONT_W, FONT_H); - _display.write(buf); + _display.setCursor(SCREEN_WIDTH - 5*FONT_W, FONT_H); + _display.printf("%2d.%1dV", _vbat.value() / 10, _vbat.value() % 10); } void AatModule::displayActive(uint32_t now, int32_t projectedAzim) diff --git a/src/module_aat.h b/src/module_aat.h index 71235a9..86f3029 100644 --- a/src/module_aat.h +++ b/src/module_aat.h @@ -25,7 +25,8 @@ class VbatSampler VbatSampler(); void update(uint32_t now); - uint32_t value() const { return _value + 1170; } + // Reported in V * 10 + uint32_t value() const { return _value; } private: const uint32_t VBAT_UPDATE_INTERVAL = 100U; const uint32_t VBAT_UPDATE_COUNT = 5U; diff --git a/targets/aat.ini b/targets/aat.ini index efde9eb..bab4cf1 100644 --- a/targets/aat.ini +++ b/targets/aat.ini @@ -17,8 +17,6 @@ build_flags = -D PIN_SERVO_ELEV=10 -D PIN_OLED_SCL=1 -D PIN_OLED_SDA=3 - ; -D PIN_OLED_SCL=9 - ; -D PIN_OLED_SDA=10 ; -D DEBUG_LOG lib_ignore = STM32UPDATE lib_deps = From 7877931391910bbd639ea7601bf073f3f305c309 Mon Sep 17 00:00:00 2001 From: Bryan Mayland Date: Sat, 6 Jan 2024 15:26:25 -0500 Subject: [PATCH 22/29] Add config webui --- html/elrs.css | 7 ++ html/scan.js | 130 +++++++++++++++++++++++++++++--------- html/txbp_index.html | 8 +-- html/vrx_index.html | 112 +++++++++++++++++++++++++++++--- include/devwifi_proxies.h | 12 ++++ lib/WIFI/devWIFI.cpp | 42 ++++++++---- lib/config/config.cpp | 66 ++++++++++++++++--- lib/config/config.h | 23 +++++-- src/devwifi_proxy_aat.cpp | 69 ++++++++++++++++++++ src/module_aat.cpp | 85 +++++++++++++++++++++---- src/module_aat.h | 14 +++- 11 files changed, 480 insertions(+), 88 deletions(-) create mode 100644 include/devwifi_proxies.h create mode 100644 src/devwifi_proxy_aat.cpp diff --git a/html/elrs.css b/html/elrs.css index a684327..4e6502e 100644 --- a/html/elrs.css +++ b/html/elrs.css @@ -289,4 +289,11 @@ body, input, select, textarea { background: #88cef7; } + datalist { + display: flex; + flex-direction: row; + justify-content: space-between; + writing-mode: horizontal-tb; + width: 100%; +} /*==========================*/ \ No newline at end of file diff --git a/html/scan.js b/html/scan.js index 89ae10e..0934e92 100644 --- a/html/scan.js +++ b/html/scan.js @@ -1,41 +1,81 @@ -document.addEventListener("DOMContentLoaded", get_mode, false); -var scanTimer = undefined; +document.addEventListener("DOMContentLoaded", init, false); function _(el) { return document.getElementById(el); } -function get_mode() { - var json_url = 'mode.json'; - xmlhttp = new XMLHttpRequest(); - xmlhttp.onreadystatechange = function () { - if (this.readyState == 4 && this.status == 200) { - var data = JSON.parse(this.responseText); - if (data.mode==="STA") { - _('stamode').style.display = 'block'; - if (_('rtctab')) _('rtctab').style.display = 'block'; - _('ssid').textContent = data.ssid; - } else { - _('apmode').style.display = 'block'; - if (data.ssid) { - _('homenet').textContent = data.ssid; - } else { - _('connect').style.display = 'none'; - } - scanTimer = setInterval(get_networks, 2000); - } - if((!data.stm32 || data.stm32==="no") && _('tx_tab')) { - mui.tabs.activate('pane-justified-2'); - _('tx_tab').style.display = 'none'; - } - if(data['product-name']) _('product-name').textContent = data['product-name']; - } +function init() { + initAat(); + + // sends XMLHttpRequest, so do it last + initOptions(); +} + +function initAat() { + _('aatsubmit').addEventListener('click', callback('Update AAT Parameters', 'An error occurred changing values', '/aatconfig', + () => { return new URLSearchParams(new FormData(_('aatconfig'))); } + )); + _('azim_center').addEventListener('change', aatAzimCenterChanged); + document.querySelectorAll('.aatlive').forEach( + el => el.addEventListener('change', aatLineElementChanged) + ); +} + +function initOptions() { + const xmlhttp = new XMLHttpRequest(); + xmlhttp.onreadystatechange = function() { + if (this.readyState == 4 && this.status == 200) { + const data = JSON.parse(this.responseText); + updateConfig(data); + setTimeout(get_networks, 2000); + } }; - xmlhttp.open("POST", json_url, true); - xmlhttp.setRequestHeader("Content-type", "application/x-www-form-urlencoded"); + xmlhttp.open('GET', '/config', true); xmlhttp.send(); } +function updateConfig(data) { + let config = data.config; + if (config.mode==="STA") { + _('stamode').style.display = 'block'; + if (_('rtctab')) _('rtctab').style.display = 'table-cell'; + _('ssid').textContent = config.ssid; + } else { + _('apmode').style.display = 'block'; + if (config.ssid) { + _('homenet').textContent = config.ssid; + } else { + _('connect').style.display = 'none'; + } + } + if((!data.stm32 || data.stm32==="no") && _('tx_tab')) { + mui.tabs.activate('pane-justified-2'); + _('tx_tab').style.display = 'none'; + } + if(config['product_name']) _('product_name').textContent = config['product_name']; + + updateAatConfig(config); +} + +function updateAatConfig(config) +{ + if (!config.hasOwnProperty('aat')) + return; + _('aattab').style.display = 'table-cell'; + + // AAT + _('servosmoo').value = config.aat.servosmoo; + _('azim_center').value = config.aat.azim_center; + _('azim_min').value = config.aat.azim_min; + _('azim_max').value = config.aat.azim_max; + _('elev_min').value = config.aat.elev_min; + _('elev_max').value = config.aat.elev_max; + + // VBAT + _('vbat_offset').value = config.vbat.offset; + _('vbat_scale').value = config.vbat.scale; +} + function get_networks() { var json_url = 'networks.json'; xmlhttp = new XMLHttpRequest(); @@ -44,7 +84,6 @@ function get_networks() { var data = JSON.parse(this.responseText); _('loader').style.display = 'none'; autocomplete(_('network'), data); - clearInterval(scanTimer); } }; xmlhttp.open("POST", json_url, true); @@ -340,6 +379,37 @@ function callback(title, msg, url, getdata) { } } +function aatAzimCenterChanged() +{ + // Update the slider labels to represent the new orientation + let labels; + switch (parseInt(this.selectedIndex)) + { + default: /* fallthrough */ + case 0: labels = 'SWNES'; break; // N + case 1: labels = 'WNESW'; break; // E + case 2: labels = 'NESWN'; break; // S + case 3: labels = 'ESWNE'; break; // W + } + let markers = _('azim_markers'); + for (i=0; i
-

Welcome to your ExpressLRS update page
-

-

From here you can update your TX Backpack module with @PLATFORM@ firmware
-

- Firmware Rev. @VERSION@ +

ExpressLRS

+ TX Backpack
+ Firmware Rev. @VERSION@

diff --git a/html/vrx_index.html b/html/vrx_index.html index 2e35d51..dfb270b 100644 --- a/html/vrx_index.html +++ b/html/vrx_index.html @@ -10,17 +10,14 @@ -
- -

Welcome to your ExpressLRS
update page
-

-

From here you can update your VRx Backpack module with @PLATFORM@ firmware
-

- Firmware Rev. @VERSION@ -

-

+
+ +

ExpressLRS

+ VRx Backpack @PLATFORM@
+ Firmware Rev. @VERSION@ +
-
+
@@ -28,6 +25,7 @@

Welcome to your ExpressLRS
update page
  • Update
  • Network
    • +
    @@ -105,6 +103,100 @@

    RTC Update via NTP

    + +
    +
    +
    +
    Servo maximum speed + Use the slider to limit the maximum rotational speed of the servo. Note that when the azimuth servo + must flip from one side to the other, the servo always will move at full speed. + + + + + + + + + + + + + +
    +
    +
    +
    Azimuth servo + Enter the micrsecond (us) values for the min and max position of the horizontal servo, using the slider to test positions. + + + + + + + + +
    + + +
    +
    + + +
    +
    + + +
    +
    +
    Elevation servo + Enter the micrsecond (us) values for the min and max position of the vertical servo, using the slider to test positions. + + + + + + + + + + +
    + + +
    +
    +   +
    +
    + + +
    +
    +
    +
    +
    Battery voltage + Battery voltage is calculated using the formula VBAT = (ADC - offset) / scale. If voltage is reading too high, increase scale. +
    + + +
    +
    + + +
    +
    +
    + + +
    +
    +