├── .gitattributes ├── .gitignore ├── Build manual.md ├── Documents └── RFM95_96_97_98W.pdf ├── Generic.ino ├── LoRaRC.ino ├── README.md ├── RFM95-Arduino-connection.png ├── RFM95-pinout.png ├── Reports └── link_test_19.08.2018.xlsx ├── Servos.ino ├── TODO.ino ├── config.h ├── ibus.ino ├── msp.h ├── msp.ino ├── ppm.ino └── sbus.ino /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | Testing 2 | -------------------------------------------------------------------------------- /Build manual.md: -------------------------------------------------------------------------------- 1 | #Manual 2 | 3 | Build your own module. 4 | 5 | To build your first LoRaRC module you need: 6 | 7 | - Arduino Pro Mini 16Mhz, 5V (unless you can buy 16MHz 3,3V), 8 | - AMS1117 3,3V voltage regulator, 9 | - RFM95 module, 10 | - 868MHz dipole antenna. 11 | 12 | #Step 1 - Upload Optiboot bootloader into Arduino's Atmega microcontroller. 13 | You can use various methodos to do it. The instruction is described in the 14 | Optiboot Project's site: https://github.com/Optiboot/optiboot/wiki/InstallingOnChips 15 | 16 | If you have any spare Arduino board, the easiest way is to use Optiloader 17 | sketch which will upload the right bootloader and set the fusebit for you. 18 | See Optiloader site: https://github.com/WestfW/OptiLoader. 19 | 20 | All you need to do is to connect the boards as follows and power on the "programmer board": 21 | Programmer board LoRaRC board 22 | Pin 13 Pin 13 23 | Pin 12 Pin 12 24 | Pin 11 Pin 11 25 | Pin 10 Pin RESET 26 | +5V VCC +5V VCC / RAW * 27 | GND GND 28 | 29 | * IMPORTANT NOTE: If you already have soldered Arduino and RFM togeather you cannot 30 | connect +5V to LoRaRC module VCC. RFM is not 5V tolerant. Make sure you will supply 31 | the module with 3.3V. See points below. 32 | 33 | Finally, try to upload any sketch to your Arduino Pro Mini. Remember, from now on 34 | your Pro Mini's bootloader will communicate with baudrate of 115200bps. Therefore, 35 | select Board Type as: Arduino/Genuino Uno. 36 | 37 | #Step 2 - Change Arduino's voltage regulator to 3.3V 38 | Since RFM95 is not 5V tolerant, but Atmega is 3.3V tolerant, the easiest way is to 39 | change Arduino's VCC to 3.3V. In order to achieve that, we need to change voltage 40 | regulator. 41 | 42 | First, desolder original 5V regulator. 43 | 44 | AMS1117 spec sheet: http://www.advanced-monolithic.com/pdf/ds1117.pdf 45 | 46 | #Step 3 - Solder RFM95 and Arduino togeather 47 | http://www.hoperf.com/upload/rf/RFM95_96_97_98W.pdf 48 | 49 | Kynar 50 | 51 | Wiring: 52 | RFM95 Arduino 53 | MISO <-> MISO (Pin 12) 54 | MOSI <-> MOSI (Pin 11) 55 | SCK <-> SCK (Pin 13) 56 | NSS <-> Pin 10 57 | NRESET <-> Pin A1 58 | DIO0 <-> Pin 2 59 | 3.3V <-> VCC (3.3V) 60 | GND <-> GND 61 | 62 | #Step 4 - Build your own 868MHz antenna 63 | 64 | -------------------------------------------------------------------------------- /Documents/RFM95_96_97_98W.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winiak/LoRaRC/b2b00fb1e371ad0e55ad8b3e0729e3fd24f82d21/Documents/RFM95_96_97_98W.pdf -------------------------------------------------------------------------------- /Generic.ino: -------------------------------------------------------------------------------- 1 | 2 | 3 | uint8_t calculate_lost_frames_rssi(unsigned long lost_frames) { 4 | // calculate RSSI basing on lost frames 5 | // lost frames may be given as 6 | // - a counter of lost frames - no filter - short response, first good frame resets to zero, difficult to see when frame losts are shorter than half a second 7 | // - an average of frames lost in portion of time - slight delay in response, easier to see when conditions are going bad 8 | return 100 - round(100 * (lost_frames > LOST_FRAMES_COUNT ? LOST_FRAMES_COUNT : lost_frames) / LOST_FRAMES_COUNT); // 30 frames is a history 9 | } 10 | 11 | /** 12 | * Calculate RSSI based RSSI reported by transmitter 13 | * Return in %. 14 | * Easy but not reliable - I reported significant packets losts (Failsafe activated) when signal dropped below 55% (-102dB) 15 | * It looks like is would be good to adapt the dynamics to sensitivity (SF and BW dependent) and TX power 16 | */ 17 | uint8_t calculate_rssi(int tr_rssi) { 18 | // Simpliest method used from Lora-net project 19 | tr_rssi = 157 + tr_rssi; // entire link budget 20 | 21 | // Method based on OpenLRSng (although not identical) 22 | // if the result from the link budget is less than 50 23 | // use lost packets as the remaining 50% calculation 24 | if (tr_rssi < 50) { 25 | // TODO 26 | } 27 | // Setting frames to show it in "%" 28 | if (tr_rssi > 100) tr_rssi = 100; 29 | if (tr_rssi < 0) tr_rssi = 0; 30 | return (failsafe_state ? 0 : tr_rssi); 31 | } 32 | 33 | void manage_servos() { 34 | if (failsafe_state) 35 | set_servos_failsafe(); 36 | else 37 | for (uint8_t i = 0; i < SERVO_CHANNELS; i++) 38 | Servos[i] = Servo_Buffer[i]; 39 | #ifdef INJECT_RSSI_IN_CH 40 | Servos[INJECT_RSSI_IN_CH] = map(calculated_rssi, 0, 100, 1000, 2000); // inject calculated RSSI to % 41 | //Servos[INJECT_RSSI_IN_CH-1] = map(calculated_lost_frames_rssi, 0, 100, 1000, 2000); // inject RSSI based on lost frames 42 | //Servos[INJECT_RSSI_IN_CH+2] = map(current_power, tx_power_low, tx_power_high, 1000, 2000); // inject current power level 43 | #endif 44 | } 45 | 46 | void set_servos_failsafe() { 47 | for (uint8_t i = 0; i < SERVO_CHANNELS; i++) 48 | Servos[i] = Servo_Failsafe[i]; 49 | } 50 | 51 | byte power_increase() { 52 | if (current_power < tx_power_high) 53 | current_power = current_power + tx_power_step; 54 | else 55 | return current_power; 56 | LoRa.sleep(); 57 | LoRa.setTxPower(current_power); 58 | LoRa.idle(); 59 | return current_power; 60 | } 61 | 62 | byte power_decrease() { 63 | if (current_power > tx_power_low) 64 | current_power = current_power - tx_power_step; 65 | else 66 | return current_power; 67 | LoRa.sleep(); 68 | LoRa.setTxPower(current_power); 69 | LoRa.idle(); 70 | return current_power; 71 | } 72 | 73 | 74 | void Hopping() { 75 | long f; 76 | LoRa.sleep(); 77 | current_channel++; 78 | if (current_channel >= sizeof(hop_list)) 79 | current_channel = 0; 80 | f = base_frequency + (hop_list[current_channel] * frequency_step); // 81 | #ifdef DEBUG_CH_FREQ 82 | Serial.print(f); 83 | #endif 84 | LoRa.setFrequency(f); 85 | LoRa.idle(); 86 | } 87 | 88 | int receiveData(char data_len) { 89 | int packetSize = LoRa.parsePacket(data_len); 90 | char i = 0; 91 | if (packetSize) { 92 | //Serial.print("["); 93 | while (LoRa.available()) { 94 | RX_Buffer[i] = (char)LoRa.read(); 95 | //Serial.print(RX_Buffer[i]); 96 | i++; 97 | } 98 | //Serial.print("] \t"); 99 | // print RSSI of packet 100 | RX_RSSI = LoRa.packetRssi(); // remove +255 to display byte values instead of dBm 101 | //Serial.print("RX RSSI: "); 102 | //Serial.println(RX_RSSI); 103 | //Serial.print(" SNR: "); 104 | //Serial.println(LoRa.packetSnr()); 105 | } 106 | RX_Buffer_Len = packetSize; 107 | return packetSize; 108 | } 109 | 110 | bool sendBufferData() { 111 | char sent_data_len; 112 | if (TX_Buffer_Len == 0) 113 | return false; 114 | LoRa.beginPacket(true); // false - explicit, true - implicit 115 | sent_data_len = LoRa.write(TX_Buffer, TX_Buffer_Len); 116 | LoRa.endPacket(); 117 | TX_Buffer_Len = 0; 118 | return sent_data_len; 119 | } 120 | 121 | void PrintHex8(uint8_t *data, uint8_t length) // prints 8-bit data in hex with leading zeroes 122 | { 123 | Serial.print("0x"); 124 | for (int i=0; i 11 | #include 12 | #include "config.h" 13 | #include 14 | 15 | 16 | #ifdef TX_module 17 | stateMachineDef stateMachine = TRANSMIT; 18 | #endif 19 | #ifdef RX_module 20 | stateMachineDef stateMachine = RECEIVE; 21 | #endif 22 | static unsigned long TX_period = F_rate_low; // 7700 / 20000 / 40000 us 23 | static unsigned long ibus_frame_period = 7500; //us (so far the smallest delays with 7,5ms, measured frame rate 7,7 to 8,5ms 24 | static unsigned long sbus_frame_period = 15000; 25 | static unsigned long RX_last_frame_received, RX_hopping_time; 26 | static unsigned long RX_frame_time; 27 | static unsigned long timer_start, timer_stop; 28 | 29 | unsigned long lost_frames = 0; 30 | uint8_t current_channel = 0; 31 | byte current_power = tx_power_low; 32 | byte power_delay_counter = TX_POWER_DELAY_FILTER; 33 | unsigned char TX_Buffer[12]; 34 | unsigned char TX_Buffer_Len = 0; 35 | unsigned char RX_Buffer[12]; 36 | unsigned char RX_Buffer_Len = 0; 37 | int RX_RSSI; // RSSI on receiver side 38 | int TX_RSSI; // RSSI on transmitter side 39 | static uint8_t calculated_rssi = 0; 40 | static uint8_t calculated_lost_frames_rssi = 0; 41 | int counter = 0; 42 | 43 | static bool failsafe_state = true; // sending Failsafe values by default 44 | 45 | void setup() { 46 | #ifdef SBUS_module 47 | Serial.begin(100000, SERIAL_8E2); 48 | #else 49 | Serial.begin(115200); 50 | #endif 51 | 52 | // while (!Serial); 53 | wdt_enable(WDTO_250MS); 54 | 55 | Serial.println("LoRa for Remote Control"); 56 | /* 57 | LoRa.setSignalBandwidth(125E3); // 7.8E3, 10.4E3, 15.6E3, 20.8E3, 31.25E3, 41.7E3, 62.5E3, 125E3, and 250E3. 58 | LoRa.setSpreadingFactor(7); // 6 to 12 - 6 requires IMPLICIT 59 | LoRa.setCodingRate4(5); // 4 to 8, default 5 60 | LoRa.enableCrc(); // LoRa.disableCrc(); 61 | LoRa.setTxPower(5); // 2 to 20, default 17 62 | 63 | */ 64 | LoRa.setPins(NSS_PIN, NRESET_PIN, DIO0_PIN); //(ss, reset, dio0) dio is optional but must be interrupt capable via attachInterrupt(...): prefered: nSS-10, nRESET-9, DI0-2 65 | if (!LoRa.begin(base_frequency + (hop_list[0] * 50000))) { 66 | Serial.println("Starting LoRa failed!"); 67 | while (1); 68 | } 69 | LoRa.setTxPower(tx_power_low); 70 | LoRa.setSpreadingFactor(spread_factor); // 6 to 12 - 6 requires IMPLICIT 71 | 72 | LoRa.setSignalBandwidth(BW_high); 73 | TX_period = F_rate_high; 74 | 75 | LoRa.enableCrc(); 76 | //LoRa.disableCrc(); 77 | wdt_reset(); 78 | #ifdef DEBUG_ANALYZER 79 | #ifdef TX_module 80 | Serial.println("HC\tTX RSSI\tRX RSSI\tLost\tPacket t[ms]\tTX Pwr[dBm]\tRX Pwr[dBm]\tInput"); 81 | #endif //TX_module 82 | #endif // DEBUG_ANALYZER 83 | 84 | #ifdef TX_module 85 | #ifdef PPM_module 86 | setup_module(); 87 | #endif 88 | #endif 89 | } 90 | 91 | void loop() { 92 | static long int packet_timer; 93 | static unsigned long transmit_time = 0; 94 | static unsigned long receive_time = 0; 95 | static unsigned long twenty_ms_time = 0; 96 | static unsigned long ibus_frame_time = 0; 97 | static uint8_t no_RX_ack = 0; 98 | 99 | 100 | /** 101 | * Transmitter 102 | */ 103 | #ifdef TX_module 104 | switch (stateMachine) { 105 | case TRANSMIT: // transmit data then switch to RECEIVE 106 | #ifdef TX_SERVO_TESTER 107 | servoTester(); // servo tester - change servo values continuously 108 | #endif 109 | #if defined(DEBUG_RADIO_EXCH) || defined(DEBUG_CH_FREQ) || defined(DEBUG_ANALYZER) || defined(PPM_module) 110 | Serial.println(); 111 | #endif 112 | Hopping(); 113 | if (no_RX_ack > 0) { // ACKonwlege, check for return of the telemetry data 114 | #ifdef DEBUG_RADIO_EXCH 115 | Serial.print("\t\t\t NO ACK: "); 116 | Serial.print(no_RX_ack); 117 | #endif 118 | if (no_RX_ack > TX_POWER_DELAY_FILTER) { // if no ACK for (TX_POWER_DELAY_FILTER - two times hopping) frames, then assume no telemetry (and RSSI) is available 119 | TX_RSSI = -254; 120 | RX_RSSI = -254; 121 | if (no_RX_ack % TX_POWER_DELAY_FILTER * current_power * current_power == 0) // the higher power is set, the longer time is needed to increase power by the STEP 122 | power_increase(); 123 | } 124 | } 125 | TX_Buffer_Len = buildServoData(); 126 | packet_timer = micros(); 127 | sendBufferData(); 128 | //Serial.print(micros() - packet_timer); 129 | stateMachine = RECEIVE; 130 | no_RX_ack++; 131 | break; 132 | 133 | case RECEIVE: // stay in RECEIVE and wait for data until next TX period 134 | if (receiveData(6)) { 135 | no_RX_ack = 0; 136 | TX_RSSI = RX_Buffer[0] - 255;// - 255; "0" to display byte values instead of dBm 137 | #ifdef DEBUG_CH_FREQ 138 | Serial.print("\tHC: "); 139 | Serial.print(current_channel); 140 | #endif 141 | #ifdef DEBUG_RADIO_EXCH 142 | Serial.print("\tTr RSSI: "); 143 | Serial.print(TX_RSSI); 144 | Serial.print("\tRe RSSI: "); 145 | Serial.print(RX_RSSI); 146 | Serial.print("\tPacket exchange time: "); 147 | Serial.print(micros() - packet_timer); 148 | #endif 149 | if (TX_RSSI < power_thr_low && power_delay_counter-- == 0) { 150 | power_increase(); 151 | power_delay_counter = TX_POWER_DELAY_FILTER; 152 | } 153 | if (TX_RSSI > power_thr_high && power_delay_counter-- == 0) { 154 | power_decrease(); 155 | power_delay_counter = TX_POWER_DELAY_FILTER; 156 | } 157 | } 158 | break; 159 | case SETUP: 160 | break; 161 | default: // controller 162 | break; 163 | } 164 | // State machine controller 165 | if (micros() > transmit_time) { 166 | #ifdef DEBUG_ANALYZER 167 | //Serial.println("TX RSSI\tRX RSSI\tLost"); 168 | Serial.print(current_channel); Serial.print("\t"); 169 | Serial.print(TX_RSSI); Serial.print("\t"); 170 | Serial.print(RX_RSSI); Serial.print("\t"); 171 | Serial.print(no_RX_ack); Serial.print("\t"); 172 | Serial.print(byte((micros() - packet_timer)/1000)); Serial.print("\t"); 173 | Serial.print(current_power); Serial.print("\t"); 174 | Serial.print(RX_Buffer[1]); 175 | #ifdef PPM_module 176 | Serial.print("\t"); Serial.print(check_PPM_corrupted() ? "0" : "111"); 177 | #endif 178 | #endif 179 | transmit_time = micros() + TX_period; 180 | timer_start = micros(); 181 | stateMachine = TRANSMIT; 182 | } 183 | 184 | #endif // TX_module 185 | 186 | /** 187 | * Receiver 188 | */ 189 | #ifdef RX_module 190 | TX_Buffer_Len = 6; 191 | RX_Buffer_Len = 12; 192 | // last_frame_received 193 | // TX_period 194 | // lost_frames 195 | 196 | switch (stateMachine) { 197 | case RECEIVE: 198 | //Serial.println("Test"); 199 | if (int s = receiveData(RX_Buffer_Len)) { 200 | #ifdef DEBUG_ANALYZER 201 | Serial.println(""); Serial.print(s); Serial.print("\t"); 202 | #endif 203 | stateMachine = TRANSMIT; 204 | decodeServoData(); 205 | #ifdef DEBUG_ANALYZER 206 | Serial.print(millis() - RX_last_frame_received); Serial.print("ms\t"); 207 | #endif 208 | RX_last_frame_received = millis(); 209 | 210 | RX_hopping_time = micros() + (TX_period * 2); 211 | if (RX_RSSI < power_thr_low && power_delay_counter-- == 0) { 212 | power_increase(); 213 | power_delay_counter = TX_POWER_DELAY_FILTER; 214 | } 215 | if (RX_RSSI > power_thr_high && power_delay_counter-- == 0) { 216 | power_decrease(); 217 | power_delay_counter = TX_POWER_DELAY_FILTER; 218 | } 219 | failsafe_state = false; 220 | } 221 | break; 222 | case TRANSMIT: 223 | TX_Buffer[0] = RX_RSSI; 224 | TX_Buffer[1] = current_power; 225 | TX_Buffer[2] = 0; 226 | TX_Buffer[3] = 0; 227 | TX_Buffer[4] = 0; 228 | TX_Buffer[5] = 0; 229 | packet_timer = micros(); 230 | sendBufferData(); 231 | Hopping(); 232 | stateMachine = RECEIVE; 233 | 234 | calculated_rssi = calculate_rssi(RX_RSSI); 235 | //calculated_lost_frames_rssi= calculate_lost_frames_rssi(lost_frames); 236 | 237 | manage_servos(); 238 | 239 | 240 | //Serial.println(micros() - packet_timer); 241 | #ifdef DEBUG_ANALYZER 242 | for (int i = 0; i < SERVO_CHANNELS; i++) { 243 | Serial.print("\t"); 244 | Serial.print(Servo_Buffer[i]); 245 | } 246 | #endif 247 | break; 248 | } 249 | // forced hopping if no data received 250 | // TODO: Fast recover then slow recover 251 | if (micros() > RX_hopping_time) { 252 | 253 | RX_hopping_time = micros() + (TX_period * 2); 254 | 255 | Hopping(); 256 | 257 | #ifdef DEBUG_CH_FREQ 258 | Serial.print("No FR: "); 259 | Serial.print(millis() - RX_last_frame_received); 260 | Serial.print("ms\thop: "); 261 | Serial.println(current_channel); 262 | #endif 263 | } 264 | if (micros() > RX_frame_time) { 265 | // SBUS 266 | #ifdef SBUS_module 267 | send_servo_frame(); // fail safe is inside 268 | RX_frame_time = micros() + sbus_frame_period; 269 | #endif // SBUS_module 270 | 271 | // IBUS 272 | #ifdef IBUS_module 273 | #ifdef DEBUG_RX_OUTPUT 274 | // Show delay of each frame 275 | Serial.print("\t Delayed[us]="); 276 | Serial.println(micros() - RX_frame_time, DEC); 277 | #endif //DEBUG_RX_OUTPUT 278 | if (!failsafe_state) 279 | send_servo_frame(); 280 | // Show calculated RSSI 281 | //Serial.print("\t"); 282 | //Serial.println(calculate_rssi(RX_RSSI)); 283 | RX_frame_time = micros() + ibus_frame_period; 284 | #endif // IBUS_module 285 | 286 | // MSP 287 | #ifdef MSP_module 288 | // TODO: every 5th frame send request for data and block 289 | // the send_servo_frame() until data is received - do not wait too long (max 2 frames) 290 | if (!failsafe_state) 291 | send_servo_frame(); 292 | #endif // MSP_module 293 | if (failsafe_state) 294 | Serial.println("Failsafe !!!"); 295 | } 296 | // Failsafe 297 | if (millis() - RX_last_frame_received > FAILSAFE_DELAY_MS) { 298 | // IBUS, MSP - stop transmitting data 299 | failsafe_state = true; 300 | // PPM - send data out of range 301 | set_servos_failsafe(); 302 | } 303 | 304 | // obsługa konfiguracji 305 | 306 | #endif //RX_module 307 | 308 | wdt_reset(); 309 | } 310 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LoRaRC 2 | LoRa for Remote Control 3 | 4 | The goal of this project is to use LoRa modules (RFM95/96) with Arduino to remotely control an aircraft or drone. The expectation is that using LoRa modules shall improve flight distance due to great sensitivity. The drawback is data rate limited by the technology. 5 | 6 | Beware! 7 | This project is for hobby and technology "exploration" purposes. Not for commercial use! You are free to use it for your personal purposes. I am not taking the responsibility for the robustness or the stability of this solution. Use at your own risk. Make sure you are working within your country regulations and limitations. 8 | See frequencies over the countries: https://www.thethingsnetwork.org/docs/lorawan/frequency-plans.html 9 | Play safe, have fun :). 10 | 11 | There are following targets to be achieved in this project ("+" means DONE): 12 | + (+)Low cost communication modules (RFM95 + Arduino ProMini) 13 | - ready to use Arduino_Lora library, https://github.com/sandeepmistry/arduino-LoRa - requires updates or will be included as a part of this project 14 | + (+)TX and RX functionality in the same code - to simplify configuration, just select it by #define in config.h 15 | + (+)Bi-directional communication, possibility to read (limited) telemetry 16 | + (+)Frequency hopping, possibility to configure number of hop channels and frequencies 17 | + (+)Dynamic TX Power adjustment, based on RSSI level, configurable power range 18 | - Dynamic Lora Bandwidht, depending most likely on lost frames 19 | + (+)Maximum 8 channels (compressed with method known from SBUS) to send 12 bytes 20 | + (+)Response frame limited to 6 bytes 21 | - Possible frame rates: 12ms, 25ms, 45ms, 85ms (SF6, CRC, BW500..250..125..62.5KHz). Can be 30% less if telemetry will be turned off, but then fixed Power and fixed Bandwith must be applied - not in plans at the moment. 22 | + (+)RSSI is provided and injected to channel 8 (can be configured) and it is based on RSSI signal of LoRa modules. 23 | - RSSI based on lost frames (TODO) 24 | - External protocols: 25 | + (+)PPM as input 26 | + (+)IBUS (FlySky protocol - 7ms update time, 115kbs, non-inverted serial, can be used with native Serial of Arduino) as output 27 | - IBUS as input (TODO) 28 | + (+)SBUS as output (DONE) 29 | - SBUS as input (TODO) 30 | - protocol for telemetry: MSP (TODO) 31 | - TBS CRLF as input (some day..., anyone???) 32 | - telemetry data will be limited to several parameters, send in compressed form, with one parameter at a time (or few but limited to 4..5 bytes): 33 | - Signal strength, Voltage, Current, Distance, 34 | - Altitude, Angle Pitch, Angle Roll, Home direction 35 | - GPS Long 36 | - GPS Lat 37 | (that should be all what is mandatory when you loose Video link and may be needed to find lost plane) 38 | + (+)Failsafe in receiver to be provided (PPM data out of range, IBUS stops sending data) 39 | - Setup process is considered (to configure and store data without need of reprogramming) 40 | - Binding process is considered, although, currently there is no header in the data frame, only CRC is checked, therefore frequency hopping protects us from failure ;) 41 | 42 | Hardware: 43 | - Arduino Pro Mini 5V/16MHz with voltage regulator changed to 3,3V 44 | - RFM95 for 868MHz (see https://github.com/sandeepmistry/arduino-LoRa for wiring description) 45 | - Antenna (Dipole, Moxon) 46 | 47 | Testing: 48 | - First in flight testing (not as controller) - altitude up to 800m and distance up to 900m. Video taken during the flight: https://youtu.be/bZgweGUDXOE ; Report from the flight: https://github.com/winiak/LoRaRC/blob/master/Reports/link_test_19.08.2018.xlsx 49 | - Live testing in flight... I did already ~20 flights (status on 12.09.2018), some fixes were done (failsafe issue, power management thresholds). 50 | - The longest flight was 5,8km at altitude of 300m (another one at 200m) where the video link was a limit. Power set in both TX and RX was set to 10mW! See video, log can be provided if needed: https://www.youtube.com/watch?v=st8SkwsevAg&index=4&list=PLqifu3lTwTqqz3f-RLDulBpYAeGYek0TZ 51 | 52 | 53 | 54 | https://revspace.nl/DecodingLora 55 | https://www.thethingsnetwork.org/docs/lorawan 56 | http://www.hoperf.com/upload/rf/RFM95_96_97_98W.pdf 57 | http://www.semtech.com/apps/filedown/down.php?file=SX1272LoRaCalculatorSetup1%271.zip 58 | -------------------------------------------------------------------------------- /RFM95-Arduino-connection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winiak/LoRaRC/b2b00fb1e371ad0e55ad8b3e0729e3fd24f82d21/RFM95-Arduino-connection.png -------------------------------------------------------------------------------- /RFM95-pinout.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winiak/LoRaRC/b2b00fb1e371ad0e55ad8b3e0729e3fd24f82d21/RFM95-pinout.png -------------------------------------------------------------------------------- /Reports/link_test_19.08.2018.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winiak/LoRaRC/b2b00fb1e371ad0e55ad8b3e0729e3fd24f82d21/Reports/link_test_19.08.2018.xlsx -------------------------------------------------------------------------------- /Servos.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * Servo functions used in project. 3 | * 4 | * Thanks to Zendes for SBUS encoding/decoding code https://github.com/zendes/SBUS 5 | */ 6 | 7 | char buildServoData() 8 | { 9 | // use SBUS compression method 10 | TX_Buffer[0] = "S"; 11 | TX_Buffer[1] = (uint8_t) ((Servos[0] & 0x07FF)); 12 | TX_Buffer[2] = (uint8_t) ((Servos[0] & 0x07FF)>>8 | (Servos[1] & 0x07FF)<<3); 13 | TX_Buffer[3] = (uint8_t) ((Servos[1] & 0x07FF)>>5 | (Servos[2] & 0x07FF)<<6); 14 | TX_Buffer[4] = (uint8_t) ((Servos[2] & 0x07FF)>>2); 15 | TX_Buffer[5] = (uint8_t) ((Servos[2] & 0x07FF)>>10 | (Servos[3] & 0x07FF)<<1); 16 | TX_Buffer[6] = (uint8_t) ((Servos[3] & 0x07FF)>>7 | (Servos[4] & 0x07FF)<<4); 17 | TX_Buffer[7] = (uint8_t) ((Servos[4] & 0x07FF)>>4 | (Servos[5] & 0x07FF)<<7); 18 | TX_Buffer[8] = (uint8_t) ((Servos[5] & 0x07FF)>>1); 19 | TX_Buffer[9] = (uint8_t) ((Servos[5] & 0x07FF)>>9 | (Servos[6] & 0x07FF)<<2); 20 | TX_Buffer[10] = (uint8_t) ((Servos[6] & 0x07FF)>>6 | (Servos[7] & 0x07FF)<<5); 21 | TX_Buffer[11] = (uint8_t) ((Servos[7] & 0x07FF)>>3); 22 | return 12; 23 | } 24 | 25 | void decodeServoData() { 26 | char i = 1; 27 | Servo_Buffer[0] = (uint16_t) ((RX_Buffer[i+0] |RX_Buffer[i+1] <<8) & 0x07FF); 28 | Servo_Buffer[1] = (uint16_t) ((RX_Buffer[i+1]>>3 |RX_Buffer[i+2] <<5) & 0x07FF); 29 | Servo_Buffer[2] = (uint16_t) ((RX_Buffer[i+2]>>6 |RX_Buffer[i+3] <<2 |RX_Buffer[i+4]<<10) & 0x07FF); 30 | Servo_Buffer[3] = (uint16_t) ((RX_Buffer[i+4]>>1 |RX_Buffer[i+5] <<7) & 0x07FF); 31 | Servo_Buffer[4] = (uint16_t) ((RX_Buffer[i+5]>>4 |RX_Buffer[i+6] <<4) & 0x07FF); 32 | Servo_Buffer[5] = (uint16_t) ((RX_Buffer[i+6]>>7 |RX_Buffer[i+7] <<1 |RX_Buffer[i+8]<<9) & 0x07FF); 33 | Servo_Buffer[6] = (uint16_t) ((RX_Buffer[i+8]>>2 |RX_Buffer[i+9] <<6) & 0x07FF); 34 | Servo_Buffer[7] = (uint16_t) ((RX_Buffer[i+9]>>5 |RX_Buffer[i+10] <<3) & 0x07FF); 35 | } 36 | 37 | void servoTester() { 38 | static unsigned int swipeval = 1000; 39 | static char sign = 1; 40 | static unsigned int sstep = 20; 41 | 42 | if (swipeval <= 1000) 43 | sign = 1; 44 | if (swipeval >= 2000) 45 | sign = -1; 46 | swipeval = swipeval + sstep * sign; 47 | 48 | for (char i=0; i<8; i++) 49 | Servos[i] = (i != 2 ? swipeval : Servos[i]); 50 | } 51 | -------------------------------------------------------------------------------- /TODO.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * TODO: 3 | * + Connection - CHECKED 4 | * + Frequency Hopping - CHECKED 5 | * + Servo signal operation - CHECKED 6 | * + dynamic TX power CHECKED 7 | * + dynamic RX power CHECKED 8 | * - read iBus (Serial -> Servo Buffer) 9 | * + generate iBus w/o Interruption (Check if possible using Interrupts) 10 | * - test ibus 11 | * + failsafe 12 | * - test link 13 | * 14 | * - dynamic Signal Bandwidth 15 | * - in-flight setup 16 | * - add 4 bytes for binding 17 | * - add binding process 18 | * - lost frames statistics (lost frames per hopping channel) 19 | * 20 | * - Protocols: 21 | * - iBus 22 | * - MSP 23 | * - sBus 24 | * - PPM 25 | */ 26 | -------------------------------------------------------------------------------- /config.h: -------------------------------------------------------------------------------- 1 | 2 | enum stateMachineDef {SETUP = 0, TRANSMIT = 1, RECEIVE = 2, BIND = 3 }; 3 | 4 | // Dubugging - select any 5 | //#define DEBUG_CH_FREQ 6 | //#define DEBUG_RADIO_EXCH 7 | //#define DEBUG_ANALYZER 8 | //#define TX_SERVO_TESTER 9 | //#define DEBUG_RX_OUTPUT 10 | 11 | // Transmitter or Receiver - select one 12 | //#define TX_module 13 | #define RX_module 14 | 15 | // Communication type - select one 16 | //#define PPM_module // using ICP for TX or declared for TX 17 | //#define IBUS_module // using UART 18 | #define SBUS_module // using UART at 100000N2 19 | //#define MSP_module // using UART 20 | 21 | // Transmitting power in dBm: 2 to 20, default 17 22 | byte tx_power_low = 4; // 4=>2.5mW; 6=>5mW; 10=>10mW 23 | byte tx_power_high = 10; // 12=>16mW; 14=>25mW; 16=>40mW; 18=>63mW; 20=>100mW 24 | 25 | int power_thr_high = -90; // testing: 190 (-65); flying: 180 (-75) 26 | int power_thr_low = -100; // testing: 180 (-75); flying: 160 (-95) 27 | byte tx_power_step = 2; 28 | #define TX_POWER_DELAY_FILTER sizeof(hop_list)*2 29 | 30 | // Signal bandwidth and frame time 31 | // full range 7.8E3, 10.4E3, 15.6E3, 20.8E3, 31.25E3, 41.7E3, 62.5E3, 125E3, 250E3, 500E3. 32 | // measured (16Bytes from Transmitter, 6Bytes from Receiver : 33 | // SF6,BW500 => 10ms; SF6,BW250 => 20ms; SF6,BW125 => 40ms; SF6,BW62,5 => 80ms 34 | unsigned long BW_low = 125E3; //Hz 35 | unsigned long BW_high = 250E3; //Hz 36 | unsigned long F_rate_low = 45000; //us 37 | unsigned long F_rate_high = 25000; //us 38 | byte spread_factor = 6; 39 | 40 | #define base_frequency 868100000 41 | #define frequency_step 100000 42 | const uint8_t hop_list[] = {5,7,12}; 43 | #define LOST_FRAMES_COUNT 30 // sizeof(hop_list) * 10 44 | 45 | 46 | // Servos & channels 47 | #define SERVO_CHANNELS 8 48 | volatile unsigned int Servo_Buffer[SERVO_CHANNELS] = {1500, 1500, 1000, 1500, 1500, 1500, 1500, 1500}; 49 | unsigned int Servos[SERVO_CHANNELS] = {1500, 1500, 1000, 1500, 1500, 1500, 1500, 1500}; 50 | unsigned int Servo_Failsafe[SERVO_CHANNELS] = {1500, 1500, 900, 1500, 1500, 1500, 1500, 150}; 51 | #define FAILSAFE_DELAY_MS 800 52 | 53 | #define SERVO_SHIFT 0 //(-16) // PPM = -16 | others = 0 54 | 55 | // inject RSSI signal into Channel (CH1 = 0) 56 | #define INJECT_RSSI_IN_CH 7 57 | 58 | // PPM 59 | #define PPM_OUT 8 60 | #define PPM_OUT_HIGH PORTB |= _BV(0) 61 | #define PPM_OUT_LOW PORTB &= ~_BV(0) 62 | #define PPM_IN 8 // ICP1 63 | 64 | // RFM95 to Arduino HW connection 65 | #define NSS_PIN 10 66 | #define NRESET_PIN A0 67 | #define DIO0_PIN 2 // must be IRQ assignable 68 | -------------------------------------------------------------------------------- /ibus.ino: -------------------------------------------------------------------------------- 1 | #ifdef IBUS_module 2 | 3 | /** 4 | * iBus protocol generator by Konrad Winiarski 5 | * - why iBus? Latency! Frame sent to FC every 7ms! (can vary in wide range without impact on link quality) 6 | * - 14 channels by default !! in 32 bytes 7 | * - support bi-directional communication (just in case) 8 | * - consumes UART, SoftSerial does not work at 115200 :( 9 | * 10 | * Frame example: 11 | * x20x40 = Header (length of 32 bytes + command) 12 | * xDCx05 = 1500 ch 1 13 | * xDBx05 = 1499 ch 2 14 | * xEFx03 = 1007 ch 3 15 | * xDDx05 = 1501 ch 4 16 | * xD0x07 = 2000 ch 5 17 | * xD0x07 = 2000 ch 6 18 | * xDCx05 = 1500 ch 7 19 | * xDCx05 = 1500 ch 8 20 | * xDCx05 = 1500 ch 9 21 | * xDCx05 = 1500 ch 10 22 | * xDCx05 = 1500 ch 11 23 | * xDCx05 = 1500 ch 12 24 | * xDCx05 = 1500 ch 13 25 | * xDCx05 = 1500 ch 14 26 | * x54xF3 = Checksum: 0xFFFF - (0x20 + 0x40 ... sum of all !) 27 | * 28 | * Thanks to Basejunction for introduction :) 29 | * https://basejunction.wordpress.com/2015/08/23/en-flysky-i6-14-channels-part1/ 30 | */ 31 | 32 | #ifdef RX_module 33 | 34 | #define IBUS_MAXCHANNELS 14 35 | 36 | void setup_module() { 37 | // module works on hardware serial - must be 115200 38 | } 39 | 40 | void send_servo_frame() { 41 | static long ibus_frame_timer; 42 | if (failsafe_state) { 43 | Serial.println("Failsafe"); 44 | return; 45 | } 46 | char frame_buffer[32]; 47 | char ptr, len; 48 | unsigned int chksum = 65535 - 32 - 64; //; // - 0x20 - 0x40; // 0x0000 - 0x20 - 0x40; 49 | unsigned int temp_servo, temp; 50 | 51 | frame_buffer[0] = 0x20; // frame length = 32 bytes 52 | frame_buffer[1] = 0x40; // command - servo data 53 | 54 | // Build frame 55 | for (byte i = 0; i < IBUS_MAXCHANNELS; i++) { 56 | temp_servo = (i < SERVO_CHANNELS ? (Servos[i] + SERVO_SHIFT) : 1500); // add shift (config) 57 | frame_buffer[(i * 2) + 2] = temp_servo % 256; 58 | chksum -= (temp_servo % 256); 59 | frame_buffer[(i * 2) + 3] = temp_servo / 256; 60 | chksum -= (temp_servo / 256); 61 | } 62 | 63 | // Add CRC to frame 64 | frame_buffer[30] = chksum % 256; 65 | frame_buffer[31] = chksum / 256; 66 | 67 | // Send to serial (soft serial does not work at 115200) - HW UART MUST BE USED 68 | Serial.write(frame_buffer); 69 | 70 | 71 | #ifdef DEBUG_RX_OUTPUT 72 | // Show all data in ASCII 73 | PrintHex8(frame_buffer, 32); 74 | Serial.println(); 75 | 76 | /* 77 | for (byte i = 0; i < 8; i++) 78 | { 79 | Serial.print((Servos[i] + SERVO_SHIFT) / 2); 80 | Serial.print(" "); 81 | } 82 | Serial.println(); 83 | */ 84 | #endif // DEBUG_RX_OUTPUT 85 | } 86 | 87 | 88 | #endif //RX_module 89 | #endif // IBUS 90 | -------------------------------------------------------------------------------- /msp.h: -------------------------------------------------------------------------------- 1 | 2 | // requests & replies 3 | #define MSP_API_VERSION 1 4 | #define MSP_FC_VARIANT 2 5 | #define MSP_FC_VERSION 3 6 | #define MSP_BOARD_INFO 4 7 | #define MSP_BUILD_INFO 5 8 | #define MSP_CALIBRATION_DATA 14 9 | #define MSP_FEATURE 36 10 | #define MSP_BOARD_ALIGNMENT 38 11 | #define MSP_CURRENT_METER_CONFIG 40 12 | #define MSP_RX_CONFIG 44 13 | #define MSP_SONAR_ALTITUDE 58 14 | #define MSP_ARMING_CONFIG 61 15 | #define MSP_RX_MAP 64 // get channel map (also returns number of channels total) 16 | #define MSP_LOOP_TIME 73 // FC cycle time i.e looptime parameter 17 | #define MSP_STATUS 101 18 | #define MSP_RAW_IMU 102 19 | #define MSP_SERVO 103 20 | #define MSP_MOTOR 104 21 | #define MSP_RC 105 22 | #define MSP_RAW_GPS 106 23 | #define MSP_COMP_GPS 107 // distance home, direction home 24 | #define MSP_ATTITUDE 108 25 | #define MSP_ALTITUDE 109 26 | #define MSP_ANALOG 110 27 | #define MSP_RC_TUNING 111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID 28 | #define MSP_PID 112 // P I D coeff 29 | #define MSP_MISC 114 30 | #define MSP_SERVO_CONFIGURATIONS 120 31 | #define MSP_NAV_STATUS 121 // navigation status 32 | #define MSP_SENSOR_ALIGNMENT 126 // orientation of acc,gyro,mag 33 | #define MSP_STATUS_EX 150 34 | #define MSP_SENSOR_STATUS 151 35 | #define MSP_BOXIDS 119 36 | #define MSP_UID 160 // Unique device ID 37 | #define MSP_GPSSVINFO 164 // get Signal Strength (only U-Blox) 38 | #define MSP_GPSSTATISTICS 166 // get GPS debugging data 39 | #define MSP_SET_PID 202 // set P I D coeff 40 | 41 | 42 | // commands 43 | #define MSP_SET_HEAD 211 // define a new heading hold direction 44 | #define MSP_SET_RAW_RC 200 // 8 rc chan 45 | #define MSP_SET_RAW_GPS 201 // fix, numsat, lat, lon, alt, speed 46 | #define MSP_SET_WP 209 // sets a given WP (WP#, lat, lon, alt, flags) 47 | 48 | 49 | // bits of getActiveModes() return value 50 | #define MSP_MODE_ARM 0 51 | #define MSP_MODE_ANGLE 1 52 | #define MSP_MODE_HORIZON 2 53 | #define MSP_MODE_NAVALTHOLD 3 /* cleanflight BARO */ 54 | #define MSP_MODE_MAG 4 55 | #define MSP_MODE_HEADFREE 5 56 | #define MSP_MODE_HEADADJ 6 57 | #define MSP_MODE_CAMSTAB 7 58 | #define MSP_MODE_NAVRTH 8 /* cleanflight GPSHOME */ 59 | #define MSP_MODE_NAVPOSHOLD 9 /* cleanflight GPSHOLD */ 60 | #define MSP_MODE_PASSTHRU 10 61 | #define MSP_MODE_BEEPERON 11 62 | #define MSP_MODE_LEDLOW 12 63 | #define MSP_MODE_LLIGHTS 13 64 | #define MSP_MODE_OSD 14 65 | #define MSP_MODE_TELEMETRY 15 66 | #define MSP_MODE_GTUNE 16 67 | #define MSP_MODE_SONAR 17 68 | #define MSP_MODE_BLACKBOX 18 69 | #define MSP_MODE_FAILSAFE 19 70 | #define MSP_MODE_NAVWP 20 /* cleanflight AIRMODE */ 71 | #define MSP_MODE_AIRMODE 21 /* cleanflight DISABLE3DSWITCH */ 72 | #define MSP_MODE_HOMERESET 22 /* cleanflight FPVANGLEMIX */ 73 | #define MSP_MODE_GCSNAV 23 /* cleanflight BLACKBOXERASE */ 74 | #define MSP_MODE_HEADINGLOCK 24 75 | #define MSP_MODE_SURFACE 25 76 | #define MSP_MODE_FLAPERON 26 77 | #define MSP_MODE_TURNASSIST 27 78 | #define MSP_MODE_NAVLAUNCH 28 79 | #define MSP_MODE_AUTOTRIM 29 80 | 81 | -------------------------------------------------------------------------------- /msp.ino: -------------------------------------------------------------------------------- 1 | #ifdef MSP_module 2 | 3 | /** 4 | * This module is to provide bidirectional communication between RX and FC 5 | * in Multiwii Serial Protocol standard. 6 | * The goal is to send RC values by MSP commands and get any needed data 7 | * out of FC and send it to the TX - this will provide full telemetry capability. 8 | * 9 | * Due to the limitation of downlink (currenlty limited to 6 bytes, but 5 available), the downlink 10 | * will be limited to few, selected paramaters (Voltage, Amps, GPS coords). 11 | * 12 | * It is based on the MSP documentation available on Multiwii project WIKI: 13 | * http://www.multiwii.com/wiki/index.php?title=Multiwii_Serial_Protocol 14 | * 15 | */ 16 | #ifdef RX_module 17 | 18 | #include "msp.h" 19 | #define MSP_MAXCHANNELS 8 // max 16 (or alawys 16?) 20 | 21 | void setup_module() { 22 | // module works on hardware serial - shall be 57600 or 115200 23 | } 24 | 25 | void send_message(uint8_t messageID, void * payload, uint8_t messageSize) { 26 | uint8_t frame_size = 0; 27 | char frame_buffer[32]; 28 | 29 | frame_buffer[0] = '$'; 30 | frame_buffer[1] = 'M'; 31 | frame_buffer[2] = '<'; 32 | frame_buffer[3] = messageSize; 33 | frame_buffer[4] = messageID; 34 | 35 | uint8_t checksum = frame_size ^ messageID; 36 | uint8_t * payloadPtr = (uint8_t*)payload; 37 | 38 | for (uint8_t i = 5; i < messageSize + 5 ; ++i) { 39 | uint8_t b = *(payloadPtr++); 40 | checksum ^= b; 41 | frame_buffer[i] = b; 42 | } 43 | 44 | frame_buffer[messageSize + 5] = checksum; 45 | 46 | Serial.write(frame_buffer); 47 | } 48 | 49 | void send_request(uint8_t messageID) { 50 | uint8_t frame_size = 0; 51 | uint8_t checksum = frame_size ^ messageID; 52 | char frame_buffer[32]; 53 | 54 | frame_buffer[0] = '$'; 55 | frame_buffer[1] = 'M'; 56 | frame_buffer[2] = '<'; 57 | frame_buffer[3] = '0'; 58 | frame_buffer[4] = messageID; 59 | frame_buffer[5] = checksum; 60 | 61 | Serial.write(frame_buffer); 62 | } 63 | 64 | void send_servo_frame() { 65 | uint8_t frame_size = MSP_MAXCHANNELS * 2; 66 | uint8_t messageID = MSP_RC; // Send RC data message 67 | uint8_t checksum = frame_size ^ messageID; 68 | char frame_buffer[32]; 69 | unsigned int temp_servo, temp; 70 | uint8_t i; 71 | 72 | frame_buffer[0] = '$'; 73 | frame_buffer[1] = 'M'; 74 | frame_buffer[2] = '<'; 75 | frame_buffer[3] = frame_size; //frame_size 76 | frame_buffer[4] = messageID; 77 | 78 | // Build frame 79 | for (i = 5; i < MSP_MAXCHANNELS + 5; i++) { 80 | temp_servo = (i < SERVO_CHANNELS ? (Servos[i] + SERVO_SHIFT)/2 : 1500); // add shift (config) and make compatible (divide by 2) 81 | frame_buffer[(i * 2) + 2] = temp_servo % 256; 82 | checksum ^= (temp_servo % 256); 83 | frame_buffer[(i * 2) + 3] = temp_servo / 256; 84 | checksum ^= (temp_servo / 256); 85 | } 86 | 87 | // Add CRC to frame 88 | frame_buffer[MSP_MAXCHANNELS * 2 + 5]; 89 | 90 | // Send to serial (soft serial does not work at 115200) - HW UART MUST BE USED 91 | Serial.println(frame_buffer); 92 | } 93 | 94 | #endif // RX_module 95 | 96 | #endif // MSP_module 97 | -------------------------------------------------------------------------------- /ppm.ino: -------------------------------------------------------------------------------- 1 | #ifdef TX_module 2 | #ifdef PPM_module 3 | 4 | static bool PPM_corrupted = true; 5 | static char PPM_corrupted_counter = 0; 6 | static char channel_no = 0; 7 | static char channel_counter = 0; 8 | 9 | void setup_module() { 10 | Config_ICP1_PPM(); 11 | sei(); 12 | } 13 | 14 | bool check_PPM_corrupted () { 15 | return PPM_corrupted; 16 | } 17 | 18 | /** 19 | * Configure Timer to capture PPM values 20 | */ 21 | void Config_ICP1_PPM() // Use ICP1 in input capture mode 22 | { 23 | pinMode(PPM_IN, INPUT); 24 | 25 | // Setup timer1 for input capture (PSC=8 -> 0.5ms precision) 26 | TCCR1A = ((1 << WGM10) | (1 << WGM11)); 27 | TCCR1B = ((1 << WGM12) | (1 << WGM13) | (1 << CS11) | (1 < 6000) // new frame detection : >4ms LOW 46 | { 47 | channel_counter = channel_no; 48 | channel_no = 0; 49 | } 50 | else { 51 | // check the signal time and update the channel if it is between 750us-2500us 52 | if ((time_temp > 1500) && (time_temp < 5000)) { 53 | Servos[channel_no] = time_temp / 2; // write the low byte of the value into the servo value buffer. 54 | if (channel_no < 8) channel_no++; 55 | PPM_corrupted_counter = 0; 56 | PPM_corrupted = false; 57 | } else { 58 | PPM_corrupted_counter++; 59 | if (PPM_corrupted_counter > 10) { 60 | PPM_corrupted = true; 61 | channel_counter = 0; 62 | } 63 | 64 | } 65 | } 66 | } 67 | 68 | #endif // PPM_module 69 | #endif // TX_module 70 | 71 | 72 | /** 73 | * 74 | * Receiver - PPM signal generation 75 | * 76 | */ 77 | #ifdef RX_module 78 | #ifdef PPM_module 79 | 80 | void setup_module() { 81 | ; 82 | } 83 | 84 | #endif // PPM_module 85 | #endif // TX_module 86 | 87 | -------------------------------------------------------------------------------- /sbus.ino: -------------------------------------------------------------------------------- 1 | #ifdef SBUS_module 2 | /** 3 | * Tested with SP Racing f3 flight controller and iNav. 4 | * Still Beta, not tested in flight yet. 5 | * With this communication type, UART is set to 100000 8E2. Standard Arduino monitor is not supporting this speed, so don't bother. Use Putty. 6 | * 7 | * Based on code of Pawel Spychalski: 8 | * https://quadmeup.com/generate-s-bus-with-arduino-in-a-simple-way/ 9 | * 10 | * For some reason, there is an error when trying to create pocket first, then send it by Serial.write(pocket); 11 | * If there is "\0" Serial function deos not send it. When trying to change data type or setting fixed buffer length, there is an error during compilation. 12 | * Maybe someone will find and fix the proble someday. For now, there are plenty of Serial.write() calls. 13 | */ 14 | #ifdef RX_module 15 | 16 | #define RC_CHANNEL_MIN 990 17 | #define RC_CHANNEL_MAX 2010 18 | 19 | #define SBUS_MIN_OFFSET 173 20 | #define SBUS_MID_OFFSET 992 21 | #define SBUS_MAX_OFFSET 1811 22 | #define SBUS_CHANNEL_NUMBER 16 23 | #define SBUS_PACKET_LENGTH 25 24 | #define SBUS_FRAME_HEADER 0x0f 25 | #define SBUS_FRAME_FOOTER 0x00 26 | #define SBUS_FRAME_FOOTER_V2 0x04 27 | #define SBUS_STATE_FAILSAFE 0x08 28 | #define SBUS_STATE_SIGNALLOSS 0x04 29 | #define SBUS_UPDATE_RATE 15 //ms 30 | 31 | void setup_module() { 32 | // module works on hardware serial - must be 100000/8E2 33 | //Serial.end(); 34 | //Serial.begin(100000, SERIAL_8E2); 35 | } 36 | 37 | 38 | void send_servo_frame(){ 39 | 40 | unsigned char packet[25]; 41 | static int output[SBUS_CHANNEL_NUMBER] = {0}; 42 | 43 | /* 44 | * Map 1000-2000 with middle at 1500 chanel values to 45 | * 173-1811 with middle at 992 S.BUS protocol requires 46 | */ 47 | for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) { 48 | output[i] = map( (i < SERVO_CHANNELS ? (Servos[i] + SERVO_SHIFT) : 1500), RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET); 49 | } 50 | 51 | uint8_t stateByte = 0x00; 52 | 53 | if (lost_frames > 12) { 54 | stateByte |= SBUS_STATE_SIGNALLOSS; 55 | } 56 | if (failsafe_state) { 57 | stateByte |= SBUS_STATE_FAILSAFE; 58 | } 59 | /* 60 | packet[0] = SBUS_FRAME_HEADER; //Header 61 | packet[1] = (uint8_t) (output[0] & 0x07FF); 62 | packet[2] = (uint8_t) ((output[0] & 0x07FF)>>8 | (output[1] & 0x07FF)<<3); 63 | packet[3] = (uint8_t) ((output[1] & 0x07FF)>>5 | (output[2] & 0x07FF)<<6); 64 | packet[4] = (uint8_t) ((output[2] & 0x07FF)>>2); 65 | packet[5] = (uint8_t) ((output[2] & 0x07FF)>>10 | (output[3] & 0x07FF)<<1); 66 | packet[6] = (uint8_t) ((output[3] & 0x07FF)>>7 | (output[4] & 0x07FF)<<4); 67 | packet[7] = (uint8_t) ((output[4] & 0x07FF)>>4 | (output[5] & 0x07FF)<<7); 68 | packet[8] = (uint8_t) ((output[5] & 0x07FF)>>1); 69 | packet[9] = (uint8_t) ((output[5] & 0x07FF)>>9 | (output[6] & 0x07FF)<<2); 70 | packet[10] = (uint8_t) ((output[6] & 0x07FF)>>6 | (output[7] & 0x07FF)<<5); 71 | packet[11] = (uint8_t) ((output[7] & 0x07FF)>>3); 72 | packet[12] = (uint8_t) ((output[8] & 0x07FF)); 73 | packet[13] = (uint8_t) ((output[8] & 0x07FF)>>8 | (output[9] & 0x07FF)<<3); 74 | packet[14] = (uint8_t) ((output[9] & 0x07FF)>>5 | (output[10] & 0x07FF)<<6); 75 | packet[15] = (uint8_t) ((output[10] & 0x07FF)>>2); 76 | packet[16] = (uint8_t) ((output[10] & 0x07FF)>>10 | (output[11] & 0x07FF)<<1); 77 | packet[17] = (uint8_t) ((output[11] & 0x07FF)>>7 | (output[12] & 0x07FF)<<4); 78 | packet[18] = (uint8_t) ((output[12] & 0x07FF)>>4 | (output[13] & 0x07FF)<<7); 79 | packet[19] = (uint8_t) ((output[13] & 0x07FF)>>1); 80 | packet[20] = (uint8_t) ((output[13] & 0x07FF)>>9 | (output[14] & 0x07FF)<<2); 81 | packet[21] = (uint8_t) ((output[14] & 0x07FF)>>6 | (output[15] & 0x07FF)<<5); 82 | packet[22] = (uint8_t) ((output[15] & 0x07FF)>>3); 83 | packet[23] = (uint8_t) stateByte; //Flags byte 84 | packet[24] = (uint8_t) SBUS_FRAME_FOOTER; //Footer 85 | */ 86 | 87 | Serial.write(SBUS_FRAME_HEADER); 88 | Serial.write(output[0] & 0x07FF); 89 | Serial.write((output[0] & 0x07FF)>>8 | (output[1] & 0x07FF)<<3); 90 | 91 | Serial.write((output[1] & 0x07FF)>>5 | (output[2] & 0x07FF)<<6); 92 | Serial.write((output[2] & 0x07FF)>>2); 93 | Serial.write((output[2] & 0x07FF)>>10 | (output[3] & 0x07FF)<<1); 94 | Serial.write((output[3] & 0x07FF)>>7 | (output[4] & 0x07FF)<<4); 95 | Serial.write((output[4] & 0x07FF)>>4 | (output[5] & 0x07FF)<<7); 96 | Serial.write((output[5] & 0x07FF)>>1); 97 | Serial.write((output[5] & 0x07FF)>>9 | (output[6] & 0x07FF)<<2); 98 | Serial.write((output[6] & 0x07FF)>>6 | (output[7] & 0x07FF)<<5); 99 | Serial.write((output[7] & 0x07FF)>>3); 100 | Serial.write((output[8] & 0x07FF)); 101 | Serial.write((output[8] & 0x07FF)>>8 | (output[9] & 0x07FF)<<3); 102 | Serial.write((output[9] & 0x07FF)>>5 | (output[10] & 0x07FF)<<6); 103 | Serial.write((output[10] & 0x07FF)>>2); 104 | Serial.write((output[10] & 0x07FF)>>10 | (output[11] & 0x07FF)<<1); 105 | Serial.write((output[11] & 0x07FF)>>7 | (output[12] & 0x07FF)<<4); 106 | Serial.write((output[12] & 0x07FF)>>4 | (output[13] & 0x07FF)<<7); 107 | Serial.write((output[13] & 0x07FF)>>1); 108 | Serial.write((output[13] & 0x07FF)>>9 | (output[14] & 0x07FF)<<2); 109 | Serial.write((output[14] & 0x07FF)>>6 | (output[15] & 0x07FF)<<5); 110 | Serial.write((output[15] & 0x07FF)>>3); 111 | Serial.write (stateByte); //Flags byte 112 | Serial.write(SBUS_FRAME_FOOTER); //Footer 113 | } 114 | 115 | 116 | 117 | #endif // RX module 118 | 119 | #endif // IBUS_module 120 | --------------------------------------------------------------------------------