Inital

OpenFlightTX.ino

@@ -0,0 +1,224 @@
+#define BUZZER_PIN      15
+#define THROTTLE_PIN    36
+#define YAW_PIN         39
+#define PITCH_PIN       34
+#define ROLL_PIN        35
+
+uint16_t throttle_raw = 0;
+uint16_t yaw_raw = 0;
+uint16_t pitch_raw = 0;
+uint16_t roll_raw = 0;
+
+uint16_t throttle_value = 0;
+uint16_t yaw_value = 0;
+uint16_t pitch_value = 0;
+uint16_t roll_value = 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
+
+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 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;
+    }
+}
+
+/*
+ * 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];
+    }
+
+    // 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];
+        }
+
+        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);
+}
+
+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);
+
+    throttle_value = throttle_insert(throttle_raw);    
+    yaw_value = yaw_insert(yaw_raw);    
+    pitch_value = pitch_insert(pitch_raw);    
+    roll_value = roll_insert(roll_raw);    
+}
+
+
+void setup() {
+    Serial.begin(115200);
+
+    // PIN Initialization
+    pinMode(BUZZER_PIN, OUTPUT);
+
+    median_initialize();
+
+    // Startup Sound
+    intro();
+}
+
+void loop() {
+    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_raw);
+    Serial.println("]");
+    */
+}