Merge branch 'feature_extend_ATR833_driver' of https://github.com/Scumi/XCSoar

Author: MaxKellermann

NEWS.txt

@@ -4,6 +4,8 @@ Version 7.29 - not yet released
   - fix crash when arc airspace has no center
   - allow xci files to be downloaded from repository
   - in OpenAir format use AC as Class and AY as Type (#1118)
+* devices
+  - ATR833: read-out active and standby frequencies
 * Android
   - get rid of not usable USB interfaces in the 'Device -> Port' list
 * Kobo

src/Device/Driver/ATR833.cpp

@@ -5,15 +5,31 @@
 #include "Device/Driver.hpp"
 #include "Device/Port/Port.hpp"
 #include "RadioFrequency.hpp"
+#include "NMEA/Info.hpp"
+#include "Operation/Operation.hpp"
+#include "time/TimeoutClock.hpp"
+#include "util/StaticFifoBuffer.hxx"
+
+#include <cstdint>
 
 static constexpr uint8_t STX = 0x02;
 static constexpr uint8_t SYNC = 'r';
 
 class ATR833Device final : public AbstractDevice {
+  static constexpr uint8_t ACK = 0x06;
+  static constexpr uint8_t NAK = 0x15;
+  static constexpr uint8_t ALIVE = 0x10;
+  static constexpr uint8_t EXCHANGE = 0x11;
   static constexpr uint8_t SETSTANDBY = 0x12;
   static constexpr uint8_t SETACTIVE = 0x13;
+  static constexpr uint8_t ALLDATA = 0x42;
+  static constexpr uint8_t REQUESTDATA = 0x82;
 
   Port &port;
+  /**
+   * Buffer which receives the messages send from the radio.
+  */ 
+  StaticFifoBuffer<uint8_t, 256u> rx_buf;
 
 public:
   explicit ATR833Device(Port &_port):port(_port) {}
@@ -27,6 +43,32 @@ class ATR833Device final : public AbstractDevice {
   bool PutStandbyFrequency(RadioFrequency frequency,
                            const TCHAR *name,
                            OperationEnvironment &env) override;
+  bool EnableNMEA(OperationEnvironment &env) override;
+  void OnSysTicker() override;
+  void LinkTimeout() override;
+
+private:
+  PeriodClock status_clock;
+
+  /**
+   * Handles responses from the radio.
+   */
+  void HandleResponse(const uint8_t *data, struct NMEAInfo &info);
+  /**
+   * Calculates the length of the message just receiving.
+   *
+   * @param data Pointer to the first character of the message.
+   * @param length Number of characters received.
+   * @return Expected message length.
+   */
+  static size_t ExpectedMsgLength(const uint8_t *data, size_t length);
+  /**
+   * Calculates the length of the command message just receiving.
+   *
+   * @param code Command code received after the STX+'r' character.
+   * @return Expected message length after the code character.
+   */
+  static size_t ExpectedMsgLengthCommand(uint8_t code);
 };
 
 class ATRBuffer {
@@ -57,14 +99,150 @@ class ATRBuffer {
 };
 
 bool
-ATR833Device::DataReceived(std::span<const std::byte>,
+ATR833Device::DataReceived(std::span<const std::byte> s,
                            [[maybe_unused]] NMEAInfo &info) noexcept
 {
-  // actually made no use of radio information
-  // TODO: interpret data delivered by ATR833 to display Radio settings...
+  assert(!s.empty());
+  const auto *data = s.data();
+  const auto *const end = data + s.size();
+  size_t expected_msg_length{};
+
+  do {
+    // Append new data to the buffer, as much as fits in there
+    auto range = rx_buf.Write();
+
+    if (range.empty()) {
+      rx_buf.Clear();
+      continue;
+    }
+
+    size_t nbytes = std::min(range.size(), size_t(end - data));
+    memcpy(range.data(), data, nbytes);
+    data += nbytes;
+    rx_buf.Append(nbytes);
+
+    for (;;) {
+      // Read data from buffer to handle the messages
+      range = rx_buf.Read();;
+
+      if (range.empty() || range.size() < expected_msg_length)
+        break;
+
+      expected_msg_length = ExpectedMsgLength(range.data(), range.size());
+
+      if (range.size() >= expected_msg_length) {
+        if (range.front() == STX) {
+            HandleResponse(range.data(), info);
+        }
+        // Message handled -> remove message
+        rx_buf.Consume(expected_msg_length);
+        expected_msg_length = 0;
+      }
+    }
+  } while (data < end);
+
   return true;
 }
 
+/**
+  The expected length of a received message may change,
+  when the first character is STX and the second character
+  is not received yet.
+*/
+size_t
+ATR833Device::ExpectedMsgLength(const uint8_t *data, size_t length)
+{
+  assert(data != nullptr);
+  assert(length > 0);
+
+  if (data[0] == STX) {
+    if (length > 2) {
+      return 3 + ExpectedMsgLengthCommand(data[2]);
+    } else {
+      // minimum 3 chars
+      return 3;
+    }
+  } else
+    return 1;
+}
+
+size_t
+ATR833Device::ExpectedMsgLengthCommand(uint8_t code)
+{
+  switch (code) {
+  case SETACTIVE:
+    // Active frequency
+    return 2;
+  case SETSTANDBY:
+    // Standby frequency
+    return 2;
+  case EXCHANGE:
+    // Exchange frequencies
+    return 1;
+  case ALLDATA:
+    // receive full dataset (freq, VOL, etc...)
+    return 12;
+  case ACK:
+    return 1;
+  case NAK:
+    return 2;
+  case ALIVE:
+    return 1;
+  default:
+    // Received unknown msg id (code)
+    return 0;
+  }
+}
+
+void
+ATR833Device::HandleResponse(const uint8_t *data, struct NMEAInfo &info)
+{
+  info.alive.Update(info.clock);
+
+  if(data[2] != EXCHANGE && data[2] != ALLDATA &&
+     data[2] != SETACTIVE && data[2] != SETSTANDBY)
+    return;
+
+  if(data[2] == SETACTIVE) {
+    info.settings.has_active_frequency.Update(info.clock);
+    info.settings.active_frequency =
+      RadioFrequency::FromMegaKiloHertz(data[3], data[4] * 5);
+  }
+
+  if(data[2] == SETSTANDBY) {
+    info.settings.has_standby_frequency.Update(info.clock);
+    info.settings.standby_frequency =
+      RadioFrequency::FromMegaKiloHertz(data[3], data[4] * 5);
+  }
+
+  if(data[2] == EXCHANGE) {
+    const auto old_active_freq = info.settings.active_frequency;
+
+    info.settings.has_active_frequency.Update(info.clock);
+    info.settings.active_frequency = info.settings.standby_frequency;
+
+    info.settings.has_standby_frequency.Update(info.clock);
+    info.settings.standby_frequency = old_active_freq;
+  }
+
+  if(data[2] == ALLDATA) {
+    /*
+      byte 4: MHz active
+      byte 5: kHz/5 active
+      byte 6: MHz standby
+      byte 7: kHz/5 standby     
+      byte 8-15: not used by XCSoar (VOL, SQ, VOX, INT, DIM, EXT, spacing, dual)   
+    */
+    info.settings.has_active_frequency.Update(info.clock);
+    info.settings.active_frequency = 
+      RadioFrequency::FromMegaKiloHertz(data[3], data[4] * 5);
+
+    info.settings.has_standby_frequency.Update(info.clock);
+    info.settings.standby_frequency = 
+      RadioFrequency::FromMegaKiloHertz(data[5], data[6] * 5);
+  }
+}
+
 bool
 ATR833Device::PutActiveFrequency(RadioFrequency frequency,
                                  [[maybe_unused]] const TCHAR *name,
@@ -90,17 +268,46 @@ ATR833Device::PutStandbyFrequency(RadioFrequency frequency,
   return true;
 }
 
+void
+ATR833Device::LinkTimeout()
+{
+  NullOperationEnvironment env;
+
+  ATRBuffer{ALIVE}.Send(port, env);
+  ATRBuffer{REQUESTDATA}.Send(port, env);
+}
+
+bool
+ATR833Device::EnableNMEA(OperationEnvironment &env)
+{
+  ATRBuffer{ALIVE}.Send(port, env);
+  ATRBuffer{REQUESTDATA}.Send(port, env);
+
+  return true;
+}
+
+void
+ATR833Device::OnSysTicker()
+{
+  // ALIVE shall be send every 5 seconds
+  if (status_clock.CheckUpdate(std::chrono::seconds(5))) {
+    NullOperationEnvironment env;
+
+    ATRBuffer{ALIVE}.Send(port, env);
+
+    ATRBuffer{REQUESTDATA}.Send(port, env);
+  }
+}
+
 static Device *
 ATR833CreateOnPort([[maybe_unused]] const DeviceConfig &config, Port &com_port)
 {
   return new ATR833Device(com_port);
 }
 
-
 const DeviceRegister atr833_driver = {
   _T("ATR833"),
   _T("ATR833"),
-  DeviceRegister::NO_TIMEOUT |
   DeviceRegister::RAW_GPS_DATA,
   ATR833CreateOnPort,
 };