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

Finally fixed with median

master
Englebert 10 months ago
parent
722c704491
commit
69a7a4b39e
2 changed files with 91 additions and 45 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. 134
      src/main.cpp
  2. 2
      src/main.h

134
src/main.cpp
View File

@ -65,16 +65,16 @@ int16_t gimbal_right_y = 0; // For Pitch
// Active calibration
int16_t min_throttle = 32767;
int16_t max_throttle = 0;
int16_t max_throttle = -32767;
int16_t min_yaw = 32767;
int16_t max_yaw = 0;
int16_t max_yaw = -32767;
int16_t min_pitch = 32767;
int16_t max_pitch = 0;
int16_t max_pitch = -32767;
int16_t min_roll = 32767;
int16_t max_roll = 0;
int16_t max_roll = -32767;
// Gimbal Medians
#define MEDIAN_MAX 5
#define MEDIAN_MAX 31
int16_t med_throttle[MEDIAN_MAX];
int16_t med_yaw[MEDIAN_MAX];
int16_t med_pitch[MEDIAN_MAX];
@ -504,18 +504,28 @@ void load_rccontroller(void) {
touch_state = true;
} else {
if(
(uint32_t)(millis() - last_touch) > 150
(uint32_t)(millis() - last_touch) > 250
) {
if((uint32_t)(millis() - last_toggle) > 500) {
// Temporary
last_toggle = millis();
// Gestures Detection
} else {
touch_gestures(x, y);
// Reset state for next cycle to detect
touch_state = false;
if(
x > 120 && y > 0 &&
x < 200 && y < 85
) {
transmission_mode = MODE_ESPNOW;
last_toggle = millis();
} else if(
x > 120 && y > 85 &&
x < 200 && y < 170
) {
transmission_mode = MODE_BLE;
last_toggle = millis();
}
}
touch_gestures(x, y);
// Reset state for next cycle to detect
touch_state = false;
}
}
@ -536,19 +546,23 @@ void load_rccontroller(void) {
case TP_SLIDE_UP:
// current_hack--;
/***
if(transmission_mode > MODE_NONE)
transmission_mode--;
if(transmission_mode < MODE_NONE) {
transmission_mode = MODE_NONE;
}
***/
break;
case TP_SLIDE_DOWN:
// current_hack++;
/****
transmission_mode++;
if(transmission_mode > MODE_BLE) {
transmission_mode = MODE_BLE;
}
***/
break;
case TP_NONE:
@ -1373,10 +1387,21 @@ void taskGimbal(void *pvParameters) {
for(;;) {
vTaskDelay(1);
yaw_steps = ADS.readADC(3);
throttle_steps = ADS.readADC(2);
pitch_steps = ADS.readADC(0);
roll_steps = ADS.readADC(1);
tmp_yaw = ADS.readADC(3);
// if(ADS.getError() == ADS1X15_OK)
// yaw_steps = tmp_yaw;
tmp_throttle = ADS.readADC(2);
// if(ADS.getError() == ADS1X15_OK)
// throttle_steps = tmp_throttle;
tmp_pitch = ADS.readADC(0);
// if(ADS.getError() == ADS1X15_OK)
// pitch_steps = tmp_pitch;
tmp_roll = ADS.readADC(1);
// if(ADS.getError() == ADS1X15_OK)
// roll_steps = tmp_roll;
// Median Processor
// Shift to left....
@ -1388,18 +1413,25 @@ void taskGimbal(void *pvParameters) {
}
// Insert to the last
med_throttle[MEDIAN_MAX - 1] = throttle_steps;
med_yaw[MEDIAN_MAX - 1] = yaw_steps;
med_pitch[MEDIAN_MAX - 1] = pitch_steps;
med_roll[MEDIAN_MAX - 1] = roll_steps;
// med_throttle[MEDIAN_MAX - 1] = throttle_steps;
// med_yaw[MEDIAN_MAX - 1] = yaw_steps;
// med_pitch[MEDIAN_MAX - 1] = pitch_steps;
// med_roll[MEDIAN_MAX - 1] = roll_steps;
med_throttle[MEDIAN_MAX - 1] = tmp_throttle;
med_yaw[MEDIAN_MAX - 1] = tmp_yaw;
med_pitch[MEDIAN_MAX - 1] = tmp_pitch;
med_roll[MEDIAN_MAX - 1] = tmp_roll;
// Sorting
throttle_val = get_median(med_throttle, MEDIAN_MAX);
yaw_val = get_median(med_yaw, MEDIAN_MAX);
pitch_val = get_median(med_pitch, MEDIAN_MAX);
roll_val = get_median(med_roll, MEDIAN_MAX);
// tmp_throttle = get_median(med_throttle, MEDIAN_MAX);
// tmp_yaw = get_median(med_yaw, MEDIAN_MAX);
// tmp_pitch = get_median(med_pitch, MEDIAN_MAX);
// tmp_roll = get_median(med_roll, MEDIAN_MAX);
throttle_steps = get_median(0);
yaw_steps = get_median(1);
pitch_steps = get_median(2);
roll_steps = get_median(3);
// throttle_val = map(throttle_steps, min_throttle, max_throttle, 1000, 2000);
throttle_val = map(throttle_steps, min_throttle, max_throttle, 2000, 1000);
yaw_val = map(yaw_steps, min_yaw, max_yaw, 2000, 1000);
@ -1412,19 +1444,40 @@ void taskGimbal(void *pvParameters) {
}
int16_t get_median(int16_t dataset[], uint8_t total_data) {
int16_t key, j;
int16_t get_median(uint8_t gimbal) {
int16_t i, key, j;
uint8_t total_data = MEDIAN_MAX;
uint8_t pos = total_data >> 1;
int16_t arr[total_data];
int16_t dataset[total_data];
switch(gimbal) {
case 0:
for(i = 0; i < total_data; i++) {
dataset[i] = med_throttle[i];
}
break;
case 1:
for(i = 0; i < total_data; i++) {
dataset[i] = med_yaw[i];
}
break;
case 2:
for(i = 0; i < total_data; i++) {
dataset[i] = med_pitch[i];
}
break;
case 3:
for(i = 0; i < total_data; i++) {
dataset[i] = med_roll[i];
}
break;
/**
for(uint8_t i = 0; i < total_data; i++) {
Serial.print(dataset[i]); Serial.print(" ");
}
Serial.println("");
**/
for(uint8_t i = 0; i < total_data; i++) {
for(i = 1; i < total_data; i++) {
key = dataset[i];
j = i - 1;
while( j >= 0 && dataset[j] > key) {
@ -1434,13 +1487,6 @@ int16_t get_median(int16_t dataset[], uint8_t total_data) {
dataset[j + 1] = key;
}
/**
for(uint8_t i = 0; i < total_data; i++) {
Serial.print(dataset[i]); Serial.print(" ");
}
Serial.println("");
**/
return dataset[pos];
}

2
src/main.h
View File

@ -138,7 +138,7 @@ void load_mainscreen(void);
void load_rccontroller(void);
void draw_gimbal_huge(uint16_t x, uint16_t y);
int16_t get_median(int16_t dataset[], uint8_t total_data);
int16_t get_median(uint8_t gimbal);
void display_on(void);
void display_state_processor(void);

Write Preview
Loading…
Cancel
Save