diff --git a/ADF7021.cpp b/ADF7021.cpp
index 647c4a4..dc4fa24 100644
--- a/ADF7021.cpp
+++ b/ADF7021.cpp
@@ -43,7 +43,7 @@ uint32_t           ADF7021_REG1;
 uint32_t           div2;
 uint32_t           f_div;
 
-static  void Send_AD7021_control_shift()
+static void Send_AD7021_control_shift()
 {
   int AD7021_counter;
 
@@ -62,7 +62,7 @@ static  void Send_AD7021_control_shift()
   io.SDATA_pin(LOW);
 }
 
-static  void Send_AD7021_control_slePulse()
+static void Send_AD7021_control_slePulse()
 {
   io.SLE_pin(HIGH);
   io.dlybit();
@@ -626,9 +626,9 @@ void CIO::ifConf2(MMDVM_STATE modemState)
   Send_AD7021_control2();
 
   // MODULATION (2)
-  ADF7021_REG2 |= (uint32_t) 0b0010;               // register 2
-  ADF7021_REG2 |= (uint32_t) m_power       << 13;  // power level
-  ADF7021_REG2 |= (uint32_t) 0b110001      << 7;   // PA  
+  ADF7021_REG2 |= (uint32_t) 0b0010;                  // register 2
+  ADF7021_REG2 |= (uint32_t) (m_power & 0x3F) << 13;  // power level
+  ADF7021_REG2 |= (uint32_t) 0b110001         << 7;   // PA  
   AD7021_control_word = ADF7021_REG2;
   Send_AD7021_control2();
  
@@ -786,7 +786,6 @@ void CIO::interrupt2()
 }
 #endif
 
-//======================================================================================================================
 void CIO::setTX()
 { 
   // PTT pin on (doing it earlier helps to measure timing impact)
@@ -806,7 +805,6 @@ void CIO::setTX()
   while(CLK_pin());
 }
 
-//======================================================================================================================
 void CIO::setRX(bool doSle)
 { 
   // PTT pin off (doing it earlier helps to measure timing impact)
@@ -827,6 +825,11 @@ void CIO::setRX(bool doSle)
   }
 }
 
+void CIO::setPower(uint8_t power)
+{
+  m_power = power >> 2;
+}
+
 uint32_t CIO::RXfreq()
 {
   return (uint32_t)((float)(ADF7021_PFD / f_div) * ((float)((32768 * m_RX_N_divider) + m_RX_F_divider) / 32768.0)) + 100000;
diff --git a/IO.cpp b/IO.cpp
index c04b7bc..ea0f491 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -269,7 +269,7 @@ bool CIO::hasRXOverflow()
 uint8_t CIO::setFreq(uint32_t frequency_rx, uint32_t frequency_tx, uint8_t rf_power)
 {
   // Configure power level
-  m_power = rf_power;
+  setPower(rf_power);
 
   // Check frequency ranges
   if( !( ((frequency_rx >= VHF1_MIN)&&(frequency_rx < VHF1_MAX)) || ((frequency_tx >= VHF1_MIN)&&(frequency_tx < VHF1_MAX)) || \
diff --git a/IO.h b/IO.h
index e366c36..a52db83 100644
--- a/IO.h
+++ b/IO.h
@@ -92,6 +92,7 @@ public:
   bool      hasTXOverflow(void);
   bool      hasRXOverflow(void);
   uint8_t   setFreq(uint32_t frequency_rx, uint32_t frequency_tx, uint8_t rf_power);
+  void      setPower(uint8_t power);
   void      setMode(MMDVM_STATE modemState);
   void      setDecode(bool dcd);
   void      setLoDevYSF(bool ysfLoDev);