Kim Youngsik / Mbed 2 deprecated 0SAS_FCC_V12

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:   MPU6050 mbed

Fork of 0SAS_FCC_V11 by Kim Youngsik

ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp@0:a1ad0eb8b619, 2018-04-16 (annotated)

Committer:
skyyoungsik
Date:
Mon Apr 16 07:16:00 2018 +0000
Revision:
0:a1ad0eb8b619
zg

Who changed what in which revision?

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