englebert
/
FPVController
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Prefixed the Yaw and Pitch orientation using the fastest way without interrupting too much on calculations.

master
Englebert 2 years ago
parent
771925a3d2
commit
956a702120
7 changed files with 172 additions and 39 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      openscad/BoxMode.scad
  2. 57
      src/ESPNow.cpp
  3. 8
      src/ESPNow2.h
  4. 104
      src/ESPNowRC.cpp
  5. 28
      src/Inputs.cpp
  6. 6
      src/Inputs.h
  7. 2
      src/main.cpp

6
openscad/BoxMode.scad
View File

@ -1,9 +1,9 @@
round_quality=500;
draft=true;
show_gimbal=false;
show_body=false;
show_all=false;
show_m5_back=false;
show_body=true;
show_all=true;
show_m5_back=true;
show_gimbal_back=true;
if(show_body) {

57
src/ESPNow.cpp
View File

@ -42,6 +42,17 @@ void ESPNow2::begin(void) {
// uint8_t temp_addr[] = { 0x08, 0xB6, 0x1F, 0x34, 0x89, 0x98 };
memcpy(default_server_address, temp_addr, sizeof(uint8_t[6]));
pairing_status = PAIR_REQUEST;
// Initialize the auxilary buttons
for(uint8_t i = 0; i < 9; i++) {
aux_state[i] = false;
}
// Initialize virtual ruby buttons
for(uint8_t i = 0; i < 9; i++) {
last_rubybutton[i] = 0;
rubybutton_state[i] = false;
}
}
@ -91,16 +102,54 @@ void ESPNow2::update(void) {
if((uint32_t)(millis() - last_updated) > 1) {
data_to_send.message_type = DATA;
data_to_send.throttle = map(inputs.throttle, stickcalibration.throttle_min, stickcalibration.throttle_max, 1000, 2000); // Throttle
data_to_send.yaw = map(inputs.yaw, stickcalibration.yaw_min, stickcalibration.yaw_max, 1000, 2000); // Yaw
data_to_send.pitch = map(inputs.pitch, stickcalibration.pitch_min, stickcalibration.pitch_max, 1000, 2000); // Pitch
data_to_send.roll = map(inputs.roll, stickcalibration.roll_min, stickcalibration.roll_max, 1000, 2000); // Roll
data_to_send.throttle = map(inputs.throttle, stickcalibration.throttle_min, stickcalibration.throttle_max + inputs.trim_throttle, 1000, 2000); // Throttle
data_to_send.yaw = map(inputs.yaw, stickcalibration.yaw_min, stickcalibration.yaw_max + inputs.trim_yaw, 2000, 1000); // Yaw
data_to_send.pitch = map(inputs.pitch, stickcalibration.pitch_min, stickcalibration.pitch_max + inputs.trim_pitch, 2000, 1000); // Pitch
data_to_send.roll = map(inputs.roll, stickcalibration.roll_min, stickcalibration.roll_max + inputs.trim_roll, 1000, 2000); // Roll
// Converting Ruby Button state to send.
data_to_send.rubybutton = (
rubybutton_state[0] |
rubybutton_state[1] << 1 |
rubybutton_state[2] << 2 |
rubybutton_state[3] << 3 |
rubybutton_state[4] << 4 |
rubybutton_state[5] << 5 |
rubybutton_state[6] << 6 |
rubybutton_state[7] << 7 |
rubybutton_state[8] << 8
);
// Convert the state into one variable
data_to_send.aux = (
aux_state[0] |
aux_state[1] << 1 |
aux_state[2] << 2 |
aux_state[3] << 3 |
aux_state[4] << 4 |
aux_state[5] << 5 |
aux_state[6] << 6 |
aux_state[7] << 7 |
aux_state[8] << 8 |
aux_state[9] << 9 |
aux_state[10] << 10 |
aux_state[11] << 11
);
// Sending Data
esp_now_send(destination_server_address, (uint8_t *) &data_to_send, sizeof(data_to_send));
last_updated = millis();
}
// Unpress the buttons (virtually)
for(uint8_t i = 0; i < 9; i++) {
if(rubybutton_state[i]) {
if((uint32_t)(millis() - last_rubybutton[i]) > 100) {
rubybutton_state[i] = false;
}
}
}
}
}

8
src/ESPNow2.h
View File

