Kim Youngsik
test
Dependencies: mbed BufferedSerial ConfigFile
Revision 1:9530746906b6, committed 2018-11-28
- Comitter:
- skyyoungsik
- Date:
- Wed Nov 28 13:06:23 2018 +0000
- Parent:
- 0:3473b92e991e
- Commit message:
- test1
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ConfigFile.lib Wed Nov 28 13:06:23 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/shintamainjp/code/ConfigFile/#f6ceafabe9f8
--- a/PID.lib Tue Jun 12 01:05:50 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_GUI/GUI_Config.h Wed Nov 28 13:06:23 2018 +0000
@@ -0,0 +1,114 @@
+#pragma once
+
+#define MODEL_INFO "ROBOFRIEN FCC "
+#define FIRMWARE_INFO 1.01
+
+#define interrupts() sei()
+#define noInterrupts() cli()
+#define TO_GCS 255
+#define FROM_FCS 0
+
+#define PIN_ULTRA_TRIG A8
+#define PIN_ULTRA_ECHO A9
+#define AHRS_INTERRUPT_PIN A10 // use pin A10 on Arduino Uno & most boards
+
+
+//// EEPROM //////
+// DPN1 //
+#define EEPROM_MODEL_TYPE1 0
+#define EEPROM_MODEL_TYPE2_UP 1
+#define EEPROM_MODEL_TYPE2_DOWN 2
+
+// DPN3 //
+#define EEPROM_RECV_MIN_1 30
+#define EEPROM_RECV_MIN_2 31
+#define EEPROM_RECV_MIN_3 32
+#define EEPROM_RECV_MIN_4 33
+#define EEPROM_RECV_MIN_5 34
+#define EEPROM_RECV_MIN_6 35
+#define EEPROM_RECV_MIN_7 36
+#define EEPROM_RECV_MIN_8 37
+
+#define EEPROM_RECV_NEU_1 38
+#define EEPROM_RECV_NEU_2 39
+#define EEPROM_RECV_NEU_3 40
+#define EEPROM_RECV_NEU_4 41
+#define EEPROM_RECV_NEU_5 42
+#define EEPROM_RECV_NEU_6 43
+#define EEPROM_RECV_NEU_7 44
+#define EEPROM_RECV_NEU_8 45
+
+#define EEPROM_RECV_MAX_1 46
+#define EEPROM_RECV_MAX_2 47
+#define EEPROM_RECV_MAX_3 48
+#define EEPROM_RECV_MAX_4 49
+#define EEPROM_RECV_MAX_5 50
+#define EEPROM_RECV_MAX_6 51
+#define EEPROM_RECV_MAX_7 52
+#define EEPROM_RECV_MAX_8 53
+
+// DPN 4 //
+#define EEPROM_MOTOR_MIN_1_UP 54
+#define EEPROM_MOTOR_MIN_1_DOWN 55
+#define EEPROM_MOTOR_MIN_2_UP 56
+#define EEPROM_MOTOR_MIN_2_DOWN 57
+#define EEPROM_MOTOR_MIN_3_UP 58
+#define EEPROM_MOTOR_MIN_3_DOWN 59
+#define EEPROM_MOTOR_MIN_4_UP 60
+#define EEPROM_MOTOR_MIN_4_DOWN 61
+#define EEPROM_MOTOR_MIN_5_UP 62
+#define EEPROM_MOTOR_MIN_5_DOWN 63
+#define EEPROM_MOTOR_MIN_6_UP 64
+#define EEPROM_MOTOR_MIN_6_DOWN 65
+#define EEPROM_MOTOR_MIN_7_UP 66
+#define EEPROM_MOTOR_MIN_7_DOWN 67
+#define EEPROM_MOTOR_MIN_8_UP 68
+#define EEPROM_MOTOR_MIN_8_DOWN 69
+
+// DPN 5 //
+#define EEPROM_HEADLIGHT_PERIOD 70
+#define EEPROM_HEADLIGHT_DUTYRATE 71
+#define EEPROM_SIDELIGHT_PERIOD 72
+#define EEPROM_SIDELIGHT_DUTYRATE 73
+
+// DPN 6 //
+#define EEPROM_AHRS_ROLL_GAP_UP 74
+#define EEPROM_AHRS_ROLL_GAP_DOWN 75
+#define EEPROM_AHRS_PITCH_GAP_UP 76
+#define EEPROM_AHRS_PITCH_GAP_DOWN 77
+#define EEPROM_AHRS_YAW_GAP_UP 78
+#define EEPROM_AHRS_YAW_GAP_DOWN 79
+
+
+#define EEPROM_AHRS_YAW_X_GAP_1 80
+#define EEPROM_AHRS_YAW_X_GAP_2 81
+#define EEPROM_AHRS_YAW_Y_GAP_1 82
+#define EEPROM_AHRS_YAW_Y_GAP_2 83
+#define EEPROM_AHRS_YAW_Z_GAP_1 84
+#define EEPROM_AHRS_YAW_Z_GAP_2 85
+
+#define EEPROM_AHRS_DECLINATION_ANGLE_UP 86
+#define EEPROM_AHRS_DECLINATION_ANGLE_DOWN 87
+
+// DPN 7 //
+#define EEPROM_LIMIT_ANGLE_ROLL_UP 88
+#define EEPROM_LIMIT_ANGLE_ROLL_DOWN 89
+#define EEPROM_LIMIT_ANGLE_PITCH_UP 90
+#define EEPROM_LIMIT_ANGLE_PITCH_DOWN 91
+#define EEPROM_LIMIT_RATE_ROLL_UP 92
+#define EEPROM_LIMIT_RATE_ROLL_DOWN 93
+#define EEPROM_LIMIT_RATE_PITCH_UP 94
+#define EEPROM_LIMIT_RATE_PITCH_DOWN 95
+#define EEPROM_LIMIT_RATE_YAW_UP 96
+#define EEPROM_LIMIT_RATE_YAW_DOWN 97
+
+// DPN 8 //
+// --Gain 1 //
+int EEPROM_GAIN_P_UP[20] = {100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119};
+int EEPROM_GAIN_P_DOWN[20] = {120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139};
+int EEPROM_GAIN_D_UP[20] = {140,141,142,143,144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159};
+int EEPROM_GAIN_D_DOWN[20] = {160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175,176,177,178,179};
+int EEPROM_GAIN_I_UP[20] = {180,181,182,183,184,185,186,187,188,189,190,191,192,193,194,195,196,197,198,199};
+int EEPROM_GAIN_I_DOWN[20] = {200,201,202,203,204,205,206,207,208,209,210,211,212,213,214,215,216,217,218,219};
+
+
--- a/ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp Wed Nov 28 13:06:23 2018 +0000
@@ -1,10 +1,16 @@
#include "ROBOFRIEN_GUI.h"
+#include "GUI_Config.h"
#include "eeprom.h"
-#include "Config.h"
#include "BufferedSerial.h"
+#define PC_DEBUG 0
-BufferedSerial pc(USBTX, USBRX);
+#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;
@@ -19,11 +25,7 @@
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_ascii_trans(char* input){
- pc.printf(input); pc.printf("\r\n");
-}
void ROBOFRIEN_GUI::pc_rx_update(){
while (pc.readable()){
@@ -90,7 +92,7 @@
}
}
void ROBOFRIEN_GUI::Init(){
- pc.baud(38400);
+ pc.baud(115200);
eeprom_init();
/// EEPROM R/W ////
// -------- MODEL -------------- //
@@ -130,6 +132,8 @@
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);
@@ -140,9 +144,9 @@
// --------- 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;
+ 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++) {
@@ -184,25 +188,33 @@
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;
+ 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);
+// trans_flight_data(TO_GCS, Modem_DC_DPN);
}
else {
- trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]);
+// 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);
+// trans_empty_data(TO_GCS, Modem_DC_DPN);
switch (Modem_DC_DPN) { // Check DPN //
case 0:
break;
@@ -212,7 +224,7 @@
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();
+ eeprom_refresh_bool = true;
break;
case 3: /// RC Receiver ///
if ((Modem_DATA[0] >= 1) & (Modem_DATA[0] <= 8)) {
@@ -261,7 +273,7 @@
eeprom_write(EEPROM_RECV_MAX_8, cap_max[Modem_DATA[0] - 1] + 127);
break;
}
- eeprom_refresh();
+ eeprom_refresh_bool = true;
}
break;
case 4: // Motor - OUTPUT //
@@ -269,30 +281,44 @@
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();
+ 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();
+ 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();
+ 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();
+ 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;
@@ -305,7 +331,7 @@
eeprom_write(EEPROM_HEADLIGHT_DUTYRATE, headlight_dutyrate);
eeprom_write(EEPROM_SIDELIGHT_PERIOD, sidelight_period);
eeprom_write(EEPROM_SIDELIGHT_DUTYRATE, sidelight_dutyrate);
- eeprom_refresh();
+ eeprom_refresh_bool = true;
break;
case 6: // AHRS - ROLL , Pitch //
/// Attitude ///
@@ -327,26 +353,26 @@
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();
+ 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_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;
+ 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_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();
+ 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];
@@ -360,7 +386,7 @@
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();
+ if(gain_number == 20)eeprom_refresh_bool = true;
}
break;
}
@@ -369,6 +395,24 @@
}
}
+
+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];
@@ -398,8 +442,11 @@
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++) {
- pc.putc(modem_buffer[i]);
+ TRANS_BUF[i] = modem_buffer[i];
+ TRANS_BUF_LEN = 11;
+// pc.putc(modem_buffer[i]);
}
}
@@ -485,12 +532,12 @@
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;
+ 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:
@@ -530,8 +577,11 @@
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++) {
- pc.putc(modem_buffer[i]);
+ TRANS_BUF[i] = modem_buffer[i];
+ TRANS_BUF_LEN = 27;
+// pc.putc(modem_buffer[i]);
}
}
@@ -556,28 +606,47 @@
// 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(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 += 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;
+ 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 + Mode2;
+ 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] = Bat2;
+ 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;
@@ -658,10 +727,10 @@
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;
+ 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++) {
@@ -682,8 +751,11 @@
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++) {
- pc.putc(modem_buffer[i]);
+ TRANS_BUF[i] = modem_buffer[i];
+ TRANS_BUF_LEN = 84;
+// pc.putc(modem_buffer[i]);
}
}
@@ -699,7 +771,7 @@
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();
+ eeprom_refresh_bool = true;
}
void ROBOFRIEN_GUI::write_compass_setting_to_eeprom(){
@@ -718,5 +790,46 @@
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();
+ 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;
+ }
+}
--- a/ROBOFRIEN_GUI/ROBOFRIEN_GUI.h Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_GUI/ROBOFRIEN_GUI.h Wed Nov 28 13:06:23 2018 +0000
@@ -8,21 +8,22 @@
class ROBOFRIEN_GUI {
public:
- void pc_ascii_trans(char*);
void pc_rx_update();
bool rx_bool();
void Init();
void Refresh();
+ void SET_DATA();
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////// [H/M] HomePoint and Marker //////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
signed long Homepoint_Lat, Homepoint_Lng, Homepoint_Alt;
signed long Marker_Mode[20],Marker_Lat[20], Marker_Lng[20], Marker_Alt[20], Marker_Speed[20];
+ signed long Controller_Joystick[4], Gimbal_Joystick[3];
/////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////// [M/S] Mode and State ///////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
- int8_t Mode1, Mode2,MissionState,CurrentMarker;
- uint8_t Bat1,Bat2;
+ int8_t Mode1, Alarm,MissionState,CurrentMarker;
+ uint8_t Bat1;
bool button[5];
/////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////// [GPS] GPS //////////////////////////////////////////////
@@ -47,7 +48,7 @@
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////// [E/D] Extra & Debug /////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
- uint16_t DEBUGx100[8];
+ int32_t DEBUGx100[8];
@@ -57,24 +58,31 @@
uint8_t headlight_period, headlight_dutyrate, sidelight_period, sidelight_dutyrate;
int16_t raw_cap[8];
int16_t cap_min[8], cap_neu[8], cap_max[8];
- int motor_min[4];
+ int motor_min[6];
uint8_t Compass_Calibration_Mode;
bool attitude_configuration_bool;
float declination_angle;
int16_t limit_rollx100, limit_pitchx100;
- int32_t limit_roll_rate, limit_pitch_rate, limit_yaw_rate;
+ int32_t limit_roll_ratex100, limit_pitch_ratex100, limit_yaw_ratex100;
int gain_px100[20], gain_dx100[20], gain_ix100[20];
float mag_x_avg,mag_y_avg,mag_z_avg;
// float calibrate_gap_roll,calibrate_gap_pitch;
float cal_accel_bias[3];
uint8_t DPN_Info;
- int pwm_info[4];
+ int pwm_info[6];
void attitude_calibrate(float , float , float);
void write_compass_setting_to_eeprom();
void trans_flight_data(int id_dest, int data_parse_num);
+ void trans_configuration_data(int id_dest, int data_parse_num, int data_parse_detail_num);
+ void Trans();
+ void TRANS_BUFFER();
+ long MarkerChksum;
private:
- void trans_configuration_data(int id_dest, int data_parse_num, int data_parse_detail_num);
+ bool eeprom_refresh_bool;
void trans_empty_data(int id_dest, int data_parse_num);
+ bool TRANS_BUFFER_BUSY;
+ uint8_t TRANS_BUF[100],TRANS_BUF_LEN,TRANS_BUF_CNT;
+ int HomePointChksum;
};
#endif
\ No newline at end of file
--- a/ROBOFRIEN_SUB_FUNC/Config.h Tue Jun 12 01:05:50 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,119 +0,0 @@
-#pragma once
-
-#define MODEL_INFO "ROBOFRIEN FCC "
-#define FIRMWARE_INFO 1.01
-
-#define interrupts() sei()
-#define noInterrupts() cli()
-#define TO_GCS 255
-#define FROM_FCS 0
-
-#define PIN_ULTRA_TRIG A8
-#define PIN_ULTRA_ECHO A9
-#define AHRS_INTERRUPT_PIN A10 // use pin A10 on Arduino Uno & most boards
-
-
-//// EEPROM //////
-// DPN1 //
-#define EEPROM_MODEL_TYPE1 0
-#define EEPROM_MODEL_TYPE2_UP 1
-#define EEPROM_MODEL_TYPE2_DOWN 2
-
-// DPN3 //
-#define EEPROM_RECV_MIN_1 30
-#define EEPROM_RECV_MIN_2 31
-#define EEPROM_RECV_MIN_3 32
-#define EEPROM_RECV_MIN_4 33
-#define EEPROM_RECV_MIN_5 34
-#define EEPROM_RECV_MIN_6 35
-#define EEPROM_RECV_MIN_7 36
-#define EEPROM_RECV_MIN_8 37
-
-#define EEPROM_RECV_NEU_1 38
-#define EEPROM_RECV_NEU_2 39
-#define EEPROM_RECV_NEU_3 40
-#define EEPROM_RECV_NEU_4 41
-#define EEPROM_RECV_NEU_5 42
-#define EEPROM_RECV_NEU_6 43
-#define EEPROM_RECV_NEU_7 44
-#define EEPROM_RECV_NEU_8 45
-
-#define EEPROM_RECV_MAX_1 46
-#define EEPROM_RECV_MAX_2 47
-#define EEPROM_RECV_MAX_3 48
-#define EEPROM_RECV_MAX_4 49
-#define EEPROM_RECV_MAX_5 50
-#define EEPROM_RECV_MAX_6 51
-#define EEPROM_RECV_MAX_7 52
-#define EEPROM_RECV_MAX_8 53
-
-// DPN 4 //
-#define EEPROM_MOTOR_MIN_1_UP 54
-#define EEPROM_MOTOR_MIN_1_DOWN 55
-#define EEPROM_MOTOR_MIN_2_UP 56
-#define EEPROM_MOTOR_MIN_2_DOWN 57
-#define EEPROM_MOTOR_MIN_3_UP 58
-#define EEPROM_MOTOR_MIN_3_DOWN 59
-#define EEPROM_MOTOR_MIN_4_UP 60
-#define EEPROM_MOTOR_MIN_4_DOWN 61
-#define EEPROM_MOTOR_MIN_5_UP 62
-#define EEPROM_MOTOR_MIN_5_DOWN 63
-#define EEPROM_MOTOR_MIN_6_UP 64
-#define EEPROM_MOTOR_MIN_6_DOWN 65
-#define EEPROM_MOTOR_MIN_7_UP 66
-#define EEPROM_MOTOR_MIN_7_DOWN 67
-#define EEPROM_MOTOR_MIN_8_UP 68
-#define EEPROM_MOTOR_MIN_8_DOWN 69
-
-// DPN 5 //
-#define EEPROM_HEADLIGHT_PERIOD 70
-#define EEPROM_HEADLIGHT_DUTYRATE 71
-#define EEPROM_SIDELIGHT_PERIOD 72
-#define EEPROM_SIDELIGHT_DUTYRATE 73
-
-// DPN 6 //
-#define EEPROM_AHRS_ROLL_GAP_UP 74
-#define EEPROM_AHRS_ROLL_GAP_DOWN 75
-#define EEPROM_AHRS_PITCH_GAP_UP 76
-#define EEPROM_AHRS_PITCH_GAP_DOWN 77
-#define EEPROM_AHRS_YAW_GAP_UP 78
-#define EEPROM_AHRS_YAW_GAP_DOWN 79
-
-
-#define EEPROM_AHRS_YAW_X_GAP_1 80
-#define EEPROM_AHRS_YAW_X_GAP_2 81
-#define EEPROM_AHRS_YAW_Y_GAP_1 82
-#define EEPROM_AHRS_YAW_Y_GAP_2 83
-#define EEPROM_AHRS_YAW_Z_GAP_1 84
-#define EEPROM_AHRS_YAW_Z_GAP_2 85
-
-#define EEPROM_AHRS_DECLINATION_ANGLE_UP 86
-#define EEPROM_AHRS_DECLINATION_ANGLE_DOWN 87
-
-// DPN 7 //
-#define EEPROM_LIMIT_ANGLE_ROLL_UP 88
-#define EEPROM_LIMIT_ANGLE_ROLL_DOWN 89
-#define EEPROM_LIMIT_ANGLE_PITCH_UP 90
-#define EEPROM_LIMIT_ANGLE_PITCH_DOWN 91
-#define EEPROM_LIMIT_RATE_ROLL_UP 92
-#define EEPROM_LIMIT_RATE_ROLL_DOWN 93
-#define EEPROM_LIMIT_RATE_PITCH_UP 94
-#define EEPROM_LIMIT_RATE_PITCH_DOWN 95
-#define EEPROM_LIMIT_RATE_YAW_UP 96
-#define EEPROM_LIMIT_RATE_YAW_DOWN 97
-
-// DPN 8 //
-// --Gain 1 //
-int EEPROM_GAIN_P_UP[20] = {100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119};
-int EEPROM_GAIN_P_DOWN[20] = {120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139};
-int EEPROM_GAIN_D_UP[20] = {140,141,142,143,144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159};
-int EEPROM_GAIN_D_DOWN[20] = {160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175,176,177,178,179};
-int EEPROM_GAIN_I_UP[20] = {180,181,182,183,184,185,186,187,188,189,190,191,192,193,194,195,196,197,198,199};
-int EEPROM_GAIN_I_DOWN[20] = {200,201,202,203,204,205,206,207,208,209,210,211,212,213,214,215,216,217,218,219};
-
-
-const int PIN_BLDC_PWM1 = 46; // PL3 //
-const int PIN_BLDC_PWM2 = 45; // PL4 //
-const int PIN_BLDC_PWM3 = 44; // PL5 //
-const int PIN_BLDC_PWM4 = 10; // PB4 //
-const int PIN_BLDC_PWM5 = 9; // PH6 //
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/Function.h Wed Nov 28 13:06:23 2018 +0000
@@ -0,0 +1,61 @@
+#define M_PI 3.141592
+
+double deg2rad(double deg) {
+ return deg * (M_PI/180.0);
+}
+
+
+double getDistance(double lat1,double lon1,double lat2,double lon2) {
+ double R = 6371; // Radius of the earth in km
+ double dLat = deg2rad(lat2-lat1); // deg2rad below
+ double dLon = deg2rad(lon2-lon1);
+ double a =
+ sin(dLat/2) * sin(dLat/2) +
+ cos(deg2rad(lat1)) * cos(deg2rad(lat2)) *
+ sin(dLon/2) * sin(dLon/2)
+ ;
+ double c = 2 * atan2(sqrt(a), sqrt(1-a));
+ double d = R * c; // Distance in km
+ d = d * 1000; // Distance in meter
+ return d;
+}
+
+double MIN_MAX(double input, double min, double max){
+ double output;
+ if( input < min) output = min;
+ else if(input > max) output = max;
+ else output = input;
+
+ return output;
+}
+
+
+float bearing(float lat1,float lon1,float lat2,float lon2){
+
+ float teta1 = (lat1)*M_PI/180.0;
+ float teta2 = (lat2)*M_PI/180.0;;
+// float delta1 = (lat2-lat1)*M_PI/180.0;;
+ float delta2 = (lon2-lon1)*M_PI/180.0;;
+
+ //==================Heading Formula Calculation================//
+
+ float y = sin(delta2) * cos(teta2);
+ float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
+ float brng = atan2(y,x);
+ brng = (brng)*180.0/M_PI;// radians to degrees
+ brng = ( ((int)brng + 360) % 360 );
+// if(brng > 180) brng -= 360;
+ if(brng > 360) brng -= 360;
+ else if(brng < 0) brng += 360;
+ return brng;
+}
+
+float ABSOL(float input){
+ float output;
+ if( input > 0){
+ output = input;
+ }else{
+ output = -input;
+ }
+ return output;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_BAT.cpp Wed Nov 28 13:06:23 2018 +0000
@@ -0,0 +1,22 @@
+#include "ROBOFRIEN_BAT.h"
+
+AnalogIn BAT_ANALOG(A0);
+
+#define BAT_1CELL_MIN 3.8
+#define BAT_1CELL_MAX 4.1
+#define DC_DC_REFERENCE 3.3
+#define REGISTER_RATIO 11
+#define BAT_LPF 0.9
+
+void ROBOFRIEN_BAT::Init(int cell_info){
+ BAT_MIN = BAT_1CELL_MIN * cell_info;
+ BAT_MAX = BAT_1CELL_MAX * cell_info;
+}
+
+float ROBOFRIEN_BAT::update(){
+ float BAT_PERCENT = 0;
+ ORIGIN_DATA = BAT_ANALOG;
+ BAT_PERCENT = ((BAT_ANALOG*DC_DC_REFERENCE*REGISTER_RATIO)-BAT_MIN)/(BAT_MAX-BAT_MIN)*100;
+ LPF_BAT_DATA = LPF_BAT_DATA * BAT_LPF + BAT_PERCENT * (1-BAT_LPF);
+ return BAT_PERCENT;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_BAT.h Wed Nov 28 13:06:23 2018 +0000
@@ -0,0 +1,18 @@
+#ifndef MBED_ROBOFRIEN_BAT_H
+#define MBED_ROBOFRIEN_BAT_H
+
+#include "mbed.h"
+
+
+
+class ROBOFRIEN_BAT {
+public:
+ void Init(int);
+ float update( );
+ float ORIGIN_DATA;
+private:
+ float BAT_MIN,BAT_MAX;
+ float LPF_BAT_DATA;
+};
+
+#endif
\ No newline at end of file
--- a/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_LED.cpp Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_LED.cpp Wed Nov 28 13:06:23 2018 +0000
@@ -2,13 +2,36 @@
#include "millis.h"
DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
DigitalOut myled4(LED4);
void ROBOFRIEN_LED::Init(){
myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
myled4 = 0;
}
-
+void ROBOFRIEN_LED::ALARM(uint8_t ALARM_DATA){
+ switch(ALARM_DATA){
+ case 0:{ // NORMAL
+ myled2 = 0;
+ myled3 = 0;
+ }break;
+ case 1:{ // ALARM1
+ myled2 = 1;
+ myled3 = 0;
+ }break;
+ case 2:{ // ALARM2
+ myled2 = 0;
+ myled3 = 1;
+ }break;
+ case 3:{ // ALARM3
+ myled2 = 1;
+ myled3 = 1;
+ }break;
+ }
+}
int headlight_millis, sidelight_millis;
void ROBOFRIEN_LED::update(uint8_t HD_PD, uint8_t HD_DR, uint8_t SD_PD, uint8_t SD_DR){
//////// HEAD LIGHT /////////////
--- a/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_LED.h Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_LED.h Wed Nov 28 13:06:23 2018 +0000
@@ -10,6 +10,7 @@
public:
void Init();
void update(uint8_t, uint8_t, uint8_t, uint8_t);
+ void ALARM(uint8_t);
private:
--- a/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_MOTOR.cpp Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_MOTOR.cpp Wed Nov 28 13:06:23 2018 +0000
@@ -1,14 +1,14 @@
#include "ROBOFRIEN_MOTOR.h"
-float PWM_TRIM[6] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
float PWM_MIN = 1/3.0f;
float PWM_MAX = 2/3.0f;
-PwmOut pwm1(p21);
-PwmOut pwm2(p22);
-PwmOut pwm3(p23);
-PwmOut pwm4(p24);
-PwmOut pwm5(p25);
-PwmOut pwm6(p26);
+
+PwmOut pwm1(p21);
+PwmOut pwm2(p22);
+PwmOut pwm3(p23);
+PwmOut pwm4(p24);
+PwmOut pwm5(p25);
+PwmOut pwm6(p26);
void ROBOFRIEN_MOTOR::OFF(){
pwm1.write(PWM_MIN);
@@ -36,14 +36,14 @@
// Motor Power : 0 ~ 10000
float PWM_DATA[6];
for(int i=0; i<6; i++){
- PWM_DATA[i] = PWM_MIN + PWM_TRIM[i] + (1/3.0)*(Value[i]/10000.0);
+ PWM_DATA[i] = PWM_MIN + (PWM_MAX - PWM_MIN) * (Value[i]/10000.0);
if(PWM_DATA[i] > PWM_MAX)PWM_DATA[i] = PWM_MAX;
}
- pwm1.period(PWM_DATA[0]);
- pwm2.period(PWM_DATA[1]);
- pwm3.period(PWM_DATA[2]);
- pwm4.period(PWM_DATA[3]);
- pwm5.period(PWM_DATA[4]);
- pwm6.period(PWM_DATA[5]);
+ pwm1.write(PWM_DATA[0]);
+ pwm2.write(PWM_DATA[1]);
+ pwm3.write(PWM_DATA[2]);
+ pwm4.write(PWM_DATA[3]);
+ pwm5.write(PWM_DATA[4]);
+ pwm6.write(PWM_DATA[5]);
}
--- a/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_MOTOR.h Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_MOTOR.h Wed Nov 28 13:06:23 2018 +0000
@@ -11,7 +11,7 @@
void Init();
void update();
void OFF();
- int16_t Value[6];
+ int16_t Value[6]; // 0 ~ 10000
private:
};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_PID.cpp Wed Nov 28 13:06:23 2018 +0000
@@ -0,0 +1,37 @@
+#include "ROBOFRIEN_PID.h"
+
+
+#define OUTPUT_LIMIT 3000
+#define RAT_INTEGER_LIMIT 500
+void ROBOFRIEN_PID::Init(){
+ RAT_INTEGER[0] = 0;
+ RAT_INTEGER[1] = 0;
+ RAT_INTEGER[2] = 0;
+}
+
+void ROBOFRIEN_PID::update(){
+ output_Data[0] = err_Rate[0] * RAT_KP[0]; // P GAIN
+ RAT_INTEGER[0] += err_Rate[0]/100.0 * RAT_KI[0];
+ output_Data[0] += RAT_INTEGER[0];
+ if(output_Data[0] > OUTPUT_LIMIT) output_Data[0] = OUTPUT_LIMIT;
+ else if(output_Data[0] < -OUTPUT_LIMIT) output_Data[0] = -OUTPUT_LIMIT;
+
+ output_Data[1] = err_Rate[1] * RAT_KP[1]; // P GAIN
+ RAT_INTEGER[1] += err_Rate[1]/100.0 * RAT_KI[1];
+ output_Data[1] += RAT_INTEGER[1];
+ if(output_Data[1] > OUTPUT_LIMIT) output_Data[1] = OUTPUT_LIMIT;
+ else if(output_Data[1] < -OUTPUT_LIMIT) output_Data[1] = -OUTPUT_LIMIT;
+
+ output_Data[2] = err_Rate[2] * RAT_KP[2]; // P GAIN
+ RAT_INTEGER[2] += err_Rate[2]/100.0 * RAT_KI[2];
+ output_Data[2] += RAT_INTEGER[2];
+ if(output_Data[2] > OUTPUT_LIMIT) output_Data[2] = OUTPUT_LIMIT;
+ else if(output_Data[2] < -OUTPUT_LIMIT) output_Data[2] = -OUTPUT_LIMIT;
+}
+
+void ROBOFRIEN_PID::RAT_INTEGER_RESET(){
+ RAT_INTEGER[0] = 0;
+ RAT_INTEGER[1] = 0;
+ RAT_INTEGER[2] = 0;
+}
+
--- a/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_PID.h Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_PID.h Wed Nov 28 13:06:23 2018 +0000
@@ -10,10 +10,13 @@
public:
void Init();
void update( );
- float err_Rate[3],err_Angle[3];
+ float err_Rate[3];
float output_Data[3];
- void ROLL_RATE_PID_RUN();
+ float RAT_KP[3], RAT_KD[3], RAT_KI[3];
+ float ANG_KP[3], ANG_KD[3], ANG_KI[3];
+ void RAT_INTEGER_RESET();
private:
+ int16_t RAT_INTEGER[3];
};
#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_SBUS.cpp Wed Nov 28 13:06:23 2018 +0000
@@ -0,0 +1,115 @@
+#include "ROBOFRIEN_SBUS.h"
+#include "millis.h"
+#include "BufferedSerial.h"
+
+BufferedSerial SBUS_(p13,p14);
+
+#define CAP_MIN 487
+#define CAP_NEU 1024
+#define CAP_MAX 1561
+bool reverse_bool[16] = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false};
+
+void ROBOFRIEN_SBUS::Init(){
+ SBUS_.baud(100000);
+ SBUS_.format(8, Serial::Even, 2);
+ CONTROLLER_STATE = SIGNAL_OFF;
+}
+
+uint8_t BIT_REVERSE(uint8_t INPUT){
+ uint8_t OUTPUT;
+ OUTPUT = INPUT << 7;
+ OUTPUT |= ( ((INPUT >> 1)&0b00000001) << 6 );
+ OUTPUT |= ( ((INPUT >> 2)&0b00000001) << 5 );
+ OUTPUT |= ( ((INPUT >> 3)&0b00000001) << 4 );
+ OUTPUT |= ( ((INPUT >> 4)&0b00000001) << 3 );
+ OUTPUT |= ( ((INPUT >> 5)&0b00000001) << 2 );
+ OUTPUT |= ( ((INPUT >> 6)&0b00000001) << 1 );
+ OUTPUT |= ( ((INPUT >> 7)&0b00000001) << 0 );
+ return OUTPUT;
+}
+uint16_t BIT11_REVERSE(uint16_t INPUT){
+ uint16_t OUTPUT;
+ INPUT = INPUT & 0b0000011111111111;
+ OUTPUT = INPUT << 10;
+ OUTPUT |= ( ((INPUT >> 1)&0b0000000000000001) << 9 );
+ OUTPUT |= ( ((INPUT >> 2)&0b0000000000000001) << 8 );
+ OUTPUT |= ( ((INPUT >> 3)&0b0000000000000001) << 7 );
+ OUTPUT |= ( ((INPUT >> 4)&0b0000000000000001) << 6 );
+ OUTPUT |= ( ((INPUT >> 5)&0b0000000000000001) << 5 );
+ OUTPUT |= ( ((INPUT >> 6)&0b0000000000000001) << 4 );
+ OUTPUT |= ( ((INPUT >> 7)&0b0000000000000001) << 3 );
+ OUTPUT |= ( ((INPUT >> 8)&0b0000000000000001) << 2 );
+ OUTPUT |= ( ((INPUT >> 9)&0b0000000000000001) << 1 );
+ OUTPUT |= ( ((INPUT >>10)&0b0000000000000001) << 0 );
+ OUTPUT = OUTPUT & 0b0000011111111111;
+ return OUTPUT;
+
+}
+
+void ROBOFRIEN_SBUS::update(){
+ while( SBUS_.readable() > 0){
+ SBUS_ID[2] = SBUS_ID[1];
+ SBUS_ID[1] = SBUS_ID[0];
+ SBUS_ID[0] = BIT_REVERSE(SBUS_.getc());
+ SBUS_CNT ++;
+ if( start_bool == false ){
+ if( ( SBUS_ID[1] == 0b00000000 ) & ( SBUS_ID[0] == 0b11110000 ) ){
+ start_bool = true;
+ check_cnt = SBUS_CNT;
+ SBUS_CNT = 0;
+ }
+ }else{
+ SBUS_BUFF[SBUS_CNT] = SBUS_ID[0];
+ if(SBUS_CNT == 24){
+ start_bool = false;
+ channel[ 0] = ( (uint16_t)(SBUS_BUFF[ 1]&0b11111111)<<3 | (uint16_t)((uint16_t)SBUS_BUFF[ 2]&0b11100000)>>5 );
+ channel[ 1] = ( (int16_t)(SBUS_BUFF[ 2]&0b00011111)<<6 | (int16_t)(SBUS_BUFF[ 3]&0b11111100)>>2 );
+ channel[ 2] = ( (int16_t)(SBUS_BUFF[ 3]&0b00000011)<<9 | (int16_t)(SBUS_BUFF[ 4]&0b11111111)<<1 | (int16_t)(SBUS_BUFF[5]&0b10000000)>>7 );
+ channel[ 3] = ( (int16_t)(SBUS_BUFF[ 5]&0b01111111)<<4 | (int16_t)(SBUS_BUFF[ 6]&0b11110000)>>4 );
+ channel[ 4] = ( (int16_t)(SBUS_BUFF[ 6]&0b00001111)<<7 | (int16_t)(SBUS_BUFF[ 7]&0b11111110)>>1 );
+ channel[ 5] = ( (int16_t)(SBUS_BUFF[ 7]&0b00000001)<<10| (int16_t)(SBUS_BUFF[ 8]&0b11111111)<<2 | (int16_t)(SBUS_BUFF[9]&0b11000000)>>6 );
+ channel[ 6] = ( (int16_t)(SBUS_BUFF[ 9]&0b00111111)<<5 | (int16_t)(SBUS_BUFF[10]&0b11111000)>>3 );
+ channel[ 7] = ( (int16_t)(SBUS_BUFF[10]&0b00000111)<<8 | (int16_t)(SBUS_BUFF[11]&0b11111111)>>0 );
+ channel[ 8] = ( (int16_t)(SBUS_BUFF[11]&0b11111111)<<3 | (int16_t)(SBUS_BUFF[12]&0b11100000)>>5 );
+ channel[ 9] = ( (int16_t)(SBUS_BUFF[12]&0b00011111)<<6 | (int16_t)(SBUS_BUFF[13]&0b11111100)>>2 );
+ channel[10] = ( (int16_t)(SBUS_BUFF[13]&0b00000011)<<9 | (int16_t)(SBUS_BUFF[14]&0b11111111)<<1 | (int16_t)(SBUS_BUFF[15]&0b10000000)>>7 );
+ channel[11] = ( (int16_t)(SBUS_BUFF[15]&0b01111111)<<4 | (int16_t)(SBUS_BUFF[16]&0b11110000)>>4 );
+ channel[12] = ( (int16_t)(SBUS_BUFF[16]&0b00001111)<<7 | (int16_t)(SBUS_BUFF[17]&0b11111110)>>1 );
+ channel[13] = ( (int16_t)(SBUS_BUFF[17]&0b00000001)<<10| (int16_t)(SBUS_BUFF[18]&0b11111111)<<2 | (int16_t)(SBUS_BUFF[19]&0b11000000)>>6 );
+ channel[14] = ( (int16_t)(SBUS_BUFF[19]&0b00111111)<<5 | (int16_t)(SBUS_BUFF[20]&0b11111000)>>3 );
+ channel[15] = ( (int16_t)(SBUS_BUFF[20]&0b00000111)<<8 | (int16_t)(SBUS_BUFF[21]&0b11111111)>>0 );
+ for(int i=0; i<16; i++){
+ channel[ i] = BIT11_REVERSE(channel[ i]);
+ if( channel[i] < CAP_MIN ) channel[i] = CAP_MIN;
+ else if( channel[i] > CAP_MAX ) channel[i] = CAP_MAX;
+ channel[i] = (channel[i] - 1024) * 1000 / 537.0;
+ // 487, 1024, 1561 // RANGE : -1000 ~ 1000
+ }
+ if( channel[4] < -500 ){ // Motor OFF
+ CONTROLLER_STATE = MOTOR_OFF;
+ }else if( channel[4] < 500) { // Manual Mode
+ if( channel[7] < -500){ // Manual - Attitude Mode
+ if( channel[2] < -950){
+ CONTROLLER_STATE = MOTOR_OFF;
+ }else{
+ CONTROLLER_STATE = MANUAL_ATTITUDE;
+ }
+ }else if( channel[7] < 500){ // Manual - Velocity Mode
+ CONTROLLER_STATE = MANUAL_VELOCITY;
+ }else{ // Manual - Position Mode
+ CONTROLLER_STATE = MANUAL_POSITION;
+ }
+ }else{ // Auto Flight Mode
+ CONTROLLER_STATE = AUTO_FLIGHT;
+ if( channel[ 5] < -500){
+ HOMEPOINT_BOOL = false;
+ }else if( channel[ 5] > 500){
+ HOMEPOINT_BOOL = true;
+ }else{
+ HOMEPOINT_BOOL = false;
+ }
+ }
+ }
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_SBUS.h Wed Nov 28 13:06:23 2018 +0000
@@ -0,0 +1,38 @@
+#ifndef MBED_ROBOFRIEN_SBUS_H
+#define MBED_ROBOFRIEN_SBUS_H
+
+#include "mbed.h"
+
+
+#define SIGNAL_OFF 0
+#define MOTOR_OFF 1
+#define MANUAL_ATTITUDE 2
+#define MANUAL_VELOCITY 3
+#define MANUAL_POSITION 4
+#define AUTO_FLIGHT 5
+#define HOME_POINT 6
+
+class ROBOFRIEN_SBUS {
+public:
+ void Init();
+ void update();
+ bool end_bool,start_bool;
+ bool read_check;
+ int16_t channel[16]; // Range : -1000 ~ 1000
+ int check_cnt;
+ uint8_t CONTROLLER_STATE;
+ /* **** CONTROL STATE **** */
+ // 0 = SIGNAL OFF
+ // 1 = MOTOR OFF
+ // 2 = MANUAL - ATTITUDE
+ // 3 = MANUAL - VELOCITY
+ // 4 = MANUAL - POSITION
+ // 5 = AUTO FLIGHT
+ bool HOMEPOINT_BOOL;
+private:
+ uint8_t SBUS_CNT;
+ uint8_t SBUS_ID[3];
+ uint8_t SBUS_BUFF[25];
+};
+
+#endif
\ No newline at end of file
--- a/ROBOFRIEN_SUB_FUNC/ROBOFRIE_PID.cpp Tue Jun 12 01:05:50 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,29 +0,0 @@
-#include "ROBOFRIEN_PID.h"
-#include "PID.h"
-
-#define RATE 0.01
-PID ROLL_RATE_PID(10.0, 0.0, 0.0, RATE);
-
-void ROBOFRIEN_PID::Init(){
- ROLL_RATE_PID.setInputLimits(0.0, 10.0);
- ROLL_RATE_PID.setOutputLimits(0.0, 1.0);
- ROLL_RATE_PID.setMode(AUTO_MODE);
- ROLL_RATE_PID.setSetPoint(err_Angle[0]);
-}
-
-void ROBOFRIEN_PID::ROLL_RATE_PID_RUN(){
- if(err_Rate[0] >= 0){
- ROLL_RATE_PID.setProcessValue(err_Rate[0]);
- output_Data[0] = ROLL_RATE_PID.compute();
- }
- else{
- ROLL_RATE_PID.setProcessValue(-err_Rate[0]);
- output_Data[0] = -ROLL_RATE_PID.compute();
- }
-}
-
-void ROBOFRIEN_PID::update(){
- ROLL_RATE_PID_RUN();
-}
-
-
--- a/ROBOFRIEN_SUB_FUNC/eeprom.h Tue Jun 12 01:05:50 2018 +0000
+++ b/ROBOFRIEN_SUB_FUNC/eeprom.h Wed Nov 28 13:06:23 2018 +0000
@@ -1,33 +1,64 @@
-#define eeprom_length 256
+#define eeprom_length 255
LocalFileSystem local("local"); // Create the local filesystem under the name "local"
volatile int eeprom_address[eeprom_length],eeprom_data[eeprom_length];
void eeprom_init(){
+ int eeprom_chksum;
FILE *file = fopen("/local/eeprom.txt", "r"); // Open "out.txt" on the local file system for writing
if(file == NULL){
FILE *file = fopen("/local/eeprom.txt", "w"); // Write "out.txt ~~//
for(int i=0; i<eeprom_length; i++){
fprintf(file, "%d\t%d\r\n", i, 0);
+ wait(0.0001);
}
- fclose(file);
}else{
for(int i=0; i<eeprom_length; i++) {eeprom_address[i] = 0; eeprom_data[i] = 0; }
for(int i=0; i<eeprom_length; i++){
fscanf(file,"%d\t%d\r\n",&eeprom_address[i], &eeprom_data[i]);
- }
+ wait(0.0001);
+ }
+ int tmp;
+ fscanf(file,"%d\t%d\t\n",&tmp, &eeprom_chksum);
}
fclose(file);
+ int cal_eeprom_chksum = 0;
+ for(int i=0; i<eeprom_length; i++){
+ cal_eeprom_chksum += eeprom_data[i];
+ }
+ if( cal_eeprom_chksum != eeprom_chksum){
+ while(1){
+ DigitalOut myled1(LED1);
+ DigitalOut myled2(LED2);
+ DigitalOut myled3(LED3);
+ DigitalOut myled4(LED4);
+ myled1 = 1; myled2 = 1; myled3 = 1; myled4 = 1;
+ wait(1);
+ myled1 = 0; myled2 = 0; myled3 = 0; myled4 = 0;
+ wait(1);
+ }
+ }
}
-void eeprom_refresh(){
+bool eeprom_refresh(){
/// Write Data to EEPROM //
+ bool OUTPUT_VALUE = false;
+ int eeprom_chksum = 0;
+ __disable_irq();
FILE *file = fopen("/local/eeprom.txt", "w"); // Open "out.txt" on the local file system for writing
- for(int i=0; i<eeprom_length; i++){
- fprintf(file,"%d\t%d\r\n",i, eeprom_data[i]);
- }
- fclose(file);
+// if(file != NULL){
+ OUTPUT_VALUE = true;
+ for(int i=0; i<eeprom_length; i++){
+ fprintf(file,"%d\t%d\r\n",i, eeprom_data[i]);
+ wait(0.0001);
+ eeprom_chksum += eeprom_data[i];
+ }
+ fprintf(file,"%d\t%d",eeprom_length,eeprom_chksum);
+// }
+ fclose(file);
+ __enable_irq();
+ return OUTPUT_VALUE;
}
void eeprom_write(int addr, int data){
@@ -41,10 +72,3 @@
}
-void eeprom_reset(){
- FILE *file = fopen("/local/eeprom.txt", "w"); // Open "out.txt" on the local file system for writing
- for(int i=0; i<256; i++){
- fprintf(file, "%d\t%d\r\n", i,0);
- }
- fclose(file);
-}
--- a/SPATIAL/SPATIAL.cpp Tue Jun 12 01:05:50 2018 +0000
+++ b/SPATIAL/SPATIAL.cpp Wed Nov 28 13:06:23 2018 +0000
@@ -7,6 +7,18 @@
BufferedSerial SPATIAL_UART(p28, p27); // tx, rx
+/* ****** 100Hz Receive ********** */
+// Packet ID : 42, Length : 12 --> Angural Velocity X,Y,Z (rad/s)
+// Packet ID : 39, Length : 12 --> Roll, Pitch, Yaw (rad)
+
+/* ****** 10Hz Receive *********** */
+// Packet ID : 35, Length = 12 --> NED Velocity X, Y, Z (m/s)
+// Packet ID : 32, Length = 24 --> System State Packet
+
+/* ****** 1Hz Receive *********** */
+// Packet ID : 22, Length = 14 --> Time Packet
+// Packet ID : 30, Length = 13 --> Satellites Packet
+
//float 이랑 double 값뽑을때 쓰는거
union FP8
{
@@ -30,7 +42,12 @@
return crc;
}
-void SPATIAL::Init(){
+void SPATIAL::Init(int sensor_wait_input){
+ if( sensor_wait_input == 1){
+ SENSOR_WAIT_BOOL = true;
+ }else{
+ SENSOR_WAIT_BOOL = false;
+ }
SPATIAL_UART.baud(115200);
}
//이것은 SPATIAL에서 전송되는 각종 데이터를 받는 부분이다.
@@ -39,142 +56,281 @@
while(SPATIAL_UART.readable() > 0)
{
SPATIAL_ID[4] = SPATIAL_ID[3]; SPATIAL_ID[3] = SPATIAL_ID[2]; SPATIAL_ID[2] = SPATIAL_ID[1]; SPATIAL_ID[1] = SPATIAL_ID[0]; SPATIAL_ID[0] = SPATIAL_UART.getc();
- if(SPATIAL_ID_20_BOOL == true){
- SPATIAL_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
- if(SPATIAL_BUF_CNT >= 100){ SPATIAL_ID_20_BOOL = false; SPATIAL_ID_20_PARSING_BOOL = true;}
+ if((SPATIAL_ID_39_BOOL == true)|(SPATIAL_ID_42_BOOL == true)|(SPATIAL_ID_35_BOOL == true)|(SPATIAL_ID_32_BOOL == true)|(SPATIAL_ID_22_BOOL == true)|(SPATIAL_ID_30_BOOL == true)){
+// SPATIAL_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+ if(SPATIAL_ID_39_BOOL == true) {
+ SPATIAL_39_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+ if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_39_BOOL = false; SPATIAL_ID_39_PARSING_BOOL = true;}
+ }else if(SPATIAL_ID_42_BOOL == true) {
+ SPATIAL_42_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+ if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_42_BOOL = false; SPATIAL_ID_42_PARSING_BOOL = true;}
+ }else if(SPATIAL_ID_35_BOOL == true) {
+ SPATIAL_35_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+ if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_35_BOOL = false; SPATIAL_ID_35_PARSING_BOOL = true;}
+ }else if(SPATIAL_ID_32_BOOL == true) {
+ SPATIAL_32_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+ if(SPATIAL_BUF_CNT >= 24){ SPATIAL_ID_32_BOOL = false; SPATIAL_ID_32_PARSING_BOOL = true;}
+ }else if(SPATIAL_ID_22_BOOL == true) {
+ SPATIAL_22_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+ if(SPATIAL_BUF_CNT >= 14){ SPATIAL_ID_22_BOOL = false; SPATIAL_ID_22_PARSING_BOOL = true;}
+ }else if(SPATIAL_ID_30_BOOL == true) {
+ SPATIAL_30_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+ if(SPATIAL_BUF_CNT >= 13){ SPATIAL_ID_30_BOOL = false; SPATIAL_ID_30_PARSING_BOOL = true;}
+ }
SPATIAL_BUF_CNT ++;
}
else{
- if((SPATIAL_ID[3] == 20) & (SPATIAL_ID[2] == 100)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+ if((SPATIAL_ID[3] == 39) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+ {
+ //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+ if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+ {
+ SPATIAL_ID_39_BOOL = true;
+ CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+ SPATIAL_BUF_CNT = 0;
+ }
+ }
+ else if((SPATIAL_ID[3] == 42) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
{
//첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
{
- SPATIAL_ID_20_BOOL = true;
+ SPATIAL_ID_42_BOOL = true;
+ CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+ SPATIAL_BUF_CNT = 0;
+ }
+ }
+ else if((SPATIAL_ID[3] == 35) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+ {
+ //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+ if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+ {
+ SPATIAL_ID_35_BOOL = true;
+ CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+ SPATIAL_BUF_CNT = 0;
+ }
+ }
+ else if((SPATIAL_ID[3] == 32) & (SPATIAL_ID[2] == 24)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+ {
+ //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+ if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+ {
+ SPATIAL_ID_32_BOOL = true;
+ CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+ SPATIAL_BUF_CNT = 0;
+ }
+ }
+ else if((SPATIAL_ID[3] == 22) & (SPATIAL_ID[2] == 14)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+ {
+ //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+ if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+ {
+ SPATIAL_ID_22_BOOL = true;
+ CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+ SPATIAL_BUF_CNT = 0;
+ }
+ }
+ else if((SPATIAL_ID[3] == 30) & (SPATIAL_ID[2] == 13)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+ {
+ //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+ if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+ {
+ SPATIAL_ID_30_BOOL = true;
CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
SPATIAL_BUF_CNT = 0;
}
}
}
}
+ SPATIAL_ID_42_Parsing();
+ SPATIAL_ID_39_Parsing();
+ SPATIAL_ID_35_Parsing();
+ SPATIAL_ID_32_Parsing();
+ SPATIAL_ID_22_Parsing();
+ SPATIAL_ID_30_Parsing();
+}
+
+void SPATIAL::SPATIAL_ID_22_Parsing(){
+ if(SPATIAL_ID_22_PARSING_BOOL == true){
+ SPATIAL_ID_22_PARSING_BOOL = false;
+ if(CRC_INPUT == calculate_crc16(SPATIAL_22_BUF,14)){
+ UNIX_TIME = (uint32_t)SPATIAL_22_BUF[0] + (uint32_t)SPATIAL_22_BUF[13]*100 + (uint32_t)SPATIAL_22_BUF[12]*10000 + (uint32_t)SPATIAL_22_BUF[11]*1000000;
+ }
+ }
+}
+void SPATIAL::SPATIAL_ID_30_Parsing(){
+ if(SPATIAL_ID_30_PARSING_BOOL == true){
+ SPATIAL_ID_30_PARSING_BOOL = false;
+// SATELLITE_NUM ++;
+// if(CRC_INPUT == calculate_crc16(SPATIAL_30_BUF,13)){
+ SATELLITE_NUM = SPATIAL_30_BUF[8] + SPATIAL_30_BUF[9] + SPATIAL_30_BUF[10] + SPATIAL_30_BUF[11] + SPATIAL_30_BUF[12];
+// }
+ }
}
-void SPATIAL::SPATIAL_ID_20_Parsing() //데이터 파싱을 실시함 여기서.
-{
- if(SPATIAL_ID_20_PARSING_BOOL == true)
- {
- SPATIAL_ID_20_PARSING_BOOL = false;
+void SPATIAL::SPATIAL_ID_35_Parsing(){
+ if(SPATIAL_ID_35_PARSING_BOOL == true){
+ SPATIAL_ID_35_PARSING_BOOL = false;
+ if(CRC_INPUT == calculate_crc16(SPATIAL_35_BUF,12)){
+ FP4 SPATIAL_Vx; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
+ SPATIAL_Vx.DATA4[0] = SPATIAL_35_BUF[0];
+ SPATIAL_Vx.DATA4[1] = SPATIAL_35_BUF[1];
+ SPATIAL_Vx.DATA4[2] = SPATIAL_35_BUF[2];
+ SPATIAL_Vx.DATA4[3] = SPATIAL_35_BUF[3];
+ Vx = SPATIAL_Vx.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
+
+ FP4 SPATIAL_Vy; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
+ SPATIAL_Vy.DATA4[0] = SPATIAL_35_BUF[4];
+ SPATIAL_Vy.DATA4[1] = SPATIAL_35_BUF[5];
+ SPATIAL_Vy.DATA4[2] = SPATIAL_35_BUF[6];
+ SPATIAL_Vy.DATA4[3] = SPATIAL_35_BUF[7];
+ Vy = SPATIAL_Vy.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
+
+ FP4 SPATIAL_Vz; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
+ SPATIAL_Vz.DATA4[0] = SPATIAL_35_BUF[8];
+ SPATIAL_Vz.DATA4[1] = SPATIAL_35_BUF[9];
+ SPATIAL_Vz.DATA4[2] = SPATIAL_35_BUF[10];
+ SPATIAL_Vz.DATA4[3] = SPATIAL_35_BUF[11];
+ Vz = SPATIAL_Vz.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
+ Vz = -Vz;
+
+ /* ********** ECEF to Compass Heading ***** */
+ float B_Vx,B_Vy;
+ B_Vx = Vx * cos(YAW*M_PI/180.0) + Vy * sin(YAW*M_PI/180.0);
+ B_Vy = Vy * cos(YAW*M_PI/180.0) - Vx * sin(YAW*M_PI/180.0);
+ Vx = B_Vx;
+ Vy = B_Vy;
+ }
+ }
+}
+void SPATIAL::SPATIAL_ID_32_Parsing(){
+ if(SPATIAL_ID_32_PARSING_BOOL == true){
+ SPATIAL_ID_32_PARSING_BOOL = false;
+ if(CRC_INPUT == calculate_crc16(SPATIAL_32_BUF,24)){
+ HZ_CNT++;
+/*
+ SYSTEM_STATUS = (uint16_t)SPATIAL_32_BUF[0]<<8 | (uint16_t)SPATIAL_32_BUF[1];
+ FILTER_STATUS = (uint16_t)SPATIAL_32_BUF[2]<<8 | (uint16_t)SPATIAL_32_BUF[3];
- if(CRC_INPUT == calculate_crc16(SPATIAL_BUF,100)){
- UNIX_TIME = (uint32_t)SPATIAL_BUF[3]<<24 | (uint32_t)SPATIAL_BUF[4]<<16 | (uint32_t)SPATIAL_BUF[5]<<8 | (uint32_t)SPATIAL_BUF[6];
- UNIX_TIME /= 1000;
+ UNIX_TIME = (uint32_t)SPATIAL_32_BUF[4]<<24 | (uint32_t)SPATIAL_32_BUF[5]<<16 | (uint32_t)SPATIAL_32_BUF[6]<<8 | (uint32_t)SPATIAL_32_BUF[7];
+ UNIX_TIME /= 100000;
+*/
FP8 SPATIAL_LATITUDE; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
- SPATIAL_LATITUDE.DATA8[0] = SPATIAL_BUF[12];
- SPATIAL_LATITUDE.DATA8[1] = SPATIAL_BUF[13];
- SPATIAL_LATITUDE.DATA8[2] = SPATIAL_BUF[14];
- SPATIAL_LATITUDE.DATA8[3] = SPATIAL_BUF[15];
- SPATIAL_LATITUDE.DATA8[4] = SPATIAL_BUF[16];
- SPATIAL_LATITUDE.DATA8[5] = SPATIAL_BUF[17];
- SPATIAL_LATITUDE.DATA8[6] = SPATIAL_BUF[18];
- SPATIAL_LATITUDE.DATA8[7] = SPATIAL_BUF[19]; //얘들을 모아서.
+ SPATIAL_LATITUDE.DATA8[0] = SPATIAL_32_BUF[0];
+ SPATIAL_LATITUDE.DATA8[1] = SPATIAL_32_BUF[1];
+ SPATIAL_LATITUDE.DATA8[2] = SPATIAL_32_BUF[2];
+ SPATIAL_LATITUDE.DATA8[3] = SPATIAL_32_BUF[3];
+ SPATIAL_LATITUDE.DATA8[4] = SPATIAL_32_BUF[4];
+ SPATIAL_LATITUDE.DATA8[5] = SPATIAL_32_BUF[5];
+ SPATIAL_LATITUDE.DATA8[6] = SPATIAL_32_BUF[6];
+ SPATIAL_LATITUDE.DATA8[7] = SPATIAL_32_BUF[7]; //얘들을 모아서.
LATITUDE = SPATIAL_LATITUDE.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침.
FP8 SPATIAL_LONGITUDE; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
- SPATIAL_LONGITUDE.DATA8[0] = SPATIAL_BUF[20];
- SPATIAL_LONGITUDE.DATA8[1] = SPATIAL_BUF[21];
- SPATIAL_LONGITUDE.DATA8[2] = SPATIAL_BUF[22];
- SPATIAL_LONGITUDE.DATA8[3] = SPATIAL_BUF[23];
- SPATIAL_LONGITUDE.DATA8[4] = SPATIAL_BUF[24];
- SPATIAL_LONGITUDE.DATA8[5] = SPATIAL_BUF[25];
- SPATIAL_LONGITUDE.DATA8[6] = SPATIAL_BUF[26];
- SPATIAL_LONGITUDE.DATA8[7] = SPATIAL_BUF[27]; //얘들을 모아서.
+ SPATIAL_LONGITUDE.DATA8[0] = SPATIAL_32_BUF[8];
+ SPATIAL_LONGITUDE.DATA8[1] = SPATIAL_32_BUF[9];
+ SPATIAL_LONGITUDE.DATA8[2] = SPATIAL_32_BUF[10];
+ SPATIAL_LONGITUDE.DATA8[3] = SPATIAL_32_BUF[11];
+ SPATIAL_LONGITUDE.DATA8[4] = SPATIAL_32_BUF[12];
+ SPATIAL_LONGITUDE.DATA8[5] = SPATIAL_32_BUF[13];
+ SPATIAL_LONGITUDE.DATA8[6] = SPATIAL_32_BUF[14];
+ SPATIAL_LONGITUDE.DATA8[7] = SPATIAL_32_BUF[15]; //얘들을 모아서.
LONGITUDE = SPATIAL_LONGITUDE.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침.
FP8 SPATIAL_HEIGHT; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
- SPATIAL_HEIGHT.DATA8[0] = SPATIAL_BUF[28];
- SPATIAL_HEIGHT.DATA8[1] = SPATIAL_BUF[29];
- SPATIAL_HEIGHT.DATA8[2] = SPATIAL_BUF[30];
- SPATIAL_HEIGHT.DATA8[3] = SPATIAL_BUF[31];
- SPATIAL_HEIGHT.DATA8[4] = SPATIAL_BUF[32];
- SPATIAL_HEIGHT.DATA8[5] = SPATIAL_BUF[33];
- SPATIAL_HEIGHT.DATA8[6] = SPATIAL_BUF[34];
- SPATIAL_HEIGHT.DATA8[7] = SPATIAL_BUF[35]; //얘들을 모아서.
- HEIGHT = SPATIAL_HEIGHT.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침.
-
-
- FP4 SPATIAL_Vn; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_Vn.DATA4[0] = SPATIAL_BUF[36];
- SPATIAL_Vn.DATA4[1] = SPATIAL_BUF[37];
- SPATIAL_Vn.DATA4[2] = SPATIAL_BUF[38];
- SPATIAL_Vn.DATA4[3] = SPATIAL_BUF[39];
- Vn = SPATIAL_Vn.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
-
-
- FP4 SPATIAL_Ve; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_Ve.DATA4[0] = SPATIAL_BUF[40];
- SPATIAL_Ve.DATA4[1] = SPATIAL_BUF[41];
- SPATIAL_Ve.DATA4[2] = SPATIAL_BUF[42];
- SPATIAL_Ve.DATA4[3] = SPATIAL_BUF[43];
- Ve = SPATIAL_Ve.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
-
- FP4 SPATIAL_Vd; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_Vd.DATA4[0] = SPATIAL_BUF[44];
- SPATIAL_Vd.DATA4[1] = SPATIAL_BUF[45];
- SPATIAL_Vd.DATA4[2] = SPATIAL_BUF[46];
- SPATIAL_Vd.DATA4[3] = SPATIAL_BUF[47];
- Vd = SPATIAL_Vd.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
-
- Vd = -Vd;
-
+ SPATIAL_HEIGHT.DATA8[0] = SPATIAL_32_BUF[16];
+ SPATIAL_HEIGHT.DATA8[1] = SPATIAL_32_BUF[17];
+ SPATIAL_HEIGHT.DATA8[2] = SPATIAL_32_BUF[18];
+ SPATIAL_HEIGHT.DATA8[3] = SPATIAL_32_BUF[19];
+ SPATIAL_HEIGHT.DATA8[4] = SPATIAL_32_BUF[20];
+ SPATIAL_HEIGHT.DATA8[5] = SPATIAL_32_BUF[21];
+ SPATIAL_HEIGHT.DATA8[6] = SPATIAL_32_BUF[22];
+ SPATIAL_HEIGHT.DATA8[7] = SPATIAL_32_BUF[23]; //얘들을 모아서.
+ HEIGHT = SPATIAL_HEIGHT.F8; //double형으로 짠. 라디안이라 고침.
+
+ }
+ }
+}
+void SPATIAL::SPATIAL_ID_39_Parsing(){
+ if(SPATIAL_ID_39_PARSING_BOOL == true){
+ SPATIAL_ID_39_PARSING_BOOL = false;
+ if(CRC_INPUT == calculate_crc16(SPATIAL_39_BUF,12)){
FP4 SPATIAL_ROLL; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_ROLL.DATA4[0] = SPATIAL_BUF[64];
- SPATIAL_ROLL.DATA4[1] = SPATIAL_BUF[65];
- SPATIAL_ROLL.DATA4[2] = SPATIAL_BUF[66];
- SPATIAL_ROLL.DATA4[3] = SPATIAL_BUF[67];
+ SPATIAL_ROLL.DATA4[0] = SPATIAL_39_BUF[0];
+ SPATIAL_ROLL.DATA4[1] = SPATIAL_39_BUF[1];
+ SPATIAL_ROLL.DATA4[2] = SPATIAL_39_BUF[2];
+ SPATIAL_ROLL.DATA4[3] = SPATIAL_39_BUF[3];
ROLL = SPATIAL_ROLL.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
-
+
FP4 SPATIAL_PITCH; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_PITCH.DATA4[0] = SPATIAL_BUF[68];
- SPATIAL_PITCH.DATA4[1] = SPATIAL_BUF[69];
- SPATIAL_PITCH.DATA4[2] = SPATIAL_BUF[70];
- SPATIAL_PITCH.DATA4[3] = SPATIAL_BUF[71];
+ SPATIAL_PITCH.DATA4[0] = SPATIAL_39_BUF[4];
+ SPATIAL_PITCH.DATA4[1] = SPATIAL_39_BUF[5];
+ SPATIAL_PITCH.DATA4[2] = SPATIAL_39_BUF[6];
+ SPATIAL_PITCH.DATA4[3] = SPATIAL_39_BUF[7];
PITCH = SPATIAL_PITCH.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
FP4 SPATIAL_YAW; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_YAW.DATA4[0] = SPATIAL_BUF[72];
- SPATIAL_YAW.DATA4[1] = SPATIAL_BUF[73];
- SPATIAL_YAW.DATA4[2] = SPATIAL_BUF[74];
- SPATIAL_YAW.DATA4[3] = SPATIAL_BUF[75];
- YAW = SPATIAL_YAW.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
+ SPATIAL_YAW.DATA4[0] = SPATIAL_39_BUF[8];
+ SPATIAL_YAW.DATA4[1] = SPATIAL_39_BUF[9];
+ SPATIAL_YAW.DATA4[2] = SPATIAL_39_BUF[10];
+ SPATIAL_YAW.DATA4[3] = SPATIAL_39_BUF[11];
+ YAW = SPATIAL_YAW.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
+ YAW += DECLINATION_ANGLE;
-
+ if(YAW > 360) YAW -= 360;
+ else if(YAW < 0) YAW += 360;
+ }
+ }
+}
+void SPATIAL::SPATIAL_ID_42_Parsing(){
+ if(SPATIAL_ID_42_PARSING_BOOL == true){
+ SPATIAL_ID_42_PARSING_BOOL = false;
+ if(CRC_INPUT == calculate_crc16(SPATIAL_42_BUF,12)){
FP4 SPATIAL_ROLL_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_ROLL_RATE.DATA4[0] = SPATIAL_BUF[76];
- SPATIAL_ROLL_RATE.DATA4[1] = SPATIAL_BUF[77];
- SPATIAL_ROLL_RATE.DATA4[2] = SPATIAL_BUF[78];
- SPATIAL_ROLL_RATE.DATA4[3] = SPATIAL_BUF[79];
+ SPATIAL_ROLL_RATE.DATA4[0] = SPATIAL_42_BUF[0];
+ SPATIAL_ROLL_RATE.DATA4[1] = SPATIAL_42_BUF[1];
+ SPATIAL_ROLL_RATE.DATA4[2] = SPATIAL_42_BUF[2];
+ SPATIAL_ROLL_RATE.DATA4[3] = SPATIAL_42_BUF[3];
ROLL_RATE = SPATIAL_ROLL_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
FP4 SPATIAL_PITCH_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_PITCH_RATE.DATA4[0] = SPATIAL_BUF[80];
- SPATIAL_PITCH_RATE.DATA4[1] = SPATIAL_BUF[81];
- SPATIAL_PITCH_RATE.DATA4[2] = SPATIAL_BUF[82];
- SPATIAL_PITCH_RATE.DATA4[3] = SPATIAL_BUF[83];
+ SPATIAL_PITCH_RATE.DATA4[0] = SPATIAL_42_BUF[4];
+ SPATIAL_PITCH_RATE.DATA4[1] = SPATIAL_42_BUF[5];
+ SPATIAL_PITCH_RATE.DATA4[2] = SPATIAL_42_BUF[6];
+ SPATIAL_PITCH_RATE.DATA4[3] = SPATIAL_42_BUF[7];
PITCH_RATE = SPATIAL_PITCH_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
FP4 SPATIAL_YAW_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
- SPATIAL_YAW_RATE.DATA4[0] = SPATIAL_BUF[84];
- SPATIAL_YAW_RATE.DATA4[1] = SPATIAL_BUF[85];
- SPATIAL_YAW_RATE.DATA4[2] = SPATIAL_BUF[86];
- SPATIAL_YAW_RATE.DATA4[3] = SPATIAL_BUF[87];
- YAW_RATE = SPATIAL_YAW_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
-
+ SPATIAL_YAW_RATE.DATA4[0] = SPATIAL_42_BUF[8];
+ SPATIAL_YAW_RATE.DATA4[1] = SPATIAL_42_BUF[9];
+ SPATIAL_YAW_RATE.DATA4[2] = SPATIAL_42_BUF[10];
+ SPATIAL_YAW_RATE.DATA4[3] = SPATIAL_42_BUF[11];
+ YAW_RATE = SPATIAL_YAW_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
+ }
+ }
+}
+
+bool SPATIAL::SPATIAL_STABLE(){
+ if(SENSOR_WAIT_BOOL == true){
+ if( (Vx<0.1) & (Vx>-0.1) & (Vy<0.1) & (Vy>-0.1) & (Vz<0.1) & (Vz>-0.1) ){
+ SENSOR_STABLE_CNT ++;
+ }else{
+ SENSOR_STABLE_CNT = 0;
}
-
-
+ if( SENSOR_STABLE_CNT > 2000){
+ SENSOR_STABLE_OK = true;
+ }
+ ///////////////////////////////
+ if(SENSOR_STABLE_OK == true){
+ return true;
+ }else{
+ return false;
+ }
+ }else{
+ return true;
}
}
--- a/SPATIAL/SPATIAL.h Tue Jun 12 01:05:50 2018 +0000
+++ b/SPATIAL/SPATIAL.h Wed Nov 28 13:06:23 2018 +0000
@@ -4,24 +4,39 @@
#include "mbed.h"
-
class SPATIAL {
public:
- void Init();
- void SPATIAL_ID_20_Parsing();
+ void Init(int);
void SPATIAL_RECEIVE();
double LATITUDE, LONGITUDE, HEIGHT;
- float Vn, Ve, Vd, ROLL, PITCH, YAW, ROLL_RATE, PITCH_RATE, YAW_RATE;
+ float Vx, Vy, Vz, ROLL, PITCH, YAW, ROLL_RATE, PITCH_RATE, YAW_RATE;
uint32_t UNIX_TIME;
- bool SPATIAL_ID_20_BOOL, SPATIAL_ID_20_PARSING_BOOL;
+ bool SPATIAL_ID_39_BOOL, SPATIAL_ID_39_PARSING_BOOL;
+ bool SPATIAL_ID_42_BOOL, SPATIAL_ID_42_PARSING_BOOL;
+ bool SPATIAL_ID_35_BOOL, SPATIAL_ID_35_PARSING_BOOL;
+ bool SPATIAL_ID_32_BOOL, SPATIAL_ID_32_PARSING_BOOL;
+ bool SPATIAL_ID_30_BOOL, SPATIAL_ID_30_PARSING_BOOL;
+ bool SPATIAL_ID_22_BOOL, SPATIAL_ID_22_PARSING_BOOL;
+ int HZ_CNT;
int SATELLITE_NUM;
+ uint8_t SYSTEM_STATUS,FILTER_STATUS;
+ bool SPATIAL_STABLE();
+ float DECLINATION_ANGLE;
private:
int SPATIAL_BUF_CNT;
- unsigned char SPATIAL_BUF[100], SPATIAL_ID[5];
+ unsigned char SPATIAL_ID[5];
+ unsigned char SPATIAL_42_BUF[12], SPATIAL_39_BUF[12], SPATIAL_35_BUF[12], SPATIAL_32_BUF[24], SPATIAL_22_BUF[14], SPATIAL_30_BUF[13];
uint16_t CRC_INPUT;
-
-
+ void SPATIAL_ID_39_Parsing();
+ void SPATIAL_ID_42_Parsing();
+ void SPATIAL_ID_35_Parsing();
+ void SPATIAL_ID_32_Parsing();
+ void SPATIAL_ID_30_Parsing();
+ void SPATIAL_ID_22_Parsing();
+ int SENSOR_STABLE_CNT;
+ bool SENSOR_STABLE_OK;
+ bool SENSOR_WAIT_BOOL;
};
--- a/main.cpp Tue Jun 12 01:05:50 2018 +0000
+++ b/main.cpp Wed Nov 28 13:06:23 2018 +0000
@@ -1,217 +1,603 @@
#include "mbed.h"
-
-
#include <math.h>
-
+#include "Function.h"
#include "millis.h"
#include "SPATIAL.h"
#include "ROBOFRIEN_GUI.h"
#include "ROBOFRIEN_LED.h"
#include "ROBOFRIEN_MOTOR.h"
#include "ROBOFRIEN_PID.h"
+#include "ROBOFRIEN_SBUS.h"
+#include "ROBOFRIEN_BAT.h"
+
+
+
+/* ************ USER INTERFACE ************** */
+#define SENSOR_WAIT 1
+#define THROTTLE_MAX 7000
+#define THROTTLE_BASE 4000
+#define THROTTLE_MIN 3000
+#define THROTTLE_CONTROL_LIMIT 1500
+#define LIMIT_PROGRESS_VELOCITY 10
+#define LIMIT_UP_VELOCITY 4
+#define LIMIT_DOWN_VELOCITY 2
+#define LIMIT_PLANE_VEL_INTEG 5 // Degree
+#define LIMIT_VERTICAL_VEL_INTEG 500
+/* ******************************************* */
+
+/* *************************** */
+// AutoFlight Sequence
+// 0. Wait to Start
+// 1. Take Off (10m From Ground)
+// 2. Heading Align to Marker
+// 3. Go to Point ( Pitching & Altitude )
+// 4. Hovering
+// 5. Landing
+#define WAIT_TO_START 1
+#define TAKE_OFF 2
+#define HEADING_ALIGN 3
+#define GO_TO_POINT 4
+#define CHECK_MARKER 5
+#define LANDING 6
+#define WAIT 7
+int ex_AUTOFLIGHT_SEQUENCE;
+int AUTOFLIGHT_SEQUENCE = WAIT_TO_START;
+unsigned long AUTOFLIGHT_TIME_CNT;
+/* *************************** */
ROBOFRIEN_GUI GUI;
ROBOFRIEN_LED LED;
ROBOFRIEN_MOTOR MOTOR;
ROBOFRIEN_PID PID;
SPATIAL SPATIAL;
+ROBOFRIEN_SBUS SBUS;
+ROBOFRIEN_BAT BAT;
void GUI_UPDATE();
+void ALGORITHM();
+void ANGLE_CONTROL(double [3]);
+void PLANE_VEL_CONTROL(double, double, double *, double *);
+void VERTICAL_VEL_CONTROL(double, double *);
+void VEL_INTEG_RESET();
+float DEG_TO_METER(float , float , float , float );
-unsigned long millis_20hz, millis_100hz;
-int main()
+unsigned long millis_100hz, millis_10hz, millis_200hz;
+double want_Angle[3];
+double want_Rate[3];
+double err_Angle[3];
+double THROTTLE_VALUE;
+double want_LNG, want_LAT, want_ALT;
+int main()
{
- GUI.Init();
- LED.Init();
- SPATIAL.Init();
- MOTOR.Init();
- PID.Init();
- wait(1);
- millisStart();
- while(1){
- GUI.pc_rx_update();
- LED.update(GUI.headlight_period, GUI.headlight_dutyrate, GUI.sidelight_period, GUI.sidelight_dutyrate );
- SPATIAL.SPATIAL_RECEIVE();
- SPATIAL.SPATIAL_ID_20_Parsing();
- if( (millis() - millis_100hz ) > 10 ){
- millis_100hz = millis();
- PID.err_Angle[0] = 0; PID.err_Angle[1] = 0; PID.err_Angle[2] = 0;
- float want_Rate[3] = {0,0,0};
- PID.err_Rate[0] = (want_Rate[0] - SPATIAL.ROLL_RATE);
- PID.err_Rate[1] = (want_Rate[1] - SPATIAL.PITCH_RATE);
- PID.err_Rate[2] = (want_Rate[2] - SPATIAL.YAW_RATE);
- PID.update();
- MOTOR.Value[0] = 5000 + PID.output_Data[0]*3000;
- MOTOR.Value[1] = 5000 + PID.output_Data[1]*3000;
- MOTOR.Value[2] = 5000 + PID.output_Data[2]*3000;
- MOTOR.Value[3] = 5000 + PID.err_Rate[0]*100;
- MOTOR.Value[4] = 5000 + 0;
- MOTOR.Value[5] = 5000 + 0;
- MOTOR.update();
+ LED.Init();
+ MOTOR.Init();
+ wait(1);
+ SPATIAL.Init(SENSOR_WAIT);
+ SBUS.Init();
+ PID.Init();
+ GUI.Init();
+ BAT.Init(3); // 3cell
+ millisStart();
+ while (1) {
+ GUI.pc_rx_update();
+ GUI.TRANS_BUFFER();
+ LED.update(GUI.headlight_period, GUI.headlight_dutyrate, GUI.sidelight_period, GUI.sidelight_dutyrate );
+ SPATIAL.SPATIAL_RECEIVE();
+ SBUS.update();
+ if ( ( millis() - millis_200hz ) > 5 ) {
+ millis_200hz = millis();
+ if ( SPATIAL.SPATIAL_STABLE() == false ) {
+ LED.ALARM(1);
+ if (GUI.button[0] == true) {
+ ALGORITHM();
+ MOTOR.update();
+ } else {
+ MOTOR.OFF();
}
- if( (millis() - millis_20hz )>50){
- millis_20hz = millis();
- }
- if(GUI.rx_bool() == true){
- GUI_UPDATE();
- GUI.Refresh();
- }
-
+ } else {
+ LED.ALARM(0);
+ ALGORITHM();
+ MOTOR.update();
+ }
}
+
+ if ( (millis() - millis_10hz ) > 100) {
+ millis_10hz = millis();
+ GUI_UPDATE();
+ GUI.Trans();
+ GUI.SET_DATA();
+ }
+ if (GUI.rx_bool() == true) {
+ GUI.Refresh();
+ }
+ }
}
-void GUI_UPDATE(){
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- //////////////////////////////////////////// [M/S] Mode and State ///////////////////////////////////////
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- GUI.Mode1 = 0;
- GUI.Mode2 = 0;
- GUI.MissionState = 0;
- GUI.CurrentMarker = 0;
- GUI.Bat1 = 0; // 21.6 ~ 24.6 ( 6cell )
- GUI.Bat2 = 0; // 11.1 ~ 12.6 ( 3cell )
- // [ Button State & Home Point Chksum & Marker Chksum ] is change automatically in "ROBOFRIEN_GUI.cpp" //
- // You Can not change this setting, just use it ///
- if(GUI.button[0] == true) {}
- else {}
- if(GUI.button[1] == true) {}
- else {}
- if(GUI.button[2] == true) {}
- else {}
- if(GUI.button[3] == true) {}
- else {}
- if(GUI.button[4] == true) {}
- else {}
-
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- //////////////////////////////////////////////// [GPS] GPS //////////////////////////////////////////////
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- GUI.utc_time = SPATIAL.UNIX_TIME;
- GUI.latitude = (SPATIAL.LATITUDE*1000000);
- GUI.longitude = (SPATIAL.LONGITUDE*1000000);
- GUI.altitude = SPATIAL.HEIGHT*100;
- GUI.SatNum = SPATIAL.SATELLITE_NUM;
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////// AHRS //////////////////////////////////////////////////
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- GUI.rollx100 = (SPATIAL.ROLL * 100);
- GUI.pitchx100 = (SPATIAL.PITCH * 100);
- GUI.yawx100 = ((SPATIAL.YAW )* 100);
- GUI.roll_ratex100 = SPATIAL.ROLL_RATE * 100;
- GUI.pitch_ratex100 = SPATIAL.PITCH_RATE * 100;
- GUI.yaw_ratex100 = SPATIAL.YAW_RATE * 100;
- GUI.VXx100 = SPATIAL.Ve * 100;
- GUI.VYx100 = SPATIAL.Vn * 100;
- GUI.VZx100 = SPATIAL.Vd * 100;
-
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////// [C/P] CAP/PWM ////////////////////////////////////////////
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- GUI.pwm[0] = MOTOR.Value[0]/100; // (0 ~ 10000) --> ( 0 ~ 100 )
- GUI.pwm[1] = MOTOR.Value[1]/100; // (0 ~ 10000) --> ( 0 ~ 100 )
- GUI.pwm[2] = MOTOR.Value[2]/100; // (0 ~ 10000) --> ( 0 ~ 100 )
- GUI.pwm[3] = MOTOR.Value[3]/100; // (0 ~ 10000) --> ( 0 ~ 100 )
- GUI.pwm[4] = MOTOR.Value[4]/100; // (0 ~ 10000) --> ( 0 ~ 100 )
- GUI.pwm[5] = MOTOR.Value[5]/100; // (0 ~ 10000) --> ( 0 ~ 100 )
-
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- /////////////////////////////////////////// [E/D] Extra & Debug /////////////////////////////////////////
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- GUI.DEBUGx100[0] = 0;
- GUI.DEBUGx100[1] = 0;
- GUI.DEBUGx100[2] = 0;
- GUI.DEBUGx100[3] = 0;
- GUI.DEBUGx100[4] = 0;
- GUI.DEBUGx100[5] = 0;
- GUI.DEBUGx100[6] = 0;
- GUI.DEBUGx100[7] = 0;
-
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- /////////////////////////////////////////// Configuration ///////////////////////////////////////////////
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- ///////////////////////////// -----------------DPN 1-------------------- ////////////////////////////////
- // You can set the this value from "Config.h" ( EEPROM_MODEL_TYPE1, EEPROM_MODEL_TYPE2_UP, EEPROM_MODEL_TYPE2_DOWN ) //
-
- ///////////////////////////// -----------------DPN 2-------------------- ////////////////////////////////
- // You can set the this value from "Config.h" ( MODEL_INFO, FIRMWARE_INFO ) //
-
- ///////////////////////////// -----------------DPN 3-------------------- ////////////////////////////////
- /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) ///
- /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value //
-
- ///////////////////////////// -----------------DPN 4-------------------- ////////////////////////////////
- /// You can use the [pwm_info , motor_min[4]] for calibration of motor pwm value //
-
- ///////////////////////////// -----------------DPN 5-------------------- ////////////////////////////////
-/*
- LED.HeadlightPeriod = GUI.headlight_period;
- LED.HeadlightDutyrate = GUI.headlight_dutyrate;
- LED.SidelightPeriod = GUI.sidelight_period;
- LED.SidelightDutyrate = GUI.sidelight_dutyrate;
-*/
- ///////////////////////////// -----------------DPN 6-------------------- ////////////////////////////////
- /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) ///
- /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value //
- GUI.raw_cap[0] = (0 - 150)*2;
- GUI.raw_cap[1] = (0 - 150)*2;
- GUI.raw_cap[2] = (0 - 150)*2;
- GUI.raw_cap[3] = (0 - 150)*2;
- GUI.raw_cap[4] = (0 - 150)*2;
- GUI.raw_cap[5] = (0 - 150)*2;
- GUI.raw_cap[6] = 0;
- GUI.raw_cap[7] = 0;
-
- if(GUI.attitude_configuration_bool == true){
- GUI.attitude_configuration_bool = false;
- // You can calibration of attitude (Roll, Pitch) //
- // GUI.attitude_calibrate(accelData[0]*aRes,accelData[1]*aRes,(accelData[2]*aRes-1));
+
+uint8_t EX_CONTROLLER_STATE;
+bool MOTOR_FORCED_OFF_BOOL = true;
+void ALGORITHM() { // 200 Hz //
+ /* *************************** */
+ /* ******* Want Angle ******** */
+ /* *************************** */
+ switch (SBUS.CONTROLLER_STATE) {
+ case SIGNAL_OFF: {
+ want_Angle[0] = 0; want_Angle[1] = 0; want_Angle[2] = SPATIAL.YAW;
+ THROTTLE_VALUE = 0;
+ } break;
+ case MOTOR_OFF: {
+ MOTOR_FORCED_OFF_BOOL = true;
+ want_Angle[0] = 0; want_Angle[1] = 0; want_Angle[2] = SPATIAL.YAW;
+ THROTTLE_VALUE = 0;
+ } break;
+ case MANUAL_ATTITUDE: {
+ MOTOR_FORCED_OFF_BOOL = false;
+ want_Angle[0] = ((SBUS.channel[0]) / 1000.0) * GUI.limit_rollx100 / 100.0; // Want Angle : Roll
+ want_Angle[1] = ((SBUS.channel[1]) / 1000.0) * GUI.limit_pitchx100 / 100.0; // Want Angle : Pitch
+ if ( (SBUS.channel[3] > 3) | (SBUS.channel[3] < -3) ) want_Angle[2] += ((SBUS.channel[3]) / 2000.0) / 200.0 * (GUI.limit_yaw_ratex100 / 100.0);
+ if ( want_Angle[2] > 360) want_Angle[2] -= 360;
+ else if ( want_Angle[2] < 0) want_Angle[2] += 360;
+ THROTTLE_VALUE = ((SBUS.channel[2] + 1000) / 2000.0 * THROTTLE_MAX);
+ } break;
+ case MANUAL_VELOCITY ... AUTO_FLIGHT: { // MANUAL_VELOCITY(3), MANUAL_POSITION(4), AUTO_FLIGHT(5) //
+ //// [LNG]Longitude Up : East Face, Longitude Down : West Face
+ //// [LAT]Latitude Up : North Face, Latitude Down : South Face
+ if ( EX_CONTROLLER_STATE != SBUS.CONTROLLER_STATE) {
+ want_LAT = SPATIAL.LATITUDE;
+ want_LNG = SPATIAL.LONGITUDE;
+ want_ALT = SPATIAL.HEIGHT;
+ AUTOFLIGHT_TIME_CNT = 0;
+ }
+
+ double want_Vel[3]; // VEL[0] = X-axis, VEL[1] = Y-axis, VEL[2] = Z-axis
+ if ( SBUS.CONTROLLER_STATE == MANUAL_VELOCITY ) {
+ /* ******************************************************* */
+ /* ****************** MANUAL VELOCITY ******************** */
+ /* ******************************************************* */
+ MOTOR_FORCED_OFF_BOOL = false;
+ want_Vel[0] = -(SBUS.channel[1] / 1000.0) * LIMIT_PROGRESS_VELOCITY;
+ want_Vel[1] = (SBUS.channel[0] / 1000.0) * LIMIT_PROGRESS_VELOCITY;
+ if ( SBUS.channel[2] >= 0 ) want_Vel[2] = (SBUS.channel[2] / 1000.0) * LIMIT_UP_VELOCITY;
+ else if ( SBUS.channel[2] < 0 ) want_Vel[2] = (SBUS.channel[2] / 1000.0) * LIMIT_DOWN_VELOCITY;
+ if ( (SBUS.channel[3] > 3) | (SBUS.channel[3] < -3) ) want_Angle[2] += ((SBUS.channel[3]) / 2000.0) / 200.0 * (GUI.limit_yaw_ratex100 / 100.0);
+
+ } else {
+ if ( SBUS.CONTROLLER_STATE == MANUAL_POSITION ) {
+ /* ******************************************************* */
+ /* ****************** MANUAL POSITION ******************** */
+ /* ******************************************************* */
+ MOTOR_FORCED_OFF_BOOL = false;
+ double del_pos[3];
+ del_pos[0] = (-(SBUS.channel[1] / 1000.0) * LIMIT_PROGRESS_VELOCITY); //divided by time
+ del_pos[1] = ((SBUS.channel[0] / 1000.0) * LIMIT_PROGRESS_VELOCITY); //divided by time
+ if (SBUS.channel[2] >= 0) del_pos[2] = ((SBUS.channel[2] / 1000.0) * LIMIT_UP_VELOCITY); //divided by time
+ else if (SBUS.channel[2] < 0) del_pos[2] = ((SBUS.channel[2] / 1000.0) * LIMIT_DOWN_VELOCITY); //divided by time
+
+ double NED_del_pos[3];
+ NED_del_pos[0] = del_pos[0] * cos(SPATIAL.YAW * M_PI / 180.0) - del_pos[1] * sin(SPATIAL.YAW * M_PI / 180.0);
+ NED_del_pos[1] = del_pos[0] * sin(SPATIAL.YAW * M_PI / 180.0) + del_pos[1] * cos(SPATIAL.YAW * M_PI / 180.0);
+
+ if( (SBUS.channel[1] < 5) & (SBUS.channel[1] > -5) ) {}
+ else want_LAT += (NED_del_pos[0] / 200.0) / 110574.0;
+ if( (SBUS.channel[0] < 5) & (SBUS.channel[0] > -5) ) {}
+ else want_LNG += (NED_del_pos[1] / 200.0) / 111320.0;
+ if( (SBUS.channel[2] < 10) & (SBUS.channel[2] > -10) ) {}
+ else want_ALT += (del_pos[2] / 200.0);
+
+ if ( (SBUS.channel[3] > 3) | (SBUS.channel[3] < -3) ) want_Angle[2] += ((SBUS.channel[3]) / 2000.0) / 200.0 * (GUI.limit_yaw_ratex100 / 100.0);
+
+ } else if ( SBUS.CONTROLLER_STATE == AUTO_FLIGHT) {
+ /* ******************************************************* */
+ /* ****************** AUTO FLIGHT ******************** */
+ /* ******************************************************* */
+ if (AUTOFLIGHT_SEQUENCE != ex_AUTOFLIGHT_SEQUENCE) {
+ AUTOFLIGHT_TIME_CNT = 0;
+ }
+ ex_AUTOFLIGHT_SEQUENCE = AUTOFLIGHT_SEQUENCE;
+ AUTOFLIGHT_TIME_CNT ++;
+ if ( AUTOFLIGHT_SEQUENCE == WAIT_TO_START) MOTOR_FORCED_OFF_BOOL = true;
+ else MOTOR_FORCED_OFF_BOOL = false;
+ switch (AUTOFLIGHT_SEQUENCE) {
+ case WAIT_TO_START: {
+ if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) AUTOFLIGHT_SEQUENCE = TAKE_OFF;
+ want_LAT = SPATIAL.LATITUDE;
+ want_LNG = SPATIAL.LONGITUDE;
+ want_ALT = SPATIAL.HEIGHT;
+ want_Angle[2] = SPATIAL.YAW;
+ break;
+ }
+ case TAKE_OFF: { /// Take off [ Ground --> 5m ] in 1m/s
+ want_ALT += (1 / 200.0);
+ if ( AUTOFLIGHT_TIME_CNT > (5 * 200)) {
+ AUTOFLIGHT_SEQUENCE = CHECK_MARKER;
+ GUI.CurrentMarker = 1;
+ }
+ break;
+ }
+ case HEADING_ALIGN: {
+ float BEARING_ANGLE = bearing( SPATIAL.LATITUDE, SPATIAL.LONGITUDE, (float)(GUI.Marker_Lat[GUI.CurrentMarker - 1]) / 1000000.0, (float)(GUI.Marker_Lng[GUI.CurrentMarker - 1]) / 1000000.0);
+ want_Angle[2] = BEARING_ANGLE;
+ if ( ABSOL((BEARING_ANGLE - SPATIAL.YAW)) < 5 ) {
+ if (AUTOFLIGHT_TIME_CNT > (3 * 200) ) AUTOFLIGHT_SEQUENCE = GO_TO_POINT;
+ } else AUTOFLIGHT_TIME_CNT = 0;
+// GUI.DEBUGx100[0] = (BEARING_ANGLE + 100) * 100;
+// GUI.DEBUGx100[1] = (ABSOL((BEARING_ANGLE - SPATIAL.YAW)) + 100) * 100;
+ break;
+ }
+ case GO_TO_POINT: {
+ float Target_LAT = GUI.Marker_Lat[GUI.CurrentMarker - 1] / 1000000.0;
+ float Target_LNG = GUI.Marker_Lng[GUI.CurrentMarker - 1] / 1000000.0;
+ float Target_ALT = GUI.Marker_Alt[GUI.CurrentMarker - 1] / 100.0;
+// float BEARING_ANGLE = bearing( SPATIAL.LATITUDE, SPATIAL.LONGITUDE, (float)(GUI.Marker_Lat[GUI.CurrentMarker - 1]) / 1000000.0, (float)(GUI.Marker_Lng[GUI.CurrentMarker - 1]) / 1000000.0);
+// want_Angle[2] = BEARING_ANGLE;
+ // 1ms ++
+ /* if(Target_LAT > want_LAT) want_LAT += (1.5/200.0)/110574.0;
+ else want_LAT -= (1/200.0)/110574.0;
+ if(Target_LNG > want_LNG) want_LNG += (1.5/200.0)/111320.0;
+ else want_LNG -= (1/200.0)/111320.0;
+ if(Target_ALT > want_ALT) want_ALT += (1/200.0);
+ else want_ALT -= (1/200.0);
+ */
+// want_LAT = Target_LAT;
+// want_LNG = Target_LNG;
+// want_ALT = Target_ALT;
+ if(Target_LAT > want_LAT) want_LAT += (LIMIT_PROGRESS_VELOCITY/200.0)/110574.0;
+ else want_LAT -= (LIMIT_PROGRESS_VELOCITY/200.0)/110574.0;
+ if(Target_LNG > want_LNG) want_LNG += (LIMIT_PROGRESS_VELOCITY/200.0)/111320.0;
+ else want_LNG -= (LIMIT_PROGRESS_VELOCITY/200.0)/111320.0;
+ if(Target_ALT > want_ALT) want_ALT += (LIMIT_UP_VELOCITY/200.0);
+ else want_ALT -= (LIMIT_DOWN_VELOCITY/200.0);
+
+ bool escape_bool = false;
+ float POSITION_DISTANCE;
+ float HEIGHT_DISTANCE;
+ POSITION_DISTANCE = DEG_TO_METER( want_LAT, want_LNG, SPATIAL.LATITUDE, SPATIAL.LONGITUDE );
+ HEIGHT_DISTANCE = sqrt( (want_ALT - SPATIAL.HEIGHT) * (want_ALT - SPATIAL.HEIGHT) );
+ if ( ( POSITION_DISTANCE < 5 ) & (HEIGHT_DISTANCE < 5)) { // 5m
+ if ( (POSITION_DISTANCE < 3) ) { // 3m
+ if ( (POSITION_DISTANCE < 1) ) { // 1m
+ ///////////// Target in the 1m OK !!!! /////////////////
+ escape_bool = true;
+ /////////////////////////////////////////////////
+ }
+ else {
+ if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) escape_bool = true; // Time Out //
+ }
+ }
+ else {
+ if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) escape_bool = true; // Time Out //
+ }
+ }
+ else {
+ AUTOFLIGHT_TIME_CNT = 0;
+ }
+ if ( escape_bool == true) {
+ AUTOFLIGHT_SEQUENCE = CHECK_MARKER;
+ GUI.CurrentMarker ++;
+ }
+ // 1m내외
+ break;
+ }
+ case CHECK_MARKER: {
+ if (AUTOFLIGHT_TIME_CNT > (3 * 200) ) {
+ if ( (GUI.Marker_Lat[GUI.CurrentMarker - 1] != 0) & (GUI.Marker_Lng[GUI.CurrentMarker - 1] != 0) ) {
+ AUTOFLIGHT_SEQUENCE = HEADING_ALIGN;
+ } else {
+ AUTOFLIGHT_SEQUENCE = LANDING;
+ }
+ }
+ break;
+ }
+ case LANDING: {
+ want_ALT -= (0.5 / 200.0); // 0.5m/s
+ break;
+ }
+ case WAIT: {
+ break;
+ }
+ }
+ }
+ double NED_want_vel[3];
+ NED_want_vel[0] = (want_LAT - SPATIAL.LATITUDE) * 110574.0 * (GUI.gain_px100[4] / 100.0);
+ NED_want_vel[1] = (want_LNG - SPATIAL.LONGITUDE) * 111320.0 * (GUI.gain_px100[5] / 100.0);
+ NED_want_vel[2] = (want_ALT - SPATIAL.HEIGHT) * (GUI.gain_px100[6] / 100.0);
+
+ want_Vel[0] = NED_want_vel[0] * cos(SPATIAL.YAW * M_PI / 180.0) + NED_want_vel[1] * sin(SPATIAL.YAW * M_PI / 180.0);
+ want_Vel[1] = NED_want_vel[1] * cos(SPATIAL.YAW * M_PI / 180.0) - NED_want_vel[0] * sin(SPATIAL.YAW * M_PI / 180.0);
+ want_Vel[2] = NED_want_vel[2];
+
+ float LIMIT_PROGRESS_VELOCITY_CALCULATED;
+ float POSITION_DISTANCE = DEG_TO_METER( want_LAT, want_LNG, SPATIAL.LATITUDE, SPATIAL.LONGITUDE );
+ if ( POSITION_DISTANCE < 20) LIMIT_PROGRESS_VELOCITY_CALCULATED = (0.04 * POSITION_DISTANCE + 0.2) * LIMIT_PROGRESS_VELOCITY;
+ else LIMIT_PROGRESS_VELOCITY_CALCULATED = LIMIT_PROGRESS_VELOCITY;
+
+ want_Vel[0] = MIN_MAX(want_Vel[0], -LIMIT_PROGRESS_VELOCITY_CALCULATED, LIMIT_PROGRESS_VELOCITY_CALCULATED);
+ want_Vel[1] = MIN_MAX(want_Vel[1], -LIMIT_PROGRESS_VELOCITY_CALCULATED, LIMIT_PROGRESS_VELOCITY_CALCULATED);
+ if (want_Vel[2] >= 0) want_Vel[2] = MIN_MAX(want_Vel[2], -LIMIT_UP_VELOCITY, LIMIT_UP_VELOCITY);
+ else if (want_Vel[2] < 0) want_Vel[2] = MIN_MAX(want_Vel[2], -LIMIT_DOWN_VELOCITY, LIMIT_DOWN_VELOCITY);
+
+ }
+
+ /* ********** WANT ANGLE GENERATE ************ */
+ PLANE_VEL_CONTROL(want_Vel[0], want_Vel[1], &want_Angle[0], &want_Angle[1]);
+ if ( want_Angle[2] > 360) want_Angle[2] -= 360;
+ else if ( want_Angle[2] < 0) want_Angle[2] += 360;
+
+ /* ********** THROTTLE VALUE GENRATE ************* */
+ VERTICAL_VEL_CONTROL(want_Vel[2], &THROTTLE_VALUE);
+ if (MOTOR_FORCED_OFF_BOOL == true) THROTTLE_VALUE = 0;
+
+ } break;
+ }
+ EX_CONTROLLER_STATE = SBUS.CONTROLLER_STATE;
+ /* ************************** */
+ /* ******* Want Rate ******** */
+ /* ************************** */
+ ANGLE_CONTROL( want_Angle );
+
+ if (GUI.DPN_Info == 0) {
+ if ( SBUS.CONTROLLER_STATE <= MOTOR_OFF) {
+ PID.RAT_INTEGER_RESET();
+ VEL_INTEG_RESET();
+ for (int i = 0; i < 6; i++) MOTOR.Value[i] = 0;
+ } else {
+ int MOTOR_MIN_DEADZONE = 5;
+ for (int i = 0; i < 5; i++) {
+ if (GUI.motor_min[i] > 100)GUI.motor_min[i] = 100;
+ }
+ MOTOR.Value[0] = (GUI.motor_min[0] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE + PID.output_Data[0] / 2.0 - PID.output_Data[1] * 0.87 - PID.output_Data[2];
+ MOTOR.Value[1] = (GUI.motor_min[1] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE + PID.output_Data[0] + 0 + PID.output_Data[2];
+ MOTOR.Value[2] = (GUI.motor_min[2] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE + PID.output_Data[0] / 2.0 + PID.output_Data[1] * 0.87 - PID.output_Data[2];
+ MOTOR.Value[3] = (GUI.motor_min[3] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE - PID.output_Data[0] / 2.0 + PID.output_Data[1] * 0.87 + PID.output_Data[2];
+ MOTOR.Value[4] = (GUI.motor_min[4] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE - PID.output_Data[0] + 0 - PID.output_Data[2];
+ MOTOR.Value[5] = (GUI.motor_min[5] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE - PID.output_Data[0] / 2.0 - PID.output_Data[1] * 0.87 + PID.output_Data[2];
+ for (int i = 0; i < 6; i++) {
+ if ( MOTOR.Value[i] < ((GUI.motor_min[0] - MOTOR_MIN_DEADZONE) * 10 ) ) {
+ MOTOR.Value[i] = ((GUI.motor_min[0] - MOTOR_MIN_DEADZONE) * 10 );
+ }
+ }
}
- if(GUI.Compass_Calibration_Mode == 1){
- // You can calibration of compass (Yaw)//
+ } else if (GUI.DPN_Info == 4) {
+ MOTOR.Value[0] = (float)GUI.pwm_info[0] * 10;
+ MOTOR.Value[1] = (float)GUI.pwm_info[1] * 10;
+ MOTOR.Value[2] = (float)GUI.pwm_info[2] * 10;
+ MOTOR.Value[3] = (float)GUI.pwm_info[3] * 10;
+ MOTOR.Value[4] = (float)GUI.pwm_info[4] * 10;
+ MOTOR.Value[5] = (float)GUI.pwm_info[5] * 10;
+ }
+}
+
+
+void GUI_UPDATE() {
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //////////////////////////////////////////// [M/S] Mode and State ///////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /* ************ MODE 1 ************* */
+ GUI.Mode1 = SBUS.CONTROLLER_STATE;
+ GUI.Alarm = !SPATIAL.SPATIAL_STABLE();
+
+ GUI.MissionState = AUTOFLIGHT_SEQUENCE;
+ GUI.Bat1 = BAT.update();
+ // [ Button State & Home Point Chksum & Marker Chksum ] is change automatically in "ROBOFRIEN_GUI.cpp" //
+ // You Can not change this setting, just use it ///
+ if (GUI.button[0] == true) {}
+ else {}
+ if (GUI.button[1] == true) {
+ AUTOFLIGHT_SEQUENCE = WAIT_TO_START;
+ }
+ else {}
+ if (GUI.button[2] == true) {}
+ else {}
+ if (GUI.button[3] == true) {}
+ else {}
+ if (GUI.button[4] == true) {}
+ else {}
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //////////////////////////////////////////////// [GPS] GPS //////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ GUI.utc_time = SPATIAL.UNIX_TIME;
+ GUI.latitude = (SPATIAL.LATITUDE * 1000000);
+ GUI.longitude = (SPATIAL.LONGITUDE * 1000000);
+ GUI.altitude = SPATIAL.HEIGHT * 100;
+ GUI.SatNum = SPATIAL.SATELLITE_NUM;
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ ///////////////////////////////////////////////// AHRS //////////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ GUI.rollx100 = (SPATIAL.ROLL * 100);
+ GUI.pitchx100 = (SPATIAL.PITCH * 100);
+ GUI.yawx100 = ((SPATIAL.YAW ) * 100);
+ GUI.roll_ratex100 = SPATIAL.ROLL_RATE * 100;
+ GUI.pitch_ratex100 = SPATIAL.PITCH_RATE * 100;
+ GUI.yaw_ratex100 = SPATIAL.YAW_RATE * 100;
+ GUI.VXx100 = SPATIAL.Vx * 100;
+ GUI.VYx100 = SPATIAL.Vy * 100;
+ GUI.VZx100 = SPATIAL.Vz * 100;
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ ////////////////////////////////////////////// [C/P] CAP/PWM ////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ GUI.cap[0] = SBUS.channel[0] / 10.0;
+ GUI.cap[1] = SBUS.channel[1] / 10.0;
+ GUI.cap[2] = SBUS.channel[2] / 10.0;
+ GUI.cap[3] = SBUS.channel[3] / 10.0;
+ GUI.cap[4] = SBUS.channel[4] / 10.0;
+ GUI.cap[5] = SBUS.channel[5] / 10.0;
+ GUI.cap[6] = SBUS.channel[6] / 10.0;
+ GUI.cap[7] = SBUS.channel[7] / 10.0;
+
+ GUI.pwm[0] = MOTOR.Value[0] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
+ GUI.pwm[1] = MOTOR.Value[1] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
+ GUI.pwm[2] = MOTOR.Value[2] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
+ GUI.pwm[3] = MOTOR.Value[3] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
+ GUI.pwm[4] = MOTOR.Value[4] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
+ GUI.pwm[5] = MOTOR.Value[5] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /////////////////////////////////////////// [E/D] Extra & Debug /////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ GUI.DEBUGx100[2] = (want_Angle[2] + 100) * 100;
+ GUI.DEBUGx100[3] = GUI.Controller_Joystick[3];
+ GUI.DEBUGx100[4] = GUI.Gimbal_Joystick[0];
+ GUI.DEBUGx100[5] = GUI.Gimbal_Joystick[1];
+ float PNU_RPM[2];
+ PNU_RPM[0] = sin( millis() / 10000.0 ) * 10000;
+ PNU_RPM[1] = cos( millis() / 10000.0 ) * 10000;
+ GUI.DEBUGx100[6] = PNU_RPM[0];
+ GUI.DEBUGx100[7] = PNU_RPM[1];
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /////////////////////////////////////////// Configuration ///////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ ///////////////////////////// -----------------DPN 1-------------------- ////////////////////////////////
+ // You can set the this value from "Config.h" ( EEPROM_MODEL_TYPE1, EEPROM_MODEL_TYPE2_UP, EEPROM_MODEL_TYPE2_DOWN ) //
+
+ ///////////////////////////// -----------------DPN 2-------------------- ////////////////////////////////
+ // You can set the this value from "Config.h" ( MODEL_INFO, FIRMWARE_INFO ) //
+
+ ///////////////////////////// -----------------DPN 3-------------------- ////////////////////////////////
+ /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) ///
+ /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value //
+
+ ///////////////////////////// -----------------DPN 4-------------------- ////////////////////////////////
+ /// You can use the [pwm_info , motor_min[4]] for calibration of motor pwm value //
+
+ ///////////////////////////// -----------------DPN 5-------------------- ////////////////////////////////
+ /*
+ LED.HeadlightPeriod = GUI.headlight_period;
+ LED.HeadlightDutyrate = GUI.headlight_dutyrate;
+ LED.SidelightPeriod = GUI.sidelight_period;
+ LED.SidelightDutyrate = GUI.sidelight_dutyrate;
+ */
+ ///////////////////////////// -----------------DPN 6-------------------- ////////////////////////////////
+ /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) ///
+ /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value //
+ GUI.raw_cap[0] = (0 - 150) * 2;
+ GUI.raw_cap[1] = (0 - 150) * 2;
+ GUI.raw_cap[2] = (0 - 150) * 2;
+ GUI.raw_cap[3] = (0 - 150) * 2;
+ GUI.raw_cap[4] = (0 - 150) * 2;
+ GUI.raw_cap[5] = (0 - 150) * 2;
+ GUI.raw_cap[6] = 0;
+ GUI.raw_cap[7] = 0;
+
+ if (GUI.attitude_configuration_bool == true) {
+ GUI.attitude_configuration_bool = false;
+ // You can calibration of attitude (Roll, Pitch) //
+ // GUI.attitude_calibrate(accelData[0]*aRes,accelData[1]*aRes,(accelData[2]*aRes-1));
+ }
+ if (GUI.Compass_Calibration_Mode == 1) {
+ // You can calibration of compass (Yaw)//
// if(ex_Compass_Calibration_Mode == 0){
- // magnetic_offset_reset();
+ // magnetic_offset_reset();
// }else{
// //// calibrating ... /////
- // magnetic_calibration();
+ // magnetic_calibration();
// }
- }else if(GUI.Compass_Calibration_Mode == 2){
- // // You can finish the calibration of compass
+ } else if (GUI.Compass_Calibration_Mode == 2) {
+ // // You can finish the calibration of compass
// //// write to eeprom ////
// GUI.Compass_Calibration_Mode = 0;
- // GUI.write_compass_setting_to_eeprom();
- }
- // ex_Compass_Calibration_Mode = GUI.Compass_Calibration_Mode;
-
- ///////////////////////////// -----------------DPN 7-------------------- ////////////////////////////////
- /// You can use the [limit_rollx100, limit_pitchx100, limit_roll_rate, limit_pitch_rate, limit_yaw_rate] for limit the angle and angular velocity //
-
- ///////////////////////////// -----------------DPN 8-------------------- ////////////////////////////////
- /// You can use the [gain_px100[20], gain_dx100[20], gain_ix100[20]] for gain tuning //
-
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
- //////////////////////////////////////////////// Refresh ////////////////////////////////////////////////
- /////////////////////////////////////////////////////////////////////////////////////////////////////////
-// GUI.trans_flight_data(255, 0);
- GUI.pc_rx_update();
+ // GUI.write_compass_setting_to_eeprom();
+ }
+ else if (GUI.Compass_Calibration_Mode == 3) { // declination angle
+ SPATIAL.DECLINATION_ANGLE = GUI.declination_angle;
+ }
+ // ex_Compass_Calibration_Mode = GUI.Compass_Calibration_Mode;
+
+ ///////////////////////////// -----------------DPN 7-------------------- ////////////////////////////////
+ /// You can use the [limit_rollx100, limit_pitchx100, limit_roll_ratex100, limit_pitch_ratex100, limit_yaw_ratex100] for limit the angle and angular velocity //
+
+ ///////////////////////////// -----------------DPN 8-------------------- ////////////////////////////////
+ /// You can use the [gain_px100[20], gain_dx100[20], gain_ix100[20]] for gain tuning //
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //////////////////////////////////////////////// Refresh ////////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // GUI.trans_flight_data(255, 0);
+ GUI.pc_rx_update();
+}
+
+void ANGLE_CONTROL(double IN_WANT_ANGLE[3]) {
+ /* ********* MIN - MAX ************* */
+ IN_WANT_ANGLE[0] = MIN_MAX(IN_WANT_ANGLE[0], -GUI.limit_rollx100 / 100.0, GUI.limit_rollx100 / 100.0 );
+ IN_WANT_ANGLE[1] = MIN_MAX(IN_WANT_ANGLE[1], -GUI.limit_pitchx100 / 100.0, GUI.limit_pitchx100 / 100.0 );
+ err_Angle[0] = IN_WANT_ANGLE[0] - SPATIAL.ROLL;
+ err_Angle[1] = IN_WANT_ANGLE[1] - SPATIAL.PITCH;
+ err_Angle[2] = IN_WANT_ANGLE[2] - SPATIAL.YAW;
+ if (err_Angle[2] > 180) err_Angle[2] -= 360;
+ else if (err_Angle[2] < -180) err_Angle[2] += 360;
+ want_Rate[0] = err_Angle[0] * GUI.gain_px100[0] / 100.0; // Want Rate : Roll Rate
+ want_Rate[1] = err_Angle[1] * GUI.gain_dx100[0] / 100.0; // Want Rate : Pitch Rate
+ want_Rate[2] = err_Angle[2] * GUI.gain_ix100[0] / 100.0; // Want Rate : Yaw Rate
+ want_Rate[0] = MIN_MAX(want_Rate[0], -GUI.limit_roll_ratex100 / 100.0, GUI.limit_roll_ratex100 / 100.0);
+ want_Rate[1] = MIN_MAX(want_Rate[1], -GUI.limit_pitch_ratex100 / 100.0, GUI.limit_pitch_ratex100 / 100.0);
+ want_Rate[2] = MIN_MAX(want_Rate[2], -GUI.limit_yaw_ratex100 / 100.0, GUI.limit_yaw_ratex100 / 100.0);
+ PID.err_Rate[0] = (want_Rate[0] - SPATIAL.ROLL_RATE);
+ PID.err_Rate[1] = (want_Rate[1] - SPATIAL.PITCH_RATE);
+ PID.err_Rate[2] = (want_Rate[2] - SPATIAL.YAW_RATE);
+ PID.RAT_KP[0] = -GUI.gain_px100[1] / 100.0;
+ PID.RAT_KP[1] = -GUI.gain_px100[2] / 100.0;
+ PID.RAT_KP[2] = -GUI.gain_px100[3] / 100.0;
+ PID.RAT_KI[0] = -GUI.gain_ix100[1] / 100.0;
+ PID.RAT_KI[1] = -GUI.gain_ix100[2] / 100.0;
+ PID.RAT_KI[2] = -GUI.gain_ix100[3] / 100.0;
+ PID.update();
}
-/*
-void DATA_OUTPUT(){
- pc.printf("**** DATA ****\r\n");
- pc.printf("UNIX_TIME : %f\r\n", UNIX_TIME);
- pc.printf("LATITUDE : %f\r\n",LATITUDE);
- pc.printf("LONGITUDE : %f\r\n",LONGITUDE);
- pc.printf("HEIGHT : %f\r\n",HEIGHT);
- pc.printf("Roll : %f\r\n",ROLL);
- pc.printf("Pitch : %f\r\n",PITCH);
- pc.printf("YAW : %f\r\n",YAW);
- pc.printf("Roll Rate : %f\r\n",ROLL_RATE);
- pc.printf("Pitch Rate : %f\r\n",PITCH_RATE);
- pc.printf("YAW Rate : %f\r\n",YAW_RATE);
- pc.printf("Vn : %f\r\n",Vn);
- pc.printf("Ve : %f\r\n",Ve);
- pc.printf("Vd : %f\r\n",Vd);
+
+double PLANE_VEL_INTEG[2];
+void PLANE_VEL_CONTROL(double IN_VEL_X, double IN_VEL_Y, double *OUTPUT_WANT_ROLL, double *OUTPUT_WANT_PITCH) {
+ double err_Vel[2];
+ err_Vel[0] = ( IN_VEL_X - SPATIAL.Vx );
+ err_Vel[1] = ( IN_VEL_Y - SPATIAL.Vy );
+ err_Vel[0] = MIN_MAX(err_Vel[0], -LIMIT_PROGRESS_VELOCITY, LIMIT_PROGRESS_VELOCITY);
+ err_Vel[1] = MIN_MAX(err_Vel[1], -LIMIT_PROGRESS_VELOCITY, LIMIT_PROGRESS_VELOCITY);
+ *OUTPUT_WANT_PITCH = -(err_Vel[0] * (GUI.gain_dx100[4] / 100.0));
+ *OUTPUT_WANT_ROLL = +(err_Vel[1] * (GUI.gain_dx100[5] / 100.0));
+
+ /* ******** VEL PID - I **************** */
+ PLANE_VEL_INTEG[0] -= (err_Vel[0]) * (GUI.gain_ix100[4] / 100.0);
+ PLANE_VEL_INTEG[1] += (err_Vel[1]) * (GUI.gain_ix100[5] / 100.0);
+ PLANE_VEL_INTEG[0] = MIN_MAX(PLANE_VEL_INTEG[0], -LIMIT_PLANE_VEL_INTEG, LIMIT_PLANE_VEL_INTEG);
+ PLANE_VEL_INTEG[1] = MIN_MAX(PLANE_VEL_INTEG[1], -LIMIT_PLANE_VEL_INTEG, LIMIT_PLANE_VEL_INTEG);
+ *OUTPUT_WANT_PITCH += PLANE_VEL_INTEG[0];
+ *OUTPUT_WANT_ROLL += PLANE_VEL_INTEG[1];
+
}
-*/
\ No newline at end of file
+
+double VERTICAL_VEL_INTEG;
+void VERTICAL_VEL_CONTROL(double IN_VEL_Z, double *OUTPUT_THROTTLE_VALUE) {
+ double err_Vel_Z;
+ err_Vel_Z = IN_VEL_Z - SPATIAL.Vz;
+ if (err_Vel_Z >= 0) err_Vel_Z = MIN_MAX(err_Vel_Z, -LIMIT_UP_VELOCITY, LIMIT_UP_VELOCITY);
+ else err_Vel_Z = MIN_MAX(err_Vel_Z, -LIMIT_DOWN_VELOCITY, LIMIT_DOWN_VELOCITY);
+ *OUTPUT_THROTTLE_VALUE = (err_Vel_Z * 10) * (GUI.gain_dx100[6] / 100.0);
+
+ VERTICAL_VEL_INTEG += (err_Vel_Z) * (GUI.gain_ix100[6] / 100.0);
+ VERTICAL_VEL_INTEG = MIN_MAX(VERTICAL_VEL_INTEG, -LIMIT_VERTICAL_VEL_INTEG, LIMIT_VERTICAL_VEL_INTEG);
+ *OUTPUT_THROTTLE_VALUE += VERTICAL_VEL_INTEG;
+
+ *OUTPUT_THROTTLE_VALUE = MIN_MAX(*OUTPUT_THROTTLE_VALUE, -THROTTLE_CONTROL_LIMIT, THROTTLE_CONTROL_LIMIT);
+
+ *OUTPUT_THROTTLE_VALUE += THROTTLE_BASE;
+
+ *OUTPUT_THROTTLE_VALUE = MIN_MAX( *OUTPUT_THROTTLE_VALUE, THROTTLE_MIN, THROTTLE_MAX );
+
+}
+
+void VEL_INTEG_RESET() {
+ PLANE_VEL_INTEG[0] = 0; PLANE_VEL_INTEG[1] = 0; VERTICAL_VEL_INTEG = 0;
+}
+
+float DEG_TO_METER(float lat1, float lon1, float lat2, float lon2) {
+ float R = 6378.137; // Radius of earth in KM
+ float dLat = lat2 * M_PI / 180.0 - lat1 * M_PI / 180.0;
+ float dLon = lon2 * M_PI / 180.0 - lon1 * M_PI / 180.0;
+ float a = sin(dLat / 2.0) * sin(dLat / 2.0) + cos(lat1 * M_PI / 180.0) * cos(lat2 * M_PI / 180.0) * sin(dLon / 2.0) * sin(dLon / 2.0);
+ float c = 2 * atan2(sqrt(a), sqrt(1 - a));
+ float d = R * c;
+ return d * 1000; // meters
+}
--- a/mbed.bld Tue Jun 12 01:05:50 2018 +0000 +++ b/mbed.bld Wed Nov 28 13:06:23 2018 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee \ No newline at end of file +http://mbed.org/users/mbed_official/code/mbed/builds/c0f6e94411f5 \ No newline at end of file