Kim Youngsik
/
0ROBOFRIEN_FCC_v1_12
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed BufferedSerial ConfigFile
ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp
- Committer:
- skyyoungsik
- Date:
- 2018-11-28
- Revision:
- 1:9530746906b6
- Parent:
- 0:3473b92e991e
File content as of revision 1:9530746906b6:
#include "ROBOFRIEN_GUI.h"
#include "GUI_Config.h"
#include "eeprom.h"
#include "BufferedSerial.h"
#define PC_DEBUG 0
#if PC_DEBUG
Serial pc(USBTX,USBRX);
#else
BufferedSerial pc(p9, p10); // tx, rx
#endif
uint8_t ISR_Modem_ID_Dept, ISR_Modem_ID_Dest;
uint8_t ISR_Modem_DC_DPN, ISR_Modem_DC_DL;
uint8_t ISR_Modem_DATA[256];
uint8_t ISR_Modem_DC_REQUEST;
uint16_t ISR_Modem_CHKSUM;
uint8_t pc_isr_cnt;
uint8_t isr_modem_data_cnt = 0;
uint8_t pc_parsing_bool = false;
bool pc_Start_Check;
uint8_t model_type1;
uint16_t model_type2;
uint8_t pc_buffer[256],ex_pc_raw_buffer,pc_raw_buffer[256];
uint8_t pc_buffer_cnt=0;
void ROBOFRIEN_GUI::pc_rx_update(){
while (pc.readable()){
pc_buffer[1] = pc_buffer[0];
pc_buffer[0] = pc.getc();
// Check SOF //
if (( pc_buffer[1] == 254) & (pc_buffer[0] == 254)){
pc_Start_Check = true;
pc_isr_cnt = 0;
}
else if(pc_Start_Check == true){
if(pc_isr_cnt <= 4){
switch(pc_isr_cnt){
case 0: ISR_Modem_ID_Dept = pc_buffer[0]; break;
case 1: ISR_Modem_ID_Dest = pc_buffer[0]; break;
case 2: ISR_Modem_DC_REQUEST = pc_buffer[0]; break;
case 3: ISR_Modem_DC_DPN = pc_buffer[0]; break;
case 4: ISR_Modem_DC_DL = pc_buffer[0]; isr_modem_data_cnt = 0; break;
}
}
else if( pc_isr_cnt > 4 ){
if ((ISR_Modem_DC_DL >= 1) & (pc_isr_cnt <= (4 + ISR_Modem_DC_DL))) {
ISR_Modem_DATA[isr_modem_data_cnt] = pc_buffer[0];
isr_modem_data_cnt++;
}
else if(pc_isr_cnt == (4 + ISR_Modem_DC_DL + 2) ){
ISR_Modem_CHKSUM = ((uint16_t)pc_buffer[1] * 256 + pc_buffer[0]);
}
else if(pc_isr_cnt == (4 + ISR_Modem_DC_DL + 2 + 2) ){
if((pc_buffer[1] == 255) & (pc_buffer[0] == 255) ){
pc_Start_Check = false;
pc_parsing_bool = true;
}
}
}
pc_isr_cnt ++;
}
}
}
uint8_t Modem_ID_Dest;
uint8_t Modem_ID_Dept;
uint8_t Modem_DC_DPN;
uint8_t Modem_DC_DL;
uint8_t Modem_DATA[256];
uint8_t Modem_DC_REQUEST;
bool ROBOFRIEN_GUI::rx_bool(){
if(pc_parsing_bool == true){
pc_parsing_bool = false;
int cal_chksum = 0;
for(int i=0; i< ISR_Modem_DC_DL; i++){
Modem_DATA[i] = ISR_Modem_DATA[i];
cal_chksum += ISR_Modem_DATA[i];
}
if (ISR_Modem_CHKSUM == cal_chksum){
return true;
}else{
return false;
}
}
else{
return false;
}
}
void ROBOFRIEN_GUI::Init(){
pc.baud(115200);
eeprom_init();
/// EEPROM R/W ////
// -------- MODEL -------------- //
model_type1 = eeprom_read(EEPROM_MODEL_TYPE1);
model_type2 = (int)eeprom_read(EEPROM_MODEL_TYPE2_UP) * 256 + eeprom_read(EEPROM_MODEL_TYPE2_DOWN);
// -------- INPUT - CAP ------------ //
cap_min[0] = (int16_t)eeprom_read(EEPROM_RECV_MIN_1) - 127;
cap_min[1] = (int16_t)eeprom_read(EEPROM_RECV_MIN_2) - 127;
cap_min[2] = (int16_t)eeprom_read(EEPROM_RECV_MIN_3) - 127;
cap_min[3] = (int16_t)eeprom_read(EEPROM_RECV_MIN_4) - 127;
cap_min[4] = (int16_t)eeprom_read(EEPROM_RECV_MIN_5) - 127;
cap_min[5] = (int16_t)eeprom_read(EEPROM_RECV_MIN_6) - 127;
cap_min[6] = (int16_t)eeprom_read(EEPROM_RECV_MIN_7) - 127;
cap_min[7] = (int16_t)eeprom_read(EEPROM_RECV_MIN_8) - 127;
cap_neu[0] = (int16_t)eeprom_read(EEPROM_RECV_NEU_1) - 127;
cap_neu[1] = (int16_t)eeprom_read(EEPROM_RECV_NEU_2) - 127;
cap_neu[2] = (int16_t)eeprom_read(EEPROM_RECV_NEU_3) - 127;
cap_neu[3] = (int16_t)eeprom_read(EEPROM_RECV_NEU_4) - 127;
cap_neu[4] = (int16_t)eeprom_read(EEPROM_RECV_NEU_5) - 127;
cap_neu[5] = (int16_t)eeprom_read(EEPROM_RECV_NEU_6) - 127;
cap_neu[6] = (int16_t)eeprom_read(EEPROM_RECV_NEU_7) - 127;
cap_neu[7] = (int16_t)eeprom_read(EEPROM_RECV_NEU_8) - 127;
cap_max[0] = (int16_t)eeprom_read(EEPROM_RECV_MAX_1) - 127;
cap_max[1] = (int16_t)eeprom_read(EEPROM_RECV_MAX_2) - 127;
cap_max[2] = (int16_t)eeprom_read(EEPROM_RECV_MAX_3) - 127;
cap_max[3] = (int16_t)eeprom_read(EEPROM_RECV_MAX_4) - 127;
cap_max[4] = (int16_t)eeprom_read(EEPROM_RECV_MAX_5) - 127;
cap_max[5] = (int16_t)eeprom_read(EEPROM_RECV_MAX_6) - 127;
cap_max[6] = (int16_t)eeprom_read(EEPROM_RECV_MAX_7) - 127;
cap_max[7] = (int16_t)eeprom_read(EEPROM_RECV_MAX_8) - 127;
// --------- OUTPUT - MOTOR ----------- //
motor_min[0] = (int)eeprom_read(EEPROM_MOTOR_MIN_1_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_1_DOWN);
motor_min[1] = (int)eeprom_read(EEPROM_MOTOR_MIN_2_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_2_DOWN);
motor_min[2] = (int)eeprom_read(EEPROM_MOTOR_MIN_3_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_3_DOWN);
motor_min[3] = (int)eeprom_read(EEPROM_MOTOR_MIN_4_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_4_DOWN);
motor_min[4] = (int)eeprom_read(EEPROM_MOTOR_MIN_5_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_5_DOWN);
motor_min[5] = (int)eeprom_read(EEPROM_MOTOR_MIN_6_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_6_DOWN);
// --------- OUTPUT - LED ------------ //
headlight_period = eeprom_read(EEPROM_HEADLIGHT_PERIOD);
headlight_dutyrate = eeprom_read(EEPROM_HEADLIGHT_DUTYRATE);
sidelight_period = eeprom_read(EEPROM_SIDELIGHT_PERIOD);
sidelight_dutyrate = eeprom_read(EEPROM_SIDELIGHT_DUTYRATE);
// --------- GAIN - LIMIT ANGLE ------- //
limit_rollx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_DOWN) - 32767;
limit_pitchx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_DOWN) - 32767;
limit_roll_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_ROLL_DOWN) - 32767;
limit_pitch_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_PITCH_DOWN) - 32767;
limit_yaw_ratex100 = (int)eeprom_read(EEPROM_LIMIT_RATE_YAW_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_YAW_DOWN) - 32767;
// --------- GAIN - GAIN DATA --------- //
for (int i = 0; i < 20; i++) {
gain_px100[i] = (int)eeprom_read(EEPROM_GAIN_P_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_P_DOWN[i]);
gain_dx100[i] = (int)eeprom_read(EEPROM_GAIN_D_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_D_DOWN[i]);
gain_ix100[i] = (int)eeprom_read(EEPROM_GAIN_I_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_I_DOWN[i]);
}
cal_accel_bias[0] = (((int)eeprom_read(EEPROM_AHRS_ROLL_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_ROLL_GAP_DOWN)) - 32767)/10000.0;
cal_accel_bias[1] = (((int)eeprom_read(EEPROM_AHRS_PITCH_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_PITCH_GAP_DOWN)) - 32767)/10000.0;
cal_accel_bias[2] = (((int)eeprom_read(EEPROM_AHRS_YAW_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_YAW_GAP_DOWN)) - 32767)/10000.0;
// --------- Sensor ----------- ///
mag_x_avg = (float)eeprom_read(EEPROM_AHRS_YAW_X_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_X_GAP_2) - 32767 ;
mag_y_avg = (float)eeprom_read(EEPROM_AHRS_YAW_Y_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_Y_GAP_2) - 32767 ;
mag_z_avg = (float)eeprom_read(EEPROM_AHRS_YAW_Z_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_Z_GAP_2) - 32767 ;
declination_angle = (float)(((signed long)eeprom_read(EEPROM_AHRS_DECLINATION_ANGLE_UP) * 256 + eeprom_read(EEPROM_AHRS_DECLINATION_ANGLE_DOWN))-18000)/100.0;
}
void trans_configuration_data(int a, int b, int c);
void trans_empty_data(int a, int b);
void trans_flight_data(int a, int b);
void ROBOFRIEN_GUI::Refresh(){
Modem_ID_Dept = ISR_Modem_ID_Dept;
Modem_ID_Dest = ISR_Modem_ID_Dest;
Modem_DC_DPN = ISR_Modem_DC_DPN;
Modem_DC_DL = ISR_Modem_DC_DL;
Modem_DC_REQUEST = ISR_Modem_DC_REQUEST;
DPN_Info = Modem_DC_DPN;
if ((Modem_ID_Dept == 255)&(Modem_ID_Dest == 0)) {
if (Modem_DC_REQUEST == 1) {
/// REQUST - READ FROM FCS ///
if (Modem_DC_DPN == 0) {
/// FLIGHT DATA ///
if( (Modem_DATA[1] & 0b00000001) == 0b00000001)button[0] = !button[0];
if( (Modem_DATA[1] & 0b00000010) == 0b00000010)button[1] = !button[1];
if( (Modem_DATA[1] & 0b00000100) == 0b00000100)button[2] = !button[2];
if( (Modem_DATA[1] & 0b00001000) == 0b00001000)button[3] = !button[3];
if( (Modem_DATA[1] & 0b00010000) == 0b00010000)button[4] = !button[4];
// Home Point //
if( Modem_DATA[0] == 0){
Controller_Joystick[0] = (Modem_DATA[2]*256 + Modem_DATA[3])-32767;
Controller_Joystick[1] = (Modem_DATA[4]*256 + Modem_DATA[5])-32767;
Controller_Joystick[2] = (Modem_DATA[6]*256 + Modem_DATA[7])-32767;
Controller_Joystick[3] = (Modem_DATA[8]*256 + Modem_DATA[9])-32767;
Gimbal_Joystick[0] = (Modem_DATA[10]*256 + Modem_DATA[11])-32767;
Gimbal_Joystick[1] = (Modem_DATA[12]*256 + Modem_DATA[13])-32767;
Gimbal_Joystick[2] = (Modem_DATA[14]*256 + Modem_DATA[15])-32767;
}else if(Modem_DATA[0] <= 20){
Marker_Mode[Modem_DATA[0]-1] = Modem_DATA[2];
Marker_Lat[Modem_DATA[0]-1] = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
Marker_Lng[Modem_DATA[0]-1] = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
Marker_Alt[Modem_DATA[0]-1] = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;
Marker_Speed[Modem_DATA[0]-1] = (long)Modem_DATA[13] * 256 + Modem_DATA[14];
}else if( Modem_DATA[0] == 21){
Homepoint_Lat = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
Homepoint_Lng = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
Homepoint_Alt = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;
}
// trans_flight_data(TO_GCS, Modem_DC_DPN);
}
else {
// trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]);
}
}
else {
/// NOT REQUEST - WRITE TO FCS ///
// trans_empty_data(TO_GCS, Modem_DC_DPN);
switch (Modem_DC_DPN) { // Check DPN //
case 0:
break;
case 1: /// Modem ///
model_type1 = Modem_DATA[0];
model_type2 = (uint16_t)Modem_DATA[1] * 256 + Modem_DATA[2];
eeprom_write(EEPROM_MODEL_TYPE1, (int)model_type1);
eeprom_write(EEPROM_MODEL_TYPE2_UP, (int)model_type2 / 256);
eeprom_write(EEPROM_MODEL_TYPE2_DOWN, (int)model_type2 % 256);
eeprom_refresh_bool = true;
break;
case 3: /// RC Receiver ///
if ((Modem_DATA[0] >= 1) & (Modem_DATA[0] <= 8)) {
cap_min[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[5] - 127;
cap_neu[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[6] - 127;
cap_max[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[7] - 127;
switch (Modem_DATA[0]) {
case 1:
eeprom_write(EEPROM_RECV_MIN_1, cap_min[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_NEU_1, cap_neu[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_MAX_1, cap_max[Modem_DATA[0] - 1] + 127);
break;
case 2:
eeprom_write(EEPROM_RECV_MIN_2, cap_min[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_NEU_2, cap_neu[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_MAX_2, cap_max[Modem_DATA[0] - 1] + 127);
break;
case 3:
eeprom_write(EEPROM_RECV_MIN_3, cap_min[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_NEU_3, cap_neu[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_MAX_3, cap_max[Modem_DATA[0] - 1] + 127);
break;
case 4:
eeprom_write(EEPROM_RECV_MIN_4, cap_min[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_NEU_4, cap_neu[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_MAX_4, cap_max[Modem_DATA[0] - 1] + 127);
break;
case 5:
eeprom_write(EEPROM_RECV_MIN_5, cap_min[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_NEU_5, cap_neu[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_MAX_5, cap_max[Modem_DATA[0] - 1] + 127);
break;
case 6:
eeprom_write(EEPROM_RECV_MIN_6, cap_min[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_NEU_6, cap_neu[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_MAX_6, cap_max[Modem_DATA[0] - 1] + 127);
break;
case 7:
eeprom_write(EEPROM_RECV_MIN_7, cap_min[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_NEU_7, cap_neu[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_MAX_7, cap_max[Modem_DATA[0] - 1] + 127);
break;
case 8:
eeprom_write(EEPROM_RECV_MIN_8, cap_min[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_NEU_8, cap_neu[Modem_DATA[0] - 1] + 127);
eeprom_write(EEPROM_RECV_MAX_8, cap_max[Modem_DATA[0] - 1] + 127);
break;
}
eeprom_refresh_bool = true;
}
break;
case 4: // Motor - OUTPUT //
pwm_info[0] = (int)Modem_DATA[1] * 256 + Modem_DATA[2];
pwm_info[1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4];
pwm_info[2] = (int)Modem_DATA[5] * 256 + Modem_DATA[6];
pwm_info[3] = (int)Modem_DATA[7] * 256 + Modem_DATA[8];
pwm_info[4] = (int)Modem_DATA[9] * 256 + Modem_DATA[10];
pwm_info[5] = (int)Modem_DATA[11] * 256 + Modem_DATA[12];
switch (Modem_DATA[0]) {
case 1:
motor_min[0] = pwm_info[0];
eeprom_write(EEPROM_MOTOR_MIN_1_UP, motor_min[0] / 256);
eeprom_write(EEPROM_MOTOR_MIN_1_DOWN, motor_min[0] % 256);
eeprom_refresh_bool = true;
break;
case 2:
motor_min[1] = pwm_info[1];
eeprom_write(EEPROM_MOTOR_MIN_2_UP, motor_min[1] / 256);
eeprom_write(EEPROM_MOTOR_MIN_2_DOWN, motor_min[1] % 256);
eeprom_refresh_bool = true;
break;
case 3:
motor_min[2] = pwm_info[2];
eeprom_write(EEPROM_MOTOR_MIN_3_UP, motor_min[2] / 256);
eeprom_write(EEPROM_MOTOR_MIN_3_DOWN, motor_min[2] % 256);
eeprom_refresh_bool = true;
break;
case 4:
motor_min[3] = pwm_info[3];
eeprom_write(EEPROM_MOTOR_MIN_4_UP, motor_min[3] / 256);
eeprom_write(EEPROM_MOTOR_MIN_4_DOWN, motor_min[3] % 256);
eeprom_refresh_bool = true;
break;
case 5:
motor_min[4] = pwm_info[4];
eeprom_write(EEPROM_MOTOR_MIN_5_UP, motor_min[4] / 256);
eeprom_write(EEPROM_MOTOR_MIN_5_DOWN, motor_min[4] % 256);
eeprom_refresh_bool = true;
break;
case 6:
motor_min[5] = pwm_info[5];
eeprom_write(EEPROM_MOTOR_MIN_6_UP, motor_min[5] / 256);
eeprom_write(EEPROM_MOTOR_MIN_6_DOWN, motor_min[5] % 256);
eeprom_refresh_bool = true;
break;
}
break;
case 5: // LED - OUTPUT //
headlight_period = Modem_DATA[0];
headlight_dutyrate = Modem_DATA[1];
sidelight_period = Modem_DATA[2];
sidelight_dutyrate = Modem_DATA[3];
eeprom_write(EEPROM_HEADLIGHT_PERIOD, headlight_period);
eeprom_write(EEPROM_HEADLIGHT_DUTYRATE, headlight_dutyrate);
eeprom_write(EEPROM_SIDELIGHT_PERIOD, sidelight_period);
eeprom_write(EEPROM_SIDELIGHT_DUTYRATE, sidelight_dutyrate);
eeprom_refresh_bool = true;
break;
case 6: // AHRS - ROLL , Pitch //
/// Attitude ///
if (Modem_DATA[0] == 1) {
attitude_configuration_bool = true;
}
else {
attitude_configuration_bool = false;
}
/// Compass ///
if (Modem_DATA[7] == 1) {
Compass_Calibration_Mode = Modem_DATA[7];
}
else if (Modem_DATA[7] == 2) {
Compass_Calibration_Mode = Modem_DATA[7];
}
else if(Modem_DATA[7] == 3){
Compass_Calibration_Mode = Modem_DATA[7];
declination_angle = (float)(((int)Modem_DATA[10] * 256 + Modem_DATA[11]) - 18000)/100.0;
eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_UP, (int)((declination_angle+180)*100)/256);
eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_DOWN, (int)((declination_angle+180)*100)%256);
eeprom_refresh_bool = true;
}
break;
case 7: // Limit Angle //
limit_rollx100 = ((int)Modem_DATA[0] * 256 + Modem_DATA[1]) - 32767;
limit_pitchx100 = ((int)Modem_DATA[2] * 256 + Modem_DATA[3]) - 32767;
limit_roll_ratex100 = ((int)Modem_DATA[4] * 256 + Modem_DATA[5]) - 32767;
limit_pitch_ratex100 = ((int)Modem_DATA[6] * 256 + Modem_DATA[7]) - 32767;
limit_yaw_ratex100 = ((int)Modem_DATA[8] * 256 + Modem_DATA[9]) - 32767;
eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_UP, (limit_rollx100 + 32767) / 256);
eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_DOWN, (limit_rollx100 + 32767) % 256);
eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_UP, (limit_pitchx100 + 32767) / 256);
eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_DOWN, (limit_pitchx100 + 32767) % 256);
eeprom_write(EEPROM_LIMIT_RATE_ROLL_UP, (limit_roll_ratex100 + 32767) / 256);
eeprom_write(EEPROM_LIMIT_RATE_ROLL_DOWN, (limit_roll_ratex100 + 32767) % 256);
eeprom_write(EEPROM_LIMIT_RATE_PITCH_UP, (limit_pitch_ratex100 + 32767) / 256);
eeprom_write(EEPROM_LIMIT_RATE_PITCH_DOWN, (limit_pitch_ratex100 + 32767) % 256);
eeprom_write(EEPROM_LIMIT_RATE_YAW_UP, (limit_yaw_ratex100 + 32767) / 256);
eeprom_write(EEPROM_LIMIT_RATE_YAW_DOWN, (limit_yaw_ratex100 + 32767) % 256);
eeprom_refresh_bool = true;
break;
case 8:
int gain_number = Modem_DATA[0];
if ((gain_number >= 1) & (gain_number <= 20)) {
gain_px100[gain_number - 1] = (int)Modem_DATA[1] * 256 + Modem_DATA[2];
gain_dx100[gain_number - 1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4];
gain_ix100[gain_number - 1] = (int)Modem_DATA[5] * 256 + Modem_DATA[6];
eeprom_write(EEPROM_GAIN_P_UP[gain_number - 1], gain_px100[gain_number - 1] / 256);
eeprom_write(EEPROM_GAIN_P_DOWN[gain_number - 1], gain_px100[gain_number - 1] % 256);
eeprom_write(EEPROM_GAIN_D_UP[gain_number - 1], gain_dx100[gain_number - 1] / 256);
eeprom_write(EEPROM_GAIN_D_DOWN[gain_number - 1], gain_dx100[gain_number - 1] % 256);
eeprom_write(EEPROM_GAIN_I_UP[gain_number - 1], gain_ix100[gain_number - 1] / 256);
eeprom_write(EEPROM_GAIN_I_DOWN[gain_number - 1], gain_ix100[gain_number - 1] % 256);
if(gain_number == 20)eeprom_refresh_bool = true;
}
break;
}
}
}
}
void ROBOFRIEN_GUI::Trans(){
if ((Modem_ID_Dept == 255)&(Modem_ID_Dest == 0)) {
if (Modem_DC_REQUEST == 1) {
/// REQUST - READ FROM FCS ///
if (Modem_DC_DPN == 0) {
trans_flight_data(TO_GCS, Modem_DC_DPN);
}
else {
trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]);
}
}
else {
/// NOT REQUEST - WRITE TO FCS ///
trans_empty_data(TO_GCS, Modem_DC_DPN);
}
}
}
void ROBOFRIEN_GUI::trans_empty_data(int id_dest, int data_parse_num) {
uint8_t Packet_SOF[2] = { 254,254 };
uint8_t Packet_Identifier[2];
uint8_t Packet_DC[3];
uint8_t Packet_CHKSUM[2];
uint8_t Packet_EOF[2] = { 255,255 };
// Identifier //
Packet_Identifier[0] = 0;
Packet_Identifier[1] = id_dest;
// Data Control //
Packet_DC[0] = 0;
Packet_DC[1] = (uint8_t)data_parse_num;
int data_length = 0;
Packet_DC[2] = (uint8_t)data_length;
// CHKSUM //
unsigned int data_checksum = 0;
Packet_CHKSUM[0] = data_checksum / 256;
Packet_CHKSUM[1] = data_checksum % 256;
// Each Buffer Integrate to modem_buffer ///
uint8_t modem_buffer[11];
modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
modem_buffer[7] = Packet_CHKSUM[0];
modem_buffer[8] = Packet_CHKSUM[1];
modem_buffer[9] = Packet_EOF[0];
modem_buffer[10] = Packet_EOF[1];
TRANS_BUFFER_BUSY = true;
for (int i = 0; i <= 10; i++) {
TRANS_BUF[i] = modem_buffer[i];
TRANS_BUF_LEN = 11;
// pc.putc(modem_buffer[i]);
}
}
void ROBOFRIEN_GUI::trans_configuration_data(int id_dest, int data_parse_num, int data_parse_detail_num) {
uint8_t Packet_SOF[2] = { 254,254 };
uint8_t Packet_Identifier[2];
uint8_t Packet_DC[3];
uint8_t Packet_DATA[16];
uint8_t Packet_CHKSUM[2];
uint8_t Packet_EOF[2] = { 255,255 };
// Identifier //
Packet_Identifier[0] = 0;
Packet_Identifier[1] = id_dest;
// Data Control //
Packet_DC[0] = 0;
Packet_DC[1] = (uint8_t)data_parse_num;
int data_length = 16;
Packet_DC[2] = (uint8_t)data_length;
// DATA //
switch (data_parse_num) {
case 0:
//// Empty Data ////
for (int i = 0; i <= 15; i++) {
Packet_DATA[i] = 0;
}
break;
case 1:
//// MODEM ////
Packet_DATA[0] = model_type1;
Packet_DATA[1] = model_type2 / 256; Packet_DATA[2] = model_type2 % 256;
for (int i = 3; i <= 15; i++) Packet_DATA[i] = 0;
break;
case 2:
//// Firmware ////
for (int i = 0; i <= 13; i++) {
Packet_DATA[i] = MODEL_INFO[i];
}
float firmware_info;
firmware_info = FIRMWARE_INFO;
Packet_DATA[14] = (uint8_t)(firmware_info * 100 / 256); Packet_DATA[15] = (uint8_t)((uint32_t)(firmware_info * 100) % 256);
break;
case 3:
//// RC Receiver ////
if (data_parse_detail_num == 0) {
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;
}
else if(data_parse_detail_num <= 8){
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;
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;
}
else {
for (int i = 0; i <= 7; i++) Packet_DATA[i] = 0;
}
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;
break;
case 4:
for(int i=0; i<16; i++) Packet_DATA[i] = 0;
break;
case 5:
/////// LED //////////
Packet_DATA[0] = headlight_period; Packet_DATA[1] = headlight_dutyrate; Packet_DATA[2] = sidelight_period; Packet_DATA[3] = sidelight_dutyrate;
break;
case 6:
//// AHRS - Roll , Pitch ///
Packet_DATA[0] = 0;
Packet_DATA[1] = (uint8_t)((unsigned long)(((float)rollx100 / 100.0 + 180) * 100) / 256);
Packet_DATA[2] = (uint8_t)((unsigned long)(((float)rollx100 / 100.0 + 180) * 100) % 256);
Packet_DATA[3] = (uint8_t)((unsigned long)(((float)pitchx100 / 100.0 + 90) * 100) / 256);
Packet_DATA[4] = (uint8_t)((unsigned long)(((float)pitchx100 / 100.0 + 90) * 100) % 256);
//// AHRS - Yaw ///
Packet_DATA[5] = 0; Packet_DATA[6] = 0;
Packet_DATA[7] = (uint8_t)Compass_Calibration_Mode;
Packet_DATA[8] = (uint8_t)((unsigned long)(yawx100) / 256);
Packet_DATA[9] = (uint8_t)((unsigned long)(yawx100) % 256);
Packet_DATA[10] = (uint8_t)((long)((declination_angle + 180) * 100)/256);
Packet_DATA[11] = (uint8_t)((long)((declination_angle + 180) * 100)%256);
for (int i = 12; i <= 15; i++)Packet_DATA[i] = 0;
break;
case 7:
/// Limit ANGLE ////
Packet_DATA[0] = (limit_rollx100 + 32767) / 256;
Packet_DATA[1] = (limit_rollx100 + 32767) % 256;
Packet_DATA[2] = (limit_pitchx100 + 32767) / 256;
Packet_DATA[3] = (limit_pitchx100 + 32767) % 256;
Packet_DATA[4] = (limit_roll_ratex100 + 32767) / 256;
Packet_DATA[5] = (limit_roll_ratex100 + 32767) % 256;
Packet_DATA[6] = (limit_pitch_ratex100 + 32767) / 256;
Packet_DATA[7] = (limit_pitch_ratex100 + 32767) % 256;
Packet_DATA[8] = (limit_yaw_ratex100 + 32767) / 256;
Packet_DATA[9] = (limit_yaw_ratex100 + 32767) % 256;
for (int i = 10; i <= 15; i++) Packet_DATA[i] = 0;
break;
case 8:
Packet_DATA[0] = data_parse_detail_num;
if ((data_parse_detail_num > 0)&(data_parse_detail_num <= 20)) {
Packet_DATA[1] = gain_px100[data_parse_detail_num - 1] / 256;
Packet_DATA[2] = gain_px100[data_parse_detail_num - 1] % 256;
Packet_DATA[3] = gain_dx100[data_parse_detail_num - 1] / 256;
Packet_DATA[4] = gain_dx100[data_parse_detail_num - 1] % 256;
Packet_DATA[5] = gain_ix100[data_parse_detail_num - 1] / 256;
Packet_DATA[6] = gain_ix100[data_parse_detail_num - 1] % 256;
for (int i = 7; i <= 15; i++) Packet_DATA[i] = 0;
}
else {
for (int i = 1; i <= 15; i++) Packet_DATA[i] = 0;
}
break;
}
// CHKSUM //
unsigned int data_checksum = 0;
for (int i = 0; i < data_length; i++) {
data_checksum += Packet_DATA[i];
}
Packet_CHKSUM[0] = data_checksum / 256;
Packet_CHKSUM[1] = data_checksum % 256;
// Each Buffer Integrate to modem_buffer ///
uint8_t modem_buffer[27];
modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
for (int i = 0; i < data_length; i++) {
modem_buffer[i + 7] = Packet_DATA[i];
}
modem_buffer[23] = Packet_CHKSUM[0];
modem_buffer[24] = Packet_CHKSUM[1];
modem_buffer[25] = Packet_EOF[0];
modem_buffer[26] = Packet_EOF[1];
TRANS_BUFFER_BUSY = true;
for (int i = 0; i <= 26; i++) {
TRANS_BUF[i] = modem_buffer[i];
TRANS_BUF_LEN = 27;
// pc.putc(modem_buffer[i]);
}
}
void ROBOFRIEN_GUI::trans_flight_data(int id_dest, int data_parse_num) {
uint8_t Packet_SOF[2] = { 254,254 };
uint8_t Packet_Identifier[2];
uint8_t Packet_DC[3];
uint8_t Packet_DATA[73];
uint8_t Packet_CHKSUM[2];
uint8_t Packet_EOF[2] = { 255,255 };
// Identifier //
Packet_Identifier[0] = 0;
Packet_Identifier[1] = id_dest;
// Data Control //
Packet_DC[0] = 0;
Packet_DC[1] = (uint8_t)data_parse_num;
int data_length;
if (data_parse_num == 0) data_length = 73;
else data_length = 16;
Packet_DC[2] = (uint8_t)data_length;
// DATA - Modem & State //
if(Mode1 >= 15) Mode1 = 15;
else if(Mode1 <= 0) Mode1 = 0;
if(Alarm >= 15) Alarm = 15;
else if(Alarm <= 0) Alarm = 0;
if(MissionState >= 15) MissionState = 15;
else if(MissionState <= 0) MissionState = 0;
if(CurrentMarker >= 20) CurrentMarker = 20;
else if(CurrentMarker <= 0) CurrentMarker = 0;
int GainChksum = 0;
for(int i=0; i<20; i++){
GainChksum += (gain_px100[i]>>8);
GainChksum += (gain_px100[i]&0xFF);
GainChksum += (gain_dx100[i]>>8);
GainChksum += (gain_dx100[i]&0xFF);
GainChksum += (gain_ix100[i]>>8);
GainChksum += (gain_ix100[i]&0xFF);
}
HomePointChksum = ((Homepoint_Lat + 90000000)/16777216) + ((Homepoint_Lat + 90000000)%16777216/65536) + ((Homepoint_Lat + 90000000)%65536/256) + ((Homepoint_Lat + 90000000)%256);
HomePointChksum += ((Homepoint_Lng + 180000000)/16777216) + ((Homepoint_Lng + 180000000)%16777216/65536) + ((Homepoint_Lng + 180000000)%65536/256) + ((Homepoint_Lng + 180000000)%256);
HomePointChksum += ((Homepoint_Alt + 10000)/256 + (Homepoint_Alt + 10000)%256);
MarkerChksum = 0;
for(int i=0; i<20; i++){
MarkerChksum += (uint8_t)(Marker_Mode[i]);
MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)/16777216);
MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%16777216/65536);
MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%65536/256);
MarkerChksum += (uint8_t)((Marker_Lat[i]+90000000)%256);
MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)/16777216);
MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%16777216/65536);
MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%65536/256);
MarkerChksum += (uint8_t)((Marker_Lng[i]+180000000)%256);
MarkerChksum += (uint8_t)((Marker_Alt[i]+10000)/256);
MarkerChksum += (uint8_t)((Marker_Alt[i]+10000)%256);
MarkerChksum += (uint8_t)(Marker_Speed[i]/256);
MarkerChksum += (uint8_t)(Marker_Speed[i]%256);
}
Packet_DATA[0] = Mode1 * 16 + Alarm;
Packet_DATA[1] = MissionState;
Packet_DATA[2] = CurrentMarker;
if(Bat1 > 100) Bat1 = 100;
else if(Bat1 < 0) Bat1 = 0;
Packet_DATA[3] = Bat1;
Packet_DATA[4] = GainChksum%256;
Packet_DATA[5] = button[0] * 1 + button[1] * 2 + button[2] * 4 + button[3] * 8 + button[4] * 16;
Packet_DATA[6] = HomePointChksum%256;
Packet_DATA[7] = MarkerChksum % 256;
// DATA - GPS //
unsigned long send_gps_time = utc_time;
Packet_DATA[8] = send_gps_time / 16777216;
Packet_DATA[9] = send_gps_time % 16777216 / 65536;
Packet_DATA[10] = send_gps_time % 65536 / 256;
Packet_DATA[11] = send_gps_time % 256;
signed long send_gps_latitude = latitude + 90000000;
Packet_DATA[12] = send_gps_latitude / 16777216;
Packet_DATA[13] = send_gps_latitude % 16777216 / 65536;
Packet_DATA[14] = send_gps_latitude % 65536 / 256;
Packet_DATA[15] = send_gps_latitude % 256;
signed long send_gps_longitude = longitude + 180000000;
Packet_DATA[16] = send_gps_longitude / 16777216;
Packet_DATA[17] = send_gps_longitude % 16777216 / 65536;
Packet_DATA[18] = send_gps_longitude % 65536 / 256;
Packet_DATA[19] = send_gps_longitude % 256;
signed long send_altitude;
if ((altitude + 10000) < 0) send_altitude = -10000;
else send_altitude = (altitude + 10000);
Packet_DATA[20] = send_altitude / 256;
Packet_DATA[21] = send_altitude % 256;
Packet_DATA[22] = SatNum;
// AHRS //
unsigned long send_roll = rollx100 + 32767;
Packet_DATA[23] = send_roll / 256;
Packet_DATA[24] = send_roll % 256;
unsigned long send_pitch = pitchx100 + 32767;
Packet_DATA[25] = send_pitch / 256;
Packet_DATA[26] = send_pitch % 256;
unsigned long send_yaw = yawx100;
Packet_DATA[27] = send_yaw / 256;
Packet_DATA[28] = send_yaw % 256;
unsigned long send_roll_rate = (roll_ratex100 +32767);
Packet_DATA[29] = send_roll_rate / 256;
Packet_DATA[30] = send_roll_rate % 256;
unsigned long send_pitch_rate = (pitch_ratex100 + 32767);
Packet_DATA[31] = send_pitch_rate / 256;
Packet_DATA[32] = send_pitch_rate % 256;
unsigned long send_yaw_rate = (yaw_ratex100 + 32767);
Packet_DATA[33] = send_yaw_rate / 256;
Packet_DATA[34] = send_yaw_rate % 256;
unsigned long send_velocity_x = (VXx100 + 32767);
Packet_DATA[35] = send_velocity_x / 256;
Packet_DATA[36] = send_velocity_x % 256;
unsigned long send_velocity_y = (VYx100 + 32767);
Packet_DATA[37] = send_velocity_y / 256;
Packet_DATA[38] = send_velocity_y % 256;
unsigned long send_velocity_z = (VZx100 + 32767);
Packet_DATA[39] = send_velocity_z / 256;
Packet_DATA[40] = send_velocity_z % 256;
// CAP and PWM //
Packet_DATA[41] = (cap[0] + 127);
Packet_DATA[42] = (cap[1] + 127);
Packet_DATA[43] = (cap[2] + 127);
Packet_DATA[44] = (cap[3] + 127);
Packet_DATA[45] = (cap[4] + 127);
Packet_DATA[46] = (cap[5] + 127);
Packet_DATA[47] = (cap[6] + 127);
Packet_DATA[48] = (cap[7] + 127);
// PWM //
for (int i = 49; i <= 56; i++) {
Packet_DATA[i] = pwm[i-49];
}
// DEBUG //
Packet_DATA[57] = (int)((DEBUGx100[0]))/256;
Packet_DATA[58] = (int)((DEBUGx100[0]))%256;
Packet_DATA[59] = (int)((DEBUGx100[1]))/256;
Packet_DATA[60] = (int)((DEBUGx100[1]))%256;
Packet_DATA[61] = (int)((DEBUGx100[2]))/256;
Packet_DATA[62] = (int)((DEBUGx100[2]))%256;
Packet_DATA[63] = (int)((DEBUGx100[3]))/256;
Packet_DATA[64] = (int)((DEBUGx100[3]))%256;
Packet_DATA[65] = (int)((DEBUGx100[4]))/256;
Packet_DATA[66] = (int)((DEBUGx100[4]))%256;
Packet_DATA[67] = (int)((DEBUGx100[5]))/256;
Packet_DATA[68] = (int)((DEBUGx100[5]))%256;
Packet_DATA[69] = (int)((DEBUGx100[6]+32767))/256;
Packet_DATA[70] = (int)((DEBUGx100[6]+32767))%256;
Packet_DATA[71] = (int)((DEBUGx100[7]+32767))/256;
Packet_DATA[72] = (int)((DEBUGx100[7]+32767))%256;
// CHKSUM //
unsigned int data_checksum = 0;
for (int i = 0; i < data_length; i++) {
data_checksum += Packet_DATA[i];
}
Packet_CHKSUM[0] = data_checksum / 256;
Packet_CHKSUM[1] = data_checksum % 256;
// Each Buffer Integrate to modem_buffer ///
uint8_t modem_buffer[84];
modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
for (int i = 0; i < 73; i++) {
modem_buffer[i + 7] = Packet_DATA[i];
}
modem_buffer[80] = Packet_CHKSUM[0];
modem_buffer[81] = Packet_CHKSUM[1];
modem_buffer[82] = Packet_EOF[0];
modem_buffer[83] = Packet_EOF[1];
TRANS_BUFFER_BUSY = true;
for (int i = 0; i <= 83; i++) {
TRANS_BUF[i] = modem_buffer[i];
TRANS_BUF_LEN = 84;
// pc.putc(modem_buffer[i]);
}
}
void ROBOFRIEN_GUI::attitude_calibrate(float in_acc_bias_x, float in_acc_bias_y, float in_acc_bias_z){
// calibrate_gap_roll += rollAngle;
// calibrate_gap_pitch += pitchAngle;
cal_accel_bias[0] = in_acc_bias_x;
cal_accel_bias[1] = in_acc_bias_y;
cal_accel_bias[2] = (in_acc_bias_z);
eeprom_write(EEPROM_AHRS_ROLL_GAP_UP, (((unsigned long)(in_acc_bias_x * 10000 + 32767) / 256)));
eeprom_write(EEPROM_AHRS_ROLL_GAP_DOWN, (((unsigned long)(in_acc_bias_x * 10000 + 32767) % 256)));
eeprom_write(EEPROM_AHRS_PITCH_GAP_UP, (((unsigned long)(in_acc_bias_y * 10000 + 32767) / 256)));
eeprom_write(EEPROM_AHRS_PITCH_GAP_DOWN, (((unsigned long)(in_acc_bias_y * 10000 + 32767) % 256)));
eeprom_write(EEPROM_AHRS_YAW_GAP_UP, (((unsigned long)(in_acc_bias_z * 10000 + 32767) / 256)));
eeprom_write(EEPROM_AHRS_YAW_GAP_DOWN, (((unsigned long)(in_acc_bias_z * 10000 + 32767) % 256)));
eeprom_refresh_bool = true;
}
void ROBOFRIEN_GUI::write_compass_setting_to_eeprom(){
eeprom_write(EEPROM_AHRS_YAW_X_GAP_1, ( (uint32_t)(mag_x_avg + 32767)/256 ) );
eeprom_write(EEPROM_AHRS_YAW_X_GAP_2, ( (uint32_t)(mag_x_avg + 32767)%256 ) );
// eeprom_write(EEPROM_AHRS_YAW_X_GAP_FLOAT, ( (unsigned long)(mag_x_avg * 100)%100 ) );
// eeprom_write(EEPROM_AHRS_YAW_X_GAP_3, ( (uint32_t)(mag_x_avg * 100 + 2147483648)%65536/256 ) );
// eeprom_write(EEPROM_AHRS_YAW_X_GAP_4, ( (uint32_t)(mag_x_avg * 100 + 2147483648)%256 ) );
eeprom_write(EEPROM_AHRS_YAW_Y_GAP_1, ( (uint32_t)(mag_y_avg + 32767)/256 ) );
eeprom_write(EEPROM_AHRS_YAW_Y_GAP_2, ( (uint32_t)(mag_y_avg + 32767)%256 ) );
// eeprom_write(EEPROM_AHRS_YAW_Y_GAP_FLOAT, ( (unsigned long)(mag_y_avg * 100)%100 ) );
// eeprom_write(EEPROM_AHRS_YAW_Y_GAP_3, ( (uint32_t)(mag_y_avg * 100 + 2147483648)%65536/256 ) );
// eeprom_write(EEPROM_AHRS_YAW_Y_GAP_4, ( (uint32_t)(mag_y_avg * 100 + 2147483648)%256 ) );
eeprom_write(EEPROM_AHRS_YAW_Z_GAP_1, ( (uint32_t)(mag_z_avg + 32767)/256 ) );
eeprom_write(EEPROM_AHRS_YAW_Z_GAP_2, ( (uint32_t)(mag_z_avg + 32767)%256 ) );
eeprom_refresh_bool = true;
}
int eeprom_refresh_cnt = 0;
bool eeprom_refresh_cnt_bool = false;
void ROBOFRIEN_GUI::SET_DATA(){
if( eeprom_refresh_bool == true){
// True 일때, False로 만들고, 10초동안 true로 변경되지 않으면 그때 eeprom_refresh 실시 ( 계속 써서 시스템 망가져서 ~ )
eeprom_refresh_bool = false;
eeprom_refresh_cnt_bool = true;
eeprom_refresh_cnt = 0;
}
if( eeprom_refresh_cnt_bool == true){
eeprom_refresh_cnt ++;
if(eeprom_refresh_cnt > 10){
eeprom_refresh();
eeprom_refresh_cnt = 0;
eeprom_refresh_cnt_bool = false;
}
}
}
void ROBOFRIEN_GUI::TRANS_BUFFER(){
if(TRANS_BUFFER_BUSY == true){
if( TRANS_BUF_CNT >= TRANS_BUF_LEN){
TRANS_BUF_CNT = 0;
TRANS_BUFFER_BUSY = false;
}else{
#if PC_DEBUG
pc.putc(TRANS_BUF[TRANS_BUF_CNT]);
TRANS_BUF_CNT ++;
#else
if( (LPC_UART3->LSR & 0x20) ){
LPC_UART3->THR = TRANS_BUF[TRANS_BUF_CNT];
TRANS_BUF_CNT ++;
}
#endif
// }
}
}else{
TRANS_BUF_CNT = 0;
}
}