Diff: ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp

Revision:

1:9530746906b6

Parent:

0:3473b92e991e

--- a/ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp	Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp	Wed Nov 28 13:06:23 2018 +0000
@@ -1,10 +1,16 @@
 #include "ROBOFRIEN_GUI.h"
+#include "GUI_Config.h"
 #include "eeprom.h"
-#include "Config.h"
 #include "BufferedSerial.h"
 
+#define PC_DEBUG        0
 
-BufferedSerial pc(USBTX, USBRX);
+#if PC_DEBUG
+    Serial pc(USBTX,USBRX);
+#else
+    BufferedSerial pc(p9, p10);      // tx, rx
+#endif
+
 
 uint8_t ISR_Modem_ID_Dept, ISR_Modem_ID_Dest;
 uint8_t ISR_Modem_DC_DPN, ISR_Modem_DC_DL;
@@ -19,11 +25,7 @@
 uint16_t model_type2;
 uint8_t pc_buffer[256],ex_pc_raw_buffer,pc_raw_buffer[256];
 uint8_t pc_buffer_cnt=0;
-int HomePointChksum,MarkerChksum;
 
-void ROBOFRIEN_GUI::pc_ascii_trans(char* input){
-    pc.printf(input);   pc.printf("\r\n");
-}
 
 void ROBOFRIEN_GUI::pc_rx_update(){
     while (pc.readable()){
@@ -90,7 +92,7 @@
     }
 }
 void ROBOFRIEN_GUI::Init(){    
-    pc.baud(38400);
+    pc.baud(115200);
     eeprom_init();
     /// EEPROM R/W ////
     // -------- MODEL -------------- //
@@ -130,6 +132,8 @@
     motor_min[1] = (int)eeprom_read(EEPROM_MOTOR_MIN_2_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_2_DOWN);
     motor_min[2] = (int)eeprom_read(EEPROM_MOTOR_MIN_3_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_3_DOWN);
     motor_min[3] = (int)eeprom_read(EEPROM_MOTOR_MIN_4_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_4_DOWN);
+    motor_min[4] = (int)eeprom_read(EEPROM_MOTOR_MIN_5_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_5_DOWN);
+    motor_min[5] = (int)eeprom_read(EEPROM_MOTOR_MIN_6_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_6_DOWN);
 
     // --------- OUTPUT - LED ------------ //
     headlight_period = eeprom_read(EEPROM_HEADLIGHT_PERIOD);
@@ -140,9 +144,9 @@
     // --------- GAIN - LIMIT ANGLE ------- //
     limit_rollx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_DOWN) - 32767;
     limit_pitchx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_DOWN) - 32767;
