Kim Youngsik / Mbed 2 deprecated 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

ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp@1:9530746906b6, 2018-11-28 (annotated)

Committer:
skyyoungsik
Date:
Wed Nov 28 13:06:23 2018 +0000
Revision:
1:9530746906b6
Parent:
0:3473b92e991e 
test1

Who changed what in which revision?

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