Kim Youngsik
/
1TEST_Flight_Protocol_v1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
1TEST_Flight_Protocol
by 
Kim Youngsik
ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp
- Committer:
- skyyoungsik
- Date:
- 2017-08-31
- Revision:
- 3:cf5a27438655
- Parent:
- 2:3fe595ba6a7e
File content as of revision 3:cf5a27438655:
#include "ROBOFRIEN_GUI.h"
#include "eeprom.h"
#include "Config.h"
Serial pc(USBTX, USBRX);
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;
int HomePointChksum,MarkerChksum;
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(38400);
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);
// --------- 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_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_ROLL_DOWN) - 32767;
limit_pitch_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_PITCH_DOWN) - 32767;
limit_yaw_rate = (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){
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;
}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];
}
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();
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();
}
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];
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();
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();
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();
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();
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();
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();
}
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_rate = ((int)Modem_DATA[4] * 256 + Modem_DATA[5]) - 32767;
limit_pitch_rate = ((int)Modem_DATA[6] * 256 + Modem_DATA[7]) - 32767;
limit_yaw_rate = ((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_rate + 32767) / 256);
eeprom_write(EEPROM_LIMIT_RATE_ROLL_DOWN, (limit_roll_rate + 32767) % 256);
eeprom_write(EEPROM_LIMIT_RATE_PITCH_UP, (limit_pitch_rate + 32767) / 256);
eeprom_write(EEPROM_LIMIT_RATE_PITCH_DOWN, (limit_pitch_rate + 32767) % 256);
eeprom_write(EEPROM_LIMIT_RATE_YAW_UP, (limit_yaw_rate + 32767) / 256);
eeprom_write(EEPROM_LIMIT_RATE_YAW_DOWN, (limit_yaw_rate + 32767) % 256);
eeprom_refresh();
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();
}
break;
}
}
}
}
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];
for (int i = 0; i <= 10; i++) {
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_rate + 32767) / 256;
Packet_DATA[5] = (limit_roll_rate + 32767) % 256;
Packet_DATA[6] = (limit_pitch_rate + 32767) / 256;
Packet_DATA[7] = (limit_pitch_rate + 32767) % 256;
Packet_DATA[8] = (limit_yaw_rate + 32767) / 256;
Packet_DATA[9] = (limit_yaw_rate + 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];
for (int i = 0; i <= 26; i++) {
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(Mode2 >= 15) Mode2 = 15;
else if(Mode2 <= 0) Mode2 = 0;
if(MissionState >= 15) MissionState = 15;
else if(MissionState <= 0) MissionState = 0;
if(CurrentMarker >= 20) CurrentMarker = 20;
else if(CurrentMarker <= 0) CurrentMarker = 0;
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 += Marker_Mode[i];
MarkerChksum += (Marker_Lat[i]+90000000)/16777216 + (Marker_Lat[i]+90000000)%16777216/65536 + (Marker_Lat[i]+90000000)%65536/256 + (Marker_Lat[i]+90000000)%256;
MarkerChksum += (Marker_Lng[i]+180000000)/16777216 + (Marker_Lng[i]+180000000)%16777216/65536 + (Marker_Lng[i]+180000000)%65536/256 + (Marker_Lng[i]+180000000)%256;
MarkerChksum += (Marker_Alt[i]+10000)/256 + (Marker_Alt[i]+10000)%256;
MarkerChksum += Marker_Speed[i]/256 + Marker_Speed[i]%256;
}
Packet_DATA[0] = Mode1 * 16 + Mode2;
Packet_DATA[1] = MissionState;
Packet_DATA[2] = CurrentMarker;
Packet_DATA[3] = Bat1;
Packet_DATA[4] = Bat2;
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]))/256;
Packet_DATA[70] = (int)((DEBUGx100[6]))%256;
Packet_DATA[71] = (int)((DEBUGx100[7]))/256;
Packet_DATA[72] = (int)((DEBUGx100[7]))%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];
for (int i = 0; i <= 83; i++) {
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();
}
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();
}