-    limit_roll_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_ROLL_DOWN) - 32767;
-    limit_pitch_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_PITCH_DOWN) - 32767;
-    limit_yaw_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_YAW_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_YAW_DOWN) - 32767;
+    limit_roll_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_ROLL_DOWN) - 32767;
+    limit_pitch_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_PITCH_DOWN) - 32767;
+    limit_yaw_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_YAW_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_YAW_DOWN) - 32767;
 
     // --------- GAIN - GAIN DATA --------- //
     for (int i = 0; i < 20; i++) {
@@ -184,25 +188,33 @@
                 if( (Modem_DATA[1] & 0b00010000) == 0b00010000)button[4] = !button[4];
                 // Home Point //
                 if( Modem_DATA[0] == 0){
-                    Homepoint_Lat = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
-                    Homepoint_Lng = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
-                    Homepoint_Alt = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;
+                    Controller_Joystick[0]  = (Modem_DATA[2]*256 + Modem_DATA[3])-32767;
+                    Controller_Joystick[1]  = (Modem_DATA[4]*256 + Modem_DATA[5])-32767;
+                    Controller_Joystick[2]  = (Modem_DATA[6]*256 + Modem_DATA[7])-32767;
+                    Controller_Joystick[3]  = (Modem_DATA[8]*256 + Modem_DATA[9])-32767;
+                    Gimbal_Joystick[0]      = (Modem_DATA[10]*256 + Modem_DATA[11])-32767;
+                    Gimbal_Joystick[1]      = (Modem_DATA[12]*256 + Modem_DATA[13])-32767;
+                    Gimbal_Joystick[2]      = (Modem_DATA[14]*256 + Modem_DATA[15])-32767;
                 }else if(Modem_DATA[0] <= 20){
                     Marker_Mode[Modem_DATA[0]-1] = Modem_DATA[2];
                     Marker_Lat[Modem_DATA[0]-1] = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
                     Marker_Lng[Modem_DATA[0]-1] = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
                     Marker_Alt[Modem_DATA[0]-1] = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;                    
                     Marker_Speed[Modem_DATA[0]-1] = (long)Modem_DATA[13] * 256 + Modem_DATA[14];
+                }else if( Modem_DATA[0] == 21){
+                    Homepoint_Lat = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
+                    Homepoint_Lng = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
+                    Homepoint_Alt = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;
                 }
-                trans_flight_data(TO_GCS, Modem_DC_DPN);
+//                trans_flight_data(TO_GCS, Modem_DC_DPN);
             }
             else {
-                trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]);
+//                trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]);
             }
         }
         else {
             /// NOT REQUEST - WRITE TO FCS ///
-                trans_empty_data(TO_GCS, Modem_DC_DPN);
+//                trans_empty_data(TO_GCS, Modem_DC_DPN);
                 switch (Modem_DC_DPN) { // Check DPN //
                 case 0:
                     break;
@@ -212,7 +224,7 @@
                     eeprom_write(EEPROM_MODEL_TYPE1, (int)model_type1);
                     eeprom_write(EEPROM_MODEL_TYPE2_UP, (int)model_type2 / 256);
                     eeprom_write(EEPROM_MODEL_TYPE2_DOWN, (int)model_type2 % 256);
-                    eeprom_refresh();
+                    eeprom_refresh_bool = true;
                     break;
                 case 3:     /// RC Receiver ///
                     if ((Modem_DATA[0] >= 1) & (Modem_DATA[0] <= 8)) {
@@ -261,7 +273,7 @@
                             eeprom_write(EEPROM_RECV_MAX_8, cap_max[Modem_DATA[0] - 1] + 127);
                             break;
                         }
-                        eeprom_refresh();
+                        eeprom_refresh_bool = true;
                     }
                     break;
                 case 4:     // Motor - OUTPUT //
@@ -269,30 +281,44 @@
                     pwm_info[1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4];
                     pwm_info[2] = (int)Modem_DATA[5] * 256 + Modem_DATA[6];
                     pwm_info[3] = (int)Modem_DATA[7] * 256 + Modem_DATA[8];
+                    pwm_info[4] = (int)Modem_DATA[9] * 256 + Modem_DATA[10];
+                    pwm_info[5] = (int)Modem_DATA[11] * 256 + Modem_DATA[12];
                     switch (Modem_DATA[0]) {
                     case 1:
                         motor_min[0] = pwm_info[0];
                         eeprom_write(EEPROM_MOTOR_MIN_1_UP, motor_min[0] / 256);
                         eeprom_write(EEPROM_MOTOR_MIN_1_DOWN, motor_min[0] % 256);
-                        eeprom_refresh();
+                        eeprom_refresh_bool = true;
                         break;
                     case 2:
                         motor_min[1] = pwm_info[1];
                         eeprom_write(EEPROM_MOTOR_MIN_2_UP, motor_min[1] / 256);
                         eeprom_write(EEPROM_MOTOR_MIN_2_DOWN, motor_min[1] % 256);
-                        eeprom_refresh();
+                        eeprom_refresh_bool = true;
                         break;
                     case 3:
                         motor_min[2] = pwm_info[2];
                         eeprom_write(EEPROM_MOTOR_MIN_3_UP, motor_min[2] / 256);
                         eeprom_write(EEPROM_MOTOR_MIN_3_DOWN, motor_min[2] % 256);
-                        eeprom_refresh();
+                        eeprom_refresh_bool = true;
                         break;
                     case 4:
                         motor_min[3] = pwm_info[3];
                         eeprom_write(EEPROM_MOTOR_MIN_4_UP, motor_min[3] / 256);
                         eeprom_write(EEPROM_MOTOR_MIN_4_DOWN, motor_min[3] % 256);
-                        eeprom_refresh();
+                        eeprom_refresh_bool = true;
+                        break;
+                    case 5:
+                        motor_min[4] = pwm_info[4];
+                        eeprom_write(EEPROM_MOTOR_MIN_5_UP, motor_min[4] / 256);
+                        eeprom_write(EEPROM_MOTOR_MIN_5_DOWN, motor_min[4] % 256);
+                        eeprom_refresh_bool = true;
+                        break;
+                    case 6:
+                        motor_min[5] = pwm_info[5];
+                        eeprom_write(EEPROM_MOTOR_MIN_6_UP, motor_min[5] / 256);
+                        eeprom_write(EEPROM_MOTOR_MIN_6_DOWN, motor_min[5] % 256);
+                        eeprom_refresh_bool = true;
                         break;
                     }
                     break;
@@ -305,7 +331,7 @@
                     eeprom_write(EEPROM_HEADLIGHT_DUTYRATE, headlight_dutyrate);
                     eeprom_write(EEPROM_SIDELIGHT_PERIOD, sidelight_period);
                     eeprom_write(EEPROM_SIDELIGHT_DUTYRATE, sidelight_dutyrate);
-                    eeprom_refresh();
+                    eeprom_refresh_bool = true;
                     break;
                 case 6:     // AHRS - ROLL , Pitch //
                             /// Attitude ///
@@ -327,26 +353,26 @@
                         declination_angle = (float)(((int)Modem_DATA[10] * 256 + Modem_DATA[11]) - 18000)/100.0;
                         eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_UP, (int)((declination_angle+180)*100)/256);
                         eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_DOWN, (int)((declination_angle+180)*100)%256);
