Kim Youngsik
/
0ROBOFRIEN_FCC_v1_12
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 BufferedSerial ConfigFile
Embed:
(wiki syntax)
Show/hide line numbers
ROBOFRIEN_GUI.cpp
00001 #include "ROBOFRIEN_GUI.h" 00002 #include "GUI_Config.h" 00003 #include "eeprom.h" 00004 #include "BufferedSerial.h" 00005 00006 #define PC_DEBUG 0 00007 00008 #if PC_DEBUG 00009 Serial pc(USBTX,USBRX); 00010 #else 00011 BufferedSerial pc(p9, p10); // tx, rx 00012 #endif 00013 00014 00015 uint8_t ISR_Modem_ID_Dept, ISR_Modem_ID_Dest; 00016 uint8_t ISR_Modem_DC_DPN, ISR_Modem_DC_DL; 00017 uint8_t ISR_Modem_DATA[256]; 00018 uint8_t ISR_Modem_DC_REQUEST; 00019 uint16_t ISR_Modem_CHKSUM; 00020 uint8_t pc_isr_cnt; 00021 uint8_t isr_modem_data_cnt = 0; 00022 uint8_t pc_parsing_bool = false; 00023 bool pc_Start_Check; 00024 uint8_t model_type1; 00025 uint16_t model_type2; 00026 uint8_t pc_buffer[256],ex_pc_raw_buffer,pc_raw_buffer[256]; 00027 uint8_t pc_buffer_cnt=0; 00028 00029 00030 void ROBOFRIEN_GUI::pc_rx_update(){ 00031 while (pc.readable()){ 00032 pc_buffer[1] = pc_buffer[0]; 00033 pc_buffer[0] = pc.getc(); 00034 // Check SOF // 00035 if (( pc_buffer[1] == 254) & (pc_buffer[0] == 254)){ 00036 pc_Start_Check = true; 00037 pc_isr_cnt = 0; 00038 } 00039 else if(pc_Start_Check == true){ 00040 if(pc_isr_cnt <= 4){ 00041 switch(pc_isr_cnt){ 00042 case 0: ISR_Modem_ID_Dept = pc_buffer[0]; break; 00043 case 1: ISR_Modem_ID_Dest = pc_buffer[0]; break; 00044 case 2: ISR_Modem_DC_REQUEST = pc_buffer[0]; break; 00045 case 3: ISR_Modem_DC_DPN = pc_buffer[0]; break; 00046 case 4: ISR_Modem_DC_DL = pc_buffer[0]; isr_modem_data_cnt = 0; break; 00047 } 00048 } 00049 else if( pc_isr_cnt > 4 ){ 00050 if ((ISR_Modem_DC_DL >= 1) & (pc_isr_cnt <= (4 + ISR_Modem_DC_DL))) { 00051 ISR_Modem_DATA[isr_modem_data_cnt] = pc_buffer[0]; 00052 isr_modem_data_cnt++; 00053 } 00054 else if(pc_isr_cnt == (4 + ISR_Modem_DC_DL + 2) ){ 00055 ISR_Modem_CHKSUM = ((uint16_t)pc_buffer[1] * 256 + pc_buffer[0]); 00056 } 00057 else if(pc_isr_cnt == (4 + ISR_Modem_DC_DL + 2 + 2) ){ 00058 if((pc_buffer[1] == 255) & (pc_buffer[0] == 255) ){ 00059 pc_Start_Check = false; 00060 pc_parsing_bool = true; 00061 } 00062 } 00063 } 00064 pc_isr_cnt ++; 00065 } 00066 } 00067 } 00068 00069 uint8_t Modem_ID_Dest; 00070 uint8_t Modem_ID_Dept; 00071 uint8_t Modem_DC_DPN; 00072 uint8_t Modem_DC_DL; 00073 uint8_t Modem_DATA[256]; 00074 uint8_t Modem_DC_REQUEST; 00075 00076 bool ROBOFRIEN_GUI::rx_bool(){ 00077 if(pc_parsing_bool == true){ 00078 pc_parsing_bool = false; 00079 int cal_chksum = 0; 00080 for(int i=0; i< ISR_Modem_DC_DL; i++){ 00081 Modem_DATA[i] = ISR_Modem_DATA[i]; 00082 cal_chksum += ISR_Modem_DATA[i]; 00083 } 00084 if (ISR_Modem_CHKSUM == cal_chksum){ 00085 return true; 00086 }else{ 00087 return false; 00088 } 00089 } 00090 else{ 00091 return false; 00092 } 00093 } 00094 void ROBOFRIEN_GUI::Init(){ 00095 pc.baud(115200); 00096 eeprom_init(); 00097 /// EEPROM R/W //// 00098 // -------- MODEL -------------- // 00099 model_type1 = eeprom_read(EEPROM_MODEL_TYPE1); 00100 model_type2 = (int)eeprom_read(EEPROM_MODEL_TYPE2_UP) * 256 + eeprom_read(EEPROM_MODEL_TYPE2_DOWN); 00101 00102 // -------- INPUT - CAP ------------ // 00103 cap_min[0] = (int16_t)eeprom_read(EEPROM_RECV_MIN_1) - 127; 00104 cap_min[1] = (int16_t)eeprom_read(EEPROM_RECV_MIN_2) - 127; 00105 cap_min[2] = (int16_t)eeprom_read(EEPROM_RECV_MIN_3) - 127; 00106 cap_min[3] = (int16_t)eeprom_read(EEPROM_RECV_MIN_4) - 127; 00107 cap_min[4] = (int16_t)eeprom_read(EEPROM_RECV_MIN_5) - 127; 00108 cap_min[5] = (int16_t)eeprom_read(EEPROM_RECV_MIN_6) - 127; 00109 cap_min[6] = (int16_t)eeprom_read(EEPROM_RECV_MIN_7) - 127; 00110 cap_min[7] = (int16_t)eeprom_read(EEPROM_RECV_MIN_8) - 127; 00111 00112 cap_neu[0] = (int16_t)eeprom_read(EEPROM_RECV_NEU_1) - 127; 00113 cap_neu[1] = (int16_t)eeprom_read(EEPROM_RECV_NEU_2) - 127; 00114 cap_neu[2] = (int16_t)eeprom_read(EEPROM_RECV_NEU_3) - 127; 00115 cap_neu[3] = (int16_t)eeprom_read(EEPROM_RECV_NEU_4) - 127; 00116 cap_neu[4] = (int16_t)eeprom_read(EEPROM_RECV_NEU_5) - 127; 00117 cap_neu[5] = (int16_t)eeprom_read(EEPROM_RECV_NEU_6) - 127; 00118 cap_neu[6] = (int16_t)eeprom_read(EEPROM_RECV_NEU_7) - 127; 00119 cap_neu[7] = (int16_t)eeprom_read(EEPROM_RECV_NEU_8) - 127; 00120 00121 cap_max[0] = (int16_t)eeprom_read(EEPROM_RECV_MAX_1) - 127; 00122 cap_max[1] = (int16_t)eeprom_read(EEPROM_RECV_MAX_2) - 127; 00123 cap_max[2] = (int16_t)eeprom_read(EEPROM_RECV_MAX_3) - 127; 00124 cap_max[3] = (int16_t)eeprom_read(EEPROM_RECV_MAX_4) - 127; 00125 cap_max[4] = (int16_t)eeprom_read(EEPROM_RECV_MAX_5) - 127; 00126 cap_max[5] = (int16_t)eeprom_read(EEPROM_RECV_MAX_6) - 127; 00127 cap_max[6] = (int16_t)eeprom_read(EEPROM_RECV_MAX_7) - 127; 00128 cap_max[7] = (int16_t)eeprom_read(EEPROM_RECV_MAX_8) - 127; 00129 00130 // --------- OUTPUT - MOTOR ----------- // 00131 motor_min[0] = (int)eeprom_read(EEPROM_MOTOR_MIN_1_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_1_DOWN); 00132 motor_min[1] = (int)eeprom_read(EEPROM_MOTOR_MIN_2_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_2_DOWN); 00133 motor_min[2] = (int)eeprom_read(EEPROM_MOTOR_MIN_3_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_3_DOWN); 00134 motor_min[3] = (int)eeprom_read(EEPROM_MOTOR_MIN_4_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_4_DOWN); 00135 motor_min[4] = (int)eeprom_read(EEPROM_MOTOR_MIN_5_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_5_DOWN); 00136 motor_min[5] = (int)eeprom_read(EEPROM_MOTOR_MIN_6_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_6_DOWN); 00137 00138 // --------- OUTPUT - LED ------------ // 00139 headlight_period = eeprom_read(EEPROM_HEADLIGHT_PERIOD); 00140 headlight_dutyrate = eeprom_read(EEPROM_HEADLIGHT_DUTYRATE); 00141 sidelight_period = eeprom_read(EEPROM_SIDELIGHT_PERIOD); 00142 sidelight_dutyrate = eeprom_read(EEPROM_SIDELIGHT_DUTYRATE); 00143 00144 // --------- GAIN - LIMIT ANGLE ------- // 00145 limit_rollx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_DOWN) - 32767; 00146 limit_pitchx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_DOWN) - 32767; 00147 limit_roll_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_ROLL_DOWN) - 32767; 00148 limit_pitch_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_PITCH_DOWN) - 32767; 00149 limit_yaw_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_YAW_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_YAW_DOWN) - 32767; 00150 00151 // --------- GAIN - GAIN DATA --------- // 00152 for (int i = 0; i < 20; i++) { 00153 gain_px100[i] = (int)eeprom_read(EEPROM_GAIN_P_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_P_DOWN[i]); 00154 gain_dx100[i] = (int)eeprom_read(EEPROM_GAIN_D_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_D_DOWN[i]); 00155 gain_ix100[i] = (int)eeprom_read(EEPROM_GAIN_I_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_I_DOWN[i]); 00156 } 00157 cal_accel_bias[0] = (((int)eeprom_read(EEPROM_AHRS_ROLL_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_ROLL_GAP_DOWN)) - 32767)/10000.0; 00158 cal_accel_bias[1] = (((int)eeprom_read(EEPROM_AHRS_PITCH_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_PITCH_GAP_DOWN)) - 32767)/10000.0; 00159 cal_accel_bias[2] = (((int)eeprom_read(EEPROM_AHRS_YAW_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_YAW_GAP_DOWN)) - 32767)/10000.0; 00160 00161 // --------- Sensor ----------- /// 00162 mag_x_avg = (float)eeprom_read(EEPROM_AHRS_YAW_X_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_X_GAP_2) - 32767 ; 00163 mag_y_avg = (float)eeprom_read(EEPROM_AHRS_YAW_Y_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_Y_GAP_2) - 32767 ; 00164 mag_z_avg = (float)eeprom_read(EEPROM_AHRS_YAW_Z_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_Z_GAP_2) - 32767 ; 00165 00166 declination_angle = (float)(((signed long)eeprom_read(EEPROM_AHRS_DECLINATION_ANGLE_UP) * 256 + eeprom_read(EEPROM_AHRS_DECLINATION_ANGLE_DOWN))-18000)/100.0; 00167 } 00168 00169 void trans_configuration_data(int a, int b, int c); 00170 void trans_empty_data(int a, int b); 00171 void trans_flight_data(int a, int b); 00172 void ROBOFRIEN_GUI::Refresh(){ 00173 Modem_ID_Dept = ISR_Modem_ID_Dept; 00174 Modem_ID_Dest = ISR_Modem_ID_Dest; 00175 Modem_DC_DPN = ISR_Modem_DC_DPN; 00176 Modem_DC_DL = ISR_Modem_DC_DL; 00177 Modem_DC_REQUEST = ISR_Modem_DC_REQUEST; 00178 DPN_Info = Modem_DC_DPN; 00179 if ((Modem_ID_Dept == 255)&(Modem_ID_Dest == 0)) { 00180 if (Modem_DC_REQUEST == 1) { 00181 /// REQUST - READ FROM FCS /// 00182 if (Modem_DC_DPN == 0) { 00183 /// FLIGHT DATA /// 00184 if( (Modem_DATA[1] & 0b00000001) == 0b00000001)button[0] = !button[0]; 00185 if( (Modem_DATA[1] & 0b00000010) == 0b00000010)button[1] = !button[1]; 00186 if( (Modem_DATA[1] & 0b00000100) == 0b00000100)button[2] = !button[2]; 00187 if( (Modem_DATA[1] & 0b00001000) == 0b00001000)button[3] = !button[3]; 00188 if( (Modem_DATA[1] & 0b00010000) == 0b00010000)button[4] = !button[4]; 00189 // Home Point // 00190 if( Modem_DATA[0] == 0){ 00191 Controller_Joystick[0] = (Modem_DATA[2]*256 + Modem_DATA[3])-32767; 00192 Controller_Joystick[1] = (Modem_DATA[4]*256 + Modem_DATA[5])-32767; 00193 Controller_Joystick[2] = (Modem_DATA[6]*256 + Modem_DATA[7])-32767; 00194 Controller_Joystick[3] = (Modem_DATA[8]*256 + Modem_DATA[9])-32767; 00195 Gimbal_Joystick[0] = (Modem_DATA[10]*256 + Modem_DATA[11])-32767; 00196 Gimbal_Joystick[1] = (Modem_DATA[12]*256 + Modem_DATA[13])-32767; 00197 Gimbal_Joystick[2] = (Modem_DATA[14]*256 + Modem_DATA[15])-32767; 00198 }else if(Modem_DATA[0] <= 20){ 00199 Marker_Mode[Modem_DATA[0]-1] = Modem_DATA[2]; 00200 Marker_Lat[Modem_DATA[0]-1] = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000; 00201 Marker_Lng[Modem_DATA[0]-1] = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000; 00202 Marker_Alt[Modem_DATA[0]-1] = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000; 00203 Marker_Speed[Modem_DATA[0]-1] = (long)Modem_DATA[13] * 256 + Modem_DATA[14]; 00204 }else if( Modem_DATA[0] == 21){ 00205 Homepoint_Lat = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000; 00206 Homepoint_Lng = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000; 00207 Homepoint_Alt = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000; 00208 } 00209 // trans_flight_data(TO_GCS, Modem_DC_DPN); 00210 } 00211 else { 00212 // trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]); 00213 } 00214 } 00215 else { 00216 /// NOT REQUEST - WRITE TO FCS /// 00217 // trans_empty_data(TO_GCS, Modem_DC_DPN); 00218 switch (Modem_DC_DPN) { // Check DPN // 00219 case 0: 00220 break; 00221 case 1: /// Modem /// 00222 model_type1 = Modem_DATA[0]; 00223 model_type2 = (uint16_t)Modem_DATA[1] * 256 + Modem_DATA[2]; 00224 eeprom_write(EEPROM_MODEL_TYPE1, (int)model_type1); 00225 eeprom_write(EEPROM_MODEL_TYPE2_UP, (int)model_type2 / 256); 00226 eeprom_write(EEPROM_MODEL_TYPE2_DOWN, (int)model_type2 % 256); 00227 eeprom_refresh_bool = true; 00228 break; 00229 case 3: /// RC Receiver /// 00230 if ((Modem_DATA[0] >= 1) & (Modem_DATA[0] <= 8)) { 00231 cap_min[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[5] - 127; 00232 cap_neu[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[6] - 127; 00233 cap_max[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[7] - 127; 00234 switch (Modem_DATA[0]) { 00235 case 1: 00236 eeprom_write(EEPROM_RECV_MIN_1, cap_min[Modem_DATA[0] - 1] + 127); 00237 eeprom_write(EEPROM_RECV_NEU_1, cap_neu[Modem_DATA[0] - 1] + 127); 00238 eeprom_write(EEPROM_RECV_MAX_1, cap_max[Modem_DATA[0] - 1] + 127); 00239 break; 00240 case 2: 00241 eeprom_write(EEPROM_RECV_MIN_2, cap_min[Modem_DATA[0] - 1] + 127); 00242 eeprom_write(EEPROM_RECV_NEU_2, cap_neu[Modem_DATA[0] - 1] + 127); 00243 eeprom_write(EEPROM_RECV_MAX_2, cap_max[Modem_DATA[0] - 1] + 127); 00244 break; 00245 case 3: 00246 eeprom_write(EEPROM_RECV_MIN_3, cap_min[Modem_DATA[0] - 1] + 127); 00247 eeprom_write(EEPROM_RECV_NEU_3, cap_neu[Modem_DATA[0] - 1] + 127); 00248 eeprom_write(EEPROM_RECV_MAX_3, cap_max[Modem_DATA[0] - 1] + 127); 00249 break; 00250 case 4: 00251 eeprom_write(EEPROM_RECV_MIN_4, cap_min[Modem_DATA[0] - 1] + 127); 00252 eeprom_write(EEPROM_RECV_NEU_4, cap_neu[Modem_DATA[0] - 1] + 127); 00253 eeprom_write(EEPROM_RECV_MAX_4, cap_max[Modem_DATA[0] - 1] + 127); 00254 break; 00255 case 5: 00256 eeprom_write(EEPROM_RECV_MIN_5, cap_min[Modem_DATA[0] - 1] + 127); 00257 eeprom_write(EEPROM_RECV_NEU_5, cap_neu[Modem_DATA[0] - 1] + 127); 00258 eeprom_write(EEPROM_RECV_MAX_5, cap_max[Modem_DATA[0] - 1] + 127); 00259 break; 00260 case 6: 00261 eeprom_write(EEPROM_RECV_MIN_6, cap_min[Modem_DATA[0] - 1] + 127); 00262 eeprom_write(EEPROM_RECV_NEU_6, cap_neu[Modem_DATA[0] - 1] + 127); 00263 eeprom_write(EEPROM_RECV_MAX_6, cap_max[Modem_DATA[0] - 1] + 127); 00264 break; 00265 case 7: 00266 eeprom_write(EEPROM_RECV_MIN_7, cap_min[Modem_DATA[0] - 1] + 127); 00267 eeprom_write(EEPROM_RECV_NEU_7, cap_neu[Modem_DATA[0] - 1] + 127); 00268 eeprom_write(EEPROM_RECV_MAX_7, cap_max[Modem_DATA[0] - 1] + 127); 00269 break; 00270 case 8: 00271 eeprom_write(EEPROM_RECV_MIN_8, cap_min[Modem_DATA[0] - 1] + 127); 00272 eeprom_write(EEPROM_RECV_NEU_8, cap_neu[Modem_DATA[0] - 1] + 127); 00273 eeprom_write(EEPROM_RECV_MAX_8, cap_max[Modem_DATA[0] - 1] + 127); 00274 break; 00275 } 00276 eeprom_refresh_bool = true; 00277 } 00278 break; 00279 case 4: // Motor - OUTPUT // 00280 pwm_info[0] = (int)Modem_DATA[1] * 256 + Modem_DATA[2]; 00281 pwm_info[1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4]; 00282 pwm_info[2] = (int)Modem_DATA[5] * 256 + Modem_DATA[6]; 00283 pwm_info[3] = (int)Modem_DATA[7] * 256 + Modem_DATA[8]; 00284 pwm_info[4] = (int)Modem_DATA[9] * 256 + Modem_DATA[10]; 00285 pwm_info[5] = (int)Modem_DATA[11] * 256 + Modem_DATA[12]; 00286 switch (Modem_DATA[0]) { 00287 case 1: 00288 motor_min[0] = pwm_info[0]; 00289 eeprom_write(EEPROM_MOTOR_MIN_1_UP, motor_min[0] / 256); 00290 eeprom_write(EEPROM_MOTOR_MIN_1_DOWN, motor_min[0] % 256); 00291 eeprom_refresh_bool = true; 00292 break; 00293 case 2: 00294 motor_min[1] = pwm_info[1]; 00295 eeprom_write(EEPROM_MOTOR_MIN_2_UP, motor_min[1] / 256); 00296 eeprom_write(EEPROM_MOTOR_MIN_2_DOWN, motor_min[1] % 256); 00297 eeprom_refresh_bool = true; 00298 break; 00299 case 3: 00300 motor_min[2] = pwm_info[2]; 00301 eeprom_write(EEPROM_MOTOR_MIN_3_UP, motor_min[2] / 256); 00302 eeprom_write(EEPROM_MOTOR_MIN_3_DOWN, motor_min[2] % 256); 00303 eeprom_refresh_bool = true; 00304 break; 00305 case 4: 00306 motor_min[3] = pwm_info[3]; 00307 eeprom_write(EEPROM_MOTOR_MIN_4_UP, motor_min[3] / 256); 00308 eeprom_write(EEPROM_MOTOR_MIN_4_DOWN, motor_min[3] % 256); 00309 eeprom_refresh_bool = true; 00310 break; 00311 case 5: 00312 motor_min[4] = pwm_info[4]; 00313 eeprom_write(EEPROM_MOTOR_MIN_5_UP, motor_min[4] / 256); 00314 eeprom_write(EEPROM_MOTOR_MIN_5_DOWN, motor_min[4] % 256); 00315 eeprom_refresh_bool = true; 00316 break; 00317 case 6: 00318 motor_min[5] = pwm_info[5]; 00319 eeprom_write(EEPROM_MOTOR_MIN_6_UP, motor_min[5] / 256); 00320 eeprom_write(EEPROM_MOTOR_MIN_6_DOWN, motor_min[5] % 256); 00321 eeprom_refresh_bool = true; 00322 break; 00323 } 00324 break; 00325 case 5: // LED - OUTPUT // 00326 headlight_period = Modem_DATA[0]; 00327 headlight_dutyrate = Modem_DATA[1]; 00328 sidelight_period = Modem_DATA[2]; 00329 sidelight_dutyrate = Modem_DATA[3]; 00330 eeprom_write(EEPROM_HEADLIGHT_PERIOD, headlight_period); 00331 eeprom_write(EEPROM_HEADLIGHT_DUTYRATE, headlight_dutyrate); 00332 eeprom_write(EEPROM_SIDELIGHT_PERIOD, sidelight_period); 00333 eeprom_write(EEPROM_SIDELIGHT_DUTYRATE, sidelight_dutyrate); 00334 eeprom_refresh_bool = true; 00335 break; 00336 case 6: // AHRS - ROLL , Pitch // 00337 /// Attitude /// 00338 if (Modem_DATA[0] == 1) { 00339 attitude_configuration_bool = true; 00340 } 00341 else { 00342 attitude_configuration_bool = false; 00343 } 00344 /// Compass /// 00345 if (Modem_DATA[7] == 1) { 00346 Compass_Calibration_Mode = Modem_DATA[7]; 00347 } 00348 else if (Modem_DATA[7] == 2) { 00349 Compass_Calibration_Mode = Modem_DATA[7]; 00350 } 00351 else if(Modem_DATA[7] == 3){ 00352 Compass_Calibration_Mode = Modem_DATA[7]; 00353 declination_angle = (float)(((int)Modem_DATA[10] * 256 + Modem_DATA[11]) - 18000)/100.0; 00354 eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_UP, (int)((declination_angle+180)*100)/256); 00355 eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_DOWN, (int)((declination_angle+180)*100)%256); 00356 eeprom_refresh_bool = true; 00357 } 00358 break; 00359 case 7: // Limit Angle // 00360 limit_rollx100 = ((int)Modem_DATA[0] * 256 + Modem_DATA[1]) - 32767; 00361 limit_pitchx100 = ((int)Modem_DATA[2] * 256 + Modem_DATA[3]) - 32767; 00362 limit_roll_ratex100 = ((int)Modem_DATA[4] * 256 + Modem_DATA[5]) - 32767; 00363 limit_pitch_ratex100 = ((int)Modem_DATA[6] * 256 + Modem_DATA[7]) - 32767; 00364 limit_yaw_ratex100 = ((int)Modem_DATA[8] * 256 + Modem_DATA[9]) - 32767; 00365 eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_UP, (limit_rollx100 + 32767) / 256); 00366 eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_DOWN, (limit_rollx100 + 32767) % 256); 00367 eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_UP, (limit_pitchx100 + 32767) / 256); 00368 eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_DOWN, (limit_pitchx100 + 32767) % 256); 00369 eeprom_write(EEPROM_LIMIT_RATE_ROLL_UP, (limit_roll_ratex100 + 32767) / 256); 00370 eeprom_write(EEPROM_LIMIT_RATE_ROLL_DOWN, (limit_roll_ratex100 + 32767) % 256); 00371 eeprom_write(EEPROM_LIMIT_RATE_PITCH_UP, (limit_pitch_ratex100 + 32767) / 256); 00372 eeprom_write(EEPROM_LIMIT_RATE_PITCH_DOWN, (limit_pitch_ratex100 + 32767) % 256); 00373 eeprom_write(EEPROM_LIMIT_RATE_YAW_UP, (limit_yaw_ratex100 + 32767) / 256); 00374 eeprom_write(EEPROM_LIMIT_RATE_YAW_DOWN, (limit_yaw_ratex100 + 32767) % 256); 00375 eeprom_refresh_bool = true; 00376 break; 00377 case 8: 00378 int gain_number = Modem_DATA[0]; 00379 if ((gain_number >= 1) & (gain_number <= 20)) { 00380 gain_px100[gain_number - 1] = (int)Modem_DATA[1] * 256 + Modem_DATA[2]; 00381 gain_dx100[gain_number - 1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4]; 00382 gain_ix100[gain_number - 1] = (int)Modem_DATA[5] * 256 + Modem_DATA[6]; 00383 eeprom_write(EEPROM_GAIN_P_UP[gain_number - 1], gain_px100[gain_number - 1] / 256); 00384 eeprom_write(EEPROM_GAIN_P_DOWN[gain_number - 1], gain_px100[gain_number - 1] % 256); 00385 eeprom_write(EEPROM_GAIN_D_UP[gain_number - 1], gain_dx100[gain_number - 1] / 256); 00386 eeprom_write(EEPROM_GAIN_D_DOWN[gain_number - 1], gain_dx100[gain_number - 1] % 256); 00387 eeprom_write(EEPROM_GAIN_I_UP[gain_number - 1], gain_ix100[gain_number - 1] / 256); 00388 eeprom_write(EEPROM_GAIN_I_DOWN[gain_number - 1], gain_ix100[gain_number - 1] % 256); 00389 if(gain_number == 20)eeprom_refresh_bool = true; 00390 } 00391 break; 00392 } 00393 00394 } 00395 } 00396 00397 } 00398 00399 void ROBOFRIEN_GUI::Trans(){ 00400 if ((Modem_ID_Dept == 255)&(Modem_ID_Dest == 0)) { 00401 if (Modem_DC_REQUEST == 1) { 00402 /// REQUST - READ FROM FCS /// 00403 if (Modem_DC_DPN == 0) { 00404 trans_flight_data(TO_GCS, Modem_DC_DPN); 00405 } 00406 else { 00407 trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]); 00408 } 00409 } 00410 else { 00411 /// NOT REQUEST - WRITE TO FCS /// 00412 trans_empty_data(TO_GCS, Modem_DC_DPN); 00413 } 00414 } 00415 } 00416 void ROBOFRIEN_GUI::trans_empty_data(int id_dest, int data_parse_num) { 00417 uint8_t Packet_SOF[2] = { 254,254 }; 00418 uint8_t Packet_Identifier[2]; 00419 uint8_t Packet_DC[3]; 00420 uint8_t Packet_CHKSUM[2]; 00421 uint8_t Packet_EOF[2] = { 255,255 }; 00422 // Identifier // 00423 Packet_Identifier[0] = 0; 00424 Packet_Identifier[1] = id_dest; 00425 // Data Control // 00426 Packet_DC[0] = 0; 00427 Packet_DC[1] = (uint8_t)data_parse_num; 00428 int data_length = 0; 00429 Packet_DC[2] = (uint8_t)data_length; 00430 00431 // CHKSUM // 00432 unsigned int data_checksum = 0; 00433 Packet_CHKSUM[0] = data_checksum / 256; 00434 Packet_CHKSUM[1] = data_checksum % 256; 00435 00436 // Each Buffer Integrate to modem_buffer /// 00437 uint8_t modem_buffer[11]; 00438 modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1]; 00439 modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1]; 00440 modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2]; 00441 modem_buffer[7] = Packet_CHKSUM[0]; 00442 modem_buffer[8] = Packet_CHKSUM[1]; 00443 modem_buffer[9] = Packet_EOF[0]; 00444 modem_buffer[10] = Packet_EOF[1]; 00445 TRANS_BUFFER_BUSY = true; 00446 for (int i = 0; i <= 10; i++) { 00447 TRANS_BUF[i] = modem_buffer[i]; 00448 TRANS_BUF_LEN = 11; 00449 // pc.putc(modem_buffer[i]); 00450 } 00451 } 00452 00453 void ROBOFRIEN_GUI::trans_configuration_data(int id_dest, int data_parse_num, int data_parse_detail_num) { 00454 00455 uint8_t Packet_SOF[2] = { 254,254 }; 00456 uint8_t Packet_Identifier[2]; 00457 uint8_t Packet_DC[3]; 00458 uint8_t Packet_DATA[16]; 00459 uint8_t Packet_CHKSUM[2]; 00460 uint8_t Packet_EOF[2] = { 255,255 }; 00461 // Identifier // 00462 Packet_Identifier[0] = 0; 00463 Packet_Identifier[1] = id_dest; 00464 // Data Control // 00465 Packet_DC[0] = 0; 00466 Packet_DC[1] = (uint8_t)data_parse_num; 00467 int data_length = 16; 00468 Packet_DC[2] = (uint8_t)data_length; 00469 // DATA // 00470 switch (data_parse_num) { 00471 case 0: 00472 //// Empty Data //// 00473 for (int i = 0; i <= 15; i++) { 00474 Packet_DATA[i] = 0; 00475 } 00476 break; 00477 case 1: 00478 //// MODEM //// 00479 Packet_DATA[0] = model_type1; 00480 Packet_DATA[1] = model_type2 / 256; Packet_DATA[2] = model_type2 % 256; 00481 for (int i = 3; i <= 15; i++) Packet_DATA[i] = 0; 00482 break; 00483 case 2: 00484 //// Firmware //// 00485 for (int i = 0; i <= 13; i++) { 00486 Packet_DATA[i] = MODEL_INFO[i]; 00487 } 00488 float firmware_info; 00489 firmware_info = FIRMWARE_INFO; 00490 Packet_DATA[14] = (uint8_t)(firmware_info * 100 / 256); Packet_DATA[15] = (uint8_t)((uint32_t)(firmware_info * 100) % 256); 00491 break; 00492 case 3: 00493 //// RC Receiver //// 00494 if (data_parse_detail_num == 0) { 00495 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; 00496 } 00497 else if(data_parse_detail_num <= 8){ 00498 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; 00499 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; 00500 } 00501 else { 00502 for (int i = 0; i <= 7; i++) Packet_DATA[i] = 0; 00503 } 00504 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; 00505 break; 00506 case 4: 00507 for(int i=0; i<16; i++) Packet_DATA[i] = 0; 00508 break; 00509 case 5: 00510 /////// LED ////////// 00511 Packet_DATA[0] = headlight_period; Packet_DATA[1] = headlight_dutyrate; Packet_DATA[2] = sidelight_period; Packet_DATA[3] = sidelight_dutyrate; 00512 break; 00513 case 6: 00514 //// AHRS - Roll , Pitch /// 00515 Packet_DATA[0] = 0; 00516 Packet_DATA[1] = (uint8_t)((unsigned long)(((float)rollx100 / 100.0 + 180) * 100) / 256); 00517 Packet_DATA[2] = (uint8_t)((unsigned long)(((float)rollx100 / 100.0 + 180) * 100) % 256); 00518 Packet_DATA[3] = (uint8_t)((unsigned long)(((float)pitchx100 / 100.0 + 90) * 100) / 256); 00519 Packet_DATA[4] = (uint8_t)((unsigned long)(((float)pitchx100 / 100.0 + 90) * 100) % 256); 00520 //// AHRS - Yaw /// 00521 Packet_DATA[5] = 0; Packet_DATA[6] = 0; 00522 Packet_DATA[7] = (uint8_t)Compass_Calibration_Mode; 00523 Packet_DATA[8] = (uint8_t)((unsigned long)(yawx100) / 256); 00524 Packet_DATA[9] = (uint8_t)((unsigned long)(yawx100) % 256); 00525 Packet_DATA[10] = (uint8_t)((long)((declination_angle + 180) * 100)/256); 00526 Packet_DATA[11] = (uint8_t)((long)((declination_angle + 180) * 100)%256); 00527 for (int i = 12; i <= 15; i++)Packet_DATA[i] = 0; 00528 break; 00529 case 7: 00530 /// Limit ANGLE //// 00531 Packet_DATA[0] = (limit_rollx100 + 32767) / 256; 00532 Packet_DATA[1] = (limit_rollx100 + 32767) % 256; 00533 Packet_DATA[2] = (limit_pitchx100 + 32767) / 256; 00534 Packet_DATA[3] = (limit_pitchx100 + 32767) % 256; 00535 Packet_DATA[4] = (limit_roll_ratex100 + 32767) / 256; 00536 Packet_DATA[5] = (limit_roll_ratex100 + 32767) % 256; 00537 Packet_DATA[6] = (limit_pitch_ratex100 + 32767) / 256; 00538 Packet_DATA[7] = (limit_pitch_ratex100 + 32767) % 256; 00539 Packet_DATA[8] = (limit_yaw_ratex100 + 32767) / 256; 00540 Packet_DATA[9] = (limit_yaw_ratex100 + 32767) % 256; 00541 for (int i = 10; i <= 15; i++) Packet_DATA[i] = 0; 00542 break; 00543 case 8: 00544 Packet_DATA[0] = data_parse_detail_num; 00545 if ((data_parse_detail_num > 0)&(data_parse_detail_num <= 20)) { 00546 Packet_DATA[1] = gain_px100[data_parse_detail_num - 1] / 256; 00547 Packet_DATA[2] = gain_px100[data_parse_detail_num - 1] % 256; 00548 Packet_DATA[3] = gain_dx100[data_parse_detail_num - 1] / 256; 00549 Packet_DATA[4] = gain_dx100[data_parse_detail_num - 1] % 256; 00550 Packet_DATA[5] = gain_ix100[data_parse_detail_num - 1] / 256; 00551 Packet_DATA[6] = gain_ix100[data_parse_detail_num - 1] % 256; 00552 for (int i = 7; i <= 15; i++) Packet_DATA[i] = 0; 00553 } 00554 else { 00555 for (int i = 1; i <= 15; i++) Packet_DATA[i] = 0; 00556 } 00557 break; 00558 } 00559 00560 // CHKSUM // 00561 unsigned int data_checksum = 0; 00562 for (int i = 0; i < data_length; i++) { 00563 data_checksum += Packet_DATA[i]; 00564 } 00565 Packet_CHKSUM[0] = data_checksum / 256; 00566 Packet_CHKSUM[1] = data_checksum % 256; 00567 00568 // Each Buffer Integrate to modem_buffer /// 00569 uint8_t modem_buffer[27]; 00570 modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1]; 00571 modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1]; 00572 modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2]; 00573 for (int i = 0; i < data_length; i++) { 00574 modem_buffer[i + 7] = Packet_DATA[i]; 00575 } 00576 modem_buffer[23] = Packet_CHKSUM[0]; 00577 modem_buffer[24] = Packet_CHKSUM[1]; 00578 modem_buffer[25] = Packet_EOF[0]; 00579 modem_buffer[26] = Packet_EOF[1]; 00580 TRANS_BUFFER_BUSY = true; 00581 for (int i = 0; i <= 26; i++) { 00582 TRANS_BUF[i] = modem_buffer[i]; 00583 TRANS_BUF_LEN = 27; 00584 // pc.putc(modem_buffer[i]); 00585 } 00586 } 00587 00588 void ROBOFRIEN_GUI::trans_flight_data(int id_dest, int data_parse_num) { 00589 00590 uint8_t Packet_SOF[2] = { 254,254 }; 00591 uint8_t Packet_Identifier[2]; 00592 uint8_t Packet_DC[3]; 00593 uint8_t Packet_DATA[73]; 00594 uint8_t Packet_CHKSUM[2]; 00595 uint8_t Packet_EOF[2] = { 255,255 }; 00596 // Identifier // 00597 Packet_Identifier[0] = 0; 00598 Packet_Identifier[1] = id_dest; 00599 // Data Control // 00600 Packet_DC[0] = 0; 00601 Packet_DC[1] = (uint8_t)data_parse_num; 00602 int data_length; 00603 if (data_parse_num == 0) data_length = 73; 00604 else data_length = 16; 00605 Packet_DC[2] = (uint8_t)data_length; 00606 // DATA - Modem & State // 00607 if(Mode1 >= 15) Mode1 = 15; 00608 else if(Mode1 <= 0) Mode1 = 0; 00609 if(Alarm >= 15) Alarm = 15; 00610 else if(Alarm <= 0) Alarm = 0; 00611 if(MissionState >= 15) MissionState = 15; 00612 else if(MissionState <= 0) MissionState = 0; 00613 if(CurrentMarker >= 20) CurrentMarker = 20; 00614 else if(CurrentMarker <= 0) CurrentMarker = 0; 00615 int GainChksum = 0; 00616 for(int i=0; i<20; i++){ 00617 GainChksum += (gain_px100[i]>>8); 00618 GainChksum += (gain_px100[i]&0xFF); 00619 GainChksum += (gain_dx100[i]>>8); 00620 GainChksum += (gain_dx100[i]&0xFF); 00621 GainChksum += (gain_ix100[i]>>8); 00622 GainChksum += (gain_ix100[i]&0xFF); 00623 } 00624 HomePointChksum = ((Homepoint_Lat + 90000000)/16777216) + ((Homepoint_Lat + 90000000)%16777216/65536) + ((Homepoint_Lat + 90000000)%65536/256) + ((Homepoint_Lat + 90000000)%256); 00625 HomePointChksum += ((Homepoint_Lng + 180000000)/16777216) + ((Homepoint_Lng + 180000000)%16777216/65536) + ((Homepoint_Lng + 180000000)%65536/256) + ((Homepoint_Lng + 180000000)%256); 00626 HomePointChksum += ((Homepoint_Alt + 10000)/256 + (Homepoint_Alt + 10000)%256); 00627 MarkerChksum = 0; 00628 for(int i=0; i<20; i++){ 00629 MarkerChksum += (uint8_t)(Marker_Mode[i]); 00630 MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)/16777216); 00631 MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%16777216/65536); 00632 MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%65536/256); 00633 MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%256); 00634 MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)/16777216); 00635 MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%16777216/65536); 00636 MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%65536/256); 00637 MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%256); 00638 MarkerChksum += (uint8_t)((Marker_Alt[i]+10000)/256); 00639 MarkerChksum += (uint8_t)((Marker_Alt[i]+10000)%256); 00640 MarkerChksum += (uint8_t)(Marker_Speed[i]/256); 00641 MarkerChksum += (uint8_t)(Marker_Speed[i]%256); 00642 } 00643 Packet_DATA[0] = Mode1 * 16 + Alarm; 00644 Packet_DATA[1] = MissionState; 00645 Packet_DATA[2] = CurrentMarker; 00646 if(Bat1 > 100) Bat1 = 100; 00647 else if(Bat1 < 0) Bat1 = 0; 00648 Packet_DATA[3] = Bat1; 00649 Packet_DATA[4] = GainChksum%256; 00650 Packet_DATA[5] = button[0] * 1 + button[1] * 2 + button[2] * 4 + button[3] * 8 + button[4] * 16; 00651 Packet_DATA[6] = HomePointChksum%256; 00652 Packet_DATA[7] = MarkerChksum % 256; 00653 // DATA - GPS // 00654 unsigned long send_gps_time = utc_time; 00655 Packet_DATA[8] = send_gps_time / 16777216; 00656 Packet_DATA[9] = send_gps_time % 16777216 / 65536; 00657 Packet_DATA[10] = send_gps_time % 65536 / 256; 00658 Packet_DATA[11] = send_gps_time % 256; 00659 signed long send_gps_latitude = latitude + 90000000; 00660 Packet_DATA[12] = send_gps_latitude / 16777216; 00661 Packet_DATA[13] = send_gps_latitude % 16777216 / 65536; 00662 Packet_DATA[14] = send_gps_latitude % 65536 / 256; 00663 Packet_DATA[15] = send_gps_latitude % 256; 00664 signed long send_gps_longitude = longitude + 180000000; 00665 Packet_DATA[16] = send_gps_longitude / 16777216; 00666 Packet_DATA[17] = send_gps_longitude % 16777216 / 65536; 00667 Packet_DATA[18] = send_gps_longitude % 65536 / 256; 00668 Packet_DATA[19] = send_gps_longitude % 256; 00669 signed long send_altitude; 00670 if ((altitude + 10000) < 0) send_altitude = -10000; 00671 else send_altitude = (altitude + 10000); 00672 Packet_DATA[20] = send_altitude / 256; 00673 Packet_DATA[21] = send_altitude % 256; 00674 Packet_DATA[22] = SatNum; 00675 // AHRS // 00676 unsigned long send_roll = rollx100 + 32767; 00677 Packet_DATA[23] = send_roll / 256; 00678 Packet_DATA[24] = send_roll % 256; 00679 unsigned long send_pitch = pitchx100 + 32767; 00680 Packet_DATA[25] = send_pitch / 256; 00681 Packet_DATA[26] = send_pitch % 256; 00682 unsigned long send_yaw = yawx100; 00683 Packet_DATA[27] = send_yaw / 256; 00684 Packet_DATA[28] = send_yaw % 256; 00685 unsigned long send_roll_rate = (roll_ratex100 +32767); 00686 Packet_DATA[29] = send_roll_rate / 256; 00687 Packet_DATA[30] = send_roll_rate % 256; 00688 unsigned long send_pitch_rate = (pitch_ratex100 + 32767); 00689 Packet_DATA[31] = send_pitch_rate / 256; 00690 Packet_DATA[32] = send_pitch_rate % 256; 00691 unsigned long send_yaw_rate = (yaw_ratex100 + 32767); 00692 Packet_DATA[33] = send_yaw_rate / 256; 00693 Packet_DATA[34] = send_yaw_rate % 256; 00694 unsigned long send_velocity_x = (VXx100 + 32767); 00695 Packet_DATA[35] = send_velocity_x / 256; 00696 Packet_DATA[36] = send_velocity_x % 256; 00697 unsigned long send_velocity_y = (VYx100 + 32767); 00698 Packet_DATA[37] = send_velocity_y / 256; 00699 Packet_DATA[38] = send_velocity_y % 256; 00700 unsigned long send_velocity_z = (VZx100 + 32767); 00701 Packet_DATA[39] = send_velocity_z / 256; 00702 Packet_DATA[40] = send_velocity_z % 256; 00703 // CAP and PWM // 00704 Packet_DATA[41] = (cap[0] + 127); 00705 Packet_DATA[42] = (cap[1] + 127); 00706 Packet_DATA[43] = (cap[2] + 127); 00707 Packet_DATA[44] = (cap[3] + 127); 00708 Packet_DATA[45] = (cap[4] + 127); 00709 Packet_DATA[46] = (cap[5] + 127); 00710 Packet_DATA[47] = (cap[6] + 127); 00711 Packet_DATA[48] = (cap[7] + 127); 00712 00713 // PWM // 00714 for (int i = 49; i <= 56; i++) { 00715 Packet_DATA[i] = pwm[i-49]; 00716 } 00717 // DEBUG // 00718 Packet_DATA[57] = (int)((DEBUGx100[0]))/256; 00719 Packet_DATA[58] = (int)((DEBUGx100[0]))%256; 00720 Packet_DATA[59] = (int)((DEBUGx100[1]))/256; 00721 Packet_DATA[60] = (int)((DEBUGx100[1]))%256; 00722 Packet_DATA[61] = (int)((DEBUGx100[2]))/256; 00723 Packet_DATA[62] = (int)((DEBUGx100[2]))%256; 00724 Packet_DATA[63] = (int)((DEBUGx100[3]))/256; 00725 Packet_DATA[64] = (int)((DEBUGx100[3]))%256; 00726 Packet_DATA[65] = (int)((DEBUGx100[4]))/256; 00727 Packet_DATA[66] = (int)((DEBUGx100[4]))%256; 00728 Packet_DATA[67] = (int)((DEBUGx100[5]))/256; 00729 Packet_DATA[68] = (int)((DEBUGx100[5]))%256; 00730 Packet_DATA[69] = (int)((DEBUGx100[6]+32767))/256; 00731 Packet_DATA[70] = (int)((DEBUGx100[6]+32767))%256; 00732 Packet_DATA[71] = (int)((DEBUGx100[7]+32767))/256; 00733 Packet_DATA[72] = (int)((DEBUGx100[7]+32767))%256; 00734 // CHKSUM // 00735 unsigned int data_checksum = 0; 00736 for (int i = 0; i < data_length; i++) { 00737 data_checksum += Packet_DATA[i]; 00738 } 00739 Packet_CHKSUM[0] = data_checksum / 256; 00740 Packet_CHKSUM[1] = data_checksum % 256; 00741 00742 // Each Buffer Integrate to modem_buffer /// 00743 uint8_t modem_buffer[84]; 00744 modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1]; 00745 modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1]; 00746 modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2]; 00747 for (int i = 0; i < 73; i++) { 00748 modem_buffer[i + 7] = Packet_DATA[i]; 00749 } 00750 modem_buffer[80] = Packet_CHKSUM[0]; 00751 modem_buffer[81] = Packet_CHKSUM[1]; 00752 modem_buffer[82] = Packet_EOF[0]; 00753 modem_buffer[83] = Packet_EOF[1]; 00754 TRANS_BUFFER_BUSY = true; 00755 for (int i = 0; i <= 83; i++) { 00756 TRANS_BUF[i] = modem_buffer[i]; 00757 TRANS_BUF_LEN = 84; 00758 // pc.putc(modem_buffer[i]); 00759 } 00760 } 00761 00762 void ROBOFRIEN_GUI::attitude_calibrate(float in_acc_bias_x, float in_acc_bias_y, float in_acc_bias_z){ 00763 // calibrate_gap_roll += rollAngle; 00764 // calibrate_gap_pitch += pitchAngle; 00765 cal_accel_bias[0] = in_acc_bias_x; 00766 cal_accel_bias[1] = in_acc_bias_y; 00767 cal_accel_bias[2] = (in_acc_bias_z); 00768 eeprom_write(EEPROM_AHRS_ROLL_GAP_UP, (((unsigned long)(in_acc_bias_x * 10000 + 32767) / 256))); 00769 eeprom_write(EEPROM_AHRS_ROLL_GAP_DOWN, (((unsigned long)(in_acc_bias_x * 10000 + 32767) % 256))); 00770 eeprom_write(EEPROM_AHRS_PITCH_GAP_UP, (((unsigned long)(in_acc_bias_y * 10000 + 32767) / 256))); 00771 eeprom_write(EEPROM_AHRS_PITCH_GAP_DOWN, (((unsigned long)(in_acc_bias_y * 10000 + 32767) % 256))); 00772 eeprom_write(EEPROM_AHRS_YAW_GAP_UP, (((unsigned long)(in_acc_bias_z * 10000 + 32767) / 256))); 00773 eeprom_write(EEPROM_AHRS_YAW_GAP_DOWN, (((unsigned long)(in_acc_bias_z * 10000 + 32767) % 256))); 00774 eeprom_refresh_bool = true; 00775 } 00776 00777 void ROBOFRIEN_GUI::write_compass_setting_to_eeprom(){ 00778 eeprom_write(EEPROM_AHRS_YAW_X_GAP_1, ( (uint32_t)(mag_x_avg + 32767)/256 ) ); 00779 eeprom_write(EEPROM_AHRS_YAW_X_GAP_2, ( (uint32_t)(mag_x_avg + 32767)%256 ) ); 00780 // eeprom_write(EEPROM_AHRS_YAW_X_GAP_FLOAT, ( (unsigned long)(mag_x_avg * 100)%100 ) ); 00781 // eeprom_write(EEPROM_AHRS_YAW_X_GAP_3, ( (uint32_t)(mag_x_avg * 100 + 2147483648)%65536/256 ) ); 00782 // eeprom_write(EEPROM_AHRS_YAW_X_GAP_4, ( (uint32_t)(mag_x_avg * 100 + 2147483648)%256 ) ); 00783 00784 eeprom_write(EEPROM_AHRS_YAW_Y_GAP_1, ( (uint32_t)(mag_y_avg + 32767)/256 ) ); 00785 eeprom_write(EEPROM_AHRS_YAW_Y_GAP_2, ( (uint32_t)(mag_y_avg + 32767)%256 ) ); 00786 // eeprom_write(EEPROM_AHRS_YAW_Y_GAP_FLOAT, ( (unsigned long)(mag_y_avg * 100)%100 ) ); 00787 // eeprom_write(EEPROM_AHRS_YAW_Y_GAP_3, ( (uint32_t)(mag_y_avg * 100 + 2147483648)%65536/256 ) ); 00788 // eeprom_write(EEPROM_AHRS_YAW_Y_GAP_4, ( (uint32_t)(mag_y_avg * 100 + 2147483648)%256 ) ); 00789 00790 eeprom_write(EEPROM_AHRS_YAW_Z_GAP_1, ( (uint32_t)(mag_z_avg + 32767)/256 ) ); 00791 eeprom_write(EEPROM_AHRS_YAW_Z_GAP_2, ( (uint32_t)(mag_z_avg + 32767)%256 ) ); 00792 00793 eeprom_refresh_bool = true; 00794 } 00795 00796 int eeprom_refresh_cnt = 0; 00797 bool eeprom_refresh_cnt_bool = false; 00798 void ROBOFRIEN_GUI::SET_DATA(){ 00799 if( eeprom_refresh_bool == true){ 00800 // True 일때, False로 만들고, 10초동안 true로 변경되지 않으면 그때 eeprom_refresh 실시 ( 계속 써서 시스템 망가져서 ~ ) 00801 eeprom_refresh_bool = false; 00802 eeprom_refresh_cnt_bool = true; 00803 eeprom_refresh_cnt = 0; 00804 } 00805 if( eeprom_refresh_cnt_bool == true){ 00806 eeprom_refresh_cnt ++; 00807 if(eeprom_refresh_cnt > 10){ 00808 eeprom_refresh(); 00809 eeprom_refresh_cnt = 0; 00810 eeprom_refresh_cnt_bool = false; 00811 } 00812 } 00813 } 00814 00815 void ROBOFRIEN_GUI::TRANS_BUFFER(){ 00816 if(TRANS_BUFFER_BUSY == true){ 00817 if( TRANS_BUF_CNT >= TRANS_BUF_LEN){ 00818 TRANS_BUF_CNT = 0; 00819 TRANS_BUFFER_BUSY = false; 00820 }else{ 00821 #if PC_DEBUG 00822 pc.putc(TRANS_BUF[TRANS_BUF_CNT]); 00823 TRANS_BUF_CNT ++; 00824 #else 00825 if( (LPC_UART3->LSR & 0x20) ){ 00826 LPC_UART3->THR = TRANS_BUF[TRANS_BUF_CNT]; 00827 TRANS_BUF_CNT ++; 00828 } 00829 #endif 00830 // } 00831 } 00832 }else{ 00833 TRANS_BUF_CNT = 0; 00834 } 00835 }
Generated on Sat Jul 16 2022 14:19:21 by
1.7.2