optimise mavlink SS packet size (#3029)
[ExpressLRS.git] / src / lib / SX127xDriver / SX127xHal.cpp
blob1a9c32df8746402117dce6cfeb8a7470ae82c76c
1 #ifndef UNIT_TEST
3 #include "SX127xHal.h"
4 #include "SX127xRegs.h"
5 #include "logging.h"
6 #include <SPIEx.h>
8 SX127xHal *SX127xHal::instance = NULL;
10 SX127xHal::SX127xHal()
12 instance = this;
15 void SX127xHal::end()
17 detachInterrupt(GPIO_PIN_DIO0);
18 if (GPIO_PIN_DIO0_2 != UNDEF_PIN)
20 detachInterrupt(GPIO_PIN_DIO0_2);
22 SPIEx.end();
23 IsrCallback_1 = nullptr; // remove callbacks
24 IsrCallback_2 = nullptr; // remove callbacks
27 void SX127xHal::init()
29 DBGLN("Hal Init");
31 pinMode(GPIO_PIN_DIO0, INPUT);
32 if (GPIO_PIN_DIO0_2 != UNDEF_PIN)
34 pinMode(GPIO_PIN_DIO0_2, INPUT);
37 pinMode(GPIO_PIN_NSS, OUTPUT);
38 digitalWrite(GPIO_PIN_NSS, HIGH);
40 #ifdef PLATFORM_ESP32
41 SPIEx.begin(GPIO_PIN_SCK, GPIO_PIN_MISO, GPIO_PIN_MOSI, GPIO_PIN_NSS); // sck, miso, mosi, ss (ss can be any GPIO)
42 gpio_pullup_en((gpio_num_t)GPIO_PIN_MISO);
43 SPIEx.setFrequency(10000000);
44 SPIEx.setHwCs(true);
45 if (GPIO_PIN_NSS_2 != UNDEF_PIN)
47 pinMode(GPIO_PIN_NSS_2, OUTPUT);
48 digitalWrite(GPIO_PIN_NSS_2, HIGH);
49 spiAttachSS(SPIEx.bus(), 1, GPIO_PIN_NSS_2);
51 spiEnableSSPins(SPIEx.bus(), SX12XX_Radio_All);
52 #elif defined(PLATFORM_ESP8266)
53 DBGLN("PLATFORM_ESP8266");
54 SPIEx.begin();
55 SPIEx.setHwCs(true);
56 SPIEx.setBitOrder(MSBFIRST);
57 SPIEx.setDataMode(SPI_MODE0);
58 SPIEx.setFrequency(10000000);
59 #endif
61 attachInterrupt(digitalPinToInterrupt(GPIO_PIN_DIO0), this->dioISR_1, RISING);
62 if (GPIO_PIN_DIO0_2 != UNDEF_PIN)
64 attachInterrupt(digitalPinToInterrupt(GPIO_PIN_DIO0_2), this->dioISR_2, RISING);
68 void SX127xHal::reset(void)
70 DBGLN("SX127x Reset");
72 if (GPIO_PIN_RST != UNDEF_PIN)
74 pinMode(GPIO_PIN_RST, OUTPUT);
75 digitalWrite(GPIO_PIN_RST, LOW);
76 if (GPIO_PIN_RST_2 != UNDEF_PIN)
78 pinMode(GPIO_PIN_RST_2, OUTPUT);
79 digitalWrite(GPIO_PIN_RST_2, LOW);
81 delay(50); // Safety buffer. Busy takes longer to go low than the 1ms timeout in WaitOnBusy().
82 pinMode(GPIO_PIN_RST, INPUT); // leave floating
83 if (GPIO_PIN_RST_2 != UNDEF_PIN)
85 pinMode(GPIO_PIN_RST_2, INPUT);
89 DBGLN("SX127x Ready!");
92 uint8_t ICACHE_RAM_ATTR SX127xHal::readRegisterBits(uint8_t reg, uint8_t mask, SX12XX_Radio_Number_t radioNumber)
94 uint8_t rawValue = readRegister(reg, radioNumber);
95 uint8_t maskedValue = rawValue & mask;
96 return (maskedValue);
99 uint8_t ICACHE_RAM_ATTR SX127xHal::readRegister(uint8_t reg, SX12XX_Radio_Number_t radioNumber)
101 uint8_t data;
102 readRegister(reg, &data, 1, radioNumber);
103 return data;
106 void ICACHE_RAM_ATTR SX127xHal::readRegister(uint8_t reg, uint8_t *data, uint8_t numBytes, SX12XX_Radio_Number_t radioNumber)
108 WORD_ALIGNED_ATTR uint8_t buf[WORD_PADDED(numBytes + 1)];
109 buf[0] = reg | SPI_READ;
111 SPIEx.read(radioNumber, buf, numBytes + 1);
113 memcpy(data, buf + 1, numBytes);
116 void ICACHE_RAM_ATTR SX127xHal::writeRegisterBits(uint8_t reg, uint8_t value, uint8_t mask, SX12XX_Radio_Number_t radioNumber)
118 if (radioNumber & SX12XX_Radio_1)
120 uint8_t currentValue = readRegister(reg, SX12XX_Radio_1);
121 uint8_t newValue = (currentValue & ~mask) | (value & mask);
122 writeRegister(reg, newValue, SX12XX_Radio_1);
125 if (GPIO_PIN_NSS_2 != UNDEF_PIN && radioNumber & SX12XX_Radio_2)
127 uint8_t currentValue = readRegister(reg, SX12XX_Radio_2);
128 uint8_t newValue = (currentValue & ~mask) | (value & mask);
129 writeRegister(reg, newValue, SX12XX_Radio_2);
133 void ICACHE_RAM_ATTR SX127xHal::writeRegister(uint8_t reg, uint8_t data, SX12XX_Radio_Number_t radioNumber)
135 writeRegister(reg, &data, 1, radioNumber);
138 void ICACHE_RAM_ATTR SX127xHal::writeRegister(uint8_t reg, uint8_t *data, uint8_t numBytes, SX12XX_Radio_Number_t radioNumber)
140 WORD_ALIGNED_ATTR uint8_t buf[WORD_PADDED(numBytes + 1)];
141 buf[0] = reg | SPI_WRITE;
142 memcpy(buf + 1, data, numBytes);
144 SPIEx.write(radioNumber, buf, numBytes + 1);
147 void ICACHE_RAM_ATTR SX127xHal::dioISR_1()
149 if (instance->IsrCallback_1)
150 instance->IsrCallback_1();
153 void ICACHE_RAM_ATTR SX127xHal::dioISR_2()
155 if (instance->IsrCallback_2)
156 instance->IsrCallback_2();
159 #endif // UNIT_TEST