-                        eeprom_refresh();
+                        eeprom_refresh_bool = true;
                     }
                     break;
                 case 7:     // Limit Angle //
                     limit_rollx100 = ((int)Modem_DATA[0] * 256 + Modem_DATA[1]) - 32767;
                     limit_pitchx100 = ((int)Modem_DATA[2] * 256 + Modem_DATA[3]) - 32767;
-                    limit_roll_rate = ((int)Modem_DATA[4] * 256 + Modem_DATA[5]) - 32767;
-                    limit_pitch_rate = ((int)Modem_DATA[6] * 256 + Modem_DATA[7]) - 32767;
-                    limit_yaw_rate = ((int)Modem_DATA[8] * 256 + Modem_DATA[9]) - 32767;
+                    limit_roll_ratex100 = ((int)Modem_DATA[4] * 256 + Modem_DATA[5]) - 32767;
+                    limit_pitch_ratex100 = ((int)Modem_DATA[6] * 256 + Modem_DATA[7]) - 32767;
+                    limit_yaw_ratex100 = ((int)Modem_DATA[8] * 256 + Modem_DATA[9]) - 32767;
                     eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_UP, (limit_rollx100 + 32767) / 256);
                     eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_DOWN, (limit_rollx100 + 32767) % 256);
                     eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_UP, (limit_pitchx100 + 32767) / 256);
                     eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_DOWN, (limit_pitchx100 + 32767) % 256);
-                    eeprom_write(EEPROM_LIMIT_RATE_ROLL_UP, (limit_roll_rate + 32767) / 256);
-                    eeprom_write(EEPROM_LIMIT_RATE_ROLL_DOWN, (limit_roll_rate + 32767) % 256);
-                    eeprom_write(EEPROM_LIMIT_RATE_PITCH_UP, (limit_pitch_rate + 32767) / 256);
-                    eeprom_write(EEPROM_LIMIT_RATE_PITCH_DOWN, (limit_pitch_rate + 32767) % 256);
-                    eeprom_write(EEPROM_LIMIT_RATE_YAW_UP, (limit_yaw_rate + 32767) / 256);
-                    eeprom_write(EEPROM_LIMIT_RATE_YAW_DOWN, (limit_yaw_rate + 32767) % 256);
-                    eeprom_refresh();
+                    eeprom_write(EEPROM_LIMIT_RATE_ROLL_UP, (limit_roll_ratex100 + 32767) / 256);
+                    eeprom_write(EEPROM_LIMIT_RATE_ROLL_DOWN, (limit_roll_ratex100 + 32767) % 256);
+                    eeprom_write(EEPROM_LIMIT_RATE_PITCH_UP, (limit_pitch_ratex100 + 32767) / 256);
+                    eeprom_write(EEPROM_LIMIT_RATE_PITCH_DOWN, (limit_pitch_ratex100 + 32767) % 256);
+                    eeprom_write(EEPROM_LIMIT_RATE_YAW_UP, (limit_yaw_ratex100 + 32767) / 256);
+                    eeprom_write(EEPROM_LIMIT_RATE_YAW_DOWN, (limit_yaw_ratex100 + 32767) % 256);
+                    eeprom_refresh_bool = true;
                     break;
                 case 8:
                     int gain_number = Modem_DATA[0];