@ -19,7 +19,10 @@ typedef struct struct_data {
uint16_t yaw;
uint16_t roll;
uint16_t pitch;
bool aux[9];
uint16_t aux;
// bool aux[9];
uint16_t rubybutton;
// bool rubybutton[9];
} struct_data;
typedef struct struct_pairing {
@ -69,7 +72,10 @@ class ESPNow2 {
uint32_t last_updated = 0;
bool isConnected = false;
bool rubybutton_state[9];
uint32_t last_rubybutton[9];
bool aux_state[9];
private:
};

104
src/ESPNowRC.cpp
View File

@ -68,6 +68,7 @@ void ESPNowRC::show(TFT_eSprite *m) {
m->printf("B:%.1fV %.0fmA", voltage, current);
// m->printf(String(voltage) + "V " + String(current) + "mA");
/***
// Draw the gimbal box
draw_gimbal(m, 69, 150);
draw_gimbal(m, 161, 150);
@ -75,52 +76,92 @@ void ESPNowRC::show(TFT_eSprite *m) {
// Convert the throttle and raw to x and y
// Temporary assume 4096 = max
uint16_t left_gimbal_y = map(inputs.throttle, stickcalibration.throttle_min, stickcalibration.throttle_max, 90, 0);
uint16_t left_gimbal_x = map(inputs.yaw, stickcalibration.yaw_min, stickcalibration.yaw_max, 90, 0);
// uint16_t left_gimbal_x = map(inputs.yaw, stickcalibration.yaw_min, stickcalibration.yaw_max, 90, 0);
uint16_t left_gimbal_x = map(inputs.yaw, stickcalibration.yaw_min, stickcalibration.yaw_max + inputs.trim_yaw, 90, 0);
m->fillCircle(left_gimbal_x + 69, left_gimbal_y + 150, 5, TFT_YELLOW);
uint16_t right_gimbal_y = map(inputs.pitch, stickcalibration.pitch_min, stickcalibration.pitch_max, 0, 90);
uint16_t right_gimbal_x = map(inputs.roll, stickcalibration.roll_min, stickcalibration.roll_max, 0, 90);
m->fillCircle(right_gimbal_x + 161, right_gimbal_y + 150, 5, TFT_YELLOW);
***/
// Draw the switches
uint32_t last_millis = millis();
if(!espnow2.data_to_send.aux[0]) {
if(!espnow2.aux_state[0]) {
m->drawRect(0, 40, 60, 60, TFT_BLUE); // Button 1 - OFF
} else {
m->drawRect(0, 40, 60, 60, TFT_BLUE); // Button 1 - ON
m->fillRect(1, 41, 60, 60, TFT_BLUE);
}
if(!espnow2.data_to_send.aux[1]) {
if(!espnow2.aux_state[1]) {
m->drawRect(65, 40, 60, 60, TFT_BLUE); // Button 2
} else {
m->drawRect(65, 40, 60, 60, TFT_BLUE); // Button 2
m->fillRect(66, 41, 60, 60, TFT_BLUE);
}
if(!espnow2.data_to_send.aux[2]) {
if(!espnow2.aux_state[2]) {
m->drawRect(130, 40, 60, 60, TFT_BLUE); // Button 3
} else {
m->drawRect(130, 40, 60, 60, TFT_BLUE); // Button 3
m->fillRect(131, 41, 60, 60, TFT_BLUE);
}
if(!espnow2.data_to_send.aux[3]) {
if(!espnow2.aux_state[3]) {
m->drawRect(195, 40, 60, 60, TFT_BLUE); // Button 4
} else {
m->drawRect(195, 40, 60, 60, TFT_BLUE); // Button 4
m->fillRect(196, 41, 60, 60, TFT_BLUE);
}
if(!espnow2.data_to_send.aux[4]) {
if(!espnow2.aux_state[4]) {
////if(!espnow2.rubybutton_state[0]) {
m->drawRect(260, 40, 60, 60, TFT_BLUE); // Button 5
} else {
m->drawRect(260, 40, 60, 60, TFT_BLUE); // Button 5
m->fillRect(261, 41, 60, 60, TFT_BLUE);
}
m->setCursor(90, 130);
m->print("ESPNowRC");
//// m->setCursor(90, 130);
//// m->print("ESPNowRC");
// Temporary RubyFPV Buttons
if(!espnow2.rubybutton_state[0]) {
m->drawRect(0, 120, 60, 60, TFT_BLUE); // Button 1 - OFF
} else {
m->drawRect(0, 120, 60, 60, TFT_BLUE); // Button 1 - ON
m->fillRect(1, 121, 60, 60, TFT_BLUE);
}
if(!espnow2.rubybutton_state[1]) {
m->drawRect(65, 120, 60, 60, TFT_BLUE); // Button 2
} else {
m->drawRect(65, 120, 60, 60, TFT_BLUE); // Button 2
m->fillRect(66, 121, 60, 60, TFT_BLUE);
}
if(!espnow2.rubybutton_state[2]) {
m->drawRect(130, 120, 60, 60, TFT_BLUE); // Button 3
} else {
m->drawRect(130, 120, 60, 60, TFT_BLUE); // Button 3
m->fillRect(131, 121, 60, 60, TFT_BLUE);
}
if(!espnow2.rubybutton_state[3]) {
m->drawRect(195, 120, 60, 60, TFT_BLUE); // Button 4
} else {
m->drawRect(195, 120, 60, 60, TFT_BLUE); // Button 4
m->fillRect(196, 121, 60, 60, TFT_BLUE);
}
if(!espnow2.rubybutton_state[4]) {
////if(!espnow2.rubybutton_state[0]) {
m->drawRect(260, 120, 60, 60, TFT_BLUE); // Button 5
} else {
m->drawRect(260, 120, 60, 60, TFT_BLUE); // Button 5
m->fillRect(261, 121, 60, 60, TFT_BLUE);
}
m->pushSprite(0,0);
@ -175,52 +216,71 @@ void ESPNowRC::show(TFT_eSprite *m) {
// - MinY: 150
// - MaxX: 160
// - MaxY: 190
/****
if(e.to.x >= 110 && e.to.x <= 120 && e.to.y >= 140 && e.to.y <= 180) {
// Minus throttle
// Serial.printf("Left: TOP\n");
inputs.trim_throttle-=20;
inputs.trim_throttle-=10;
//// speak.speak_trim_word = TRIM_THROTTLE_INCREASE;
//// speak.speak_now_trim = true;
} else if(e.to.x >= 110 && e.to.x <= 170 && e.to.y >= 200 && e.to.y <= 220) {
// Add throttle
// Serial.printf("Left: BOTTOM\n");
inputs.trim_throttle+=20;
inputs.trim_throttle+=10;
//// speak.speak_trim_word = TRIM_THROTTLE_DECREASE;
//// speak.speak_now_trim = true;
} else if(e.to.x >= 60 && e.to.x <= 105 && e.to.y >= 180 && e.to.y <= 210) {
// Minus Yaw
// Serial.printf("Left: LEFT\n");
inputs.trim_yaw-=20;
inputs.trim_yaw+=10;
//// speak.speak_trim_word = TRIM_YAW_DECREASE;
//// speak.speak_now_trim = true;
Serial.printf("Trim Yaw: %d\n", inputs.trim_yaw);
} else if(e.to.x >= 110 && e.to.x <= 160 && e.to.y >= 150 && e.to.y <= 190) {
// Add Yaw
// Serial.printf("Left: RIGHT\n");
inputs.trim_yaw+=20;
inputs.trim_yaw-=10;
//// speak.speak_trim_word = TRIM_YAW_INCREASE;
//// speak.speak_now_trim = true;
} else if(e.to.x >= 250 && e.to.x <= 230 && e.to.y >= 187 && e.to.y <= 197) {
Serial.printf("Trim Yaw: %d\n", inputs.trim_yaw);
} else if(e.to.x >= 214 && e.to.x <= 245 && e.to.y >= 148 && e.to.y <= 181) {
// Add Row
inputs.trim_roll+=20;
inputs.trim_roll+=10;
//// speak.speak_trim_word = TRIM_ROLL_INCREASE;
//// speak.speak_now_trim = true;
} else if(e.to.x >= 180 && e.to.x <= 175 && e.to.y >= 187 && e.to.y <= 197) {
Serial.printf("Trim Roll: %d\n", inputs.trim_yaw);
} else if(e.to.x >= 170 && e.to.x <= 180 && e.to.y >= 148 && e.to.y <= 181) {
// Minus Row
inputs.trim_roll+=20;
inputs.trim_roll-=10;
//// speak.speak_trim_word = TRIM_ROLL_DECREASE;
//// speak.speak_now_trim = true;
// TODO: Continue the rest.....
} else if(e.to.x >= 0 && e.to.x <= 60 && e.to.y >= 40 && e.to.y <= 100) {
espnow2.data_to_send.aux[0] = !espnow2.data_to_send.aux[0]; // Flip Switch 0
Serial.printf("Trim Roll: %d\n", inputs.trim_yaw);
} else ****/
if(e.to.x >= 0 && e.to.x <= 60 && e.to.y >= 40 && e.to.y <= 100) {
espnow2.aux_state[0] = !espnow2.aux_state[0]; // Flip Switch 0
} else if(e.to.x >= 65 && e.to.x <= 125 && e.to.y >= 40 && e.to.y <= 100) {
espnow2.data_to_send.aux[1] = !espnow2.data_to_send.aux[1]; // Flip Switch 1
espnow2.aux_state[1] = !espnow2.aux_state[1]; // Flip Switch 1
} else if(e.to.x >= 130 && e.to.x <= 190 && e.to.y >= 40 && e.to.y <= 100) {
espnow2.data_to_send.aux[2] = !espnow2.data_to_send.aux[2]; // Flip Switch 2
espnow2.aux_state[2] = !espnow2.aux_state[2]; // Flip Switch 2
} else if(e.to.x >= 195 && e.to.x <= 255 && e.to.y >= 40 && e.to.y <= 100) {
espnow2.data_to_send.aux[3] = !espnow2.data_to_send.aux[3]; // Flip Switch 3
espnow2.aux_state[3] = !espnow2.aux_state[3]; // Flip Switch 3
} else if(e.to.x >= 260 && e.to.x <= 310 && e.to.y >= 40 && e.to.y <= 100) {
espnow2.data_to_send.aux[4] = !espnow2.data_to_send.aux[4]; // Flip Switch 4
espnow2.aux_state[4] = !espnow2.aux_state[4]; // Flip Switch 4
} else if(e.to.x >= 0 && e.to.x <= 60 && e.to.y >= 120 && e.to.y <= 180) {
espnow2.rubybutton_state[0] = true;
espnow2.last_rubybutton[0] = millis();
} else if(e.to.x >= 65 && e.to.x <= 125 && e.to.y >= 120 && e.to.y <= 180) {
espnow2.rubybutton_state[1] = true;
espnow2.last_rubybutton[1] = millis();
} else if(e.to.x >= 130 && e.to.x <= 190 && e.to.y >= 120 && e.to.y <= 180) {
espnow2.rubybutton_state[2] = true;
espnow2.last_rubybutton[2] = millis();
} else if(e.to.x >= 195 && e.to.x <= 255 && e.to.y >= 120 && e.to.y <= 180) {
espnow2.rubybutton_state[3] = true;
espnow2.last_rubybutton[3] = millis();
}
touch_time = millis();
}

28
src/Inputs.cpp
View File

@ -54,6 +54,14 @@ void Inputs::begin(void) {
trim_roll = trim_str.toInt();
}
// Preload values
invert_throttle_max = stickcalibration.throttle_max + stickcalibration.throttle_min;
invert_yaw_max = stickcalibration.yaw_max + stickcalibration.yaw_min;
invert_pitch_max = stickcalibration.pitch_max + stickcalibration.pitch_min;
invert_roll_max = stickcalibration.roll_max + stickcalibration.roll_min;
// Pre-read
read();
@ -72,25 +80,29 @@ void Inputs::read(void) {
roll_raw = analogRead(ROLL_PIN);
if(invert_throttle) {
throttle_raw = 4095 - throttle_raw;
throttle_raw = invert_throttle_max - throttle_raw;
}
if(invert_yaw) {
yaw_raw = 4095 - yaw_raw;
yaw_raw = invert_yaw_max - yaw_raw;
}
if(invert_pitch) {
pitch_raw = 4095 - pitch_raw;
pitch_raw = invert_pitch_max - pitch_raw;
}
if(invert_roll) {
roll_raw = 4095 - roll_raw;
roll_raw = invert_roll_max - roll_raw;
}
throttle = pool_insert(THROTTLE, throttle_raw) + trim_throttle;
yaw = pool_insert(YAW, yaw_raw) + trim_yaw;
pitch = pool_insert(PITCH, pitch_raw) + trim_pitch;
roll = pool_insert(ROLL, roll_raw) + trim_roll;
// throttle = pool_insert(THROTTLE, throttle_raw) + trim_throttle;
throttle = pool_insert(THROTTLE, throttle_raw);
// yaw = pool_insert(YAW, yaw_raw) + trim_yaw;
yaw = pool_insert(YAW, yaw_raw);
// pitch = pool_insert(PITCH, pitch_raw) + trim_pitch;
pitch = pool_insert(PITCH, pitch_raw);
// roll = pool_insert(ROLL, roll_raw) + trim_roll;
roll = pool_insert(ROLL, roll_raw);
}

6
src/Inputs.h
View File

@ -4,6 +4,7 @@
#include <Arduino.h>
#include <M5Core2.h>
#include "Storage.h"
#include "StickCalibration.h"
#define THROTTLE_PIN 35
#define YAW_PIN 36
@ -58,6 +59,11 @@ class Inputs {
int16_t trim_pitch = 0;
int16_t trim_roll = 0;
int16_t invert_throttle_max = 0;
int16_t invert_yaw_max = 0;
int16_t invert_pitch_max = 0;
int16_t invert_roll_max = 0;
Inputs(void);
void begin(void);
void read(void);

2
src/main.cpp
View File

@ -28,8 +28,8 @@ void setup() {
storage.begin();
///////customwifi.begin();
inputs.begin();
stickcalibration.begin();
inputs.begin();
espnow2.begin();
esppair.begin();
espnowrc.begin();

Write Preview
Loading…
Cancel
Save