From b89a13680e8973b3ce1a278bbdb58712a716ee6d Mon Sep 17 00:00:00 2001
From: sh123 <sh123@users.noreply.github.com>
Date: Thu, 16 Nov 2023 23:26:34 +0200
Subject: [PATCH] Handle control data escape

---
 include/loraprs_service.h |  2 +-
 src/kiss_processor.cpp    | 12 ++++++++++--
 src/loraprs_service.cpp   |  4 ++--
 3 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/include/loraprs_service.h b/include/loraprs_service.h
index a4dec95..259040e 100644
--- a/include/loraprs_service.h
+++ b/include/loraprs_service.h
@@ -109,7 +109,7 @@ private:
     uint32_t freqRx;
     uint32_t freqTx;
     uint8_t modType;
-    uint16_t pwr;
+    int16_t pwr;
     uint32_t bw;
     uint16_t sf;
     uint16_t cr;
diff --git a/src/kiss_processor.cpp b/src/kiss_processor.cpp
index 7e7fb1b..e7ee96e 100644
--- a/src/kiss_processor.cpp
+++ b/src/kiss_processor.cpp
@@ -280,11 +280,19 @@ bool Processor::receiveByteKiss(byte rxByte)
       break;
     case State::Escape:
       if (rxByte == Marker::Tfend) {
-        onRigTx((byte)Marker::Fend);
+        if (dataType_ == DataType::Raw) {
+          onRigTx((byte)Marker::Fend);
+        } else if (dataType_ == DataType::Control) {
+          cmdBuffer_.push_back((byte)Marker::Fend);
+        }
         state_ = State::GetData;
       }
       else if (rxByte == Marker::Tfesc) {
-        onRigTx((byte)Marker::Fesc);
+        if (dataType_ == DataType::Raw) {
+          onRigTx((byte)Marker::Fesc);
+        } else if (dataType_ == DataType::Control) {
+          cmdBuffer_.push_back((byte)Marker::Fesc);
+        }
         state_ = State::GetData;
       }
       else if (rxByte != Marker::Fend) {
diff --git a/src/loraprs_service.cpp b/src/loraprs_service.cpp
index 0034579..ac5ca68 100644
--- a/src/loraprs_service.cpp
+++ b/src/loraprs_service.cpp
@@ -788,7 +788,7 @@ void Service::onRadioControlCommand(const std::vector<byte> &rawCommand) {
     config_.LoraBw = be32toh(setHardware->bw);
     config_.LoraSf = be16toh(setHardware->sf);
     config_.LoraCodingRate = be16toh(setHardware->cr);
-    config_.LoraPower = be16toh(setHardware->pwr);
+    config_.LoraPower = (int16_t)be16toh(setHardware->pwr);
     config_.LoraSync = be16toh(setHardware->sync);
     config_.FskBitRate = (float)be32toh(setHardware->fskBitRate) / 1e3;
     config_.FskFreqDev = (float)be32toh(setHardware->fskFreqDev) / 1e3; 
@@ -803,7 +803,7 @@ void Service::onRadioControlCommand(const std::vector<byte> &rawCommand) {
         config_.LoraCodingRate, config_.LoraPower, config_.LoraSync, crcType, config_.LoraExplicit);
     }
   } else {
-    LOG_ERROR("Radio control command of wrong size");
+    LOG_ERROR("Radio control command of wrong size", rawCommand.size());
   }
 }