@@ -360,7 +386,7 @@
                         eeprom_write(EEPROM_GAIN_D_DOWN[gain_number - 1], gain_dx100[gain_number - 1] % 256);
                         eeprom_write(EEPROM_GAIN_I_UP[gain_number - 1], gain_ix100[gain_number - 1] / 256);
                         eeprom_write(EEPROM_GAIN_I_DOWN[gain_number - 1], gain_ix100[gain_number - 1] % 256);    
-                        if(gain_number == 20)eeprom_refresh();
+                        if(gain_number == 20)eeprom_refresh_bool = true;
                     }
                     break;
             }
@@ -369,6 +395,24 @@
     }
 
 }
+
+void ROBOFRIEN_GUI::Trans(){
+    if ((Modem_ID_Dept == 255)&(Modem_ID_Dest == 0)) {
+        if (Modem_DC_REQUEST == 1) {
+            /// REQUST - READ FROM FCS ///
+            if (Modem_DC_DPN == 0) {
+                trans_flight_data(TO_GCS, Modem_DC_DPN);
+            }
+            else {
+                trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]);
+            }
+        }
+        else {
+            /// NOT REQUEST - WRITE TO FCS ///
+                trans_empty_data(TO_GCS, Modem_DC_DPN);
+        }
+    }
+}
 void ROBOFRIEN_GUI::trans_empty_data(int id_dest, int data_parse_num) {
     uint8_t Packet_SOF[2] = { 254,254 };
     uint8_t Packet_Identifier[2];
@@ -398,8 +442,11 @@
     modem_buffer[8] = Packet_CHKSUM[1];
     modem_buffer[9] = Packet_EOF[0];
     modem_buffer[10] = Packet_EOF[1];
+    TRANS_BUFFER_BUSY = true;
     for (int i = 0; i <= 10; i++) {
-        pc.putc(modem_buffer[i]);
+        TRANS_BUF[i] = modem_buffer[i];
+        TRANS_BUF_LEN = 11;
+//        pc.putc(modem_buffer[i]);
     }
 }
 
@@ -485,12 +532,12 @@
         Packet_DATA[1] = (limit_rollx100 + 32767) % 256;
         Packet_DATA[2] = (limit_pitchx100 + 32767) / 256;
         Packet_DATA[3] = (limit_pitchx100 + 32767) % 256;
-        Packet_DATA[4] = (limit_roll_rate + 32767) / 256;
-        Packet_DATA[5] = (limit_roll_rate + 32767) % 256;
-        Packet_DATA[6] = (limit_pitch_rate + 32767) / 256;
-        Packet_DATA[7] = (limit_pitch_rate + 32767) % 256;
-        Packet_DATA[8] = (limit_yaw_rate + 32767) / 256;
-        Packet_DATA[9] = (limit_yaw_rate + 32767) % 256;
+        Packet_DATA[4] = (limit_roll_ratex100 + 32767) / 256;
+        Packet_DATA[5] = (limit_roll_ratex100 + 32767) % 256;
+        Packet_DATA[6] = (limit_pitch_ratex100 + 32767) / 256;
+        Packet_DATA[7] = (limit_pitch_ratex100 + 32767) % 256;
+        Packet_DATA[8] = (limit_yaw_ratex100 + 32767) / 256;
+        Packet_DATA[9] = (limit_yaw_ratex100 + 32767) % 256;
         for (int i = 10; i <= 15; i++) Packet_DATA[i] = 0;
         break;
     case 8:
@@ -530,8 +577,11 @@
 modem_buffer[24] = Packet_CHKSUM[1];
 modem_buffer[25] = Packet_EOF[0];
 modem_buffer[26] = Packet_EOF[1];
+TRANS_BUFFER_BUSY = true;
 for (int i = 0; i <= 26; i++) {
-    pc.putc(modem_buffer[i]);
+    TRANS_BUF[i] = modem_buffer[i];
+    TRANS_BUF_LEN = 27;
+//    pc.putc(modem_buffer[i]);
 }
 }
 
