Fix 40 MHz bandwidth mode

Author: vertexodessa

src/RadioManagementModule.cpp

@@ -5,8 +5,24 @@
 #include <chrono>
 #include <map>
 #include <thread>
+#include <unordered_map>
 #include <vector>
 
+namespace{
+    int get_40mhz_center_channel(int channel) {
+        static const std::unordered_map<int, int> channel_map = {
+                {36, 38}, {40, 38}, {44, 46}, {48, 46},
+                {52, 54}, {56, 54}, {60, 62}, {64, 62},
+                {100, 102}, {104, 102}, {108, 110}, {112, 110},
+                {116, 118}, {120, 118}, {124, 126}, {128, 126},
+                {132, 134}, {136, 134}, {140, 142}, {144, 142},
+                {149, 151}, {153, 151}, {157, 159}, {161, 159}
+        };
+
+        auto it = channel_map.find(channel);
+        return (it != channel_map.end()) ? it->second : channel;
+    }
+}
 RadioManagementModule::RadioManagementModule(
     RtlUsbAdapter device, std::shared_ptr<EepromManager> eepromManager,
     Logger_t logger)
@@ -89,11 +105,7 @@ static uint8_t rtw_get_center_ch(uint8_t channel, ChannelWidth_t chnl_bw,
     else if (channel <= 14)
       center_ch = 7;
   } else if (chnl_bw == ChannelWidth_t::CHANNEL_WIDTH_40) {
-    if (chnl_offset == HAL_PRIME_CHNL_OFFSET_LOWER) {
-      center_ch = (uint8_t)(channel + 2);
-    } else {
-      center_ch = (uint8_t)(channel - 2);
-    }
+      center_ch = get_40mhz_center_channel(center_ch);
   } else if (chnl_bw == ChannelWidth_t::CHANNEL_WIDTH_20) {
     center_ch = channel;
   } else {