From 7134601bcd7941e6f1409dd53c1ace02717c0794 Mon Sep 17 00:00:00 2001
From: Emiel Steerneman <emielsteerneman@gmail.com>
Date: Fri, 14 Jan 2022 19:02:59 +0100
Subject: [PATCH] Implemented channel selection. no jumper = yellow

---
 Core/Inc/Wireless/Wireless.h | 18 +++++++++++++++---
 Core/Src/Wireless/Wireless.c | 15 +++++++++++----
 Core/Src/robot.c             | 10 +++++++++-
 3 files changed, 35 insertions(+), 8 deletions(-)

diff --git a/Core/Inc/Wireless/Wireless.h b/Core/Inc/Wireless/Wireless.h
index 06a7ecdb..8c24e485 100644
--- a/Core/Inc/Wireless/Wireless.h
+++ b/Core/Inc/Wireless/Wireless.h
@@ -18,12 +18,21 @@
 #define MAX_BUF_LENGTH 128
 #define AUTO_TX_TIME 120 // (us)
 
-#define WIRELESS_FEEDBACK_CHANNEL -5	// 2.395 GHz
-#define WIRELESS_COMMAND_CHANNEL -15 // 2.385 GHz
+#define WIRELESS_YELLOW_CHANNELS 0
+#define WIRELESS_BLUE_CHANNELS 1
 
+#define WIRELESS_YELLOW_FEEDBACK_CHANNEL -5 // 2.395 GHz
+#define WIRELESS_YELLOW_COMMAND_CHANNEL -15 // 2.385 GHz
+#define WIRELESS_BLUE_FEEDBACK_CHANNEL -25
+#define WIRELESS_BLUE_COMMAND_CHANNEL -35
+
+typedef enum WIRELESS_CHANNEL {
+    YELLOW_CHANNEL = 0,
+    BLUE_CHANNEL = 1
+} WIRELESS_CHANNEL;
 ///////////////////////////////////////////////////// PUBLIC FUNCTION DECLARATIONS
 
-SX1280 * Wireless_Init(float channel, SPI_HandleTypeDef * WirelessSpi);
+SX1280 * Wireless_Init(WIRELESS_CHANNEL channel, SPI_HandleTypeDef * WirelessSpi);
 void Wireless_DeInit();
 void SendPacket(SX1280* SX, uint8_t * data, uint8_t Nbytes);
 void ReceivePacket(SX1280* SX);
@@ -31,4 +40,7 @@ void Wireless_IRQ_Handler(SX1280* SX, uint8_t * data, uint8_t Nbytes);
 void Wireless_DMA_Handler(SX1280* SX, uint8_t output[]);
 bool checkWirelessConnection();
 
+void SX1280_updateChannel(WIRELESS_CHANNEL channel);
+WIRELESS_CHANNEL SX1280_getCurrentChannel();
+
 #endif /* WIRELESS_WIRELESS_H_ */
diff --git a/Core/Src/Wireless/Wireless.c b/Core/Src/Wireless/Wireless.c
index 188fd748..8199c9c1 100644
--- a/Core/Src/Wireless/Wireless.c
+++ b/Core/Src/Wireless/Wireless.c
@@ -19,6 +19,8 @@ static bool isWirelessTransmitting = false; // boolean to check whether we are t
 uint8_t TX_buffer[MAX_BUF_LENGTH] __attribute__((aligned(16)));
 uint8_t RX_buffer[MAX_BUF_LENGTH] __attribute__((aligned(16)));
 
+volatile static WIRELESS_CHANNEL current_channel = YELLOW_CHANNEL;
+
 SX1280 SX1280_struct;
 SX1280* SX; // pointer to the datastruct
 
@@ -46,9 +48,11 @@ SX1280_Settings set = {
 };
 SX1280_Packet_Status PacketStat;
 