@@ -556,28 +606,47 @@
     // DATA - Modem & State //
     if(Mode1 >= 15) Mode1 = 15;
     else if(Mode1 <= 0) Mode1 = 0;
-    if(Mode2 >= 15) Mode2 = 15;
-    else if(Mode2 <= 0) Mode2 = 0;
+    if(Alarm >= 15) Alarm = 15;
+    else if(Alarm <= 0) Alarm = 0;
     if(MissionState >= 15) MissionState = 15;
     else if(MissionState <= 0) MissionState = 0;
     if(CurrentMarker >= 20) CurrentMarker = 20;
     else if(CurrentMarker <= 0) CurrentMarker = 0;
+    int GainChksum = 0;
+    for(int i=0; i<20; i++){
+        GainChksum += (gain_px100[i]>>8);
+        GainChksum += (gain_px100[i]&0xFF);
+        GainChksum += (gain_dx100[i]>>8);
+        GainChksum += (gain_dx100[i]&0xFF);
+        GainChksum += (gain_ix100[i]>>8);
+        GainChksum += (gain_ix100[i]&0xFF);
+    }
     HomePointChksum = ((Homepoint_Lat + 90000000)/16777216) + ((Homepoint_Lat + 90000000)%16777216/65536) + ((Homepoint_Lat + 90000000)%65536/256) + ((Homepoint_Lat + 90000000)%256);
     HomePointChksum += ((Homepoint_Lng + 180000000)/16777216) + ((Homepoint_Lng + 180000000)%16777216/65536) + ((Homepoint_Lng + 180000000)%65536/256) + ((Homepoint_Lng + 180000000)%256);
     HomePointChksum += ((Homepoint_Alt + 10000)/256 + (Homepoint_Alt + 10000)%256);
     MarkerChksum = 0;
     for(int i=0; i<20; i++){
-        MarkerChksum += Marker_Mode[i];
-        MarkerChksum += (Marker_Lat[i]+90000000)/16777216 + (Marker_Lat[i]+90000000)%16777216/65536 + (Marker_Lat[i]+90000000)%65536/256 + (Marker_Lat[i]+90000000)%256;   
-        MarkerChksum += (Marker_Lng[i]+180000000)/16777216 + (Marker_Lng[i]+180000000)%16777216/65536 + (Marker_Lng[i]+180000000)%65536/256 + (Marker_Lng[i]+180000000)%256;
-        MarkerChksum += (Marker_Alt[i]+10000)/256 + (Marker_Alt[i]+10000)%256;
-        MarkerChksum += Marker_Speed[i]/256 + Marker_Speed[i]%256;   
+        MarkerChksum += (uint8_t)(Marker_Mode[i]);
+        MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)/16777216);
+        MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%16777216/65536);
+        MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%65536/256);
+        MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%256);   
+        MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)/16777216);
+        MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%16777216/65536);
+        MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%65536/256);
+        MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%256);
+        MarkerChksum += (uint8_t)((Marker_Alt[i]+10000)/256);
+        MarkerChksum += (uint8_t)((Marker_Alt[i]+10000)%256);
+        MarkerChksum += (uint8_t)(Marker_Speed[i]/256);
+        MarkerChksum += (uint8_t)(Marker_Speed[i]%256);   
     }
-    Packet_DATA[0] = Mode1 * 16 + Mode2;
+    Packet_DATA[0] = Mode1 * 16 + Alarm;
     Packet_DATA[1] = MissionState;
     Packet_DATA[2] = CurrentMarker;
+    if(Bat1 > 100) Bat1 = 100;
+    else if(Bat1 < 0) Bat1 = 0;
     Packet_DATA[3] = Bat1;
-    Packet_DATA[4] = Bat2;
+    Packet_DATA[4] = GainChksum%256;
     Packet_DATA[5] = button[0] * 1 + button[1] * 2 + button[2] * 4 + button[3] * 8 + button[4] * 16;
     Packet_DATA[6] = HomePointChksum%256;
     Packet_DATA[7] = MarkerChksum % 256;
@@ -658,10 +727,10 @@
     Packet_DATA[66] = (int)((DEBUGx100[4]))%256;
     Packet_DATA[67] = (int)((DEBUGx100[5]))/256;
     Packet_DATA[68] = (int)((DEBUGx100[5]))%256;
