wip: radio init.

2 files changed, 20 insertions, 5 deletions

diff --git a/rf_test/radio.c b/rf_test/radio.c
index bda4c77..6f8c9c4 100644
--- a/rf_test/radio.c
+++ b/rf_test/radio.c
@@ -103,12 +103,26 @@ uint8_t radio_recv(char *buf, uint8_t n)
 	return read_len;
 }
 
-void set_power_level(uint8_t pwl)
+static inline void set_high_power_pa(uint8_t enable)
 {
-	uint8_t pa;
+	write_reg(0x5A, (enable ? 0x5D : 0x55));
+	write_reg(0x5C, (enable ? 0x7C : 0x70));
+}
+
+static inline void set_power_level(uint8_t pwl)
+{
+	uint8_t pa_mask;
 
-	if (pwl < 16)
+	if (pwl < 16) {
 		pwl += 16;
+		pa_mask = 0x40;
+	} else {
+		pwl += pwl < 20 ? 10 : 8;
+		pa_mask = 0x40 | 0x20;
+	}
+
+	set_high_power_pa(1);
+	write_reg(0x11, (pa_mask | pwl));
 }
 
 void radio_high_power(void)
@@ -125,11 +139,11 @@ void radio_init(struct radio_cfg *cfg)
 	
 	do {
 		write_reg(0x2F, 0xAA);
-	} while (read_reg(0x2F) != 0xAA)
+	} while (read_reg(0x2F) != 0xAA);
 
 	do {
 		write_reg(0x2F, 0x55);
-	} while (read_reg(0x2F) != 0x55)
+	} while (read_reg(0x2F) != 0x55);
 
 	write_reg(0x01, 0x04);
 
diff --git a/rf_test/radio.h b/rf_test/radio.h
index fe715b2..719af14 100644
--- a/rf_test/radio.h
+++ b/rf_test/radio.h
@@ -4,6 +4,7 @@
 #include <stdint.h>
 
 struct radio_cfg {
+	uint8_t network_id;
 	uint8_t payload_len;
 };