Kim Youngsik / Mbed 2 deprecated 1TEST_Flight_Protocol_v1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of 1TEST_Flight_Protocol by Kim Youngsik

ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp@0:c466d770ffa7, 2017-05-17 (annotated)

Committer:
skyyoungsik
Date:
Wed May 17 11:02:27 2017 +0000
Revision:
0:c466d770ffa7
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skyyoungsik 0:c466d770ffa7 1 #include "ROBOFRIEN_GUI.h"
skyyoungsik 0:c466d770ffa7 2 #include "eeprom.h"
skyyoungsik 0:c466d770ffa7 3 #include "Config.h"
skyyoungsik 0:c466d770ffa7 4
skyyoungsik 0:c466d770ffa7 5
skyyoungsik 0:c466d770ffa7 6 Serial pc(USBTX, USBRX);
skyyoungsik 0:c466d770ffa7 7
skyyoungsik 0:c466d770ffa7 8 uint8_t ISR_Modem_ID_Dept, ISR_Modem_ID_Dest;
skyyoungsik 0:c466d770ffa7 9 uint8_t ISR_Modem_DC_DPN, ISR_Modem_DC_DL;
skyyoungsik 0:c466d770ffa7 10 uint8_t ISR_Modem_DATA[256];
skyyoungsik 0:c466d770ffa7 11 uint8_t ISR_Modem_DC_REQUEST;
skyyoungsik 0:c466d770ffa7 12 uint16_t ISR_Modem_CHKSUM;
skyyoungsik 0:c466d770ffa7 13 uint8_t pc_isr_cnt;
skyyoungsik 0:c466d770ffa7 14 uint8_t isr_modem_data_cnt = 0;
skyyoungsik 0:c466d770ffa7 15 uint8_t pc_parsing_bool = false;
skyyoungsik 0:c466d770ffa7 16 bool pc_Start_Check;
skyyoungsik 0:c466d770ffa7 17 uint8_t model_type1;
skyyoungsik 0:c466d770ffa7 18 uint16_t model_type2;
skyyoungsik 0:c466d770ffa7 19 int16_t cap_min[8], cap_neu[8], cap_max[8];
skyyoungsik 0:c466d770ffa7 20 int pwm_info[4];
skyyoungsik 0:c466d770ffa7 21 int motor_min[4];
skyyoungsik 0:c466d770ffa7 22 uint8_t headlight_period, headlight_dutyrate, sidelight_period, sidelight_dutyrate;
skyyoungsik 0:c466d770ffa7 23 bool compass_configuration_start_bool = false;
skyyoungsik 0:c466d770ffa7 24 bool compass_configuration_end_bool = false;
skyyoungsik 0:c466d770ffa7 25 bool fcs_compass_configuration_state = false;
skyyoungsik 0:c466d770ffa7 26 bool attitude_configuration_bool = false;
skyyoungsik 0:c466d770ffa7 27 int16_t limit_rollx100, limit_pitchx100;
skyyoungsik 0:c466d770ffa7 28 int32_t limit_roll_rate, limit_pitch_rate, limit_yaw_rate;
skyyoungsik 0:c466d770ffa7 29 int gain_px100[20], gain_dx100[20], gain_ix100[20];
skyyoungsik 0:c466d770ffa7 30 float mag_x_avg,mag_y_avg,mag_z_avg;
skyyoungsik 0:c466d770ffa7 31 uint8_t pc_buffer[256],ex_pc_raw_buffer,pc_raw_buffer[256];
skyyoungsik 0:c466d770ffa7 32 uint8_t pc_buffer_cnt=0;
skyyoungsik 0:c466d770ffa7 33 float declination_angle;
skyyoungsik 0:c466d770ffa7 34 int HomePointChksum,MarkerChksum;
skyyoungsik 0:c466d770ffa7 35
skyyoungsik 0:c466d770ffa7 36 void ROBOFRIEN_GUI::pc_rx_update(){
skyyoungsik 0:c466d770ffa7 37 while (pc.readable()){
skyyoungsik 0:c466d770ffa7 38 pc_buffer[1] = pc_buffer[0];
skyyoungsik 0:c466d770ffa7 39 pc_buffer[0] = pc.getc();
skyyoungsik 0:c466d770ffa7 40 // Check SOF //
skyyoungsik 0:c466d770ffa7 41 if (( pc_buffer[1] == 254) & (pc_buffer[0] == 254)){
skyyoungsik 0:c466d770ffa7 42 pc_Start_Check = true;
skyyoungsik 0:c466d770ffa7 43 pc_isr_cnt = 0;
skyyoungsik 0:c466d770ffa7 44 }
skyyoungsik 0:c466d770ffa7 45 else if(pc_Start_Check == true){
skyyoungsik 0:c466d770ffa7 46 if(pc_isr_cnt <= 4){
skyyoungsik 0:c466d770ffa7 47 switch(pc_isr_cnt){
skyyoungsik 0:c466d770ffa7 48 case 0: ISR_Modem_ID_Dept = pc_buffer[0]; break;
skyyoungsik 0:c466d770ffa7 49 case 1: ISR_Modem_ID_Dest = pc_buffer[0]; break;
skyyoungsik 0:c466d770ffa7 50 case 2: ISR_Modem_DC_REQUEST = pc_buffer[0]; break;
skyyoungsik 0:c466d770ffa7 51 case 3: ISR_Modem_DC_DPN = pc_buffer[0]; break;
skyyoungsik 0:c466d770ffa7 52 case 4: ISR_Modem_DC_DL = pc_buffer[0]; isr_modem_data_cnt = 0; break;
skyyoungsik 0:c466d770ffa7 53 }
skyyoungsik 0:c466d770ffa7 54 }
skyyoungsik 0:c466d770ffa7 55 else if( pc_isr_cnt > 4 ){
skyyoungsik 0:c466d770ffa7 56 if ((ISR_Modem_DC_DL >= 1) & (pc_isr_cnt <= (4 + ISR_Modem_DC_DL))) {
skyyoungsik 0:c466d770ffa7 57 ISR_Modem_DATA[isr_modem_data_cnt] = pc_buffer[0];
skyyoungsik 0:c466d770ffa7 58 isr_modem_data_cnt++;
skyyoungsik 0:c466d770ffa7 59 }
skyyoungsik 0:c466d770ffa7 60 else if(pc_isr_cnt == (4 + ISR_Modem_DC_DL + 2) ){
skyyoungsik 0:c466d770ffa7 61 ISR_Modem_CHKSUM = ((uint16_t)pc_buffer[1] * 256 + pc_buffer[0]);
skyyoungsik 0:c466d770ffa7 62 }
skyyoungsik 0:c466d770ffa7 63 else if(pc_isr_cnt == (4 + ISR_Modem_DC_DL + 2 + 2) ){
skyyoungsik 0:c466d770ffa7 64 if((pc_buffer[1] == 255) & (pc_buffer[0] == 255) ){
skyyoungsik 0:c466d770ffa7 65 pc_Start_Check = false;
skyyoungsik 0:c466d770ffa7 66 pc_parsing_bool = true;
skyyoungsik 0:c466d770ffa7 67 }
skyyoungsik 0:c466d770ffa7 68 }
skyyoungsik 0:c466d770ffa7 69 }
skyyoungsik 0:c466d770ffa7 70 pc_isr_cnt ++;
skyyoungsik 0:c466d770ffa7 71 }
skyyoungsik 0:c466d770ffa7 72 }
skyyoungsik 0:c466d770ffa7 73 }
skyyoungsik 0:c466d770ffa7 74
skyyoungsik 0:c466d770ffa7 75 uint8_t Modem_ID_Dest;
skyyoungsik 0:c466d770ffa7 76 uint8_t Modem_ID_Dept;
skyyoungsik 0:c466d770ffa7 77 uint8_t Modem_DC_DPN;
skyyoungsik 0:c466d770ffa7 78 uint8_t Modem_DC_DL;
skyyoungsik 0:c466d770ffa7 79 uint8_t Modem_DATA[256];
skyyoungsik 0:c466d770ffa7 80 uint8_t Modem_DC_REQUEST;
skyyoungsik 0:c466d770ffa7 81
skyyoungsik 0:c466d770ffa7 82 bool ROBOFRIEN_GUI::rx_bool(){
skyyoungsik 0:c466d770ffa7 83 if(pc_parsing_bool == true){
skyyoungsik 0:c466d770ffa7 84 pc_parsing_bool = false;
skyyoungsik 0:c466d770ffa7 85 int cal_chksum = 0;
skyyoungsik 0:c466d770ffa7 86 for(int i=0; i< ISR_Modem_DC_DL; i++){
skyyoungsik 0:c466d770ffa7 87 Modem_DATA[i] = ISR_Modem_DATA[i];
skyyoungsik 0:c466d770ffa7 88 cal_chksum += ISR_Modem_DATA[i];
skyyoungsik 0:c466d770ffa7 89 }
skyyoungsik 0:c466d770ffa7 90 if (ISR_Modem_CHKSUM == cal_chksum){
skyyoungsik 0:c466d770ffa7 91 return true;
skyyoungsik 0:c466d770ffa7 92 }else{
skyyoungsik 0:c466d770ffa7 93 return false;
skyyoungsik 0:c466d770ffa7 94 }
skyyoungsik 0:c466d770ffa7 95 }
skyyoungsik 0:c466d770ffa7 96 else{
skyyoungsik 0:c466d770ffa7 97 return false;
skyyoungsik 0:c466d770ffa7 98 }
skyyoungsik 0:c466d770ffa7 99 }
skyyoungsik 0:c466d770ffa7 100 uint8_t DPN_Info;
skyyoungsik 0:c466d770ffa7 101 float calibrate_gap_roll,calibrate_gap_pitch;
skyyoungsik 0:c466d770ffa7 102 void ROBOFRIEN_GUI::Configuration_init(){
skyyoungsik 0:c466d770ffa7 103 pc.baud(38400);
skyyoungsik 0:c466d770ffa7 104 eeprom_init();
skyyoungsik 0:c466d770ffa7 105 /// EEPROM R/W ////
skyyoungsik 0:c466d770ffa7 106 // -------- MODEL -------------- //
skyyoungsik 0:c466d770ffa7 107 model_type1 = eeprom_read(EEPROM_MODEL_TYPE1);
skyyoungsik 0:c466d770ffa7 108 model_type2 = (int)eeprom_read(EEPROM_MODEL_TYPE2_UP) * 256 + eeprom_read(EEPROM_MODEL_TYPE2_DOWN);
skyyoungsik 0:c466d770ffa7 109
skyyoungsik 0:c466d770ffa7 110 // -------- INPUT - CAP ------------ //
skyyoungsik 0:c466d770ffa7 111 cap_min[0] = (int16_t)eeprom_read(EEPROM_RECV_MIN_1) - 127;
skyyoungsik 0:c466d770ffa7 112 cap_min[1] = (int16_t)eeprom_read(EEPROM_RECV_MIN_2) - 127;
skyyoungsik 0:c466d770ffa7 113 cap_min[2] = (int16_t)eeprom_read(EEPROM_RECV_MIN_3) - 127;
skyyoungsik 0:c466d770ffa7 114 cap_min[3] = (int16_t)eeprom_read(EEPROM_RECV_MIN_4) - 127;
skyyoungsik 0:c466d770ffa7 115 cap_min[4] = (int16_t)eeprom_read(EEPROM_RECV_MIN_5) - 127;
skyyoungsik 0:c466d770ffa7 116 cap_min[5] = (int16_t)eeprom_read(EEPROM_RECV_MIN_6) - 127;
skyyoungsik 0:c466d770ffa7 117 cap_min[6] = (int16_t)eeprom_read(EEPROM_RECV_MIN_7) - 127;
skyyoungsik 0:c466d770ffa7 118 cap_min[7] = (int16_t)eeprom_read(EEPROM_RECV_MIN_8) - 127;
skyyoungsik 0:c466d770ffa7 119
skyyoungsik 0:c466d770ffa7 120 cap_neu[0] = (int16_t)eeprom_read(EEPROM_RECV_NEU_1) - 127;
skyyoungsik 0:c466d770ffa7 121 cap_neu[1] = (int16_t)eeprom_read(EEPROM_RECV_NEU_2) - 127;
skyyoungsik 0:c466d770ffa7 122 cap_neu[2] = (int16_t)eeprom_read(EEPROM_RECV_NEU_3) - 127;
skyyoungsik 0:c466d770ffa7 123 cap_neu[3] = (int16_t)eeprom_read(EEPROM_RECV_NEU_4) - 127;
skyyoungsik 0:c466d770ffa7 124 cap_neu[4] = (int16_t)eeprom_read(EEPROM_RECV_NEU_5) - 127;
skyyoungsik 0:c466d770ffa7 125 cap_neu[5] = (int16_t)eeprom_read(EEPROM_RECV_NEU_6) - 127;
skyyoungsik 0:c466d770ffa7 126 cap_neu[6] = (int16_t)eeprom_read(EEPROM_RECV_NEU_7) - 127;
skyyoungsik 0:c466d770ffa7 127 cap_neu[7] = (int16_t)eeprom_read(EEPROM_RECV_NEU_8) - 127;
skyyoungsik 0:c466d770ffa7 128
skyyoungsik 0:c466d770ffa7 129 cap_max[0] = (int16_t)eeprom_read(EEPROM_RECV_MAX_1) - 127;
skyyoungsik 0:c466d770ffa7 130 cap_max[1] = (int16_t)eeprom_read(EEPROM_RECV_MAX_2) - 127;
skyyoungsik 0:c466d770ffa7 131 cap_max[2] = (int16_t)eeprom_read(EEPROM_RECV_MAX_3) - 127;
skyyoungsik 0:c466d770ffa7 132 cap_max[3] = (int16_t)eeprom_read(EEPROM_RECV_MAX_4) - 127;
skyyoungsik 0:c466d770ffa7 133 cap_max[4] = (int16_t)eeprom_read(EEPROM_RECV_MAX_5) - 127;
skyyoungsik 0:c466d770ffa7 134 cap_max[5] = (int16_t)eeprom_read(EEPROM_RECV_MAX_6) - 127;
skyyoungsik 0:c466d770ffa7 135 cap_max[6] = (int16_t)eeprom_read(EEPROM_RECV_MAX_7) - 127;
skyyoungsik 0:c466d770ffa7 136 cap_max[7] = (int16_t)eeprom_read(EEPROM_RECV_MAX_8) - 127;
skyyoungsik 0:c466d770ffa7 137
skyyoungsik 0:c466d770ffa7 138 // --------- OUTPUT - MOTOR ----------- //
skyyoungsik 0:c466d770ffa7 139 motor_min[0] = (int)eeprom_read(EEPROM_MOTOR_MIN_1_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_1_DOWN);
skyyoungsik 0:c466d770ffa7 140 motor_min[1] = (int)eeprom_read(EEPROM_MOTOR_MIN_2_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_2_DOWN);
skyyoungsik 0:c466d770ffa7 141 motor_min[2] = (int)eeprom_read(EEPROM_MOTOR_MIN_3_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_3_DOWN);
skyyoungsik 0:c466d770ffa7 142 motor_min[3] = (int)eeprom_read(EEPROM_MOTOR_MIN_4_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_4_DOWN);
skyyoungsik 0:c466d770ffa7 143
skyyoungsik 0:c466d770ffa7 144 // --------- OUTPUT - LED ------------ //
skyyoungsik 0:c466d770ffa7 145 headlight_period = eeprom_read(EEPROM_HEADLIGHT_PERIOD);
skyyoungsik 0:c466d770ffa7 146 headlight_dutyrate = eeprom_read(EEPROM_HEADLIGHT_DUTYRATE);
skyyoungsik 0:c466d770ffa7 147 sidelight_period = eeprom_read(EEPROM_SIDELIGHT_PERIOD);
skyyoungsik 0:c466d770ffa7 148 sidelight_dutyrate = eeprom_read(EEPROM_SIDELIGHT_DUTYRATE);
skyyoungsik 0:c466d770ffa7 149
skyyoungsik 0:c466d770ffa7 150 // --------- GAIN - LIMIT ANGLE ------- //
skyyoungsik 0:c466d770ffa7 151 limit_rollx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_DOWN) - 32767;
skyyoungsik 0:c466d770ffa7 152 limit_pitchx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_DOWN) - 32767;
skyyoungsik 0:c466d770ffa7 153 limit_roll_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_ROLL_DOWN) - 32767;
skyyoungsik 0:c466d770ffa7 154 limit_pitch_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_PITCH_DOWN) - 32767;
skyyoungsik 0:c466d770ffa7 155 limit_yaw_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_YAW_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_YAW_DOWN) - 32767;
skyyoungsik 0:c466d770ffa7 156
skyyoungsik 0:c466d770ffa7 157 // --------- GAIN - GAIN DATA --------- //
skyyoungsik 0:c466d770ffa7 158 for (int i = 0; i < 20; i++) {
skyyoungsik 0:c466d770ffa7 159 gain_px100[i] = (int)eeprom_read(EEPROM_GAIN_P_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_P_DOWN[i]);
skyyoungsik 0:c466d770ffa7 160 gain_dx100[i] = (int)eeprom_read(EEPROM_GAIN_D_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_D_DOWN[i]);
skyyoungsik 0:c466d770ffa7 161 gain_ix100[i] = (int)eeprom_read(EEPROM_GAIN_I_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_I_DOWN[i]);
skyyoungsik 0:c466d770ffa7 162 }
skyyoungsik 0:c466d770ffa7 163 calibrate_gap_roll = (((int)eeprom_read(EEPROM_AHRS_ROLL_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_ROLL_GAP_DOWN)) - 32767)/100.0;
skyyoungsik 0:c466d770ffa7 164 calibrate_gap_pitch = (((int)eeprom_read(EEPROM_AHRS_PITCH_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_PITCH_GAP_DOWN)) - 32767)/100.0;
skyyoungsik 0:c466d770ffa7 165
skyyoungsik 0:c466d770ffa7 166 // --------- Sensor ----------- ///
skyyoungsik 0:c466d770ffa7 167 mag_x_avg = (float)eeprom_read(EEPROM_AHRS_YAW_X_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_X_GAP_2) - 32767 ;
skyyoungsik 0:c466d770ffa7 168 mag_y_avg = (float)eeprom_read(EEPROM_AHRS_YAW_Y_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_Y_GAP_2) - 32767 ;
skyyoungsik 0:c466d770ffa7 169 mag_z_avg = (float)eeprom_read(EEPROM_AHRS_YAW_Z_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_Z_GAP_2) - 32767 ;
skyyoungsik 0:c466d770ffa7 170
skyyoungsik 0:c466d770ffa7 171 declination_angle = (float)(((signed long)eeprom_read(EEPROM_AHRS_DECLINATION_ANGLE_UP) * 256 + eeprom_read(EEPROM_AHRS_DECLINATION_ANGLE_DOWN))-18000)/100.0;
skyyoungsik 0:c466d770ffa7 172 }
skyyoungsik 0:c466d770ffa7 173
skyyoungsik 0:c466d770ffa7 174 void trans_configuration_data(int a, int b, int c);
skyyoungsik 0:c466d770ffa7 175 void trans_empty_data(int a, int b);
skyyoungsik 0:c466d770ffa7 176 void trans_flight_data(int a, int b);
skyyoungsik 0:c466d770ffa7 177 void ROBOFRIEN_GUI::Refresh(){
skyyoungsik 0:c466d770ffa7 178 Modem_ID_Dept = ISR_Modem_ID_Dept;
skyyoungsik 0:c466d770ffa7 179 Modem_ID_Dest = ISR_Modem_ID_Dest;
skyyoungsik 0:c466d770ffa7 180 Modem_DC_DPN = ISR_Modem_DC_DPN;
skyyoungsik 0:c466d770ffa7 181 Modem_DC_DL = ISR_Modem_DC_DL;
skyyoungsik 0:c466d770ffa7 182 Modem_DC_REQUEST = ISR_Modem_DC_REQUEST;
skyyoungsik 0:c466d770ffa7 183 DPN_Info = Modem_DC_DPN;
skyyoungsik 0:c466d770ffa7 184 if ((Modem_ID_Dept == 255)&(Modem_ID_Dest == 0)) {
skyyoungsik 0:c466d770ffa7 185 if (Modem_DC_REQUEST == 1) {
skyyoungsik 0:c466d770ffa7 186 /// REQUST - READ FROM FCS ///
skyyoungsik 0:c466d770ffa7 187 if (Modem_DC_DPN == 0) {
skyyoungsik 0:c466d770ffa7 188 /// FLIGHT DATA ///
skyyoungsik 0:c466d770ffa7 189 if( (Modem_DATA[1] & 0b00000001) == 0b00000001)button[0] = !button[0];
skyyoungsik 0:c466d770ffa7 190 if( (Modem_DATA[1] & 0b00000010) == 0b00000010)button[1] = !button[1];
skyyoungsik 0:c466d770ffa7 191 if( (Modem_DATA[1] & 0b00000100) == 0b00000100)button[2] = !button[2];
skyyoungsik 0:c466d770ffa7 192 if( (Modem_DATA[1] & 0b00001000) == 0b00001000)button[3] = !button[3];
skyyoungsik 0:c466d770ffa7 193 if( (Modem_DATA[1] & 0b00010000) == 0b00010000)button[4] = !button[4];
skyyoungsik 0:c466d770ffa7 194 // Home Point //
skyyoungsik 0:c466d770ffa7 195 if( Modem_DATA[0] == 0){
skyyoungsik 0:c466d770ffa7 196 Homepoint_Lat = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
skyyoungsik 0:c466d770ffa7 197 Homepoint_Lng = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
skyyoungsik 0:c466d770ffa7 198 Homepoint_Alt = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;
skyyoungsik 0:c466d770ffa7 199 }else if(Modem_DATA[0] <= 20){
skyyoungsik 0:c466d770ffa7 200 Marker_Mode[Modem_DATA[0]-1] = Modem_DATA[2];
skyyoungsik 0:c466d770ffa7 201 Marker_Lat[Modem_DATA[0]-1] = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
skyyoungsik 0:c466d770ffa7 202 Marker_Lng[Modem_DATA[0]-1] = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
skyyoungsik 0:c466d770ffa7 203 Marker_Alt[Modem_DATA[0]-1] = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;
skyyoungsik 0:c466d770ffa7 204 Marker_Speed[Modem_DATA[0]-1] = (long)Modem_DATA[13] * 256 + Modem_DATA[14];
skyyoungsik 0:c466d770ffa7 205 }
skyyoungsik 0:c466d770ffa7 206 trans_flight_data(TO_GCS, Modem_DC_DPN);
skyyoungsik 0:c466d770ffa7 207 }
skyyoungsik 0:c466d770ffa7 208 else {
skyyoungsik 0:c466d770ffa7 209 trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]);
skyyoungsik 0:c466d770ffa7 210 }
skyyoungsik 0:c466d770ffa7 211 }
skyyoungsik 0:c466d770ffa7 212 else {
skyyoungsik 0:c466d770ffa7 213 /// NOT REQUEST - WRITE TO FCS ///
skyyoungsik 0:c466d770ffa7 214 trans_empty_data(TO_GCS, Modem_DC_DPN);
skyyoungsik 0:c466d770ffa7 215 switch (Modem_DC_DPN) { // Check DPN //
skyyoungsik 0:c466d770ffa7 216 case 0:
skyyoungsik 0:c466d770ffa7 217 break;
skyyoungsik 0:c466d770ffa7 218 case 1: /// Modem ///
skyyoungsik 0:c466d770ffa7 219 model_type1 = Modem_DATA[0];
skyyoungsik 0:c466d770ffa7 220 model_type2 = (uint16_t)Modem_DATA[1] * 256 + Modem_DATA[2];
skyyoungsik 0:c466d770ffa7 221 eeprom_write(EEPROM_MODEL_TYPE1, (int)model_type1);
skyyoungsik 0:c466d770ffa7 222 eeprom_write(EEPROM_MODEL_TYPE2_UP, (int)model_type2 / 256);
skyyoungsik 0:c466d770ffa7 223 eeprom_write(EEPROM_MODEL_TYPE2_DOWN, (int)model_type2 % 256);
skyyoungsik 0:c466d770ffa7 224 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 225 break;
skyyoungsik 0:c466d770ffa7 226 case 3: /// RC Receiver ///
skyyoungsik 0:c466d770ffa7 227 if ((Modem_DATA[0] >= 1) & (Modem_DATA[0] <= 8)) {
skyyoungsik 0:c466d770ffa7 228 cap_min[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[5] - 127;
skyyoungsik 0:c466d770ffa7 229 cap_neu[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[6] - 127;
skyyoungsik 0:c466d770ffa7 230 cap_max[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[7] - 127;
skyyoungsik 0:c466d770ffa7 231 switch (Modem_DATA[0]) {
skyyoungsik 0:c466d770ffa7 232 case 1:
skyyoungsik 0:c466d770ffa7 233 eeprom_write(EEPROM_RECV_MIN_1, cap_min[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 234 eeprom_write(EEPROM_RECV_NEU_1, cap_neu[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 235 eeprom_write(EEPROM_RECV_MAX_1, cap_max[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 236 break;
skyyoungsik 0:c466d770ffa7 237 case 2:
skyyoungsik 0:c466d770ffa7 238 eeprom_write(EEPROM_RECV_MIN_2, cap_min[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 239 eeprom_write(EEPROM_RECV_NEU_2, cap_neu[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 240 eeprom_write(EEPROM_RECV_MAX_2, cap_max[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 241 break;
skyyoungsik 0:c466d770ffa7 242 case 3:
skyyoungsik 0:c466d770ffa7 243 eeprom_write(EEPROM_RECV_MIN_3, cap_min[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 244 eeprom_write(EEPROM_RECV_NEU_3, cap_neu[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 245 eeprom_write(EEPROM_RECV_MAX_3, cap_max[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 246 break;
skyyoungsik 0:c466d770ffa7 247 case 4:
skyyoungsik 0:c466d770ffa7 248 eeprom_write(EEPROM_RECV_MIN_4, cap_min[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 249 eeprom_write(EEPROM_RECV_NEU_4, cap_neu[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 250 eeprom_write(EEPROM_RECV_MAX_4, cap_max[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 251 break;
skyyoungsik 0:c466d770ffa7 252 case 5:
skyyoungsik 0:c466d770ffa7 253 eeprom_write(EEPROM_RECV_MIN_5, cap_min[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 254 eeprom_write(EEPROM_RECV_NEU_5, cap_neu[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 255 eeprom_write(EEPROM_RECV_MAX_5, cap_max[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 256 break;
skyyoungsik 0:c466d770ffa7 257 case 6:
skyyoungsik 0:c466d770ffa7 258 eeprom_write(EEPROM_RECV_MIN_6, cap_min[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 259 eeprom_write(EEPROM_RECV_NEU_6, cap_neu[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 260 eeprom_write(EEPROM_RECV_MAX_6, cap_max[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 261 break;
skyyoungsik 0:c466d770ffa7 262 case 7:
skyyoungsik 0:c466d770ffa7 263 eeprom_write(EEPROM_RECV_MIN_7, cap_min[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 264 eeprom_write(EEPROM_RECV_NEU_7, cap_neu[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 265 eeprom_write(EEPROM_RECV_MAX_7, cap_max[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 266 break;
skyyoungsik 0:c466d770ffa7 267 case 8:
skyyoungsik 0:c466d770ffa7 268 eeprom_write(EEPROM_RECV_MIN_8, cap_min[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 269 eeprom_write(EEPROM_RECV_NEU_8, cap_neu[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 270 eeprom_write(EEPROM_RECV_MAX_8, cap_max[Modem_DATA[0] - 1] + 127);
skyyoungsik 0:c466d770ffa7 271 break;
skyyoungsik 0:c466d770ffa7 272 }
skyyoungsik 0:c466d770ffa7 273 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 274 }
skyyoungsik 0:c466d770ffa7 275 break;
skyyoungsik 0:c466d770ffa7 276 case 4: // Motor - OUTPUT //
skyyoungsik 0:c466d770ffa7 277 pwm_info[0] = (int)Modem_DATA[1] * 256 + Modem_DATA[2];
skyyoungsik 0:c466d770ffa7 278 pwm_info[1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4];
skyyoungsik 0:c466d770ffa7 279 pwm_info[2] = (int)Modem_DATA[5] * 256 + Modem_DATA[6];
skyyoungsik 0:c466d770ffa7 280 pwm_info[3] = (int)Modem_DATA[7] * 256 + Modem_DATA[8];
skyyoungsik 0:c466d770ffa7 281 switch (Modem_DATA[0]) {
skyyoungsik 0:c466d770ffa7 282 case 1:
skyyoungsik 0:c466d770ffa7 283 motor_min[0] = pwm_info[0];
skyyoungsik 0:c466d770ffa7 284 eeprom_write(EEPROM_MOTOR_MIN_1_UP, motor_min[0] / 256);
skyyoungsik 0:c466d770ffa7 285 eeprom_write(EEPROM_MOTOR_MIN_1_DOWN, motor_min[0] % 256);
skyyoungsik 0:c466d770ffa7 286 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 287 break;
skyyoungsik 0:c466d770ffa7 288 case 2:
skyyoungsik 0:c466d770ffa7 289 motor_min[1] = pwm_info[1];
skyyoungsik 0:c466d770ffa7 290 eeprom_write(EEPROM_MOTOR_MIN_2_UP, motor_min[1] / 256);
skyyoungsik 0:c466d770ffa7 291 eeprom_write(EEPROM_MOTOR_MIN_2_DOWN, motor_min[1] % 256);
skyyoungsik 0:c466d770ffa7 292 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 293 break;
skyyoungsik 0:c466d770ffa7 294 case 3:
skyyoungsik 0:c466d770ffa7 295 motor_min[2] = pwm_info[2];
skyyoungsik 0:c466d770ffa7 296 eeprom_write(EEPROM_MOTOR_MIN_3_UP, motor_min[2] / 256);
skyyoungsik 0:c466d770ffa7 297 eeprom_write(EEPROM_MOTOR_MIN_3_DOWN, motor_min[2] % 256);
skyyoungsik 0:c466d770ffa7 298 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 299 break;
skyyoungsik 0:c466d770ffa7 300 case 4:
skyyoungsik 0:c466d770ffa7 301 motor_min[3] = pwm_info[3];
skyyoungsik 0:c466d770ffa7 302 eeprom_write(EEPROM_MOTOR_MIN_4_UP, motor_min[3] / 256);
skyyoungsik 0:c466d770ffa7 303 eeprom_write(EEPROM_MOTOR_MIN_4_DOWN, motor_min[3] % 256);
skyyoungsik 0:c466d770ffa7 304 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 305 break;
skyyoungsik 0:c466d770ffa7 306 }
skyyoungsik 0:c466d770ffa7 307 break;
skyyoungsik 0:c466d770ffa7 308 case 5: // LED - OUTPUT //
skyyoungsik 0:c466d770ffa7 309 headlight_period = Modem_DATA[0];
skyyoungsik 0:c466d770ffa7 310 headlight_dutyrate = Modem_DATA[1];
skyyoungsik 0:c466d770ffa7 311 sidelight_period = Modem_DATA[2];
skyyoungsik 0:c466d770ffa7 312 sidelight_dutyrate = Modem_DATA[3];
skyyoungsik 0:c466d770ffa7 313 eeprom_write(EEPROM_HEADLIGHT_PERIOD, headlight_period);
skyyoungsik 0:c466d770ffa7 314 eeprom_write(EEPROM_HEADLIGHT_DUTYRATE, headlight_dutyrate);
skyyoungsik 0:c466d770ffa7 315 eeprom_write(EEPROM_SIDELIGHT_PERIOD, sidelight_period);
skyyoungsik 0:c466d770ffa7 316 eeprom_write(EEPROM_SIDELIGHT_DUTYRATE, sidelight_dutyrate);
skyyoungsik 0:c466d770ffa7 317 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 318 break;
skyyoungsik 0:c466d770ffa7 319 case 6: // AHRS - ROLL , Pitch //
skyyoungsik 0:c466d770ffa7 320 /// Attitude ///
skyyoungsik 0:c466d770ffa7 321 if (Modem_DATA[0] == 1) {
skyyoungsik 0:c466d770ffa7 322 attitude_configuration_bool = true;
skyyoungsik 0:c466d770ffa7 323 }
skyyoungsik 0:c466d770ffa7 324 else {
skyyoungsik 0:c466d770ffa7 325 attitude_configuration_bool = false;
skyyoungsik 0:c466d770ffa7 326 }
skyyoungsik 0:c466d770ffa7 327 /// Coompass ///
skyyoungsik 0:c466d770ffa7 328 if (Modem_DATA[7] == 1) {
skyyoungsik 0:c466d770ffa7 329 compass_configuration_start_bool = true;
skyyoungsik 0:c466d770ffa7 330 }
skyyoungsik 0:c466d770ffa7 331 else if (Modem_DATA[7] == 2) {
skyyoungsik 0:c466d770ffa7 332 compass_configuration_end_bool = true;
skyyoungsik 0:c466d770ffa7 333 }
skyyoungsik 0:c466d770ffa7 334 else if(Modem_DATA[7] == 3){
skyyoungsik 0:c466d770ffa7 335 declination_angle = (float)(((int)Modem_DATA[10] * 256 + Modem_DATA[11]) - 18000)/100.0;
skyyoungsik 0:c466d770ffa7 336 eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_UP, (int)((declination_angle+180)*100)/256);
skyyoungsik 0:c466d770ffa7 337 eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_DOWN, (int)((declination_angle+180)*100)%256);
skyyoungsik 0:c466d770ffa7 338 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 339 }
skyyoungsik 0:c466d770ffa7 340 break;
skyyoungsik 0:c466d770ffa7 341 case 7: // Limit Angle //
skyyoungsik 0:c466d770ffa7 342 limit_rollx100 = ((int)Modem_DATA[0] * 256 + Modem_DATA[1]) - 32767;
skyyoungsik 0:c466d770ffa7 343 limit_pitchx100 = ((int)Modem_DATA[2] * 256 + Modem_DATA[3]) - 32767;
skyyoungsik 0:c466d770ffa7 344 limit_roll_rate = ((int)Modem_DATA[4] * 256 + Modem_DATA[5]) - 32767;
skyyoungsik 0:c466d770ffa7 345 limit_pitch_rate = ((int)Modem_DATA[6] * 256 + Modem_DATA[7]) - 32767;
skyyoungsik 0:c466d770ffa7 346 limit_yaw_rate = ((int)Modem_DATA[8] * 256 + Modem_DATA[9]) - 32767;
skyyoungsik 0:c466d770ffa7 347 eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_UP, (limit_rollx100 + 32767) / 256);
skyyoungsik 0:c466d770ffa7 348 eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_DOWN, (limit_rollx100 + 32767) % 256);
skyyoungsik 0:c466d770ffa7 349 eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_UP, (limit_pitchx100 + 32767) / 256);
skyyoungsik 0:c466d770ffa7 350 eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_DOWN, (limit_pitchx100 + 32767) % 256);
skyyoungsik 0:c466d770ffa7 351 eeprom_write(EEPROM_LIMIT_RATE_ROLL_UP, (limit_roll_rate + 32767) / 256);
skyyoungsik 0:c466d770ffa7 352 eeprom_write(EEPROM_LIMIT_RATE_ROLL_DOWN, (limit_roll_rate + 32767) % 256);
skyyoungsik 0:c466d770ffa7 353 eeprom_write(EEPROM_LIMIT_RATE_PITCH_UP, (limit_pitch_rate + 32767) / 256);
skyyoungsik 0:c466d770ffa7 354 eeprom_write(EEPROM_LIMIT_RATE_PITCH_DOWN, (limit_pitch_rate + 32767) % 256);
skyyoungsik 0:c466d770ffa7 355 eeprom_write(EEPROM_LIMIT_RATE_YAW_UP, (limit_yaw_rate + 32767) / 256);
skyyoungsik 0:c466d770ffa7 356 eeprom_write(EEPROM_LIMIT_RATE_YAW_DOWN, (limit_yaw_rate + 32767) % 256);
skyyoungsik 0:c466d770ffa7 357 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 358 break;
skyyoungsik 0:c466d770ffa7 359 case 8:
skyyoungsik 0:c466d770ffa7 360 int gain_number = Modem_DATA[0];
skyyoungsik 0:c466d770ffa7 361 if ((gain_number >= 1) & (gain_number <= 20)) {
skyyoungsik 0:c466d770ffa7 362 gain_px100[gain_number - 1] = (int)Modem_DATA[1] * 256 + Modem_DATA[2];
skyyoungsik 0:c466d770ffa7 363 gain_dx100[gain_number - 1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4];
skyyoungsik 0:c466d770ffa7 364 gain_ix100[gain_number - 1] = (int)Modem_DATA[5] * 256 + Modem_DATA[6];
skyyoungsik 0:c466d770ffa7 365 eeprom_write(EEPROM_GAIN_P_UP[gain_number - 1], gain_px100[gain_number - 1] / 256);
skyyoungsik 0:c466d770ffa7 366 eeprom_write(EEPROM_GAIN_P_DOWN[gain_number - 1], gain_px100[gain_number - 1] % 256);
skyyoungsik 0:c466d770ffa7 367 eeprom_write(EEPROM_GAIN_D_UP[gain_number - 1], gain_dx100[gain_number - 1] / 256);
skyyoungsik 0:c466d770ffa7 368 eeprom_write(EEPROM_GAIN_D_DOWN[gain_number - 1], gain_dx100[gain_number - 1] % 256);
skyyoungsik 0:c466d770ffa7 369 eeprom_write(EEPROM_GAIN_I_UP[gain_number - 1], gain_ix100[gain_number - 1] / 256);
skyyoungsik 0:c466d770ffa7 370 eeprom_write(EEPROM_GAIN_I_DOWN[gain_number - 1], gain_ix100[gain_number - 1] % 256);
skyyoungsik 0:c466d770ffa7 371 if(gain_number == 20)eeprom_refresh();
skyyoungsik 0:c466d770ffa7 372 }
skyyoungsik 0:c466d770ffa7 373 break;
skyyoungsik 0:c466d770ffa7 374 }
skyyoungsik 0:c466d770ffa7 375
skyyoungsik 0:c466d770ffa7 376 }
skyyoungsik 0:c466d770ffa7 377 }
skyyoungsik 0:c466d770ffa7 378
skyyoungsik 0:c466d770ffa7 379 }
skyyoungsik 0:c466d770ffa7 380 void ROBOFRIEN_GUI::trans_empty_data(int id_dest, int data_parse_num) {
skyyoungsik 0:c466d770ffa7 381 uint8_t Packet_SOF[2] = { 254,254 };
skyyoungsik 0:c466d770ffa7 382 uint8_t Packet_Identifier[2];
skyyoungsik 0:c466d770ffa7 383 uint8_t Packet_DC[3];
skyyoungsik 0:c466d770ffa7 384 uint8_t Packet_CHKSUM[2];
skyyoungsik 0:c466d770ffa7 385 uint8_t Packet_EOF[2] = { 255,255 };
skyyoungsik 0:c466d770ffa7 386 // Identifier //
skyyoungsik 0:c466d770ffa7 387 Packet_Identifier[0] = 0;
skyyoungsik 0:c466d770ffa7 388 Packet_Identifier[1] = id_dest;
skyyoungsik 0:c466d770ffa7 389 // Data Control //
skyyoungsik 0:c466d770ffa7 390 Packet_DC[0] = 0;
skyyoungsik 0:c466d770ffa7 391 Packet_DC[1] = (uint8_t)data_parse_num;
skyyoungsik 0:c466d770ffa7 392 int data_length = 0;
skyyoungsik 0:c466d770ffa7 393 Packet_DC[2] = (uint8_t)data_length;
skyyoungsik 0:c466d770ffa7 394
skyyoungsik 0:c466d770ffa7 395 // CHKSUM //
skyyoungsik 0:c466d770ffa7 396 unsigned int data_checksum = 0;
skyyoungsik 0:c466d770ffa7 397 Packet_CHKSUM[0] = data_checksum / 256;
skyyoungsik 0:c466d770ffa7 398 Packet_CHKSUM[1] = data_checksum % 256;
skyyoungsik 0:c466d770ffa7 399
skyyoungsik 0:c466d770ffa7 400 // Each Buffer Integrate to modem_buffer ///
skyyoungsik 0:c466d770ffa7 401 uint8_t modem_buffer[11];
skyyoungsik 0:c466d770ffa7 402 modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
skyyoungsik 0:c466d770ffa7 403 modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
skyyoungsik 0:c466d770ffa7 404 modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
skyyoungsik 0:c466d770ffa7 405 modem_buffer[7] = Packet_CHKSUM[0];
skyyoungsik 0:c466d770ffa7 406 modem_buffer[8] = Packet_CHKSUM[1];
skyyoungsik 0:c466d770ffa7 407 modem_buffer[9] = Packet_EOF[0];
skyyoungsik 0:c466d770ffa7 408 modem_buffer[10] = Packet_EOF[1];
skyyoungsik 0:c466d770ffa7 409 for (int i = 0; i <= 10; i++) {
skyyoungsik 0:c466d770ffa7 410 pc.putc(modem_buffer[i]);
skyyoungsik 0:c466d770ffa7 411 }
skyyoungsik 0:c466d770ffa7 412 }
skyyoungsik 0:c466d770ffa7 413
skyyoungsik 0:c466d770ffa7 414 void ROBOFRIEN_GUI::trans_configuration_data(int id_dest, int data_parse_num, int data_parse_detail_num) {
skyyoungsik 0:c466d770ffa7 415
skyyoungsik 0:c466d770ffa7 416 uint8_t Packet_SOF[2] = { 254,254 };
skyyoungsik 0:c466d770ffa7 417 uint8_t Packet_Identifier[2];
skyyoungsik 0:c466d770ffa7 418 uint8_t Packet_DC[3];
skyyoungsik 0:c466d770ffa7 419 uint8_t Packet_DATA[16];
skyyoungsik 0:c466d770ffa7 420 uint8_t Packet_CHKSUM[2];
skyyoungsik 0:c466d770ffa7 421 uint8_t Packet_EOF[2] = { 255,255 };
skyyoungsik 0:c466d770ffa7 422 // Identifier //
skyyoungsik 0:c466d770ffa7 423 Packet_Identifier[0] = 0;
skyyoungsik 0:c466d770ffa7 424 Packet_Identifier[1] = id_dest;
skyyoungsik 0:c466d770ffa7 425 // Data Control //
skyyoungsik 0:c466d770ffa7 426 Packet_DC[0] = 0;
skyyoungsik 0:c466d770ffa7 427 Packet_DC[1] = (uint8_t)data_parse_num;
skyyoungsik 0:c466d770ffa7 428 int data_length = 16;
skyyoungsik 0:c466d770ffa7 429 Packet_DC[2] = (uint8_t)data_length;
skyyoungsik 0:c466d770ffa7 430 // DATA //
skyyoungsik 0:c466d770ffa7 431 switch (data_parse_num) {
skyyoungsik 0:c466d770ffa7 432 case 0:
skyyoungsik 0:c466d770ffa7 433 //// Empty Data ////
skyyoungsik 0:c466d770ffa7 434 for (int i = 0; i <= 15; i++) {
skyyoungsik 0:c466d770ffa7 435 Packet_DATA[i] = 0;
skyyoungsik 0:c466d770ffa7 436 }
skyyoungsik 0:c466d770ffa7 437 break;
skyyoungsik 0:c466d770ffa7 438 case 1:
skyyoungsik 0:c466d770ffa7 439 //// MODEM ////
skyyoungsik 0:c466d770ffa7 440 Packet_DATA[0] = model_type1;
skyyoungsik 0:c466d770ffa7 441 Packet_DATA[1] = model_type2 / 256; Packet_DATA[2] = model_type2 % 256;
skyyoungsik 0:c466d770ffa7 442 for (int i = 3; i <= 15; i++) Packet_DATA[i] = 0;
skyyoungsik 0:c466d770ffa7 443 break;
skyyoungsik 0:c466d770ffa7 444 case 2:
skyyoungsik 0:c466d770ffa7 445 //// Firmware ////
skyyoungsik 0:c466d770ffa7 446 for (int i = 0; i <= 13; i++) {
skyyoungsik 0:c466d770ffa7 447 Packet_DATA[i] = MODEL_INFO[i];
skyyoungsik 0:c466d770ffa7 448 }
skyyoungsik 0:c466d770ffa7 449 float firmware_info;
skyyoungsik 0:c466d770ffa7 450 firmware_info = FIRMWARE_INFO;
skyyoungsik 0:c466d770ffa7 451 Packet_DATA[14] = (uint8_t)(firmware_info * 100 / 256); Packet_DATA[15] = (uint8_t)((uint32_t)(firmware_info * 100) % 256);
skyyoungsik 0:c466d770ffa7 452 break;
skyyoungsik 0:c466d770ffa7 453 case 3:
skyyoungsik 0:c466d770ffa7 454 //// RC Receiver ////
skyyoungsik 0:c466d770ffa7 455 if (data_parse_detail_num == 0) {
skyyoungsik 0:c466d770ffa7 456 Packet_DATA[0] = data_parse_detail_num; Packet_DATA[1] = 0; Packet_DATA[2] = 0; Packet_DATA[3] = 0; Packet_DATA[4] = 0; Packet_DATA[5] = 0; Packet_DATA[6] = 0; Packet_DATA[7] = 0;
skyyoungsik 0:c466d770ffa7 457 }
skyyoungsik 0:c466d770ffa7 458 else if(data_parse_detail_num <= 8){
skyyoungsik 0:c466d770ffa7 459 Packet_DATA[0] = data_parse_detail_num; Packet_DATA[1] = cap_min[data_parse_detail_num - 1]; Packet_DATA[2] = cap_neu[data_parse_detail_num - 1]; Packet_DATA[3] = cap_max[data_parse_detail_num - 1]; Packet_DATA[4] = 0;
skyyoungsik 0:c466d770ffa7 460 Packet_DATA[5] = cap_min[data_parse_detail_num - 1] + 127; Packet_DATA[6] = cap_neu[data_parse_detail_num - 1] + 127; Packet_DATA[7] = cap_max[data_parse_detail_num - 1] + 127;
skyyoungsik 0:c466d770ffa7 461 }
skyyoungsik 0:c466d770ffa7 462 else {
skyyoungsik 0:c466d770ffa7 463 for (int i = 0; i <= 7; i++) Packet_DATA[i] = 0;
skyyoungsik 0:c466d770ffa7 464 }
skyyoungsik 0:c466d770ffa7 465 Packet_DATA[8] = raw_cap[0] + 127; Packet_DATA[9] = raw_cap[1] + 127; Packet_DATA[10] = raw_cap[2] + 127; Packet_DATA[11] = raw_cap[3] + 127; Packet_DATA[12] = raw_cap[4] + 127; Packet_DATA[13] = raw_cap[5] + 127; Packet_DATA[14] = raw_cap[6] + 127; Packet_DATA[15] = raw_cap[7] + 127;
skyyoungsik 0:c466d770ffa7 466 break;
skyyoungsik 0:c466d770ffa7 467 case 4:
skyyoungsik 0:c466d770ffa7 468 for(int i=0; i<16; i++) Packet_DATA[i] = 0;
skyyoungsik 0:c466d770ffa7 469 break;
skyyoungsik 0:c466d770ffa7 470 case 5:
skyyoungsik 0:c466d770ffa7 471 /////// LED //////////
skyyoungsik 0:c466d770ffa7 472 Packet_DATA[0] = headlight_period; Packet_DATA[1] = headlight_dutyrate; Packet_DATA[2] = sidelight_period; Packet_DATA[3] = sidelight_dutyrate;
skyyoungsik 0:c466d770ffa7 473 break;
skyyoungsik 0:c466d770ffa7 474 case 6:
skyyoungsik 0:c466d770ffa7 475 //// AHRS - Roll , Pitch ///
skyyoungsik 0:c466d770ffa7 476 Packet_DATA[0] = 0;
skyyoungsik 0:c466d770ffa7 477 Packet_DATA[1] = (uint8_t)((unsigned long)(((float)rollx100 / 100.0 + 180) * 100) / 256);
skyyoungsik 0:c466d770ffa7 478 Packet_DATA[2] = (uint8_t)((unsigned long)(((float)rollx100 / 100.0 + 180) * 100) % 256);
skyyoungsik 0:c466d770ffa7 479 Packet_DATA[3] = (uint8_t)((unsigned long)(((float)pitchx100 / 100.0 + 90) * 100) / 256);
skyyoungsik 0:c466d770ffa7 480 Packet_DATA[4] = (uint8_t)((unsigned long)(((float)pitchx100 / 100.0 + 90) * 100) % 256);
skyyoungsik 0:c466d770ffa7 481 //// AHRS - Yaw ///
skyyoungsik 0:c466d770ffa7 482 Packet_DATA[5] = 0; Packet_DATA[6] = 0;
skyyoungsik 0:c466d770ffa7 483 Packet_DATA[7] = (uint8_t)fcs_compass_configuration_state;
skyyoungsik 0:c466d770ffa7 484 Packet_DATA[8] = (uint8_t)((unsigned long)(yawx100) / 256);
skyyoungsik 0:c466d770ffa7 485 Packet_DATA[9] = (uint8_t)((unsigned long)(yawx100) % 256);
skyyoungsik 0:c466d770ffa7 486 for (int i = 10; i <= 15; i++)Packet_DATA[i] = 0;
skyyoungsik 0:c466d770ffa7 487 break;
skyyoungsik 0:c466d770ffa7 488 case 7:
skyyoungsik 0:c466d770ffa7 489 /// Limit ANGLE ////
skyyoungsik 0:c466d770ffa7 490 Packet_DATA[0] = (limit_rollx100 + 32767) / 256;
skyyoungsik 0:c466d770ffa7 491 Packet_DATA[1] = (limit_rollx100 + 32767) % 256;
skyyoungsik 0:c466d770ffa7 492 Packet_DATA[2] = (limit_pitchx100 + 32767) / 256;
skyyoungsik 0:c466d770ffa7 493 Packet_DATA[3] = (limit_pitchx100 + 32767) % 256;
skyyoungsik 0:c466d770ffa7 494 Packet_DATA[4] = (limit_roll_rate + 32767) / 256;
skyyoungsik 0:c466d770ffa7 495 Packet_DATA[5] = (limit_roll_rate + 32767) % 256;
skyyoungsik 0:c466d770ffa7 496 Packet_DATA[6] = (limit_pitch_rate + 32767) / 256;
skyyoungsik 0:c466d770ffa7 497 Packet_DATA[7] = (limit_pitch_rate + 32767) % 256;
skyyoungsik 0:c466d770ffa7 498 Packet_DATA[8] = (limit_yaw_rate + 32767) / 256;
skyyoungsik 0:c466d770ffa7 499 Packet_DATA[9] = (limit_yaw_rate + 32767) % 256;
skyyoungsik 0:c466d770ffa7 500 for (int i = 10; i <= 15; i++) Packet_DATA[i] = 0;
skyyoungsik 0:c466d770ffa7 501 break;
skyyoungsik 0:c466d770ffa7 502 case 8:
skyyoungsik 0:c466d770ffa7 503 Packet_DATA[0] = data_parse_detail_num;
skyyoungsik 0:c466d770ffa7 504 if ((data_parse_detail_num > 0)&(data_parse_detail_num <= 20)) {
skyyoungsik 0:c466d770ffa7 505 Packet_DATA[1] = gain_px100[data_parse_detail_num - 1] / 256;
skyyoungsik 0:c466d770ffa7 506 Packet_DATA[2] = gain_px100[data_parse_detail_num - 1] % 256;
skyyoungsik 0:c466d770ffa7 507 Packet_DATA[3] = gain_dx100[data_parse_detail_num - 1] / 256;
skyyoungsik 0:c466d770ffa7 508 Packet_DATA[4] = gain_dx100[data_parse_detail_num - 1] % 256;
skyyoungsik 0:c466d770ffa7 509 Packet_DATA[5] = gain_ix100[data_parse_detail_num - 1] / 256;
skyyoungsik 0:c466d770ffa7 510 Packet_DATA[6] = gain_ix100[data_parse_detail_num - 1] % 256;
skyyoungsik 0:c466d770ffa7 511 for (int i = 7; i <= 15; i++) Packet_DATA[i] = 0;
skyyoungsik 0:c466d770ffa7 512 }
skyyoungsik 0:c466d770ffa7 513 else {
skyyoungsik 0:c466d770ffa7 514 for (int i = 1; i <= 15; i++) Packet_DATA[i] = 0;
skyyoungsik 0:c466d770ffa7 515 }
skyyoungsik 0:c466d770ffa7 516 break;
skyyoungsik 0:c466d770ffa7 517 }
skyyoungsik 0:c466d770ffa7 518
skyyoungsik 0:c466d770ffa7 519 // CHKSUM //
skyyoungsik 0:c466d770ffa7 520 unsigned int data_checksum = 0;
skyyoungsik 0:c466d770ffa7 521 for (int i = 0; i < data_length; i++) {
skyyoungsik 0:c466d770ffa7 522 data_checksum += Packet_DATA[i];
skyyoungsik 0:c466d770ffa7 523 }
skyyoungsik 0:c466d770ffa7 524 Packet_CHKSUM[0] = data_checksum / 256;
skyyoungsik 0:c466d770ffa7 525 Packet_CHKSUM[1] = data_checksum % 256;
skyyoungsik 0:c466d770ffa7 526
skyyoungsik 0:c466d770ffa7 527 // Each Buffer Integrate to modem_buffer ///
skyyoungsik 0:c466d770ffa7 528 uint8_t modem_buffer[27];
skyyoungsik 0:c466d770ffa7 529 modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
skyyoungsik 0:c466d770ffa7 530 modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
skyyoungsik 0:c466d770ffa7 531 modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
skyyoungsik 0:c466d770ffa7 532 for (int i = 0; i < data_length; i++) {
skyyoungsik 0:c466d770ffa7 533 modem_buffer[i + 7] = Packet_DATA[i];
skyyoungsik 0:c466d770ffa7 534 }
skyyoungsik 0:c466d770ffa7 535 modem_buffer[23] = Packet_CHKSUM[0];
skyyoungsik 0:c466d770ffa7 536 modem_buffer[24] = Packet_CHKSUM[1];
skyyoungsik 0:c466d770ffa7 537 modem_buffer[25] = Packet_EOF[0];
skyyoungsik 0:c466d770ffa7 538 modem_buffer[26] = Packet_EOF[1];
skyyoungsik 0:c466d770ffa7 539 for (int i = 0; i <= 26; i++) {
skyyoungsik 0:c466d770ffa7 540 pc.putc(modem_buffer[i]);
skyyoungsik 0:c466d770ffa7 541 }
skyyoungsik 0:c466d770ffa7 542 }
skyyoungsik 0:c466d770ffa7 543
skyyoungsik 0:c466d770ffa7 544 void ROBOFRIEN_GUI::trans_flight_data(int id_dest, int data_parse_num) {
skyyoungsik 0:c466d770ffa7 545
skyyoungsik 0:c466d770ffa7 546 uint8_t Packet_SOF[2] = { 254,254 };
skyyoungsik 0:c466d770ffa7 547 uint8_t Packet_Identifier[2];
skyyoungsik 0:c466d770ffa7 548 uint8_t Packet_DC[3];
skyyoungsik 0:c466d770ffa7 549 uint8_t Packet_DATA[73];
skyyoungsik 0:c466d770ffa7 550 uint8_t Packet_CHKSUM[2];
skyyoungsik 0:c466d770ffa7 551 uint8_t Packet_EOF[2] = { 255,255 };
skyyoungsik 0:c466d770ffa7 552 // Identifier //
skyyoungsik 0:c466d770ffa7 553 Packet_Identifier[0] = 0;
skyyoungsik 0:c466d770ffa7 554 Packet_Identifier[1] = id_dest;
skyyoungsik 0:c466d770ffa7 555 // Data Control //
skyyoungsik 0:c466d770ffa7 556 Packet_DC[0] = 0;
skyyoungsik 0:c466d770ffa7 557 Packet_DC[1] = (uint8_t)data_parse_num;
skyyoungsik 0:c466d770ffa7 558 int data_length;
skyyoungsik 0:c466d770ffa7 559 if (data_parse_num == 0) data_length = 73;
skyyoungsik 0:c466d770ffa7 560 else data_length = 16;
skyyoungsik 0:c466d770ffa7 561 Packet_DC[2] = (uint8_t)data_length;
skyyoungsik 0:c466d770ffa7 562 // DATA - Modem & State //
skyyoungsik 0:c466d770ffa7 563 if(Mode1 >= 15) Mode1 = 15;
skyyoungsik 0:c466d770ffa7 564 else if(Mode1 <= 0) Mode1 = 0;
skyyoungsik 0:c466d770ffa7 565 if(Mode2 >= 15) Mode2 = 15;
skyyoungsik 0:c466d770ffa7 566 else if(Mode2 <= 0) Mode2 = 0;
skyyoungsik 0:c466d770ffa7 567 if(MissionState >= 15) MissionState = 15;
skyyoungsik 0:c466d770ffa7 568 else if(MissionState <= 0) MissionState = 0;
skyyoungsik 0:c466d770ffa7 569 if(CurrentMarker >= 20) CurrentMarker = 20;
skyyoungsik 0:c466d770ffa7 570 else if(CurrentMarker <= 0) CurrentMarker = 0;
skyyoungsik 0:c466d770ffa7 571 HomePointChksum = ((Homepoint_Lat + 90000000)/16777216) + ((Homepoint_Lat + 90000000)%16777216/65536) + ((Homepoint_Lat + 90000000)%65536/256) + ((Homepoint_Lat + 90000000)%256);
skyyoungsik 0:c466d770ffa7 572 HomePointChksum += ((Homepoint_Lng + 180000000)/16777216) + ((Homepoint_Lng + 180000000)%16777216/65536) + ((Homepoint_Lng + 180000000)%65536/256) + ((Homepoint_Lng + 180000000)%256);
skyyoungsik 0:c466d770ffa7 573 HomePointChksum += ((Homepoint_Alt + 10000)/256 + (Homepoint_Alt + 10000)%256);
skyyoungsik 0:c466d770ffa7 574 MarkerChksum = 0;
skyyoungsik 0:c466d770ffa7 575 for(int i=0; i<20; i++){
skyyoungsik 0:c466d770ffa7 576 MarkerChksum += Marker_Mode[i];
skyyoungsik 0:c466d770ffa7 577 MarkerChksum += (Marker_Lat[i]+90000000)/16777216 + (Marker_Lat[i]+90000000)%16777216/65536 + (Marker_Lat[i]+90000000)%65536/256 + (Marker_Lat[i]+90000000)%256;
skyyoungsik 0:c466d770ffa7 578 MarkerChksum += (Marker_Lng[i]+180000000)/16777216 + (Marker_Lng[i]+180000000)%16777216/65536 + (Marker_Lng[i]+180000000)%65536/256 + (Marker_Lng[i]+180000000)%256;
skyyoungsik 0:c466d770ffa7 579 MarkerChksum += (Marker_Alt[i]+10000)/256 + (Marker_Alt[i]+10000)%256;
skyyoungsik 0:c466d770ffa7 580 MarkerChksum += Marker_Speed[i]/256 + Marker_Speed[i]%256;
skyyoungsik 0:c466d770ffa7 581 }
skyyoungsik 0:c466d770ffa7 582 Packet_DATA[0] = Mode1 * 16 + Mode2;
skyyoungsik 0:c466d770ffa7 583 Packet_DATA[1] = MissionState;
skyyoungsik 0:c466d770ffa7 584 Packet_DATA[2] = CurrentMarker;
skyyoungsik 0:c466d770ffa7 585 Packet_DATA[3] = Bat1;
skyyoungsik 0:c466d770ffa7 586 Packet_DATA[4] = Bat2;
skyyoungsik 0:c466d770ffa7 587 Packet_DATA[5] = button[0] * 1 + button[1] * 2 + button[2] * 4 + button[3] * 8 + button[4] * 16;
skyyoungsik 0:c466d770ffa7 588 Packet_DATA[6] = HomePointChksum%256;
skyyoungsik 0:c466d770ffa7 589 Packet_DATA[7] = MarkerChksum % 256;
skyyoungsik 0:c466d770ffa7 590 // DATA - GPS //
skyyoungsik 0:c466d770ffa7 591 unsigned long send_gps_time = utc_time;
skyyoungsik 0:c466d770ffa7 592 Packet_DATA[8] = send_gps_time / 16777216;
skyyoungsik 0:c466d770ffa7 593 Packet_DATA[9] = send_gps_time % 16777216 / 65536;
skyyoungsik 0:c466d770ffa7 594 Packet_DATA[10] = send_gps_time % 65536 / 256;
skyyoungsik 0:c466d770ffa7 595 Packet_DATA[11] = send_gps_time % 256;
skyyoungsik 0:c466d770ffa7 596 signed long send_gps_latitude = latitude + 90000000;
skyyoungsik 0:c466d770ffa7 597 Packet_DATA[12] = send_gps_latitude / 16777216;
skyyoungsik 0:c466d770ffa7 598 Packet_DATA[13] = send_gps_latitude % 16777216 / 65536;
skyyoungsik 0:c466d770ffa7 599 Packet_DATA[14] = send_gps_latitude % 65536 / 256;
skyyoungsik 0:c466d770ffa7 600 Packet_DATA[15] = send_gps_latitude % 256;
skyyoungsik 0:c466d770ffa7 601 signed long send_gps_longitude = longitude + 180000000;
skyyoungsik 0:c466d770ffa7 602 Packet_DATA[16] = send_gps_longitude / 16777216;
skyyoungsik 0:c466d770ffa7 603 Packet_DATA[17] = send_gps_longitude % 16777216 / 65536;
skyyoungsik 0:c466d770ffa7 604 Packet_DATA[18] = send_gps_longitude % 65536 / 256;
skyyoungsik 0:c466d770ffa7 605 Packet_DATA[19] = send_gps_longitude % 256;
skyyoungsik 0:c466d770ffa7 606 signed long send_altitude;
skyyoungsik 0:c466d770ffa7 607 if ((altitude + 10000) < 0) send_altitude = -10000;
skyyoungsik 0:c466d770ffa7 608 else send_altitude = (altitude + 10000);
skyyoungsik 0:c466d770ffa7 609 Packet_DATA[20] = send_altitude / 256;
skyyoungsik 0:c466d770ffa7 610 Packet_DATA[21] = send_altitude % 256;
skyyoungsik 0:c466d770ffa7 611 Packet_DATA[22] = SatNum;
skyyoungsik 0:c466d770ffa7 612 // AHRS //
skyyoungsik 0:c466d770ffa7 613 unsigned long send_roll = rollx100 + 32767;
skyyoungsik 0:c466d770ffa7 614 Packet_DATA[23] = send_roll / 256;
skyyoungsik 0:c466d770ffa7 615 Packet_DATA[24] = send_roll % 256;
skyyoungsik 0:c466d770ffa7 616 unsigned long send_pitch = pitchx100 + 32767;
skyyoungsik 0:c466d770ffa7 617 Packet_DATA[25] = send_pitch / 256;
skyyoungsik 0:c466d770ffa7 618 Packet_DATA[26] = send_pitch % 256;
skyyoungsik 0:c466d770ffa7 619 unsigned long send_yaw = yawx100;
skyyoungsik 0:c466d770ffa7 620 Packet_DATA[27] = send_yaw / 256;
skyyoungsik 0:c466d770ffa7 621 Packet_DATA[28] = send_yaw % 256;
skyyoungsik 0:c466d770ffa7 622 unsigned long send_roll_rate = (roll_ratex100 +32767);
skyyoungsik 0:c466d770ffa7 623 Packet_DATA[29] = send_roll_rate / 256;
skyyoungsik 0:c466d770ffa7 624 Packet_DATA[30] = send_roll_rate % 256;
skyyoungsik 0:c466d770ffa7 625 unsigned long send_pitch_rate = (pitch_ratex100 + 32767);
skyyoungsik 0:c466d770ffa7 626 Packet_DATA[31] = send_pitch_rate / 256;
skyyoungsik 0:c466d770ffa7 627 Packet_DATA[32] = send_pitch_rate % 256;
skyyoungsik 0:c466d770ffa7 628 unsigned long send_yaw_rate = (yaw_ratex100 + 32767);
skyyoungsik 0:c466d770ffa7 629 Packet_DATA[33] = send_yaw_rate / 256;
skyyoungsik 0:c466d770ffa7 630 Packet_DATA[34] = send_yaw_rate % 256;
skyyoungsik 0:c466d770ffa7 631 unsigned long send_velocity_x = (VXx100 + 32767);
skyyoungsik 0:c466d770ffa7 632 Packet_DATA[35] = send_velocity_x / 256;
skyyoungsik 0:c466d770ffa7 633 Packet_DATA[36] = send_velocity_x % 256;
skyyoungsik 0:c466d770ffa7 634 unsigned long send_velocity_y = (VYx100 + 32767);
skyyoungsik 0:c466d770ffa7 635 Packet_DATA[37] = send_velocity_y / 256;
skyyoungsik 0:c466d770ffa7 636 Packet_DATA[38] = send_velocity_y % 256;
skyyoungsik 0:c466d770ffa7 637 unsigned long send_velocity_z = (VZx100 + 32767);
skyyoungsik 0:c466d770ffa7 638 Packet_DATA[39] = send_velocity_z / 256;
skyyoungsik 0:c466d770ffa7 639 Packet_DATA[40] = send_velocity_z % 256;
skyyoungsik 0:c466d770ffa7 640 // CAP and PWM //
skyyoungsik 0:c466d770ffa7 641 Packet_DATA[41] = (cap[0] + 127);
skyyoungsik 0:c466d770ffa7 642 Packet_DATA[42] = (cap[1] + 127);
skyyoungsik 0:c466d770ffa7 643 Packet_DATA[43] = (cap[2] + 127);
skyyoungsik 0:c466d770ffa7 644 Packet_DATA[44] = (cap[3] + 127);
skyyoungsik 0:c466d770ffa7 645 Packet_DATA[45] = (cap[4] + 127);
skyyoungsik 0:c466d770ffa7 646 Packet_DATA[46] = (cap[5] + 127);
skyyoungsik 0:c466d770ffa7 647 Packet_DATA[47] = (cap[6] + 127);
skyyoungsik 0:c466d770ffa7 648 Packet_DATA[48] = (cap[7] + 127);
skyyoungsik 0:c466d770ffa7 649
skyyoungsik 0:c466d770ffa7 650 // PWM //
skyyoungsik 0:c466d770ffa7 651 for (int i = 49; i <= 56; i++) {
skyyoungsik 0:c466d770ffa7 652 Packet_DATA[i] = pwm[i-49];
skyyoungsik 0:c466d770ffa7 653 }
skyyoungsik 0:c466d770ffa7 654 // DEBUG //
skyyoungsik 0:c466d770ffa7 655 Packet_DATA[57] = (int)((DEBUGx100[0]))/256;
skyyoungsik 0:c466d770ffa7 656 Packet_DATA[58] = (int)((DEBUGx100[0]))%256;
skyyoungsik 0:c466d770ffa7 657 Packet_DATA[59] = (int)((DEBUGx100[1]))/256;
skyyoungsik 0:c466d770ffa7 658 Packet_DATA[60] = (int)((DEBUGx100[1]))%256;
skyyoungsik 0:c466d770ffa7 659 Packet_DATA[61] = (int)((DEBUGx100[2]))/256;
skyyoungsik 0:c466d770ffa7 660 Packet_DATA[62] = (int)((DEBUGx100[2]))%256;
skyyoungsik 0:c466d770ffa7 661 Packet_DATA[63] = (int)((DEBUGx100[3]))/256;
skyyoungsik 0:c466d770ffa7 662 Packet_DATA[64] = (int)((DEBUGx100[3]))%256;
skyyoungsik 0:c466d770ffa7 663 Packet_DATA[65] = (int)((DEBUGx100[4]))/256;
skyyoungsik 0:c466d770ffa7 664 Packet_DATA[66] = (int)((DEBUGx100[4]))%256;
skyyoungsik 0:c466d770ffa7 665 Packet_DATA[67] = (int)((DEBUGx100[5]))/256;
skyyoungsik 0:c466d770ffa7 666 Packet_DATA[68] = (int)((DEBUGx100[5]))%256;
skyyoungsik 0:c466d770ffa7 667 Packet_DATA[69] = (int)((DEBUGx100[6]))/256;
skyyoungsik 0:c466d770ffa7 668 Packet_DATA[70] = (int)((DEBUGx100[6]))%256;
skyyoungsik 0:c466d770ffa7 669 Packet_DATA[71] = (int)((DEBUGx100[7]))/256;
skyyoungsik 0:c466d770ffa7 670 Packet_DATA[72] = (int)((DEBUGx100[7]))%256;
skyyoungsik 0:c466d770ffa7 671 // CHKSUM //
skyyoungsik 0:c466d770ffa7 672 unsigned int data_checksum = 0;
skyyoungsik 0:c466d770ffa7 673 for (int i = 0; i < data_length; i++) {
skyyoungsik 0:c466d770ffa7 674 data_checksum += Packet_DATA[i];
skyyoungsik 0:c466d770ffa7 675 }
skyyoungsik 0:c466d770ffa7 676 Packet_CHKSUM[0] = data_checksum / 256;
skyyoungsik 0:c466d770ffa7 677 Packet_CHKSUM[1] = data_checksum % 256;
skyyoungsik 0:c466d770ffa7 678
skyyoungsik 0:c466d770ffa7 679 // Each Buffer Integrate to modem_buffer ///
skyyoungsik 0:c466d770ffa7 680 uint8_t modem_buffer[84];
skyyoungsik 0:c466d770ffa7 681 modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
skyyoungsik 0:c466d770ffa7 682 modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
skyyoungsik 0:c466d770ffa7 683 modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
skyyoungsik 0:c466d770ffa7 684 for (int i = 0; i < 73; i++) {
skyyoungsik 0:c466d770ffa7 685 modem_buffer[i + 7] = Packet_DATA[i];
skyyoungsik 0:c466d770ffa7 686 }
skyyoungsik 0:c466d770ffa7 687 modem_buffer[80] = Packet_CHKSUM[0];
skyyoungsik 0:c466d770ffa7 688 modem_buffer[81] = Packet_CHKSUM[1];
skyyoungsik 0:c466d770ffa7 689 modem_buffer[82] = Packet_EOF[0];
skyyoungsik 0:c466d770ffa7 690 modem_buffer[83] = Packet_EOF[1];
skyyoungsik 0:c466d770ffa7 691 for (int i = 0; i <= 83; i++) {
skyyoungsik 0:c466d770ffa7 692 pc.putc(modem_buffer[i]);
skyyoungsik 0:c466d770ffa7 693 }
skyyoungsik 0:c466d770ffa7 694 }
skyyoungsik 0:c466d770ffa7 695
skyyoungsik 0:c466d770ffa7 696 void ROBOFRIEN_GUI::attitude_calibrate(float rollAngle, float pitchAngle){
skyyoungsik 0:c466d770ffa7 697 calibrate_gap_roll += rollAngle;
skyyoungsik 0:c466d770ffa7 698 calibrate_gap_pitch += pitchAngle;
skyyoungsik 0:c466d770ffa7 699 eeprom_write(EEPROM_AHRS_ROLL_GAP_UP, (((unsigned long)(calibrate_gap_roll * 100 + 32767) / 256)));
skyyoungsik 0:c466d770ffa7 700 eeprom_write(EEPROM_AHRS_ROLL_GAP_DOWN, (((unsigned long)(calibrate_gap_roll * 100 + 32767) % 256)));
skyyoungsik 0:c466d770ffa7 701 eeprom_write(EEPROM_AHRS_PITCH_GAP_UP, (((unsigned long)(calibrate_gap_pitch * 100 + 32767) / 256)));
skyyoungsik 0:c466d770ffa7 702 eeprom_write(EEPROM_AHRS_PITCH_GAP_DOWN, (((unsigned long)(calibrate_gap_pitch * 100 + 32767) % 256)));
skyyoungsik 0:c466d770ffa7 703 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 704 }
skyyoungsik 0:c466d770ffa7 705
skyyoungsik 0:c466d770ffa7 706 void ROBOFRIEN_GUI::write_compass_setting_to_eeprom(){
skyyoungsik 0:c466d770ffa7 707 eeprom_write(EEPROM_AHRS_YAW_X_GAP_1, ( (uint32_t)(mag_x_avg + 32767)/256 ) );
skyyoungsik 0:c466d770ffa7 708 eeprom_write(EEPROM_AHRS_YAW_X_GAP_2, ( (uint32_t)(mag_x_avg + 32767)%256 ) );
skyyoungsik 0:c466d770ffa7 709 // eeprom_write(EEPROM_AHRS_YAW_X_GAP_FLOAT, ( (unsigned long)(mag_x_avg * 100)%100 ) );
skyyoungsik 0:c466d770ffa7 710 // eeprom_write(EEPROM_AHRS_YAW_X_GAP_3, ( (uint32_t)(mag_x_avg * 100 + 2147483648)%65536/256 ) );
skyyoungsik 0:c466d770ffa7 711 // eeprom_write(EEPROM_AHRS_YAW_X_GAP_4, ( (uint32_t)(mag_x_avg * 100 + 2147483648)%256 ) );
skyyoungsik 0:c466d770ffa7 712
skyyoungsik 0:c466d770ffa7 713 eeprom_write(EEPROM_AHRS_YAW_Y_GAP_1, ( (uint32_t)(mag_y_avg + 32767)/256 ) );
skyyoungsik 0:c466d770ffa7 714 eeprom_write(EEPROM_AHRS_YAW_Y_GAP_2, ( (uint32_t)(mag_y_avg + 32767)%256 ) );
skyyoungsik 0:c466d770ffa7 715 // eeprom_write(EEPROM_AHRS_YAW_Y_GAP_FLOAT, ( (unsigned long)(mag_y_avg * 100)%100 ) );
skyyoungsik 0:c466d770ffa7 716 // eeprom_write(EEPROM_AHRS_YAW_Y_GAP_3, ( (uint32_t)(mag_y_avg * 100 + 2147483648)%65536/256 ) );
skyyoungsik 0:c466d770ffa7 717 // eeprom_write(EEPROM_AHRS_YAW_Y_GAP_4, ( (uint32_t)(mag_y_avg * 100 + 2147483648)%256 ) );
skyyoungsik 0:c466d770ffa7 718
skyyoungsik 0:c466d770ffa7 719 eeprom_write(EEPROM_AHRS_YAW_Z_GAP_1, ( (uint32_t)(mag_z_avg + 32767)/256 ) );
skyyoungsik 0:c466d770ffa7 720 eeprom_write(EEPROM_AHRS_YAW_Z_GAP_2, ( (uint32_t)(mag_z_avg + 32767)%256 ) );
skyyoungsik 0:c466d770ffa7 721
skyyoungsik 0:c466d770ffa7 722 eeprom_refresh();
skyyoungsik 0:c466d770ffa7 723 }