-    Packet_DATA[69] = (int)((DEBUGx100[6]))/256;
-    Packet_DATA[70] = (int)((DEBUGx100[6]))%256;
-    Packet_DATA[71] = (int)((DEBUGx100[7]))/256;
-    Packet_DATA[72] = (int)((DEBUGx100[7]))%256;
+    Packet_DATA[69] = (int)((DEBUGx100[6]+32767))/256;
+    Packet_DATA[70] = (int)((DEBUGx100[6]+32767))%256;
+    Packet_DATA[71] = (int)((DEBUGx100[7]+32767))/256;
+    Packet_DATA[72] = (int)((DEBUGx100[7]+32767))%256;
     // CHKSUM //
     unsigned int data_checksum = 0;
     for (int i = 0; i < data_length; i++) {
@@ -682,8 +751,11 @@
     modem_buffer[81] = Packet_CHKSUM[1];
     modem_buffer[82] = Packet_EOF[0];
     modem_buffer[83] = Packet_EOF[1];
+    TRANS_BUFFER_BUSY = true;
     for (int i = 0; i <= 83; i++) {
-        pc.putc(modem_buffer[i]);
+        TRANS_BUF[i] = modem_buffer[i];
+        TRANS_BUF_LEN = 84;
+//        pc.putc(modem_buffer[i]);
     }
 }
 
@@ -699,7 +771,7 @@
     eeprom_write(EEPROM_AHRS_PITCH_GAP_DOWN, (((unsigned long)(in_acc_bias_y * 10000 + 32767) % 256)));
     eeprom_write(EEPROM_AHRS_YAW_GAP_UP, (((unsigned long)(in_acc_bias_z * 10000 + 32767) / 256)));
     eeprom_write(EEPROM_AHRS_YAW_GAP_DOWN, (((unsigned long)(in_acc_bias_z * 10000 + 32767) % 256)));
-    eeprom_refresh();
+    eeprom_refresh_bool = true;
 }
 
 void ROBOFRIEN_GUI::write_compass_setting_to_eeprom(){
@@ -718,5 +790,46 @@
     eeprom_write(EEPROM_AHRS_YAW_Z_GAP_1, ( (uint32_t)(mag_z_avg + 32767)/256 ) );
     eeprom_write(EEPROM_AHRS_YAW_Z_GAP_2, ( (uint32_t)(mag_z_avg + 32767)%256 ) );
     
-    eeprom_refresh();
+    eeprom_refresh_bool = true;
+}
+
+int eeprom_refresh_cnt = 0;
+bool eeprom_refresh_cnt_bool = false;
+void ROBOFRIEN_GUI::SET_DATA(){
+    if( eeprom_refresh_bool == true){
+        // True 일때, False로 만들고, 10초동안 true로 변경되지 않으면 그때 eeprom_refresh 실시  ( 계속 써서 시스템 망가져서 ~ )
+        eeprom_refresh_bool = false;
+        eeprom_refresh_cnt_bool = true;
+        eeprom_refresh_cnt = 0;
+    }
+    if( eeprom_refresh_cnt_bool == true){
+        eeprom_refresh_cnt ++;
+        if(eeprom_refresh_cnt > 10){
+            eeprom_refresh();
+            eeprom_refresh_cnt = 0;
+            eeprom_refresh_cnt_bool = false;
+        }
+    }
 }
+
+void ROBOFRIEN_GUI::TRANS_BUFFER(){
+    if(TRANS_BUFFER_BUSY == true){
+        if( TRANS_BUF_CNT >= TRANS_BUF_LEN){
+            TRANS_BUF_CNT = 0;
+            TRANS_BUFFER_BUSY = false;
+        }else{
+            #if PC_DEBUG
+                pc.putc(TRANS_BUF[TRANS_BUF_CNT]);
+                TRANS_BUF_CNT ++;
+            #else
+                if(  (LPC_UART3->LSR & 0x20) ){
+                    LPC_UART3->THR = TRANS_BUF[TRANS_BUF_CNT];
+                    TRANS_BUF_CNT ++;                
+                }            
+            #endif
+//            }            
+        }
+    }else{
+        TRANS_BUF_CNT = 0;
+    }
+}