diff --git a/src/main.cpp b/src/main.cpp index f272099..052c241 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -33,6 +33,16 @@ void setup() { Serial.println("ADS1115 ERROR!"); // delay(5000); } + //// ADS.setWireClock(100000); + ADS.setGain(1); // +- 4.096V + ADS.setDataRate(7); // 7 = Fastest + + // Initialize SBUSData + for(uint8_t i = 0; i < 16; i++) + sbusData.ch[i] = 0; + sbusData.failsafe = false; + sbusData.ch17 = false; + sbusData.ch18 = false; // Setting WiFi wifi_setup(); @@ -70,6 +80,16 @@ void setup() { NULL, CPU_0 ); + + xTaskCreatePinnedToCore( + taskGimbal, + "TaskGimbal", // Name of the process + 8192, // This stack size can be checked & adjusted by reading the Stack Highwater + NULL, + 5, // Priority + NULL, + CPU_0 + ); } void ota_setup(void) { @@ -372,11 +392,27 @@ void parse_i2c_command(void) { rubybutton_last[3] = millis(); } + // QA1 + if((uint32_t)(millis() - rubybutton_last[4]) > rubybutton_delay) { + rubybutton[4] = button3; + + // Reset after read + button3 = 0; + + // digitalWrite(BUTTON_DOWN, rubybutton[3]); + rubybutton_last[4] = millis(); + } + i2c_send[0] = rubybutton[0] | // Menu/Ok button rubybutton[1] << 1 | // Cancel button rubybutton[2] << 2 | // Plus button - rubybutton[3] << 3 ; // Minus button + rubybutton[3] << 3 | // Minus button + rubybutton[4] << 4 | // QA1 button + rubybutton[5] << 5 | // QA2 button + rubybutton[6] << 6 | // QA3 button + rubybutton[7] << 7 ; // Action + button + i2c_send[1] = 0; i2c_send[2] = 0; i2c_send[3] = 0; @@ -425,8 +461,8 @@ void taskSystem(void *pvParameters) { // frame_count = frame_count_raw; // frame_count_raw = 0; - // profile_gimbal_rate = profile_gimbal_rate_raw; - // profile_gimbal_rate_raw = 0; + profile_gimbal_rate = profile_gimbal_rate_raw; + profile_gimbal_rate_raw = 0; // if(counter == 0) { /// signalNotOK = true; @@ -445,7 +481,7 @@ void taskDisplay(void *pvParameters) { (void) pvParameters; uint32_t last_update = 0; - uint8_t delay = 50; // Screen refreshing time. 50mS delay. Roughly 20Hz for display rate. + uint8_t delay = 20; // Screen refreshing time. 20mS delay. Roughly 50Hz for display rate. for(;;) { vTaskDelay(delay); @@ -511,6 +547,54 @@ void taskInput(void *pvParameters) { } +void taskGimbal(void *pvParameters) { + (void) pvParameters; + + uint32_t profiling_start = 0; + uint32_t profiling_end = 0; + + profile_gimbal_rate_raw = 0; + + for(;;) { + // /profiling_start = millis(); + // raw_throttle = analogRead(THROTTLE_PIN); + // throttle_steps = median_throttle(raw_throttle); + // Sampling throttle + // throttle_steps = sampling(THROTTLE_POOL); + throttle_steps = ADS.readADC(1); + + // raw_yaw = analogRead(YAW_PIN); + // yaw_steps = median_yaw(raw_yaw); + // Sampling yaw + // yaw_steps = sampling(YAW_POOL); + yaw_steps = ADS.readADC(0); + + // raw_roll = analogRead(ROLL_PIN); + // roll_steps = median_roll(raw_roll); + // roll_steps = sampling(ROLL_POOL); + roll_steps = ADS.readADC(3); + + // raw_pitch = analogRead(PITCH_PIN); + // $pitch_steps = median_pitch(raw_pitch); + // pitch_steps = sampling(PITCH_POOL); + pitch_steps = ADS.readADC(2); + + // profiling_end = micros(); + // uint32_t result = (uint32_t)(profiling_end - profiling_start); + // Serial.println(result); + + throttle_val = map(throttle_steps, min_throttle, max_throttle, 1000, 2000); + yaw_val = map(yaw_steps, min_yaw, max_yaw, 1000, 2000); + pitch_val = map(pitch_steps, min_pitch, max_pitch, 1000, 2000); + roll_val = map(roll_steps, min_roll, max_roll, 1000, 2000); + + profile_gimbal_rate_raw++; + + vTaskDelay(1); + } +} + + void draw_gimbal_huge(uint16_t x, uint16_t y) { // Top mainscreen_buffer.drawFastHLine(x, y, 120, TFT_DARKGREEN); @@ -541,11 +625,12 @@ void screen_rubytoggles(void) { mainscreen_buffer.print("[RUBYFPV]"); // Title - mainscreen_buffer.drawRect(16, 32, 60, 60, TFT_WHITE); - mainscreen_buffer.drawRect(16, 122, 60, 60, TFT_WHITE); + mainscreen_buffer.drawRect(16, 32, 60, 60, TFT_WHITE); // MENU + mainscreen_buffer.drawRect(16, 122, 60, 60, TFT_WHITE); // CANCEL + mainscreen_buffer.drawRect(96, 32, 60, 60, TFT_WHITE); // QA1 - mainscreen_buffer.drawRect(260, 32, 60, 60, TFT_WHITE); - mainscreen_buffer.drawRect(260, 122, 60, 60, TFT_WHITE); + mainscreen_buffer.drawRect(260, 32, 60, 60, TFT_WHITE); // UP + mainscreen_buffer.drawRect(260, 122, 60, 60, TFT_WHITE); // DOWN footer(); mainscreen_buffer.pushSprite(0, 0); // Display it @@ -587,6 +672,7 @@ void screen_rubytoggles(void) { if(e & E_TOUCH) { + // MENU if( e.to.x >= 16 && e.to.x <= 76 && @@ -597,6 +683,7 @@ void screen_rubytoggles(void) { button1 = true; } + // CANCEL if( e.to.x >= 16 && e.to.x <= 76 && @@ -607,6 +694,7 @@ void screen_rubytoggles(void) { button2 = true; } + // UP if( e.to.x >= 260 && e.to.x <= 320 && @@ -617,6 +705,7 @@ void screen_rubytoggles(void) { button6 = true; } + // DOWN if( e.to.x >= 200 && e.to.x <= 320 && @@ -627,6 +716,16 @@ void screen_rubytoggles(void) { button7 = true; } + // QA1 + if( + e.to.x >= 96 && + e.to.x <= 156 && + e.to.y >= 32 && + e.to.y <= 92 + ) { + if(DEBUG) Serial.printf("RUBY BUTTON 3\r\n"); + button3 = true; + } } // End of E_TOUCH @@ -647,6 +746,39 @@ void screen_controller(void) { draw_gimbal_huge(39, 60); draw_gimbal_huge(161, 60); + gimbal_left_x = map(yaw_val, 1000, 2000, 39, 159); + gimbal_left_y = map(throttle_val, 1000, 2000, 60, 180); + mainscreen_buffer.fillCircle(gimbal_left_x, gimbal_left_y, 3, TFT_WHITE); + + gimbal_right_x = map(roll_val, 1000, 2000, 161, 281); + gimbal_right_y = map(pitch_val, 1000, 2000, 60, 180); + mainscreen_buffer.fillCircle(gimbal_right_x, gimbal_right_y, 3, TFT_WHITE); + + + if(throttle_steps > max_throttle) { + max_throttle = throttle_steps; + } else if(throttle_steps < min_throttle) { + min_throttle = throttle_steps; + } + + if(yaw_steps > max_yaw) { + max_yaw = yaw_steps; + } else if(yaw_steps < min_yaw) { + min_yaw = yaw_steps; + } + + if(pitch_steps > max_pitch) { + max_pitch = pitch_steps; + } else if(pitch_steps < min_pitch) { + min_pitch = pitch_steps; + } + + if(roll_steps > max_roll) { + max_roll = roll_steps; + } else if(roll_steps < min_roll) { + min_roll = roll_steps; + } + footer(); mainscreen_buffer.pushSprite(0, 0); // Display it @@ -875,8 +1007,12 @@ void footer(void) { mainscreen_buffer.print(uptime); mainscreen_buffer.setTextColor(TFT_YELLOW); - mainscreen_buffer.setCursor(60, 226); - mainscreen_buffer.print(IP); + mainscreen_buffer.setCursor(70, 226); + // mainscreen_buffer.print(IP); + mainscreen_buffer.print(profile_gimbal_rate); + + mainscreen_buffer.setCursor(120, 226); + mainscreen_buffer.print(pitch_val); mainscreen_buffer.setTextColor(TFT_WHITE); mainscreen_buffer.setCursor(200, 226); diff --git a/src/main.h b/src/main.h index d78eef8..0a6319c 100644 --- a/src/main.h +++ b/src/main.h @@ -66,6 +66,30 @@ bool header_detected = false; // ADS1115 #define ADS1115_SDA 21 #define ADS1115_SCL 22 +uint16_t profile_gimbal_rate; +uint16_t profile_gimbal_rate_raw; +int16_t throttle_steps = 0; +int16_t yaw_steps = 0; +int16_t roll_steps = 0; +int16_t pitch_steps = 0; +int16_t throttle_val = 0; +int16_t yaw_val = 0; +int16_t pitch_val = 0; +int16_t roll_val = 0; +int16_t gimbal_left_x = 0; // For Yaw +int16_t gimbal_left_y = 0; // For throttle +int16_t gimbal_right_x = 0; // For Roll +int16_t gimbal_right_y = 0; // For Pitch + +// Active calibration +int16_t min_throttle = 32767; +int16_t max_throttle = 0; +int16_t min_yaw = 32767; +int16_t max_yaw = 0; +int16_t min_pitch = 32767; +int16_t max_pitch = 0; +int16_t min_roll = 32767; +int16_t max_roll = 0; #define DIALOGBOX_FOREGROUND TFT_WHITE #define DIALOGBOX_BACKGROUND TFT_NAVY @@ -115,7 +139,7 @@ uint8_t button10 = 0; bool rubybutton[16]; uint32_t rubybutton_last[16]; -uint16_t rubybutton_delay = 200; +uint16_t rubybutton_delay = 100; struct_sbus sbusData; uint8_t current_screen = SCREEN_INTRO; @@ -126,6 +150,7 @@ uint32_t last_scroll = 0; void taskSystem(void *pvParameters); void taskDisplay(void *pvParameters); void taskInput(void *pvParameters); +void taskGimbal(void *pvParameters); // Other Functions void onI2C0Receive(int len); @@ -144,5 +169,4 @@ void touch_reader(void); void footer(void); void draw_gimbal_huge(uint16_t x, uint16_t y); - #endif