-SX1280 * Wireless_Init(float channel, SPI_HandleTypeDef * WirelessSpi){
+SX1280 * Wireless_Init(WIRELESS_CHANNEL channel, SPI_HandleTypeDef * WirelessSpi){
 	SX1280 * SX = &SX1280_struct;// pointer to the global struct
 
+    current_channel = channel;
+
     SX->SPI_used = false;
 
     SX->payloadLength = 0;
@@ -69,7 +73,8 @@ SX1280 * Wireless_Init(float channel, SPI_HandleTypeDef * WirelessSpi){
 
     // link settings
     SX->SX_settings = &set;
-    SX->SX_settings->channel = channel;
+    if (current_channel == YELLOW_CHANNEL) SX->SX_settings->channel = WIRELESS_YELLOW_COMMAND_CHANNEL;
+    if (current_channel == BLUE_CHANNEL  ) SX->SX_settings->channel = WIRELESS_BLUE_COMMAND_CHANNEL;
     SX->Packet_status = &PacketStat;
 
     SX1280Setup(SX); // SX1280 init procedure
@@ -127,7 +132,8 @@ void Wireless_IRQ_Handler(SX1280* SX, uint8_t * data, uint8_t Nbytes){
     	isWirelessTransmitting = false;
     	//toggle_Pin(LED5_pin);
     	// start listening for a packet again
-    	setChannel(SX, WIRELESS_COMMAND_CHANNEL); // set to channel 40 for basestation to robot
+        if (current_channel == YELLOW_CHANNEL) setChannel(SX, WIRELESS_YELLOW_COMMAND_CHANNEL);
+        if (current_channel == BLUE_CHANNEL  ) setChannel(SX, WIRELESS_BLUE_COMMAND_CHANNEL);
 		SX->SX_settings->syncWords[0] = robot_syncWord[get_Id()];
 		setSyncWords(SX, SX->SX_settings->syncWords[0], 0x00, 0x00);
 
@@ -149,7 +155,8 @@ void Wireless_IRQ_Handler(SX1280* SX, uint8_t * data, uint8_t Nbytes){
     		if (wirelessFeedback && !isWirelessTransmitting) {
     			// feedback enabled, transmit a packet to basestation
     			isWirelessTransmitting = true;
-    			setChannel(SX, WIRELESS_FEEDBACK_CHANNEL); // set to channel 40 for feedback to basestation
+                if (current_channel == YELLOW_CHANNEL) setChannel(SX, WIRELESS_YELLOW_FEEDBACK_CHANNEL);
+                if (current_channel == BLUE_CHANNEL  ) setChannel(SX, WIRELESS_BLUE_FEEDBACK_CHANNEL);
     			SX->SX_settings->syncWords[0] = robot_syncWord[16]; // 0x82108610 for basestation Rx
     			setSyncWords(SX, SX->SX_settings->syncWords[0], 0x00, 0x00);
     			SendPacket(SX, data, Nbytes);
diff --git a/Core/Src/robot.c b/Core/Src/robot.c
index 0fb1604c..199e00f7 100644
--- a/Core/Src/robot.c
+++ b/Core/Src/robot.c
@@ -223,7 +223,15 @@ void init(void){
 	/* Initialize the SX1280 wireless chip */
 	// TODO figure out why a hardfault occurs when this is disabled
 	Putty_printf("Initializing wireless\n");
-    SX = Wireless_Init(WIRELESS_COMMAND_CHANNEL, COMM_SPI);
+
+	if(read_Pin(IN2_pin)){
+		SX = Wireless_Init(BLUE_CHANNEL, COMM_SPI);
+		Putty_printf("Listening on BLUE CHANNEL\n");
+	}else{
+		SX = Wireless_Init(YELLOW_CHANNEL, COMM_SPI);
+		Putty_printf("Listening on YELLOW CHANNEL\n");
+	}
+    
 	SX->SX_settings->syncWords[0] = robot_syncWord[ID];
     setSyncWords(SX, SX->SX_settings->syncWords[0], 0x00, 0x00);
     setRX(SX, SX->SX_settings->periodBase, WIRELESS_RX_COUNT);