englebert
/
OpenFlightTX
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
15 Commits
1 Branch
0 Tags
563 KiB
 
 
Tree: 3aa34c3779
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '3aa34c3779'
${ noResults }
OpenFlightTX/OpenFlightTX.ino

2924 lines
89 KiB
Raw Blame History

/*
* Ref:
* Manual - https://www.seeedstudio.com/blog/2019/07/05/u8g2-for-seeeduino-boards/
* Dual OLED - https://github.com/olikraus/u8g2/issues/77
*
* Every power cycle:
* 1. Read configuration from EEPROM
*
*/
#include <Arduino.h>
#include <SPI.h>
#include <U8g2lib.h>
#include <Wire.h>
#include <EEPROM.h>
#include <BleGamepad.h>
// For OTA and Settings over WiFi
#include <WiFi.h>
#include <WiFiClient.h>
#include <WebServer.h>
#include <Update.h>
// For NRF24L01 (E01) - EByte
// #include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
// Declaration for NRF24L01
#define NRF24_CE 5 // GPIO5 (CE)
#define NRF24_CSN 4 // GPIO4 (CSN)
// NRF24L01 registers we need - For 2.4G Scanning
#define _NRF24_CONFIG 0x00
#define _NRF24_EN_AA 0x01
#define _NRF24_RF_CH 0x05
#define _NRF24_RF_SETUP 0x06
#define _NRF24_RPD 0x09
// For channel scanner
#define MAX_CHANNELS 125
#define MAX_SAMPLES 100
int channel_loads[MAX_CHANNELS]; // Channel 0 ~ Channel 124
uint64_t pipeIn = 0xE8E8F0F0E1LL; // TODO: set it to EEPROM
RF24 radio(NRF24_CE, NRF24_CSN); // Starting up the module on GPIO5 (CE), GPIO4 (CSN)
bool nrf_enable = false;
bool nrf_scanner = false;
bool binding = false;
bool channel_used[MAX_CHANNELS];
uint8_t graph_type = 0;
uint32_t delay_maxrf_time = 0;
bool delay_maxrf = false;
TaskHandle_t SignalTask;
// TTTT TTTT TTTT ---- YYYY YYYY YYYY ---- PPPP PPPP PPPP ---- RRRR RRRR RRRR ---- SSSS SSSS = 72-bits ( 9 bytes )
// Data Structure
struct TxMessage {
uint16_t throttle;
uint16_t yaw;
uint16_t pitch;
uint16_t roll;
uint8_t switches;
};
TxMessage txmessage;
struct SyncData {
uint8_t freq_channel;
uint8_t dummy;
uint8_t fixed;
};
SyncData syncdata;
bool wifi_enable = false;
bool webserver_setup = false;
// Starting up Web Server
WebServer server(80);
// WebPages ----- START
/*** Firmware Upgrade Page ***/
const char* firmware_upgrade_html PROGMEM = R"rawliteral(
<script>
var iBytesUploaded = 0;
var iBytesTotal = 0;
var iPreviousBytesLoaded = 0;
var iMaxFilesize = 1048576; // 1MB
var oTimer = 0;
var sResultFileSize = '';
function secondsToTime(secs) {
var hr = Math.floor(secs / 3600);
var min = Math.floor((secs - (hr * 3600))/60);
var sec = Math.floor(secs - (hr * 3600) - (min * 60));
if (hr < 10) {hr = "0" + hr; }
if (min < 10) {min = "0" + min;}
if (sec < 10) {sec = "0" + sec;}
if (hr) {hr = "00";}
return hr + ':' + min + ':' + sec;
};
function bytesToSize(bytes) {
var sizes = ['Bytes', 'KB', 'MB'];
if (bytes == 0) return 'n/a';
var i = parseInt(Math.floor(Math.log(bytes) / Math.log(1024)));
return (bytes / Math.pow(1024, i)).toFixed(1) + ' ' + sizes[i];
};
function fileSelected() {
document.getElementById('upload_response').style.display = 'none';
document.getElementById('error').style.display = 'none';
document.getElementById('error2').style.display = 'none';
document.getElementById('abort').style.display = 'none';
document.getElementById('warnsize').style.display = 'none';
var oFile = document.getElementById('image_file').files[0];
var rFilter = /^(image\/bmp|image\/gif|image\/jpeg|image\/png|image\/tiff)$/i;
if (! rFilter.test(oFile.type)) {
document.getElementById('error').style.display = 'block';
return;
}
if (oFile.size > iMaxFilesize) {
document.getElementById('warnsize').style.display = 'block';
return;
}
var oImage = document.getElementById('preview');
var oReader = new FileReader();
oReader.onload = function(e){
oImage.src = e.target.result;
oImage.onload = function () { // binding onload event
sResultFileSize = bytesToSize(oFile.size);
document.getElementById('fileinfo').style.display = 'block';
document.getElementById('filename').innerHTML = 'Name: ' + oFile.name;
document.getElementById('filesize').innerHTML = 'Size: ' + sResultFileSize;
document.getElementById('filetype').innerHTML = 'Type: ' + oFile.type;
document.getElementById('filedim').innerHTML = 'Dimension: ' + oImage.naturalWidth + ' x ' + oImage.naturalHeight;
};
};
oReader.readAsDataURL(oFile);
}
function startUploading() {
iPreviousBytesLoaded = 0;
document.getElementById('upload_response').style.display = 'none';
document.getElementById('error').style.display = 'none';
document.getElementById('error2').style.display = 'none';
document.getElementById('abort').style.display = 'none';
document.getElementById('warnsize').style.display = 'none';
document.getElementById('progress_percent').innerHTML = '';
var oProgress = document.getElementById('progress');
oProgress.style.display = 'block';
oProgress.style.width = '0px';
var vFD = new FormData(document.getElementById('upload_form'));
var oXHR = new XMLHttpRequest();
oXHR.upload.addEventListener('progress', uploadProgress, false);
oXHR.addEventListener('load', uploadFinish, false);
oXHR.addEventListener('error', uploadError, false);
oXHR.addEventListener('abort', uploadAbort, false);
oXHR.open('POST', '/update');
oXHR.send(vFD);
oTimer = setInterval(doInnerUpdates, 300);
}
function doInnerUpdates() {
var iCB = iBytesUploaded;
var iDiff = iCB - iPreviousBytesLoaded;
if (iDiff == 0)
return;
iPreviousBytesLoaded = iCB;
iDiff = iDiff * 2;
var iBytesRem = iBytesTotal - iPreviousBytesLoaded;
var secondsRemaining = iBytesRem / iDiff;
var iSpeed = iDiff.toString() + 'B/s';
if (iDiff > 1024 * 1024) {
iSpeed = (Math.round(iDiff * 100/(1024*1024))/100).toString() + 'MB/s';
} else if (iDiff > 1024) {
iSpeed = (Math.round(iDiff * 100/1024)/100).toString() + 'KB/s';
}
document.getElementById('speed').innerHTML = iSpeed;
document.getElementById('remaining').innerHTML = '| ' + secondsToTime(secondsRemaining);
}
function uploadProgress(e) {
if (e.lengthComputable) {
iBytesUploaded = e.loaded;
iBytesTotal = e.total;
var iPercentComplete = Math.round(e.loaded * 100 / e.total);
var iBytesTransfered = bytesToSize(iBytesUploaded);
document.getElementById('progress_percent').innerHTML = iPercentComplete.toString() + '%';
document.getElementById('progress').style.width = (iPercentComplete * 4).toString() + 'px';
document.getElementById('b_transfered').innerHTML = iBytesTransfered;
if (iPercentComplete == 100) {
var oUploadResponse = document.getElementById('upload_response');
oUploadResponse.innerHTML = '<h1>Please wait...processing</h1>';
oUploadResponse.style.display = 'block';
}
} else {
document.getElementById('progress').innerHTML = 'unable to compute';
}
}
function uploadFinish(e) {
var oUploadResponse = document.getElementById('upload_response');
oUploadResponse.innerHTML = e.target.responseText;
oUploadResponse.style.display = 'block';
document.getElementById('progress_percent').innerHTML = '100%';
document.getElementById('progress').style.width = '400px';
document.getElementById('filesize').innerHTML = sResultFileSize;
document.getElementById('remaining').innerHTML = '| 00:00:00';
clearInterval(oTimer);
}
function uploadError(e) {
document.getElementById('error2').style.display = 'block';
clearInterval(oTimer);
}
function uploadAbort(e) {
document.getElementById('abort').style.display = 'block';
clearInterval(oTimer);
}
</script>
<style>
.container {
margin: auto;
width: 50%;
}
.upload_form_cont {
background: -moz-linear-gradient(#ffffff, #f2f2f2);
background: -ms-linear-gradient(#ffffff, #f2f2f2);
background: -webkit-gradient(linear, left top, left bottom, color-stop(0%, #ffffff), color-stop(100%, #f2f2f2));
background: -webkit-linear-gradient(#ffffff, #f2f2f2);
background: -o-linear-gradient(#ffffff, #f2f2f2);
filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#ffffff', endColorstr='#f2f2f2');
-ms-filter: "progid:DXImageTransform.Microsoft.gradient(startColorstr='#ffffff', endColorstr='#f2f2f2')";
background: linear-gradient(#ffffff, #f2f2f2);
color:#000;
overflow:hidden;
}
#upload_form {
float:center;
padding:20px;
width:700px;
}
#preview {
background-color:#fff;
display:block;
float:right;
width:200px;
}
#upload_form > div {
margin-bottom:10px;
}
#speed,#remaining {
float:left;
width:100px;
}
#b_transfered {
float:right;
text-align:right;
}
.clear_both {
clear:both;
}
input {
border-radius:10px;
-moz-border-radius:10px;
-ms-border-radius:10px;
-o-border-radius:10px;
-webkit-border-radius:10px;
border:1px solid #ccc;
font-size:14pt;
padding:5px 10px;
}
input[type=button] {
background: -moz-linear-gradient(#ffffff, #dfdfdf);
background: -ms-linear-gradient(#ffffff, #dfdfdf);
background: -webkit-gradient(linear, left top, left bottom, color-stop(0%, #ffffff), color-stop(100%, #dfdfdf));
background: -webkit-linear-gradient(#ffffff, #dfdfdf);
background: -o-linear-gradient(#ffffff, #dfdfdf);
filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#ffffff', endColorstr='#dfdfdf');
-ms-filter: "progid:DXImageTransform.Microsoft.gradient(startColorstr='#ffffff', endColorstr='#dfdfdf')";
background: linear-gradient(#ffffff, #dfdfdf);
}
#image_file {
width:400px;
}
#progress_info {
font-size:10pt;
}
#fileinfo,#error,#error2,#abort,#warnsize {
color:#aaa;
display:none;
font-size:10pt;
font-style:italic;
margin-top:10px;
}
#progress {
border:1px solid #ccc;
display:none;
float:left;
height:14px;
border-radius:10px;
-moz-border-radius:10px;
-ms-border-radius:10px;
-o-border-radius:10px;
-webkit-border-radius:10px;
background: -moz-linear-gradient(#66cc00, #4b9500);
background: -ms-linear-gradient(#66cc00, #4b9500);
background: -webkit-gradient(linear, left top, left bottom, color-stop(0%, #66cc00), color-stop(100%, #4b9500));
background: -webkit-linear-gradient(#66cc00, #4b9500);
background: -o-linear-gradient(#66cc00, #4b9500);
filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#66cc00', endColorstr='#4b9500');
-ms-filter: "progid:DXImageTransform.Microsoft.gradient(startColorstr='#66cc00', endColorstr='#4b9500')";
background: linear-gradient(#66cc00, #4b9500);
}
#progress_percent {
float:right;
}
#upload_response {
margin-top: 10px;
padding: 20px;
overflow: hidden;
display: none;
border: 1px solid #ccc;
border-radius:10px;
-moz-border-radius:10px;
-ms-border-radius:10px;
-o-border-radius:10px;
-webkit-border-radius:10px;
box-shadow: 0 0 5px #ccc;
background: -moz-linear-gradient(#bbb, #eee);
background: -ms-linear-gradient(#bbb, #eee);
background: -webkit-gradient(linear, left top, left bottom, color-stop(0%, #bbb), color-stop(100%, #eee));
background: -webkit-linear-gradient(#bbb, #eee);
background: -o-linear-gradient(#bbb, #eee);
filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#bbb', endColorstr='#eee');
-ms-filter: "progid:DXImageTransform.Microsoft.gradient(startColorstr='#bbb', endColorstr='#eee')";
background: linear-gradient(#bbb, #eee);
}
</style>
<body>
<header>
<h2 align='center'>*** OpenFlightTX - Firmware Uploader ***</h2>
</header>
<div class='container'>
<div class='contr'><h2>Please select a file to upload the firmware.</h2></div>
<div class='upload_form_cont'>
<form method='POST' action='/update' enctype='multipart/form-data' id='upload_form'>
<div>
<div><label for='image_file'>Please select image file</label></div>
<div><input type='file' name='update' id='image_file' onchange='fileSelected();' /></div>
</div>
<div><input type='button' value='Upload' onclick='startUploading()' /></div>
<div id='fileinfo'>
<div id='filename'></div>
<div id='filesize'></div>
<div id='filetype'></div>
<div id='filedim'></div>
</div>
<div id='error'>You should select valid image files only!</div>
<div id='error2'>An error occurred while uploading the file</div>
<div id='abort'>The upload has been canceled by the user or the browser dropped the connection</div>
<div id='warnsize'>Your file is very big. We can't accept it. Please select more small file</div>
<div id='progress_info'>
<div id='progress'></div>
<div id='progress_percent'>&nbsp;</div>
<div class='clear_both'></div>
<div>
<div id='speed'>&nbsp;</div>
<div id='remaining'>&nbsp;</div>
<div id='b_transfered'>&nbsp;</div>
<div class='clear_both'></div>
</div>
<div id='upload_response'></div>
</div>
</form>
</div>
</div>
</div>
</body>
)rawliteral";
/*** Index Page ***/
const char* index_html PROGMEM = R"rawliteral(<pre><strong>OpenFlightTX v1.0.10</strong>
)rawliteral";
// WebPages ----- END
void unload_webserver(void) {
// Unload webserver....
server.stop();
// Unload WiFi...
WiFi.disconnect();
}
void setup_webserver(void) {
//************ TEMPORARY HARDCODE
WiFi.begin("veeone", "trustno1");
// WiFi.begin("Pi3_2G", "trustno1");
// init and get the time
// configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
// Setting up OTA
/*** Index Page ***/
server.on("/", HTTP_GET, []() {
server.sendHeader("Connection", "close");
server.send(200, "text/html", firmware_upgrade_html);
});
/*** Firmware Upgrade Page ***/
server.on("/firmware_upgrade", HTTP_GET, []() {
server.sendHeader("Connection", "close");
server.send(200, "text/html", firmware_upgrade_html);
});
// ################### HANDLING UPLOAD FIRMWARE HERE #########################
/*** handling uploading firmware file ***/
server.on("/update", HTTP_POST, []() {
server.sendHeader("Connection", "close");
server.send(200, "text/plain", (Update.hasError()) ? "FAIL" : "OK. Device is rebooting");
// Try to delay a bit...
delay(1000);
ESP.restart();
}, []() {
HTTPUpload& upload = server.upload();
if(upload.status == UPLOAD_FILE_START) {
// Serial.printf("Update: %s\n", upload.filename.c_str());
if(!Update.begin(UPDATE_SIZE_UNKNOWN)) { //start with max available size
Update.printError(Serial);
}
} else if(upload.status == UPLOAD_FILE_WRITE) {
/*** flashing firmware to ESP ***/
if(Update.write(upload.buf, upload.currentSize) != upload.currentSize) {
Update.printError(Serial);
}
} else if(upload.status == UPLOAD_FILE_END) {
if(Update.end(true)) {
//true to set the size to the current progress
// Serial.printf("Update Success: %u\nRebooting...\n", upload.totalSize);
} else {
Update.printError(Serial);
}
}
});
// Starting up Server
server.begin();
// Just to debug...
if(WiFi.status() == WL_CONNECTED)
Serial.println(WiFi.localIP());
}
BleGamepad bleGamepad;
/* create a hardware timer */
hw_timer_t * timer = NULL;
typedef enum {
SCREEN_INTRO = 0,
SCREEN_SELECTIONS,
SCREEN_CONTROL
} OPENFLIGHT_SCREEN;
/*
Icon configuration
Width and height must match the icon font size
GAP: Space between the icons
BGAP: Gap between the display border and the cursor.
*/
#define ICON_WIDTH 16
#define ICON_HEIGHT 16
#define ICON_GAP 4
#define ICON_BGAP 16
#define ICON_Y 16+ ICON_GAP
#define ICONX_Y 32+ ICON_GAP
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2_left(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
// U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2_right(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
struct menu_entry_type {
const uint8_t *font;
uint16_t icon;
const char *name;
};
struct menu_state {
int16_t menu_start; /* in pixel */
int16_t frame_position; /* in pixel */
uint8_t position; /* position, array index */
};
struct menu_entry_type menu_entry_alpha_list[] = {
{ u8g2_font_open_iconic_all_2x_t, 224, "RC Controller"},
{ u8g2_font_open_iconic_all_2x_t, 94, "Bluetooth"},
{ u8g2_font_open_iconic_all_2x_t, 155, "Stick Calibration"},
{ u8g2_font_open_iconic_all_2x_t, 242, "Stick Positions"},
{ u8g2_font_open_iconic_all_2x_t, 246, "Trims"},
{ u8g2_font_open_iconic_all_2x_t, 281, "WiFi"},
{ u8g2_font_open_iconic_all_2x_t, 198, "RX Binding"},
{ u8g2_font_open_iconic_all_2x_t, 253, "RF Settings"},
{ u8g2_font_open_iconic_all_2x_t, 207, "RF Scanners"},
{ u8g2_font_open_iconic_all_2x_t, 90, "Battery"},
{ u8g2_font_open_iconic_all_2x_t, 142, "Save Settings"},
{ u8g2_font_open_iconic_all_2x_t, 205, "Reset Default"},
{ NULL, 0, NULL }
};
struct menu_state current_alpha_state = { ICON_BGAP, ICON_BGAP, 0 };
struct menu_state destination_alpha_state = { ICON_BGAP, ICON_BGAP, 0 };
void draw_menu(struct menu_state *state) {
int16_t x;
uint8_t i;
x = state->menu_start;
i = 0;
while( menu_entry_alpha_list[i].icon > 0 ) {
if ( x >= -ICON_WIDTH && x < u8g2_left.getDisplayWidth()) {
u8g2_left.setFont(menu_entry_alpha_list[i].font);
u8g2_left.drawGlyph(x, ICON_Y, menu_entry_alpha_list[i].icon );
}
i++;
x += ICON_WIDTH + ICON_GAP;
}
u8g2_left.drawFrame(state->frame_position-3, ICON_Y-ICON_HEIGHT-3, ICON_WIDTH+6, ICON_WIDTH+6);
//u8g2_left.drawFrame(state->frame_position-2, ICON_Y-ICON_HEIGHT-2, ICON_WIDTH+4, ICON_WIDTH+4);
//u8g2_left.drawFrame(state->frame_position-3, ICON_Y-ICON_HEIGHT-3, ICON_WIDTH+6, ICON_WIDTH+6);
}
void to_right(struct menu_state *state) {
if(menu_entry_alpha_list[state->position+1].font != NULL) {
if((int16_t)state->frame_position+ 2*(int16_t)ICON_WIDTH + (int16_t)ICON_BGAP < (int16_t)u8g2_left.getDisplayWidth()) {
state->position++;
state->frame_position += ICON_WIDTH + (int16_t)ICON_GAP;
} else {
state->position++;
state->frame_position = (int16_t)u8g2_left.getDisplayWidth() - (int16_t)ICON_WIDTH - (int16_t)ICON_BGAP;
state->menu_start = state->frame_position - state->position*((int16_t)ICON_WIDTH + (int16_t)ICON_GAP);
}
}
}
void to_left(struct menu_state *state){
if(state->position > 0){
if ( (int16_t)state->frame_position >= (int16_t)ICON_BGAP+(int16_t)ICON_WIDTH+ (int16_t)ICON_GAP ){
state->position--;
state->frame_position -= ICON_WIDTH + (int16_t)ICON_GAP;
} else {
state->position--;
state->frame_position = ICON_BGAP;
state->menu_start = state->frame_position - state->position*((int16_t)ICON_WIDTH + (int16_t)ICON_GAP);
}
}
}
uint8_t towards_int16(int16_t *current, int16_t dest) {
if ( *current < dest ){
(*current)++;
return 1;
} else if ( *current > dest ) {
(*current)--;
return 1;
}
return 0;
}
uint8_t towards(struct menu_state *current, struct menu_state *destination) {
uint8_t r = 0;
uint8_t i;
for( i = 0; i < 6; i++ ) {
r |= towards_int16( &(current->frame_position), destination->frame_position);
r |= towards_int16( &(current->menu_start), destination->menu_start);
}
return r;
}
// For Menu Navigation and Setup
#define MENU_STICK_ROLL_MAX 3000
#define MENU_STICK_ROLL_MIN 800
#define MENU_STICK_PITCH_MAX 3000
#define MENU_STICK_PITCH_MIN 800
#define MENU_STICK_YAW_MAX 3000
#define MENU_STICK_YAW_MIN 800
typedef enum {
MENU_NONE = 0,
MENU_LEFT,
MENU_RIGHT,
MENU_BACK,
MENU_OK,
MENU_EXTEND_LEFT,
MENU_EXTEND_RIGHT
} MENU_NAVIGATION;
MENU_NAVIGATION stick_navigation_position;
bool menu_entry_alpha_selected = false;
bool require_unlock = false;
bool ble_begin = false;
uint32_t ble_profiling = 0;
uint32_t ble_profiling_raw = 0;
uint32_t nrf_profiling = 0;
uint32_t nrf_profiling_raw = 0;
uint32_t last_seconds = 0;
uint32_t last_nrf_scanned = 0;
//define the pins used by the transceiver module
#define SS 5
#define RST 14
#define DIO0 2
int counter = 0;
#define BUZZER_PIN 15
#define THROTTLE_PIN 36
#define YAW_PIN 39
#define PITCH_PIN 34
#define ROLL_PIN 35
#define VBAT_PIN 32
#define VCC_PIN 33
#define SW1_PIN 25
#define SW2_PIN 26
#define SW3_PIN 27
#define SW4_PIN 14
#define SW5_PIN 12
#define SW6_PIN 13
uint16_t throttle_raw = 0;
uint16_t yaw_raw = 0;
uint16_t pitch_raw = 0;
uint16_t roll_raw = 0;
uint16_t vbat_raw = 0;
uint16_t vcc_raw = 0;
uint16_t throttle_value = 0;
uint16_t yaw_value = 0;
uint16_t pitch_value = 0;
uint16_t roll_value = 0;
/*
uint16_t tx_throttle = 0;
uint16_t tx_yaw = 0;
uint16_t tx_pitch = 0;
uint16_t tx_roll = 0;
*/
// For Center sticks trim
uint16_t throttle_max = 0;
uint16_t throttle_min = 4096;
uint16_t yaw_max = 0;
uint16_t yaw_min = 4096;
uint16_t pitch_max = 0;
uint16_t pitch_min = 4096;
uint16_t roll_max = 0;
uint16_t roll_min = 4096;
// For NRF24L01 frequency channels + TxPower
uint8_t freq_channel = 0;
uint8_t freq_txpower = 0; // 0: LOW, 1: MIN, 2: HIGH, 3: MAX
uint64_t vbat_value = 0;
bool sw1 = 0;
bool sw2 = 0;
bool sw3 = 0;
bool sw4 = 0;
bool sw5 = 0;
bool sw6 = 0;
uint8_t switches_value = 0;
bool invert_throttle = false;
bool invert_yaw = false;
bool invert_pitch = false;
bool invert_roll = false;
int8_t trim_throttle = 0;
int8_t trim_yaw = 0;
int8_t trim_pitch = 0;
int8_t trim_roll = 0;
uint32_t frequency = 919999000;
uint8_t syncword = 0xA0;
uint16_t packet_sent = 0;
uint16_t packet_sent_count = 0;
uint32_t uptime = 0;
#define MEDIAN_TOTAL 11
#define MEDIAN_POS MEDIAN_TOTAL/2
// For function call used only
#define THROTTLE 0
#define YAW 1
#define PITCH 2
#define ROLL 3
#define VBAT 4
uint64_t last_update = 0;
uint64_t last_menu_stick_read = 0;
uint64_t last_menu_item_moved = 0;
uint16_t throttle_pool[MEDIAN_TOTAL];
uint16_t yaw_pool[MEDIAN_TOTAL];
uint16_t pitch_pool[MEDIAN_TOTAL];
uint16_t roll_pool[MEDIAN_TOTAL];
uint16_t vbat_pool[MEDIAN_TOTAL];
uint16_t tmp_median_store[MEDIAN_TOTAL];
void median_initialize() {
// Prepare the storage engine for TYPR
for(int i = 0; i < MEDIAN_TOTAL; i++) {
throttle_pool[i] = 0;
yaw_pool[i] = 0;
pitch_pool[i] = 0;
roll_pool[i] = 0;
}
}
void IRAM_ATTR onTimer(){
uptime++;
packet_sent = packet_sent_count;
packet_sent_count = 0;
}
/*
* median_get:
* To get the median from the data depending on the median_type
*
* E.g.:
* retval = median_get(THROTTLE);
*/
uint16_t median_get(uint8_t median_type) {
bool inserted = false;
uint16_t median_tmp[MEDIAN_TOTAL];
// Loop through the variable to determine position to insert
// [ 12, 32, 4 ,0, 50, 2, 10, 10, 5, 20, 0 ]
/*
[ 12 ]
[ 12, 32 ]
[ 4, 12, 32 ]
[ 0, 4, 12, 32 ]
[ 0, 4, 12, 32, 50 ]
[ 0, 2, 4, 12, 32, 50 ]
*/
// Initial
uint16_t temp_val = 0;
uint16_t total_insert = 1;
// Search on type
if(median_type == THROTTLE) {
median_tmp[0] = throttle_pool[0];
} else if(median_type == YAW) {
median_tmp[0] = yaw_pool[0];
} else if(median_type == PITCH) {
median_tmp[0] = pitch_pool[0];
} else if(median_type == ROLL) {
median_tmp[0] = roll_pool[0];
} else if(median_type == VBAT) {
median_tmp[0] = vbat_pool[0];
}
// Loop insert and sort
for(int raw_count = 1; raw_count < MEDIAN_TOTAL; raw_count++) {
inserted = false;
if(median_type == THROTTLE) {
temp_val = throttle_pool[raw_count];
} else if(median_type == YAW) {
temp_val = yaw_pool[raw_count];
} else if(median_type == PITCH) {
temp_val = pitch_pool[raw_count];
} else if(median_type == ROLL) {
temp_val = roll_pool[raw_count];
} else if(median_type == VBAT) {
temp_val = vbat_pool[raw_count];
}
for(int median_count = 0; median_count < total_insert; median_count++) {
if(!inserted) {
if(temp_val < median_tmp[median_count]) {
inserted = true;
// Reverse copy and insert
for(int median_reverse = total_insert + 1; median_reverse > median_count; median_reverse--) {
median_tmp[median_reverse] = median_tmp[median_reverse - 1];
}
// Insert the detected
median_tmp[median_count] = temp_val;
// Increase total insertion
total_insert++;
}
}
}
// Nothing was inserted... so put at the last.
if(!inserted) {
median_tmp[total_insert++] = temp_val;
}
}
// Return the result
return median_tmp[MEDIAN_POS];
}
uint16_t throttle_insert(uint16_t val) {
// Insert to the array...at the last
for(int i = 1; i < MEDIAN_TOTAL; i++)
// Shift to left
throttle_pool[i - 1] = throttle_pool[i];
throttle_pool[MEDIAN_TOTAL - 1] = val;
// Find median....
return median_get(THROTTLE);
}
uint16_t yaw_insert(uint16_t val) {
// Insert to the array...at the last
for(int i = 1; i < MEDIAN_TOTAL; i++)
// Shift to left
yaw_pool[i - 1] = yaw_pool[i];
yaw_pool[MEDIAN_TOTAL - 1] = val;
// Find median....
return median_get(YAW);
}
uint16_t pitch_insert(uint16_t val) {
// Insert to the array...at the last
for(int i = 1; i < MEDIAN_TOTAL; i++)
// Shift to left
pitch_pool[i - 1] = pitch_pool[i];
pitch_pool[MEDIAN_TOTAL - 1] = val;
// Find median....
return median_get(PITCH);
}
uint16_t roll_insert(uint16_t val) {
// Insert to the array...at the last
for(int i = 1; i < MEDIAN_TOTAL; i++)
// Shift to left
roll_pool[i - 1] = roll_pool[i];
roll_pool[MEDIAN_TOTAL - 1] = val;
// Find median....
return median_get(ROLL);
}
/*
* pool_insert:
* To insert the data into the median pool depending on type of the variable
*
* E.g.:
* throttle_value = pool_insert(THROTTLE);
*/
uint16_t pool_insert(uint8_t pool_type, uint16_t val) {
// Insert to the array...at the last of the array
for(int i = 1; i < MEDIAN_TOTAL; i++) {
// Shift to left
if(pool_type == THROTTLE) {
throttle_pool[i - 1] = throttle_pool[i];
} else if(pool_type == YAW) {
yaw_pool[i - 1] = yaw_pool[i];
} else if(pool_type == PITCH) {
pitch_pool[i - 1] = pitch_pool[i];
} else if(pool_type == ROLL) {
roll_pool[i - 1] = roll_pool[i];
} else if(pool_type == VBAT) {
vbat_pool[i - 1] = vbat_pool[i];
}
}
// Find median....
if(pool_type == THROTTLE) {
throttle_pool[MEDIAN_TOTAL - 1] = val;
} else if(pool_type == YAW) {
yaw_pool[MEDIAN_TOTAL - 1] = val;
} else if(pool_type == ROLL) {
roll_pool[MEDIAN_TOTAL - 1] = val;
} else if(pool_type == PITCH) {
pitch_pool[MEDIAN_TOTAL - 1] = val;
} else if(pool_type == VBAT) {
vbat_pool[MEDIAN_TOTAL - 1] = val;
}
return median_get(pool_type);
}
void intro() {
for(int i = 0; i < 2; i++) {
digitalWrite(BUZZER_PIN, HIGH);
delay(10);
digitalWrite(BUZZER_PIN, LOW);
delay(1);
}
}
void read_all_voltage() {
throttle_raw = analogRead(THROTTLE_PIN);
yaw_raw = analogRead(YAW_PIN);
pitch_raw = analogRead(PITCH_PIN);
// roll_raw = analogRead(ROLL_PIN);
// Hardcode to increase the value a bit...
roll_raw = analogRead(ROLL_PIN) + 150;
vbat_raw = analogRead(VBAT_PIN);
sw1 = !digitalRead(SW1_PIN);
sw2 = !digitalRead(SW2_PIN);
sw3 = !digitalRead(SW3_PIN);
sw4 = !digitalRead(SW4_PIN);
sw5 = !digitalRead(SW5_PIN);
sw6 = !digitalRead(SW6_PIN);
// Combine values to one byte...
// NNssssss
// IIwwwwww
// switches_value = LL654321
switches_value = 0;
switches_value |= (sw6) ? 1 << 5 : 0;
switches_value |= (sw5) ? 1 << 4 : 0;
switches_value |= (sw4) ? 1 << 3 : 0;
switches_value |= (sw3) ? 1 << 2 : 0;
switches_value |= (sw2) ? 1 << 1 : 0;
switches_value |= (sw1) ? 1 << 0 : 0;
throttle_value = pool_insert(THROTTLE, throttle_raw);
if(!invert_throttle)
throttle_value = throttle_value + (trim_throttle << 3);
else
throttle_value = throttle_value - (trim_throttle << 3);
if(!invert_yaw)
yaw_value = pool_insert(YAW, yaw_raw) + (trim_yaw << 3);
else
yaw_value = pool_insert(YAW, yaw_raw) - (trim_yaw << 3);
if(!invert_pitch)
pitch_value = pool_insert(PITCH, pitch_raw) + (trim_pitch << 3);
else
pitch_value = pool_insert(PITCH, pitch_raw) - (trim_pitch << 3);
if(!invert_roll)
roll_value = pool_insert(ROLL, roll_raw) + (trim_roll << 3);
else
roll_value = pool_insert(ROLL, roll_raw) - (trim_roll << 3);
vbat_value = pool_insert(VBAT, vbat_raw);
}
void battery_management() {
/*
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(menu_entry_alpha_huge_list[destination_alpha_state.position].font);
u8g2_left.drawGlyph(40, ICONX_Y, menu_entry_alpha_huge_list[destination_alpha_state.position].icon);
u8g2_left.nextPage();
*/
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFontMode(1);
u8g2_left.setFont(u8g2_font_VCR_OSD_tr);
static char buf[6];
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(0,12);
u8g2_left.print(F("Battery LEVEL"));
// u8g2_left.drawStr(45, 15, buf);
// strcpy(buf, ltoa(vbat_value, buf, 10));
// u8g2_left.drawStr(0, 30, buf);
float vbat_real_value = vbat_value * .0012142857;
//strcpy(buf, ltoa(vbat_real_value, buf, 10));
dtostrf(vbat_real_value, 2, 1, buf);
u8g2_left.setFont(u8g2_font_maniac_tf);
u8g2_left.drawStr(0, 64, buf);
u8g2_left.setCursor(44, 64);
u8g2_left.print(F("V"));
// The battery frame
u8g2_left.drawFrame(96, 0, 32, 64);
// Bars Calculations
uint8_t bars = map(vbat_value, 2650, 3460, 0, 31);
for(int i = 0; i < bars; i++) {
u8g2_left.drawHLine(98, 61 - (i*2), 28);
}
u8g2_left.nextPage();
}
void setup() {
Serial.begin(115200);
// EEPROM initializing
EEPROM.begin(512);
// PIN Initialization
pinMode(BUZZER_PIN, OUTPUT);
pinMode(SW1_PIN, INPUT_PULLUP);
pinMode(SW2_PIN, INPUT_PULLUP);
pinMode(SW3_PIN, INPUT_PULLUP);
pinMode(SW4_PIN, INPUT_PULLUP);
pinMode(SW5_PIN, INPUT_PULLUP);
pinMode(SW6_PIN, INPUT_PULLUP);
// Display Setup
// u8g2_SetI2CAddress(u8g2_right.getU8g2(), 0x3d*2);
u8g2_left.begin();
// u8g2_right.begin();
//Serial.println(u8g2_left.u8g2_GetI2CAddress(u8g2_right));
// u8g2_left.setI2CAddress(0x3C);
//u8g2_left.setFont(u8g2_font_6x12_tr);
u8g2_left.setFont(u8g2_font_6x12_tr);
// u8g2_right.setFont(u8g2_font_6x12_tr);
median_initialize();
// Startup Sound
// intro();
// LoRA Setup
// setup LoRa transceiver module
// LoRa.setPins(SS, RST, DIO0);
// Replace the LoRa.begin(---E-) argument with your location's frequency
// 433E6 for Asia
// 866E6 for Europe
// 915E6 for North America
// while (!LoRa.begin(915000000)) {
// Serial.println(".");
// delay(500);
// }
// Change sync word (0xF3) to match the receiver
// The sync word assures you don't get LoRa messages from other LoRa transceivers
// ranges from 0-0xFF
// LoRa.setSyncWord(0xA0);
// Set to the highest PA Level
// LoRa.setSignalBandwidth(20.8E3);
// LoRa.setTxPower(20);
// LoRa.setSpreadingFactor(7);
// LoRa.setSignalBandwidth(125E3);
// LoRa.setSignalBandwidth(250E3);
// LoRa.setCodingRate4(5);
// LoRa.setSignalBandwidth(15.6E3);
// LoRa.setSpreadingFactor(6);
// Serial.println("LoRa Initializing OK!");
// Reading all settings
read_settings();
/* Use 1st timer of 4 */
/* 1 tick take 1/(80MHZ/80) = 1us so we set divider 80 and count up */
timer = timerBegin(0, 80, true);
/* Attach onTimer function to our timer */
timerAttachInterrupt(timer, &onTimer, true);
/* Set alarm to call onTimer function every second 1 tick is 1us => 1 second is 1000000us */
/* Repeat the alarm (third parameter) */
timerAlarmWrite(timer, 1000000, true);
/* Start an alarm */
timerAlarmEnable(timer);
// u8g2_right.firstPage();
// u8g2_right.setFontMode(1);
// u8g2_right.setFont(u8g2_font_helvB10_tr);
// u8g2_right.setFont(u8g2_font_VCR_OSD_tr);
// u8g2_right.setFont(u8g2_font_ImpactBits_tr);
// u8g2_right.setCursor(48, 40);
// u8g2_right.print(F("READY"));
// u8g2_right.nextPage();
// setup_lora();
/* Starting up 2nd core... */
// Creating Lora Task on Core 0. This will keep sending over and over again...
xTaskCreatePinnedToCore(
SignalHandler, /* Task function. */
"SignalHandler", /* name of task. */
10000, /* Stack size of task */
NULL, /* parameter of the task */
0, /* priority of the task */
&SignalTask, /* Task handle to keep track of created task */
0); /* pin task to core 0 */
}
// Sending / Receive signals at Core 0
void SignalHandler(void *pvParameters) {
// Forever loop in this loop :P
for(;;) {
if(nrf_scanner == true) {
if(millis() > last_nrf_scanned) {
Serial.println("Inside nrf_scanner loop...");
// Collecting X number of samples on each channel
for(int i = 0; i < MAX_CHANNELS; i++) {
channel_loads[i] = 0x00;
for(int j = 0; j < MAX_SAMPLES; j++) {
// Select a new channel...
radio.setChannel(i);
// Listening for a while
radio.startListening();
// Delay for a while...
delayMicroseconds(128);
// Stop Listening
radio.stopListening();
// Read out RPD flag... set to 1 if received power > -64dBm
// return ( read_register(RPD) & 1 ) ;
if(radio.testCarrier()) {
channel_loads[i]++;
}
}
// Debug
Serial.print("Channel: ");
Serial.println(i);
}
// Wait for next second...
last_nrf_scanned = millis() + 100;
}
} else if(nrf_enable) {
// Disable it first...
if(nrf_scanner)
nrf_scanner = false;
// Assign values...
throttle_value = (throttle_value > throttle_max) ? throttle_max : throttle_value; // Play safe... before mapping
throttle_value = (throttle_value < throttle_min) ? throttle_min : throttle_value;
yaw_value = (yaw_value > yaw_max) ? yaw_max : yaw_value; // Play safe... before mapping
yaw_value = (yaw_value < yaw_min) ? yaw_min : yaw_value;
pitch_value = (pitch_value > pitch_max) ? pitch_max : pitch_value; // Play safe... before mapping
pitch_value = (pitch_value < pitch_min) ? pitch_min : pitch_value;
roll_value = (roll_value > roll_max) ? roll_max : roll_value; // Play safe... before mapping
roll_value = (roll_value < roll_min) ? roll_min : roll_value;
// Mapping before sending...
txmessage.throttle = map(throttle_value, throttle_min, throttle_max, 1000, 2000);
txmessage.yaw = map(yaw_value, yaw_min, yaw_max, 2000, 1000);
txmessage.pitch = map(pitch_value, pitch_min, pitch_max, 2000, 1000);
txmessage.roll = map(roll_value, roll_min, roll_max, 1000, 2000);
txmessage.switches = switches_value;
// Sending Data over NRF
radio.write(&txmessage, sizeof(txmessage));
// Profiling..
nrf_profiling_raw++;
// delay(1);
} else if(binding) {
Serial.print(F("Sending Binding Channel:"));
Serial.println(freq_channel);
syncdata.freq_channel = (uint8_t) freq_channel;
syncdata.dummy = 100;
syncdata.fixed = 88;
// syncdata.pipe = pipeIn;
// Sending Data over NRF
radio.write(&syncdata, sizeof(SyncData));
nrf_profiling_raw++;
} else {
delay(1);
} // End of if(nrf_scanner)
} // End of for(;;)
}
void loop() {
// Each seconds tick
if(millis() > last_seconds) {
ble_profiling = ble_profiling_raw;
ble_profiling_raw = 0;
nrf_profiling = nrf_profiling_raw;
nrf_profiling_raw = 0;
/*
Serial.print(F("NRF:"));
Serial.println(nrf_profiling);
*/
last_seconds = millis() + 1000;
// For maximize the RF
/*
if(delay_maxrf) {
if(millis() > delay_maxrf_time) {
radio.setPALevel(RF24_PA_MAX); // Set the transmit power to Maximum.
delay_maxrf = false; // Only once.
}
}
*/
// This is the way to reduce CPU cycles. Only enable it when require
if(wifi_enable) {
if(!webserver_setup) {
/* Setting up OTA */
setup_webserver();
webserver_setup = true;
} else {
if(WiFi.status() == WL_CONNECTED) {
server.handleClient();
} else {
Serial.println(F("WiFi not connected"));
}
}
} else {
if(webserver_setup) {
unload_webserver();
webserver_setup = false;
}
}
}
read_all_voltage();
// Serial.println(throttle_raw + " : " + yaw_raw);
/*
Serial.print("T [");
Serial.print(throttle_raw);
Serial.print("] Y[");
Serial.print(yaw_raw);
Serial.print("] P[");
Serial.print(pitch_raw);
Serial.print("] R[");
Serial.print(roll_value);
Serial.println("]");
Serial.print(sw1);
Serial.print(sw2);
Serial.print(sw3);
Serial.print(sw4);
Serial.print(sw5);
Serial.println(sw6);
*/
if(millis() > last_update) {
// Determine stick position
if(millis() > last_menu_stick_read) {
if(roll_value > MENU_STICK_ROLL_MAX) {
stick_navigation_position = MENU_RIGHT;
} else if(roll_value < MENU_STICK_ROLL_MIN) {
stick_navigation_position = MENU_LEFT;
} else if(pitch_value > MENU_STICK_PITCH_MAX) {
stick_navigation_position = MENU_OK;
} else if(pitch_value < MENU_STICK_PITCH_MIN) {
stick_navigation_position = MENU_BACK;
} else if(yaw_value > MENU_STICK_YAW_MAX) {
if(!invert_yaw)
stick_navigation_position = MENU_EXTEND_RIGHT;
else
stick_navigation_position = MENU_EXTEND_LEFT;
} else if(yaw_value < MENU_STICK_YAW_MIN) {
if(!invert_yaw)
stick_navigation_position = MENU_EXTEND_LEFT;
else
stick_navigation_position = MENU_EXTEND_RIGHT;
} else {
stick_navigation_position = MENU_NONE;
}
// Next reading...
last_menu_stick_read = millis() + 200;
} else {
stick_navigation_position = MENU_NONE;
}
/*** DEBUG START **/
// rf_scanners();
// return;
/*** DEBUG END **/
if(
!menu_entry_alpha_selected
) {
// Determine the menu position
if(stick_navigation_position == MENU_RIGHT) {
to_right(&destination_alpha_state);
} else if(stick_navigation_position == MENU_LEFT) {
to_left(&destination_alpha_state);
} else if(stick_navigation_position == MENU_OK) {
menu_entry_alpha_selected = true;
}
last_menu_item_moved = millis() + 80;
u8g2_left.firstPage();
draw_menu(&current_alpha_state);
// u8g2_left.setFont(u8g2_font_helvB10_tr);
u8g2_left.setFont(u8g2_font_DigitalDiscoThin_tr);
u8g2_left.setCursor((u8g2_left.getDisplayWidth()-u8g2_left.getStrWidth(menu_entry_alpha_list[destination_alpha_state.position].name))/2, u8g2_left.getDisplayHeight()-25);
u8g2_left.print(menu_entry_alpha_list[destination_alpha_state.position].name);
// Title....
// u8g2_left.setFont(u8g2_font_micro_tr);
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(0,62);
u8g2_left.print(F("OpenFlightTX - 1.0.10"));
// Battery
show_battery_level(115, 56);
// WiFi (if connected)
if(wifi_enable) {
u8g2_left.setFont(u8g2_font_open_iconic_all_1x_t);
u8g2_left.drawGlyph(105, 64, 281);
// If connected then print the IP
if(WiFi.status() == WL_CONNECTED) {
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(0, 55);
u8g2_left.print(WiFi.localIP());
}
} else if(binding) {
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(0, 53);
u8g2_left.print(F("RX Binding in Progress"));
}
u8g2_left.nextPage();
towards(&current_alpha_state, &destination_alpha_state);
} else {
if(stick_navigation_position == MENU_BACK) {
if(require_unlock) {
if(sw6) {
menu_entry_alpha_selected = false;
// u8g2_right.clearBuffer();
// u8g2_right.sendBuffer();
}
} else {
menu_entry_alpha_selected = false;
// u8g2_right.clearBuffer();
// u8g2_right.sendBuffer();
}
} else {
if(menu_entry_alpha_list[destination_alpha_state.position].name == "RC Controller") {
require_unlock = true;
rc_controller();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Battery") {
require_unlock = false;
battery_management();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Bluetooth") {
require_unlock = true;
bluetooth_gamepad();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Stick Calibration") {
require_unlock = true;
stick_calibration();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Save Settings") {
require_unlock = false;
save_settings();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Reset Default") {
require_unlock = false;
reset_default();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Stick Positions") {
require_unlock = true;
stick_position();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Trims") {
require_unlock = true;
stick_trims();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "WiFi") {
require_unlock = false;
wifi_enabler();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "RF Settings") {
require_unlock = false;
rf_settings();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "RF Scanners") {
require_unlock = true;
rf_scanners();
} else if(menu_entry_alpha_list[destination_alpha_state.position].name == "RX Binding") {
require_unlock = false;
rx_binding();
}
//// u8g2_right.clearBuffer();
// u8g2_right.setFontMode(1);
// u8g2_right.setFont(u8g2_font_helvB10_tr);
// u8g2_right.setCursor(0,15);
// u8g2_right.print(F("Selected:"));
// u8g2_right.setCursor(0,30);
// u8g2_right.print(F(menu_entry_alpha_list[destination_alpha_state.position].name));
//// u8g2_right.firstPage();
// Based on the selection and do the task
//// u8g2_right.setFont(menu_entry_alpha_huge_list[destination_alpha_state.position].font);
//// u8g2_right.drawGlyph(35, ICONX_Y, menu_entry_alpha_huge_list[destination_alpha_state.position].icon);
//// u8g2_right.nextPage();
//u8g2_right.sendBuffer();
}
}
last_update = millis() + 1;
}
/*
//if(millis() - last_update >= 2) {
// Send LoRa packet to receiver
//while(true) {
LoRa.setTxPower(20);
LoRa.beginPacket();
// LoRa.print("hello ");
LoRa.print(counter);
LoRa.endPacket();
counter++;
//delay(100);
// delayMicroseconds(1500);
// }
last_update = millis();
//}
*/
}
void rx_checker(void) {
}
void rc_controller(void) {
static char buf[16];
if(!nrf_enable) {
setup_nrf_tx();
}
/*
// Trying to send in this loop
if(!lora_enable) {
setup_lora();
lora_enable = true;
// } else {
//sendFixedMessage( byte ADDH,byte ADDL, byte CHAN, const void *message, const uint8_t size){
// ResponseStatus rs = e32ttl100.sendFixedMessage(ADDR_H, ADDR_L, CHANNEL, "Welcome1");
}
*/
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(0, 13);
u8g2_left.print(F("OpenFlightTX RC"));
show_battery_level(0, 56);
draw_gimbalbox(53, 31, 32);
draw_gimbalbox(90, 31, 32);
uint16_t gimbal_y_raw = throttle_value;
uint16_t gimbal_x_raw = yaw_value;
if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;
uint8_t gimbal_y;
if(!invert_throttle)
gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31); // Throttle
else
gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31); // Throttle
uint8_t gimbal_x;
if(!invert_yaw)
gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31); // Yaw
else
gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31); // Yaw
// u8g2_left.drawFilledEllipse(64 + gimbal_x, gimbal_y, 2, 2, U8G2_DRAW_ALL);
u8g2_left.drawFilledEllipse(53 + gimbal_x, 31 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
// Mapping based on the live values
gimbal_y_raw = pitch_value;
gimbal_x_raw = roll_value;
if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;
if(!invert_pitch)
gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31); // Pitch
else
gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31); // Pitch
if(!invert_roll)
gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31); // Roll
else
gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31); // Roll
// u8g2_right.drawFilledEllipse(gimbal_x, gimbal_y, 3, 3, U8G2_DRAW_ALL);
u8g2_left.drawFilledEllipse(90 + gimbal_x, 31 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
// Calculate uptime
int hours = uptime / 3600;
int minutes = (uptime / 60) % 60;
int seconds = uptime % 60;
u8g2_left.setFont(u8g2_font_5x7_tf);
sprintf(buf, "%d packets/s", nrf_profiling);
u8g2_left.drawStr(0, 24, buf);
sprintf(buf, "%d MHz", freq_channel + 2400);
u8g2_left.drawStr(0, 32, buf);
sprintf(buf, "%02d:%02d:%02d", hours, minutes, seconds);
u8g2_left.drawStr(84, 24, buf);
switch(freq_txpower) {
case 0:
sprintf(buf, "TX MIN");
break;
case 1:
sprintf(buf, "TX LOW");
break;
case 2:
sprintf(buf, "TX HIGH");
break;
case 3:
sprintf(buf, "TX MAX");
break;
}
u8g2_left.drawStr(0, 40, buf);
if(!sw6) {
u8g2_left.setCursor(20, 63);
u8g2_left.print(F("LOCKED"));
}
if(sw1) {
u8g2_left.setCursor(0, 48);
u8g2_left.print(F("A"));
}
if(sw2) {
u8g2_left.setCursor(5, 48);
u8g2_left.print(F("B"));
}
if(sw3) {
u8g2_left.setCursor(10, 48);
u8g2_left.print(F("C"));
}
if(sw4) {
u8g2_left.setCursor(15, 48);
u8g2_left.print(F("D"));
}
if(sw5) {
u8g2_left.setCursor(20, 48);
u8g2_left.print(F("E"));
}
u8g2_left.nextPage();
}
void stick_trims(void) {
static int8_t stick_position_pointer = 0;
// Data processing
if(stick_navigation_position == MENU_LEFT) {
if(stick_position_pointer == 0) {
trim_throttle--;
} else if(stick_position_pointer == 1) {
trim_yaw--;
} else if(stick_position_pointer == 2) {
trim_pitch--;
} else if(stick_position_pointer == 3) {
trim_roll--;
}
} else if(stick_navigation_position == MENU_RIGHT) {
if(stick_position_pointer == 0) {
trim_throttle++;
} else if(stick_position_pointer == 1) {
trim_yaw++;
} else if(stick_position_pointer == 2) {
trim_pitch++;
} else if(stick_position_pointer == 3) {
trim_roll++;
}
} else if(stick_navigation_position == MENU_OK) {
stick_position_pointer++;
}
// Done...Lets wait for next
stick_navigation_position = MENU_NONE;
// Limit counts
if(stick_position_pointer > 3) stick_position_pointer = 0;
if(trim_throttle < -15) trim_throttle = -15;
if(trim_throttle > 15) trim_throttle = 15;
if(trim_yaw < -15) trim_yaw = -15;
if(trim_yaw > 15) trim_yaw = 15;
if(trim_pitch < -15) trim_pitch = -15;
if(trim_pitch > 15) trim_pitch = 15;
if(trim_roll < -15) trim_roll = -15;
if(trim_roll > 15) trim_roll = 15;
// Displaying...
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(24, 11);
u8g2_left.print(F("Stick Trims"));
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(0, 25);
u8g2_left.print(F("THR:"));
u8g2_left.setCursor(0, 35);
u8g2_left.print(F("YAW:"));
u8g2_left.setCursor(0, 45);
u8g2_left.print(F("PIT:"));
u8g2_left.setCursor(0, 55);
u8g2_left.print(F("ROL:"));
// Drawing the Bars
// Throttle Trims
int8_t trim_values = trim_throttle;
u8g2_left.drawFrame(20, 18, 34, 8);
if(trim_throttle >= 0) {
u8g2_left.drawBox(37, 20, trim_throttle, 4);
} else {
u8g2_left.drawBox(37 + trim_throttle, 20, -(trim_throttle), 4);
}
u8g2_left.drawFrame(20, 28, 34, 9);
if(trim_yaw >= 0) {
u8g2_left.drawBox(37, 30, trim_yaw, 4);
} else {
u8g2_left.drawBox(37 + trim_yaw, 30, -(trim_yaw), 4);
}
u8g2_left.drawFrame(20, 38, 34, 9);
if(trim_pitch >= 0) {
u8g2_left.drawBox(37, 40, trim_pitch, 4);
} else {
u8g2_left.drawBox(37 + trim_pitch, 40, -(trim_pitch), 4);
}
u8g2_left.drawFrame(20, 48, 34, 9);
if(trim_roll >= 0) {
u8g2_left.drawBox(37, 50, trim_roll, 4);
} else {
u8g2_left.drawBox(37 + trim_roll, 50, -(trim_roll), 4);
}
// Pointers
// u8g2_left.setCursor(25, 25 + (stick_position_pointer * 10));
// u8g2_left.print(F("<"));
// u8g2_left.setFont(u8g2_font_m2icon_9_tf);
// u8g2_left.drawGlyph(38, 27 + (stick_position_pointer * 10), 97); // <- Pointer icon
/*
static char buf[6];
strcpy(buf, ltoa(trim_throttle, buf, 10));
u8g2_right.drawStr(100, 35,buf);
if(reset_selections) {
u8g2_left.drawFrame(26, 40, 30, 20);
} else {
u8g2_left.drawFrame(62, 40, 30, 20);
}
*/
draw_gimbalbox(60, 25, 33);
draw_gimbalbox(95, 25, 33);
uint16_t gimbal_y_raw = throttle_value;
uint16_t gimbal_x_raw = yaw_value;
if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;
uint8_t gimbal_y;
if(!invert_throttle)
gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31); // Throttle
else
gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31); // Throttle
uint8_t gimbal_x;
if(!invert_yaw)
gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31); // Yaw
else
gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31); // Yaw
// u8g2_left.drawFilledEllipse(64 + gimbal_x, gimbal_y, 2, 2, U8G2_DRAW_ALL);
u8g2_left.drawFilledEllipse(60 + gimbal_x, 25 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
// Mapping based on the live values
gimbal_y_raw = pitch_value;
gimbal_x_raw = roll_value;
if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;
if(!invert_pitch)
gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31); // Pitch
else
gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31); // Pitch
if(!invert_roll)
gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31); // Roll
else
gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31); // Roll
// u8g2_right.drawFilledEllipse(gimbal_x, gimbal_y, 3, 3, U8G2_DRAW_ALL);
u8g2_left.drawFilledEllipse(95 + gimbal_x, 25 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
u8g2_left.setFont(u8g2_font_m2icon_9_tf);
u8g2_left.drawGlyph(60, 27 + (stick_position_pointer * 10), 97); // <- Pointer icon
u8g2_left.nextPage();
}
void stick_position(void) {
static int8_t stick_position_pointer = 0;
// Data processing
if(stick_navigation_position == MENU_LEFT) {
if(stick_position_pointer == 0) {
trim_throttle--;
} else if(stick_position_pointer == 1) {
trim_yaw--;
} else if(stick_position_pointer == 2) {
trim_pitch--;
} else if(stick_position_pointer == 3) {
trim_roll--;
}
} else if(stick_navigation_position == MENU_RIGHT) {
if(stick_position_pointer == 0) {
trim_throttle++;
} else if(stick_position_pointer == 1) {
trim_yaw++;
} else if(stick_position_pointer == 2) {
trim_pitch++;
} else if(stick_position_pointer == 3) {
trim_roll++;
}
} else if(stick_navigation_position == MENU_EXTEND_LEFT) {
if(stick_position_pointer == 0) {
invert_throttle = false;
} else if(stick_position_pointer == 1) {
invert_yaw = false;
} else if(stick_position_pointer == 2) {
invert_pitch = false;
} else if(stick_position_pointer == 3) {
invert_roll = false;
}
} else if(stick_navigation_position == MENU_EXTEND_RIGHT) {
if(stick_position_pointer == 0) {
invert_throttle = true;
} else if(stick_position_pointer == 1) {
invert_yaw = true;
} else if(stick_position_pointer == 2) {
invert_pitch = true;
} else if(stick_position_pointer == 3) {
invert_roll = true;
}
} else if(stick_navigation_position == MENU_OK) {
stick_position_pointer++;
}
// Done...Lets wait for next
stick_navigation_position = MENU_NONE;
// Limit counts
if(stick_position_pointer > 3) stick_position_pointer = 0;
if(trim_throttle < -15) trim_throttle = -15;
if(trim_throttle > 15) trim_throttle = 15;
if(trim_yaw < -15) trim_yaw = -15;
if(trim_yaw > 15) trim_yaw = 15;
if(trim_pitch < -15) trim_pitch = -15;
if(trim_pitch > 15) trim_pitch = 15;
if(trim_roll < -15) trim_roll = -15;
if(trim_roll > 15) trim_roll = 15;
// Displaying...
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(10, 11);
u8g2_left.print(F("Stick Positions"));
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(0, 25);
u8g2_left.print(F("THR:"));
if(invert_throttle) {
u8g2_left.print(F("INV"));
} else {
u8g2_left.print(F("NOR"));
}
u8g2_left.setCursor(0, 35);
u8g2_left.print(F("YAW:"));
if(invert_yaw) {
u8g2_left.print(F("INV"));
} else {
u8g2_left.print(F("NOR"));
}
u8g2_left.setCursor(0, 45);
u8g2_left.print(F("PIT:"));
if(invert_pitch) {
u8g2_left.print(F("INV"));
} else {
u8g2_left.print(F("NOR"));
}
u8g2_left.setCursor(0, 55);
u8g2_left.print(F("ROL:"));
if(invert_roll) {
u8g2_left.print(F("INV"));
} else {
u8g2_left.print(F("NOR"));
}
// Pointers
// u8g2_left.setCursor(25, 25 + (stick_position_pointer * 10));
// u8g2_left.print(F("<"));
u8g2_left.setFont(u8g2_font_m2icon_9_tf);
u8g2_left.drawGlyph(38, 27 + (stick_position_pointer * 10), 97); // <- Pointer icon
/*
static char buf[6];
strcpy(buf, ltoa(trim_throttle, buf, 10));
u8g2_right.drawStr(100, 35,buf);
if(reset_selections) {
u8g2_left.drawFrame(26, 40, 30, 20);
} else {
u8g2_left.drawFrame(62, 40, 30, 20);
}
*/
draw_gimbalbox(53, 25, 32);
draw_gimbalbox(90, 25, 32);
uint16_t gimbal_y_raw = throttle_value;
uint16_t gimbal_x_raw = yaw_value;
if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;
uint8_t gimbal_y;
if(!invert_throttle)
gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31); // Throttle
else
gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31); // Throttle
uint8_t gimbal_x;
if(!invert_yaw)
gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31); // Yaw
else
gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31); // Yaw
// u8g2_left.drawFilledEllipse(64 + gimbal_x, gimbal_y, 2, 2, U8G2_DRAW_ALL);
u8g2_left.drawFilledEllipse(53 + gimbal_x, 25 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
// Mapping based on the live values
gimbal_y_raw = pitch_value;
gimbal_x_raw = roll_value;
if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;
if(!invert_pitch)
gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31); // Pitch
else
gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31); // Pitch
if(!invert_roll)
gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31); // Roll
else
gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31); // Roll
// u8g2_right.drawFilledEllipse(gimbal_x, gimbal_y, 3, 3, U8G2_DRAW_ALL);
u8g2_left.drawFilledEllipse(90 + gimbal_x, 25 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
u8g2_left.nextPage();
// u8g2_right.clearBuffer();
// u8g2_right.firstPage();
}
void read_settings(void) {
uint16_t addr = 0x00;
// Throttle Max
throttle_max = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;
// New unit / reset.... dont need to read....
if(throttle_max == 0) return;
// Throttle Min
addr = 2;
throttle_min = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;
// Yaw Max
addr = 4;
yaw_max = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;
// Yaw Min
addr = 6;
yaw_min = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;
// Pitch Max
addr = 8;
pitch_max = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;
// Pitch Min
addr = 10;
pitch_min = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;
// Roll Max
addr = 12;
roll_max = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;
// Roll Min
addr = 14;
roll_min = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;
// Inverted Values
addr = 16;
int8_t val = EEPROM.read(addr);
invert_throttle = ((val & 0b00001000) >> 3) ? true : false;
invert_yaw = ((val & 0b00000100) >> 2) ? true : false;
invert_pitch = ((val & 0b00000010) >> 1) ? true : false;
invert_roll = ((val & 0b00000001) >> 0) ? true : false;
// Throttle Trim
addr = 17;
trim_throttle = EEPROM.read(addr);
// Yaw Trim
addr = 18;
trim_yaw = EEPROM.read(addr);
// Pitch Trim
addr = 19;
trim_pitch = EEPROM.read(addr);
// Roll Trim
addr = 20;
trim_roll = EEPROM.read(addr);
// Frequency Channel
addr = 21;
freq_channel = EEPROM.read(addr);
// Frequency TxPower
addr = 22;
freq_txpower = EEPROM.read(addr);
}
void commit_all(void) {
/*
* Memory Format:
*
* TTttYYyyPPppRRrrPABCDFf
*
* TT: Throttle Max
* tt: Throttle Min
* YY: Yaw Max
* yy: Yaw Min
* PP: Pitch Max
* pp: Pitch Min
* RR: Roll Max
* rr: Roll Min
* P : Invert Gimbals (0000TYPR)
* A : Throttle Trim
* B : Yaw Trim
* C : Pitch Trim
* D : Roll Trim
* F : NRF Channels (0 ~ 124) Total 125 channels
* f : TX Power (MIN, LOW, HIGH and MAX)
*/
uint16_t addr = 0x00;
// Throttle Max
write_data(addr++, throttle_max & 0xFF); // Low bits
write_data(addr++, throttle_max >> 8 & 0xFF); // High bits
// Throttle Min
write_data(addr++, throttle_min & 0xFF); // Low bits
write_data(addr++, throttle_min >> 8 & 0xFF); // High bits
// Yaw Max
write_data(addr++, yaw_max & 0xFF); // Low bits
write_data(addr++, yaw_max >> 8 & 0xFF); // High bits
// Yaw Min
write_data(addr++, yaw_min & 0xFF); // Low bits
write_data(addr++, yaw_min >> 8 & 0xFF); // High bits
// Pitch Max
write_data(addr++, pitch_max & 0xFF); // Low bits
write_data(addr++, pitch_max >> 8 & 0xFF); // High bits
// Pitch Min
write_data(addr++, pitch_min & 0xFF); // Low bits
write_data(addr++, pitch_min >> 8 & 0xFF); // High bits
// Roll Max
write_data(addr++, roll_max & 0xFF); // Low bits
write_data(addr++, roll_max >> 8 & 0xFF); // High bits
// Roll Min
write_data(addr++, roll_min & 0xFF); // Low bits
write_data(addr++, roll_min >> 8 & 0xFF); // High bits
// Inverted Gimbals
// uint8_t val = invert_throttle << 3 || invert_yaw << 2 || invert_pitch << 1 || invert_roll;
uint8_t val = ((invert_throttle) ? 1 << 3 : 0) + ((invert_yaw) ? 1 << 2 : 0) + ((invert_pitch) ? 1 << 1 : 0) + ((invert_roll) ? 1 : 0);
Serial.println(val);
write_data(addr++, val);
// Throttle Trim
write_data(addr++, trim_throttle);
// Yaw Trim
write_data(addr++, trim_yaw);
// Pitch Trim
write_data(addr++, trim_pitch);
// Roll Trim
write_data(addr++, trim_roll);
// Frequency Channel
write_data(addr++, freq_channel);
// Frequency Tx Power
write_data(addr++, freq_txpower);
}
void write_data(uint16_t addr, uint8_t val) {
// timerAlarmDisable(timer);
EEPROM.write(addr, val);
EEPROM.commit();
// timerAlarmEnable(timer);
}
void reset_settings(void) {
for(int i = 0; i < 255; i++) {
write_data(i, 0x00);
}
// Reboot
reboot();
}
void reboot(void) {
ESP.restart();
}
void reset_default(void) {
static bool reset_selections = false;
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(10, 25);
u8g2_left.print(F("Reset Settings?"));
u8g2_left.setCursor(30, 55);
u8g2_left.print(F("Yes No"));
if(stick_navigation_position == MENU_LEFT) {
reset_selections = true;
} else if(stick_navigation_position == MENU_RIGHT) {
reset_selections = false;
} else if(stick_navigation_position == MENU_OK) {
// Reset Settings..
if(reset_selections)
reset_settings();
// Simulate...so it will return back..
menu_entry_alpha_selected = false;
last_menu_stick_read = millis() + 200;
}
if(reset_selections) {
u8g2_left.drawFrame(26, 40, 30, 20);
} else {
u8g2_left.drawFrame(62, 40, 30, 20);
}
u8g2_left.nextPage();
// u8g2_right.clearBuffer();
// u8g2_right.firstPage();
// u8g2_right.nextPage();
}
void save_settings(void) {
static bool save_selections = false;
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(10, 25);
u8g2_left.print(F("Save Settings?"));
u8g2_left.setCursor(30, 55);
u8g2_left.print(F("Yes No"));
if(stick_navigation_position == MENU_LEFT) {
save_selections = true;
} else if(stick_navigation_position == MENU_RIGHT) {
save_selections = false;
} else if(stick_navigation_position == MENU_OK) {
// Save settings
if(save_selections)
commit_all();
// Simulate...so it will return back..
menu_entry_alpha_selected = false;
last_menu_stick_read = millis() + 200;
}
if(save_selections) {
u8g2_left.drawFrame(26, 40, 30, 20);
} else {
u8g2_left.drawFrame(62, 40, 30, 20);
}
u8g2_left.nextPage();
// u8g2_right.clearBuffer();
// u8g2_right.firstPage();
// u8g2_right.nextPage();
}
// Draw the gimbal box
void draw_gimbalbox(uint8_t x, uint8_t y) {
uint8_t w = 63;
uint8_t hm = 31;
uint8_t vm = x + hm;
// showing the gimbal range
u8g2_left.drawFrame(x, y, w, w);
// showing the crosshair
// horizontal
for(uint8_t i = x; i < (x+w); i+=5) {
u8g2_left.drawPixel(i, hm);
}
// vertical
for(uint8_t i = y; i < (y+w); i+=5) {
u8g2_left.drawPixel(vm, i);
}
}
void draw_gimbalbox(uint8_t x, uint8_t y, uint8_t size) {
uint8_t w = size;
uint8_t hm = y + (size / 2);
uint8_t vm = x + (size / 2);
// showing the gimbal range
u8g2_left.drawFrame(x, y, w, w);
// showing the crosshair
// horizontal
for(uint8_t i = x; i < (x+w); i+=2) {
u8g2_left.drawPixel(i, hm);
}
// vertical
for(uint8_t i = y; i < (y+w); i+=2) {
u8g2_left.drawPixel(vm, i);
}
}
void stick_calibration() {
static char buf[6];
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(10, 11);
u8g2_left.print(F("Stick Calibration"));
// showing the gimbal range
// draw_gimbalbox(64, 0);
draw_gimbalbox(31, 32, 32);
draw_gimbalbox(65, 32, 32);
// Getting the throttle minimum
if(throttle_value < throttle_min) {
throttle_min = throttle_value;
}
if(throttle_value > throttle_max) {
throttle_max = throttle_value;
}
if(yaw_value < yaw_min) {
yaw_min = yaw_value;
}
if(yaw_value > yaw_max) {
yaw_max = yaw_value;
}
if(pitch_value < pitch_min) pitch_min = pitch_value;
if(pitch_value > pitch_max) pitch_max = pitch_value;
if(roll_value < roll_min) roll_min = roll_value;
if(roll_value > roll_max) roll_max = roll_value;
// Mapping based on the live values
uint16_t gimbal_y_raw = throttle_value;
uint16_t gimbal_x_raw = yaw_value;
if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;
uint8_t gimbal_y;
if(!invert_throttle)
gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31); // Throttle
else
gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31); // Throttle
uint8_t gimbal_x;
if(!invert_yaw)
gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31); // Yaw
else
gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31); // Yaw
// u8g2_left.drawFilledEllipse(64 + gimbal_x, gimbal_y, 2, 2, U8G2_DRAW_ALL);
u8g2_left.drawFilledEllipse(32 + gimbal_x, 32 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
// Throttle
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(0, 24);
u8g2_left.print(F("Throttle:"));
strcpy(buf, ltoa(throttle_min, buf, 10));
u8g2_left.drawStr(0, 32, buf);
strcpy(buf, ltoa(throttle_max, buf, 10));
u8g2_left.drawStr(0, 40, buf);
// Yaw
u8g2_left.setCursor(0, 48);
u8g2_left.print(F("Yaw:"));
strcpy(buf, ltoa(yaw_min, buf, 10));
u8g2_left.drawStr(0, 56, buf);
strcpy(buf, ltoa(yaw_max, buf, 10));
u8g2_left.drawStr(0, 64, buf);
// Getting the pitch minimum
if(pitch_value < pitch_min) {
pitch_min = pitch_value;
}
if(pitch_value > pitch_max) {
pitch_max = pitch_value;
}
if(roll_value < roll_min) {
roll_min = roll_value;
}
if(roll_value > roll_max) {
roll_max = roll_value;
}
if(pitch_value < pitch_min) pitch_min = pitch_value;
if(pitch_value > pitch_max) pitch_max = pitch_value;
if(roll_value < roll_min) roll_min = roll_value;
if(roll_value > roll_max) roll_max = roll_value;
// Mapping based on the live values
gimbal_y_raw = pitch_value;
gimbal_x_raw = roll_value;
if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;
if(!invert_pitch)
gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31); // Pitch
else
gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31); // Pitch
if(!invert_roll)
gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31); // Roll
else
gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31); // Roll
// u8g2_right.drawFilledEllipse(gimbal_x, gimbal_y, 3, 3, U8G2_DRAW_ALL);
u8g2_left.drawFilledEllipse(66 + gimbal_x, 32 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
// Pitch
u8g2_left.setCursor(99, 24);
u8g2_left.print(F("Pitch:"));
strcpy(buf, ltoa(pitch_min, buf, 10));
u8g2_left.drawStr(99, 32, buf);
strcpy(buf, ltoa(pitch_max, buf, 10));
u8g2_left.drawStr(99, 40, buf);
// Roll
u8g2_left.setCursor(99, 48);
u8g2_left.print(F("Roll:"));
strcpy(buf, ltoa(roll_min, buf, 10));
u8g2_left.drawStr(99, 56, buf);
strcpy(buf, ltoa(roll_max, buf, 10));
u8g2_left.drawStr(99, 64, buf);
u8g2_left.nextPage();
}
void bluetooth_gamepad() {
u8g2_left.clearBuffer();
u8g2_left.firstPage();
/*
u8g2_left.setFont(menu_entry_alpha_huge_list[destination_alpha_state.position].font);
u8g2_left.drawGlyph(40, ICONX_Y, menu_entry_alpha_huge_list[destination_alpha_state.position].icon);
u8g2_left.nextPage();
u8g2_right.clearBuffer();
u8g2_right.firstPage();
u8g2_right.setFontMode(1);
u8g2_right.setFont(u8g2_font_ImpactBits_tr);
*/
static char buf[6];
int throttle = map(throttle_value, 0, 4096, 127, -127); // Throttle
int yaw = map(yaw_value, 0, 4096, -127, 127); // Yaw
int pitch = map(pitch_value, 0, 4096, 127, -127); // Pitch
int roll = map(roll_value, 0, 4096, -127, 127); // Roll
u8g2_left.setFontMode(1);
// u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setFont(u8g2_font_tenfatguys_tf);
u8g2_left.setCursor(10, 16);
u8g2_left.print(F("BT GAMEPAD"));
show_battery_level(0, 56);
draw_gimbalbox(31, 32, 32);
draw_gimbalbox(65, 32, 32);
// Mapping based on the live values
uint16_t gimbal_y_raw = throttle_value;
uint16_t gimbal_x_raw = yaw_value;
if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;
uint8_t gimbal_y;
if(!invert_throttle)
gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31); // Throttle
else
gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31); // Throttle
uint8_t gimbal_x;
if(!invert_yaw)
gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31); // Yaw
else
gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31); // Yaw
u8g2_left.drawFilledEllipse(32 + gimbal_x, 32 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
// Mapping based on the live values
gimbal_y_raw = pitch_value;
gimbal_x_raw = roll_value;
if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;
if(!invert_pitch)
gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31); // Pitch
else
gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31); // Pitch
if(!invert_roll)
gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31); // Roll
else
gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31); // Roll
u8g2_left.drawFilledEllipse(66 + gimbal_x, 32 + gimbal_y, 2, 2, U8G2_DRAW_ALL);
// u8g2_left.setFont(u8g2_font_TimesNewPixel_tr);
/*
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(0, 8);
u8g2_left.print(F("T:"));
strcpy(buf, ltoa(throttle, buf, 10));
u8g2_left.drawStr(15, 8, buf);
u8g2_left.setCursor(0,45);
u8g2_left.print(F("Y:"));
strcpy(buf, ltoa(yaw, buf, 10));
u8g2_left.drawStr(15,45,buf);
u8g2_left.setCursor(60,30);
u8g2_left.print(F("P:"));
strcpy(buf, ltoa(pitch, buf, 10));
u8g2_left.drawStr(75,30,buf);
u8g2_left.setCursor(60,45);
u8g2_left.print(F("R:"));
strcpy(buf, ltoa(roll, buf, 10));
u8g2_left.drawStr(75,45,buf);
*/
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(99, 50);
u8g2_left.print(F("Rate:"));
strcpy(buf, ltoa(ble_profiling, buf, 10));
u8g2_left.drawStr(99, 57,buf);
if(!sw6) {
u8g2_left.setCursor(99, 64);
u8g2_left.print(F("LOCKED"));
}
u8g2_left.nextPage();
if(!ble_begin) {
bleGamepad.begin();
ble_begin = true;
}
if(bleGamepad.isConnected()) {
ble_profiling_raw++;
// [YAW, THROTTLE, ROLL, PITCH]
bleGamepad.setAxes(yaw, throttle, roll, pitch, 0, 0, DPAD_CENTERED);
}
}
void rf_settings(void) {
// Data processing
if(stick_navigation_position == MENU_LEFT) {
if(freq_txpower > 0) {
freq_txpower--;
radio.setPALevel(freq_txpower);
}
} else if(stick_navigation_position == MENU_RIGHT) {
if(freq_txpower < 3) {
freq_txpower++;
radio.setPALevel(freq_txpower);
}
} else if(stick_navigation_position == MENU_EXTEND_LEFT) {
if(freq_channel > 0) {
freq_channel--;
radio.setChannel(freq_channel);
}
} else if(stick_navigation_position == MENU_EXTEND_RIGHT) {
if(freq_channel < MAX_CHANNELS) {
freq_channel++;
radio.setChannel(freq_channel);
}
}
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(0, 15);
u8g2_left.print(F("RF Settings"));
// To List the current frequency... and also current power. Using the sticks to change it. Once back, it will commit the setting by pushing down the stick. If pulling up...nothing change.
u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(0, 32);
u8g2_left.print(F("Frequency:"));
u8g2_left.setCursor(64, 32);
u8g2_left.print(F("TX Power:"));
// Getting the common variables
static char buf[10];
int frequency_mhz = 2400 + freq_channel;
sprintf(buf, "%d MHz", frequency_mhz);
u8g2_left.drawStr(0, 40, buf);
u8g2_left.setCursor(0, 56);
if(channel_used[freq_channel]) {
u8g2_left.print(F("USED"));
} else {
u8g2_left.print(F("FREE"));
}
switch(freq_txpower) {
case 0:
sprintf(buf, "LOW");
break;
case 1:
sprintf(buf, "MIN");
break;
case 2:
sprintf(buf, "HIGH");
break;
case 3:
sprintf(buf, "MAX");
break;
default:
sprintf(buf, "UNKNOWN");
break;
}
u8g2_left.drawStr(64, 40, buf);
u8g2_left.nextPage();
}
void show_battery_level(uint8_t x, uint8_t y) {
u8g2_left.drawFrame(x, y, 12, 8);
// Tips of the battery
u8g2_left.drawLine(x+12, y+2, x+12, y+5);
// Battery level indicator
int vbat_bar_value = ((vbat_value * .0012142857) - 3.2) * 8;
// int vbat_bar_value = vbat_value * 10 / 4096;
uint8_t x1 = 0;
for(int i = 0; i < vbat_bar_value; i++) {
x1 = x + i + 2;
u8g2_left.drawLine(x1, y + 2, x1, y + 5);
}
}
void wifi_enabler(void) {
/*
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFont(menu_entry_alpha_huge_list[destination_alpha_state.position].font);
u8g2_left.drawGlyph(40, ICONX_Y, menu_entry_alpha_huge_list[destination_alpha_state.position].icon);
u8g2_left.nextPage();
*/
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFontMode(1);
u8g2_left.setFont(u8g2_font_VCR_OSD_tr);
static char buf[6];
// u8g2_left.drawStr(45, 15, buf);
// strcpy(buf, ltoa(vbat_value, buf, 10));
// u8g2_left.drawStr(0, 30, buf);
static bool wifi_selections = false;
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(15, 25);
u8g2_left.print(F("Enable WiFi?"));
u8g2_left.setCursor(30, 55);
u8g2_left.print(F("Yes No"));
// TODO: Working on WiFi. Manually on...
if(stick_navigation_position == MENU_LEFT) {
wifi_selections = true;
} else if(stick_navigation_position == MENU_RIGHT) {
wifi_selections = false;
} else if(stick_navigation_position == MENU_OK) {
// Wifi...
if(wifi_selections) {
wifi_enable = true;
} else {
wifi_enable = false;
}
// Simulate...so it will return back..
menu_entry_alpha_selected = false;
last_menu_stick_read = millis() + 200;
}
if(wifi_selections) {
u8g2_left.drawFrame(26, 40, 30, 20);
} else {
u8g2_left.drawFrame(62, 40, 30, 20);
}
u8g2_left.nextPage();
}
void setup_nrf_scanner() {
// Setting NRF24L01
// printf_begin();
radio.begin();
radio.setAutoAck(false);
// Get into standby mode
radio.startListening();
radio.stopListening();
// radio.printDetails();
// Initialize variables...
for(int i = 0; i < MAX_CHANNELS; i++) {
channel_loads[i] = 0;
}
Serial.println("SET nrf_scanner = TRUE");
nrf_scanner = true;
}
void setup_nrf_sync(void) {
// Setting NRF24L01
// printf_begin();
radio.begin();
// radio.setAutoAck(true);
radio.setAutoAck(false); // Seems like i am doing this on the last version...
radio.setPayloadSize(sizeof(SyncData));
radio.setChannel(125); // 0: Sync channel (2525MHz)
radio.setDataRate(RF24_250KBPS); // Lowest.. at 250kbps
radio.setPALevel(RF24_PA_MIN);
// delay_maxrf_time = millis() + 2000; // Delay 2 seconds later
// delay_maxrf = true;
// radio.setPALevel(RF24_PA_MAX); // Set the transmit power to Maximum... Later make it configurable.
const uint64_t pipe = 0xE8E8F0F0E1LL;
radio.openWritingPipe(pipe);
// radio.printDetails();
}
void setup_nrf_tx(void) {
// Setting NRF24L01
// printf_begin();
radio.begin();
// radio.setAutoAck(true);
radio.setAutoAck(false); // Seems like i am doing this on the last version...
radio.setPayloadSize(sizeof(TxMessage));
radio.setChannel(freq_channel); // This will need to set from EEPROM settings
radio.setDataRate(RF24_250KBPS); // Lowest.. at 250kbps
switch(freq_txpower) { // Set the transmit power
case 0:
radio.setPALevel(RF24_PA_MIN);
break;
case 1:
radio.setPALevel(RF24_PA_LOW);
break;
case 2:
radio.setPALevel(RF24_PA_HIGH);
break;
case 3:
radio.setPALevel(RF24_PA_MAX);
break;
}
// delay_maxrf_time = millis() + 2000; // Delay 2 seconds later
// delay_maxrf = true;
// radio.setPALevel(RF24_PA_MAX); // Set the transmit power to Maximum... Later make it configurable.
const uint64_t pipe = 0xE8E8F0F0E1LL;
radio.openWritingPipe(pipe);
// radio.printDetails();
Serial.println("SET nrf_scanner = TRUE");
nrf_enable = true;
}
void restore_radio(void) {
radio.setAutoAck(false);
radio.setPayloadSize(sizeof(TxMessage));
radio.setChannel(freq_channel);
switch(freq_txpower) { // Set the transmit power
case 0:
radio.setPALevel(RF24_PA_MIN);
break;
case 1:
radio.setPALevel(RF24_PA_LOW);
break;
case 2:
radio.setPALevel(RF24_PA_HIGH);
break;
case 3:
radio.setPALevel(RF24_PA_MAX);
break;
}
const uint64_t pipe = 0xE8E8F0F0E1LL;
radio.openWritingPipe(pipe);
nrf_enable = true;
}
void setup_rxsync_server(void) {
static char sync_html[128];
sprintf(sync_html, "%d|%ld", freq_channel, pipeIn);
server.on("/", HTTP_GET, []() {
server.sendHeader("Connection", "close");
server.send(200, "text/html", sync_html);
});
// Starting up Server
server.begin();
}
void rx_binding(void) {
u8g2_left.clearBuffer();
u8g2_left.firstPage();
static bool binding_selections = false;
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(15, 25);
u8g2_left.print(F("Enable Binding?"));
u8g2_left.setCursor(30, 55);
u8g2_left.print(F("Yes No"));
// TODO: Working on WiFi. Manually on...
if(stick_navigation_position == MENU_LEFT) {
binding_selections = true;
} else if(stick_navigation_position == MENU_RIGHT) {
binding_selections = false;
} else if(stick_navigation_position == MENU_OK) {
// Wifi...
if(binding_selections) {
if(!binding) {
// WiFi.softAP("OpenFlightTX", "12345678");
setup_nrf_sync();
// setup_rxsync_server();
binding = true;
}
} else {
binding = false;
restore_radio();
}
// Simulate...so it will return back..
menu_entry_alpha_selected = false;
last_menu_stick_read = millis() + 200;
}
if(binding_selections) {
u8g2_left.drawFrame(26, 40, 30, 20);
} else {
u8g2_left.drawFrame(62, 40, 30, 20);
}
// Just need to know the receiver's channel, encryption code
u8g2_left.nextPage();
}
void rf_scanners(void) {
int y = 0;
static char buf[10];
static int result_x = 0;
if(!nrf_scanner) {
setup_nrf_scanner();
for(int i = 0; i < MAX_CHANNELS; i++) {
channel_used[i] = false;
}
}
// Storing...
int channel_unused = 0;
for(int i = 0; i < MAX_CHANNELS; i++) {
if(channel_loads[i] > 0) {
channel_used[i] = true;
}
if(!channel_used[i]) channel_unused++;
}
// Data processing
if(stick_navigation_position == MENU_LEFT) {
graph_type = 0;
} else if(stick_navigation_position == MENU_RIGHT) {
graph_type = 1;
} else if(stick_navigation_position == MENU_EXTEND_LEFT) {
result_x += 10;
} else if(stick_navigation_position == MENU_EXTEND_RIGHT) {
result_x -= 10;
}
u8g2_left.clearBuffer();
u8g2_left.firstPage();
u8g2_left.setFontMode(1);
u8g2_left.setFont(u8g2_font_VCR_OSD_tr);
u8g2_left.setFont(u8g2_font_ImpactBits_tr);
u8g2_left.setCursor(0,12);
u8g2_left.print(F("RF SCANNER"));
u8g2_left.setFont(u8g2_font_5x7_tf);
if(!sw6) {
u8g2_left.setCursor(85, 15);
u8g2_left.print(F("LOCKED"));
}
// List all free channels..
bool continuemore = true;
int c = 0;
int col = 0;
int frequency_mhz = 0;
for(int i = 0; i < MAX_CHANNELS; i++) {
if(channel_used[i]) {
continue;
}
frequency_mhz = 2400 + i;
sprintf(buf, "%d ", frequency_mhz);
col = c / 4;
int y = c % 4;
u8g2_left.drawStr(0 + (col * 22) - result_x, 40 + (y * 8), buf);
c++;
if(c >= 28) break;
}
// u8g2_left.drawStr(45, 15, buf);
// strcpy(buf, ltoa(vbat_value, buf, 10));
// u8g2_left.drawStr(0, 30, buf);
//strcpy(buf, ltoa(vbat_real_value, buf, 10));
//sprintf(buf, "RATE:%i", lora_receive_profiling);
//u8g2_left.setFont(u8g2_font_5x7_tf);
//u8g2_left.drawStr(0, 32, buf);
// Drawing from the memory...
if(graph_type == 0) {
for(int x = 0; x < MAX_CHANNELS; x++) {
if(channel_loads[x] == 0) continue;
// y = 63 - (channel_loads[x] * 32 / MAX_SAMPLES);
y = 32 - rf24_min(0xF, channel_loads[x]);
u8g2_left.drawLine(x, 32, x, y);
}
} else {
for(int x = 0; x < MAX_CHANNELS; x++) {
if(!channel_used[x]) continue;
// Meaning ... already use..... then draw a line
u8g2_left.drawLine(x, 16, x, 32);
}
}
// Stats
// u8g2_left.setFont(u8g2_font_5x7_tf);
u8g2_left.setCursor(85, 7);
u8g2_left.print(F("FREE:"));
itoa(channel_unused, buf, 10);
u8g2_left.drawStr(110, 7, buf);
u8g2_left.nextPage();
}
/*
// set the value of a nRF24L01 register
void setRegister(byte r, byte v) {
digitalWrite(NRF24_CE, 0);
SPI.transfer((r&0x1F)|0x20);
SPI.transfer(v);
digitalWrite(NRF24_CE, 1);
}
// Get the value of a nRF24L01 register
byte getRegister(byte r) {
byte c;
digitalWrite(NRF24_CE, 0);
c = SPI.transfer(r&0x1F);
c = SPI.transfer(0);
digitalWrite(NRF24_CE, 1);
return(c);
}
void disableRX(void) {
digitalWrite(NRF24_CE, LOW);
}
// power up the nRF24L01p chip
void powerUp(void) {
setRegister(_NRF24_CONFIG,getRegister(_NRF24_CONFIG)|0x02);
delayMicroseconds(130);
}
// switch nRF24L01p off
void powerDown(void) {
setRegister(_NRF24_CONFIG,getRegister(_NRF24_CONFIG)&~0x02);
}
// enable RX
void enableRX(void) {
digitalWrite(NRF24_CE, HIGH);
}
// setup RX-Mode of nRF24L01p
void setRX(void) {
setRegister(_NRF24_CONFIG,getRegister(_NRF24_CONFIG)|0x01);
enableRX();
// this is slightly shorter than
// the recommended delay of 130 usec
// - but it works for me and speeds things up a little...
delayMicroseconds(100);
}
*/